some mlat changes and update to work with the "real" tags interface
This commit is contained in:
Nick Foster
parent
6beb78fcf3
commit
90fbf70c5e
@ -28,6 +28,7 @@
|
||||
#include <gr_io_signature.h>
|
||||
#include <string.h>
|
||||
#include <iostream>
|
||||
#include <gr_tag_info.h>
|
||||
|
||||
air_modes_preamble_sptr air_make_modes_preamble(int channel_rate, float threshold_db)
|
||||
{
|
||||
@ -45,7 +46,7 @@ air_modes_preamble::air_modes_preamble(int channel_rate, float threshold_db) :
|
||||
d_check_width = 120 * d_samples_per_symbol; //only search to this far from the end of the stream buffer
|
||||
d_threshold_db = threshold_db;
|
||||
d_threshold = powf(10., threshold_db/10.); //the level that the sample must be above the moving average in order to qualify as a pulse
|
||||
|
||||
d_secs_per_sample = 1.0 / channel_rate;
|
||||
set_output_multiple(1+d_check_width*2);
|
||||
|
||||
std::stringstream str;
|
||||
@ -77,6 +78,18 @@ static double correlate_preamble(const float *in, int samples_per_chip) {
|
||||
return corr;
|
||||
}
|
||||
|
||||
|
||||
static double pmt_to_timestamp(pmt::pmt_t tstamp, uint64_t abs_sample_cnt, double secs_per_sample) {
|
||||
uint64_t ts_sample, ticks;
|
||||
|
||||
if(pmt::pmt_symbol_to_string(gr_tags::get_key(tstamp)) != "timestamp") return 0;
|
||||
|
||||
ticks = pmt_to_uint64(gr_tags::get_value(tstamp));
|
||||
ts_sample = gr_tags::get_nitems(tstamp);
|
||||
//std::cout << "HEY WE GOT A STAMP AT " << ticks << " TICKS AT SAMPLE " << ts_sample << " ABS SAMPLE CNT IS " << abs_sample_cnt << std::endl;
|
||||
return double(ticks+abs_sample_cnt) * secs_per_sample;
|
||||
}
|
||||
|
||||
int air_modes_preamble::general_work(int noutput_items,
|
||||
gr_vector_int &ninput_items,
|
||||
gr_vector_const_void_star &input_items,
|
||||
@ -94,7 +107,13 @@ int air_modes_preamble::general_work(int noutput_items,
|
||||
int(9 * d_samples_per_chip)
|
||||
};
|
||||
|
||||
uint64_t abs_out_sample_cnt = nitems_written(0);
|
||||
uint64_t abs_sample_cnt = nitems_read(0);
|
||||
std::vector<pmt::pmt_t> tstamp_tags;
|
||||
get_tags_in_range(tstamp_tags, 0, abs_sample_cnt, abs_sample_cnt + ninputs, pmt::pmt_string_to_symbol("timestamp"));
|
||||
//tags.back() is the most recent timestamp, then.
|
||||
if(tstamp_tags.size() > 0) {
|
||||
d_timestamp = tstamp_tags.back();
|
||||
}
|
||||
|
||||
for(int i=0; i < ninputs; i++) {
|
||||
float pulse_threshold = inavg[i] * d_threshold;
|
||||
@ -148,13 +167,21 @@ int air_modes_preamble::general_work(int noutput_items,
|
||||
integrate_and_dump(out, &in[i], 240, d_samples_per_chip);
|
||||
}
|
||||
|
||||
//get the timestamp of the preamble
|
||||
double tstamp = 0;
|
||||
|
||||
if(d_timestamp) {
|
||||
tstamp = pmt_to_timestamp(d_timestamp, abs_sample_cnt + i, d_secs_per_sample);
|
||||
}
|
||||
|
||||
//now tag the preamble
|
||||
add_item_tag(0, //stream ID
|
||||
nitems_written(0), //sample
|
||||
d_key, //frame_info
|
||||
pmt::pmt_from_double((double) space_threshold),
|
||||
pmt::pmt_from_double(tstamp),
|
||||
d_me //block src id
|
||||
);
|
||||
|
||||
//std::cout << "PREAMBLE" << std::endl;
|
||||
|
||||
//produce only one output per work call
|
||||
|
||||
@ -48,6 +48,8 @@ private:
|
||||
float d_threshold_db;
|
||||
float d_threshold;
|
||||
pmt::pmt_t d_me, d_key;
|
||||
pmt::pmt_t d_timestamp;
|
||||
double d_secs_per_sample;
|
||||
|
||||
public:
|
||||
int general_work (int noutput_items,
|
||||
|
||||
@ -55,9 +55,8 @@ air_modes_slicer::air_modes_slicer(int channel_rate, gr_msg_queue_sptr queue) :
|
||||
d_samples_per_symbol = d_samples_per_chip * 2;
|
||||
d_check_width = 120 * d_samples_per_symbol; //how far you will have to look ahead
|
||||
d_queue = queue;
|
||||
d_secs_per_sample = 1.0 / d_chip_rate;
|
||||
|
||||
set_output_multiple(1+d_check_width * 2); //how do you specify buffer size for sinks?
|
||||
set_output_multiple(1+d_check_width); //how do you specify buffer size for sinks?
|
||||
}
|
||||
|
||||
//FIXME i'm sure this exists in gr
|
||||
@ -106,21 +105,6 @@ static slice_result_t slicer(const float bit0, const float bit1, const float ref
|
||||
return result;
|
||||
}
|
||||
|
||||
/*
|
||||
static double pmt_to_timestamp(pmt::pmt_t tstamp, uint64_t sample_cnt, double secs_per_sample) {
|
||||
double frac;
|
||||
uint64_t secs, sample, sample_age;
|
||||
|
||||
if(gr_tags::get_name(tstamp) != "time") return 0;
|
||||
|
||||
secs = pmt_to_uint64(pmt_tuple_ref(gr_tags::get_value(tstamp), 0));
|
||||
frac = pmt_to_double(pmt_tuple_ref(gr_tags::get_value(tstamp), 1));
|
||||
sample = gr_tags::get_nitems(d_timestamp);
|
||||
//now we have to offset the timestamp based on the current sample number
|
||||
sample_age = (sample_cnt + i) - sample;
|
||||
return sample_age * secs_per_sample + frac + secs;
|
||||
}
|
||||
*/
|
||||
int air_modes_slicer::work(int noutput_items,
|
||||
gr_vector_const_void_star &input_items,
|
||||
gr_vector_void_star &output_items)
|
||||
@ -128,8 +112,6 @@ int air_modes_slicer::work(int noutput_items,
|
||||
const float *in = (const float *) input_items[0];
|
||||
int size = noutput_items - d_check_width; //since it's a sync block, i assume that it runs with ninput_items = noutput_items
|
||||
|
||||
int i;
|
||||
|
||||
std::vector<pmt::pmt_t> tags;
|
||||
uint64_t abs_sample_cnt = nitems_read(0);
|
||||
get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + size, pmt::pmt_string_to_symbol("preamble_found"));
|
||||
@ -181,23 +163,7 @@ int air_modes_slicer::work(int noutput_items,
|
||||
}
|
||||
|
||||
/******************** BEGIN TIMESTAMP BS ******************/
|
||||
rx_packet.timestamp = 0;
|
||||
/*
|
||||
uint64_t abs_sample_cnt = nitems_read(0);
|
||||
std::vector<pmt::pmt_t> tags;
|
||||
uint64_t timestamp_secs, timestamp_sample, timestamp_delta;
|
||||
double timestamp_frac;
|
||||
|
||||
get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + i, pmt::pmt_string_to_symbol("time"));
|
||||
//tags.back() is the most recent timestamp, then.
|
||||
if(tags.size() > 0) {
|
||||
d_timestamp = tags.back();
|
||||
}
|
||||
|
||||
if(d_timestamp) {
|
||||
rx_packet.timestamp = pmt_to_timestamp(d_timestamp, abs_sample_cnt + i, d_secs_per_sample);
|
||||
}
|
||||
*/
|
||||
rx_packet.timestamp = pmt_to_double(gr_tags::get_value(*tag_iter));
|
||||
/******************* END TIMESTAMP BS *********************/
|
||||
|
||||
//increment for the next round
|
||||
|
||||
@ -45,10 +45,8 @@ private:
|
||||
int d_chip_rate;
|
||||
int d_samples_per_chip;
|
||||
int d_samples_per_symbol;
|
||||
double d_secs_per_sample;
|
||||
gr_msg_queue_sptr d_queue;
|
||||
std::ostringstream d_payload;
|
||||
pmt::pmt_t d_timestamp;
|
||||
|
||||
public:
|
||||
int work (int noutput_items,
|
||||
|
||||
@ -3,7 +3,7 @@ import mlat
|
||||
import numpy
|
||||
|
||||
#here's some test data to validate the algorithm
|
||||
teststations = [[37.76225, -122.44254, 100], [37.409044, -122.077748, 100], [37.63816,-122.378082, 100], [37.701207,-122.309418, 100]]
|
||||
teststations = [[37.76225, -122.44254, 100], [37.680016,-121.772461, 100], [37.385844,-122.083082, 100], [37.701207,-122.309418, 100]]
|
||||
testalt = 8000
|
||||
testplane = numpy.array(mlat.llh2ecef([37.617175,-122.400843, testalt]))
|
||||
testme = mlat.llh2geoid(teststations[0])
|
||||
|
||||
@ -13,62 +13,6 @@ from scipy.ndimage import map_coordinates
|
||||
#NB: because of the way this solver works, at least 3 stations and timestamps
|
||||
#are required. this function will not return hyperbolae for underconstrained systems.
|
||||
#TODO: get HDOP out of this so we can draw circles of likely position and indicate constraint
|
||||
|
||||
##########################NOTES#########################################
|
||||
#you should test your solver with a reference dataset that you've calculated. let's put one together.
|
||||
#let's say we have you here in SF, ER in MV, and some place in Fremont. the plane can be at SFO at 24000'.
|
||||
#your pos: 37.76225, -122.44254, 300'
|
||||
#ettus: 37.409044, -122.077748, 300'
|
||||
#fremont: 37.585085, -121.986395, 300'
|
||||
#airplane: 37.617175,-122.380843, 24000'
|
||||
|
||||
#calculated using a third-party tool and verified against the algorithms in this file:
|
||||
#you: -2708399, -4260759, 3884677
|
||||
#ettus: -2693916, -4298177, 3853611
|
||||
#fremont: -2680759, -4292379, 3869113
|
||||
#airplane: -2712433, -4277271, 3876757
|
||||
|
||||
#here's how I did it in Octave...
|
||||
#prange_est = ((stations(:, 1) - xguess(1)).^2 + (stations(:, 2) - xguess(2)).^2 + (stations(:,3) - xguess(3)).^2).^0.5;
|
||||
#dphat = prange_obs - prange_est;
|
||||
#H = [[-(stations(:,1)-xguess(1)) ./ prange_est, -(stations(:,2)-xguess(2)) ./ prange_est, -(stations(:,3)-xguess(3)) ./ prange_est]];
|
||||
#xerr = (H'*H\H'*dphat)';
|
||||
#xguess = xguess + xerr
|
||||
#remember the last line of stations is the 0 vector. remember the last line of prange_obs is the height of the a/c above geoid.
|
||||
|
||||
#use one of the station positions as a starting guess. calculate alt-above-geoid once as though it were located directly above the starting guess; 300 miles
|
||||
#is like 4 degrees of latitude and i don't think it'll introduce more than a few meters of error.
|
||||
|
||||
#well, there are some places where 4 degrees of latitude would count for 20 meters of error. but since our accuracy is really +/- 250m anyway it's probably not
|
||||
#a big deal. saves a lot of CPU to only do the lookup once.
|
||||
|
||||
#so, the above solver works for pseudorange, but what if you only have time-of-flight info?
|
||||
#let's modify the prange eq to deal with time difference of arrival instead of pseudorange
|
||||
#knowns: time of arrival at each station, positions of each station, altitude of a/c
|
||||
#from this, you can say "it arrived x ns sooner at each of the other stations"
|
||||
#thus you can say "it's this much closer/farther to station y than to me"
|
||||
|
||||
#the stations vector is RELATIVE TO YOU -- so it's [ettus-me; fremont-me; [0,0,0]-me]
|
||||
#prange_obs is a vector of TDOAs; the first value (earliest) is truncated since the difference is zero (just like stations)
|
||||
#this implies the TDOAs should arrive sorted, which is probably a good idea, or at the very least the closest station should be first
|
||||
|
||||
#prange_est = [norm(stations(1,:)-xguess);
|
||||
# norm(stations(2,:)-xguess);
|
||||
# norm(stations(3,:)-xguess)]; #only valid for the three-station case we're testing Octave with
|
||||
#dphat = prange_obs - prange_est;
|
||||
#H = [[-(stations(:,1)-xguess(1)) ./ prange_est, -(stations(:,2)-xguess(2)) ./ prange_est, -(stations(:,3)-xguess(3)) ./ prange_est]];
|
||||
#xguess += (H'*H\H'*dphat)';
|
||||
#err=norm(airplane-(xguess+me)) #just for calculating convergence
|
||||
|
||||
#it converges for 500km position error in the initial guess, so it seems pretty good.
|
||||
#seems to converge quickly in the terminal phase. 250m timing offset gives 450m position error
|
||||
#250m time offset in the local receiver (830ns) gives 325m position error
|
||||
|
||||
#the last question is how to use the altitude data in prange_obs; calculate height above geoid for YOUR position at a/c alt and use that to get height above [0,0,0]
|
||||
#this will be close enough. you could iterate this along with the rest of it but it won't change more than the variation in geoid height between you and a/c.
|
||||
#maybe after convergence you can iterate a few more times with the a/c geoid height if you're worried about it.
|
||||
#there's probably a way to use YOU as the alt. station and just use height above YOU instead. but this works.
|
||||
|
||||
########################END NOTES#######################################
|
||||
|
||||
|
||||
@ -216,8 +160,8 @@ def mlat(replies, altitude):
|
||||
prange_obs = numpy.array(prange_obs)
|
||||
|
||||
#xguess = llh2ecef([37.617175,-122.400843, 8000])-numpy.array(me)
|
||||
xguess = [0,0,0]
|
||||
#xguess = numpy.array(llh2ecef([stations[2][0], stations[2][1], altitude])) - numpy.array(me)
|
||||
#xguess = [0,0,0]
|
||||
xguess = numpy.array(llh2ecef([me_llh[0], me_llh[1], altitude])) - numpy.array(me)
|
||||
|
||||
xyzpos = mlat_iter(rel_stations, prange_obs, xguess)
|
||||
llhpos = ecef2llh(xyzpos+me)
|
||||
@ -228,7 +172,6 @@ def mlat(replies, altitude):
|
||||
#so now we solve AGAIN, but this time with the corrected pseudorange of the aircraft altitude
|
||||
#this might not be really useful in practice but the sim shows >50m errors without it
|
||||
prange_obs[-1] = [numpy.linalg.norm(llh2ecef((llhpos[0], llhpos[1], altitude)))]
|
||||
|
||||
xyzpos_corr = mlat_iter(rel_stations, prange_obs, xyzpos) #start off with a really close guess
|
||||
llhpos = ecef2llh(xyzpos_corr+me)
|
||||
|
||||
|
||||
@ -61,7 +61,7 @@ class modes_output_print(modes_parse.modes_parse):
|
||||
refdb = 10.0*math.log10(reference)
|
||||
|
||||
if output is not None:
|
||||
output = "(%.0f %f) " % (refdb, timestamp) + output
|
||||
output = "(%.0f %.10f) " % (refdb, timestamp) + output
|
||||
print output
|
||||
|
||||
def print0(self, shortdata, parity, ecc):
|
||||
|
||||
Loading…
Reference in New Issue
Block a user