some mlat changes and update to work with the "real" tags interface

src/lib/air_modes_preamble.cc

@@ -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

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,

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<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"));
 	std::vector<pmt::pmt_t>::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;
+		rx_packet.timestamp = pmt_to_double(gr_tags::get_value(*tag_iter));
-		/*
-		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);
-		}
-		*/
 		/******************* END TIMESTAMP BS *********************/
 			
 		//increment for the next round

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,

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])

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 = [0,0,0]
-    #xguess = numpy.array(llh2ecef([stations[2][0], stations[2][1], altitude])) - numpy.array(me)
+    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)
 

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):