From 90fbf70c5e1125f5779faba47a64e98d6242ae4a Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Wed, 24 Aug 2011 18:19:57 -0700 Subject: [PATCH] some mlat changes and update to work with the "real" tags interface --- src/lib/air_modes_preamble.cc | 33 +++++++++++++++++-- src/lib/air_modes_preamble.h | 2 ++ src/lib/air_modes_slicer.cc | 40 ++--------------------- src/lib/air_modes_slicer.h | 2 -- src/python/mlat-test.py | 2 +- src/python/mlat.py | 61 ++--------------------------------- src/python/modes_print.py | 2 +- 7 files changed, 39 insertions(+), 103 deletions(-) diff --git a/src/lib/air_modes_preamble.cc b/src/lib/air_modes_preamble.cc index 591a4c4..b07d76f 100644 --- a/src/lib/air_modes_preamble.cc +++ b/src/lib/air_modes_preamble.cc @@ -28,6 +28,7 @@ #include #include #include +#include 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 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 diff --git a/src/lib/air_modes_preamble.h b/src/lib/air_modes_preamble.h index 31780d6..03d9bbe 100644 --- a/src/lib/air_modes_preamble.h +++ b/src/lib/air_modes_preamble.h @@ -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, diff --git a/src/lib/air_modes_slicer.cc b/src/lib/air_modes_slicer.cc index 5ace9fe..6ee3c1c 100644 --- a/src/lib/air_modes_slicer.cc +++ b/src/lib/air_modes_slicer.cc @@ -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,35 +105,18 @@ 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) { 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 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")); std::vector::iterator tag_iter; - + for(tag_iter = tags.begin(); tag_iter != tags.end(); tag_iter++) { uint64_t i = gr_tags::get_nitems(*tag_iter) - abs_sample_cnt; modes_packet rx_packet; @@ -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 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 diff --git a/src/lib/air_modes_slicer.h b/src/lib/air_modes_slicer.h index 636fc72..5fe1da3 100644 --- a/src/lib/air_modes_slicer.h +++ b/src/lib/air_modes_slicer.h @@ -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, diff --git a/src/python/mlat-test.py b/src/python/mlat-test.py index ffbc274..67a53d0 100755 --- a/src/python/mlat-test.py +++ b/src/python/mlat-test.py @@ -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]) diff --git a/src/python/mlat.py b/src/python/mlat.py index 6c61e9d..d6b3ebd 100755 --- a/src/python/mlat.py +++ b/src/python/mlat.py @@ -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) diff --git a/src/python/modes_print.py b/src/python/modes_print.py index 4d60ed5..1448c29 100644 --- a/src/python/modes_print.py +++ b/src/python/modes_print.py @@ -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):