From eb18bdc308c09872be72a00db3e878893bfb9eb2 Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Thu, 18 Nov 2010 14:25:07 -0800 Subject: [PATCH 01/11] First stab at tagged timestamps using TR's tagging branch. Currently segfaults due to some oddness in the tag subsystem. --- src/lib/air_modes_slicer.cc | 54 +++++++++++++++++++++++++++++++++++-- src/lib/air_modes_types.h | 2 ++ src/python/modes_print.py | 8 +++--- src/python/uhd_modes.py | 52 ++++++++++++++++++----------------- 4 files changed, 87 insertions(+), 29 deletions(-) diff --git a/src/lib/air_modes_slicer.cc b/src/lib/air_modes_slicer.cc index a2b150c..942869e 100644 --- a/src/lib/air_modes_slicer.cc +++ b/src/lib/air_modes_slicer.cc @@ -31,6 +31,7 @@ #include #include #include +#include extern "C" { @@ -58,6 +59,14 @@ air_modes_slicer::air_modes_slicer(int channel_rate, gr_msg_queue_sptr queue) : set_output_multiple(1+d_check_width * 2); //how do you specify buffer size for sinks? } +static bool pmtcompare(pmt::pmt_t x, pmt::pmt_t y) +{ + uint64_t t_x, t_y; + t_x = pmt::pmt_to_uint64(pmt::pmt_tuple_ref(x, 0)); + t_y = pmt::pmt_to_uint64(pmt::pmt_tuple_ref(y, 0)); + return t_x < t_y; +} + int air_modes_slicer::work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) @@ -146,8 +155,48 @@ int air_modes_slicer::work(int noutput_items, } else { if(rx_packet.numlowconf < 24) rx_packet.lowconfbits[rx_packet.numlowconf++] = j; } - } + + /******************** BEGIN TIMESTAMP BS ******************/ + uint64_t abs_sample_cnt = nitems_read(0); + std::vector tags; + + //printf("nitems_read: %i", abs_sample_cnt); + pmt::pmt_t timestamp = pmt::mp(pmt::mp(0), pmt::mp(0)); //so we don't barf if there isn't one + + get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + i); + //tags.back() is the most recent timestamp, then. + if(tags.size() > 0) { + std::sort(tags.begin(), tags.end(), pmtcompare); + + do { + timestamp = tags.back(); + tags.pop_back(); + } while(!pmt::pmt_eqv(gr_tags::get_key(timestamp), pmt::pmt_string_to_symbol("time"))); //only interested in timestamps + + uint64_t timestamp_secs = pmt_to_uint64(pmt_tuple_ref(timestamp, 0)); + double timestamp_frac = pmt_to_double(pmt_tuple_ref(timestamp, 1)); + uint64_t timestamp_sample = gr_tags::get_nitems(timestamp); + //now we have to offset the timestamp based on the current sample number + uint64_t timestamp_delta = (abs_sample_cnt + i) - timestamp_sample; + + timestamp_frac += timestamp_delta * (1.0 / (d_samples_per_chip * d_chip_rate)); + if(timestamp_frac > 1.0) { + timestamp_frac -= 1.0; + timestamp_secs++; + } + + rx_packet.timestamp_secs = timestamp_secs; + rx_packet.timestamp_frac = timestamp_frac; + } + else { + rx_packet.timestamp_secs = 0; + rx_packet.timestamp_frac = 0; + } + + /******************* END TIMESTAMP BS *********************/ + + //increment for the next round i += packet_length * d_samples_per_symbol; //here you might want to traverse the whole packet and if you find all 0's, just toss it. don't know why these packets turn up, but they pass ECC. @@ -237,7 +286,8 @@ int air_modes_slicer::work(int noutput_items, } } - d_payload << " " << std::setw(6) << rx_packet.parity << " " << std::dec << rx_packet.reference_level; + d_payload << " " << std::setw(6) << rx_packet.parity << " " << std::dec << rx_packet.reference_level + << " " << rx_packet.timestamp_secs << " " << rx_packet.timestamp_frac; gr_message_sptr msg = gr_make_message_from_string(std::string(d_payload.str())); d_queue->handle(msg); diff --git a/src/lib/air_modes_types.h b/src/lib/air_modes_types.h index bc12bcd..bc8af53 100644 --- a/src/lib/air_modes_types.h +++ b/src/lib/air_modes_types.h @@ -36,6 +36,8 @@ struct modes_packet { framer_packet_type type; //what length packet are we unsigned int message_type; float reference_level; + unsigned long timestamp_secs; //timestamp from tags + double timestamp_frac; }; #endif diff --git a/src/python/modes_print.py b/src/python/modes_print.py index c1af26f..3d3d6ea 100644 --- a/src/python/modes_print.py +++ b/src/python/modes_print.py @@ -30,7 +30,9 @@ class modes_output_print(modes_parse.modes_parse): modes_parse.modes_parse.__init__(self, mypos) def parse(self, message): - [msgtype, shortdata, longdata, parity, ecc, reference] = message.split() + [msgtype, shortdata, longdata, parity, ecc, reference, time_secs, time_frac] = message.split() + time_secs = long(time_secs) + time_frac = float(time_frac) shortdata = long(shortdata, 16) longdata = long(longdata, 16) @@ -53,7 +55,7 @@ class modes_output_print(modes_parse.modes_parse): elif msgtype == 17: output = self.print17(shortdata, longdata, parity, ecc) else: - output = "No handler for message type " + str(msgtype) + " from " + str(ecc) + output = "No handler for message type " + str(msgtype) + " from %x" % ecc if reference == 0.0: refdb = 0.0 @@ -61,7 +63,7 @@ class modes_output_print(modes_parse.modes_parse): refdb = 10.0*math.log10(reference) if output is not None: - output = "(%.0f) " % (refdb) + output + output = "(%.0f %u %f) " % (refdb, time_secs, time_frac) + output print output def print0(self, shortdata, parity, ecc): diff --git a/src/python/uhd_modes.py b/src/python/uhd_modes.py index 6c16c32..f569a56 100755 --- a/src/python/uhd_modes.py +++ b/src/python/uhd_modes.py @@ -54,11 +54,11 @@ class adsb_rx_block (gr.top_block): self.args = args if options.filename is None: - self.u = uhd.simple_source("", uhd.io_type_t.COMPLEX_FLOAT32) + self.u = uhd.single_usrp_source("", uhd.io_type_t.COMPLEX_FLOAT32) - if(options.rx_subdev_spec is None): - options.rx_subdev_spec = "" - self.u.set_subdev_spec(options.rx_subdev_spec) + #if(options.rx_subdev_spec is None): + # options.rx_subdev_spec = "" + #self.u.set_subdev_spec(options.rx_subdev_spec) rate = options.rate self.u.set_samp_rate(rate) @@ -66,7 +66,7 @@ class adsb_rx_block (gr.top_block): if options.gain is None: #set to halfway g = self.u.get_gain_range() - options.gain = (g.min+g.max) / 2.0 + options.gain = (g.start()+g.stop()) / 2.0 if not(self.tune(options.freq)): print "Failed to set initial frequency" @@ -89,34 +89,38 @@ class adsb_rx_block (gr.top_block): #the DBSRX especially tends to be spur-prone; the LPF keeps out the #spur multiple that shows up at 2MHz - self.filtcoeffs = gr.firdes.low_pass(1, rate, 1.8e6, 200e3) - self.filter = gr.fir_filter_fff(1, self.filtcoeffs) +# self.filtcoeffs = gr.firdes.low_pass(1, rate, 1.8e6, 200e3) +# self.filter = gr.fir_filter_fff(1, self.filtcoeffs) self.preamble = air.modes_preamble(rate, options.threshold) self.framer = air.modes_framer(rate) self.slicer = air.modes_slicer(rate, queue) - self.connect(self.u, self.demod, self.filter) - self.connect(self.filter, self.avg) - self.connect(self.filter, (self.preamble, 0)) - self.connect(self.avg, (self.preamble, 1)) - self.connect(self.filter, (self.framer, 0)) - self.connect(self.preamble, (self.framer, 1)) - self.connect(self.filter, (self.slicer, 0)) - self.connect(self.framer, (self.slicer, 1)) + #self.nullsink = gr.file_sink(gr.sizeof_gr_complex, "/dev/null") + + #self.connect(self.u, self.nullsink) + +# self.connect(self.u, self.demod, self.filter) +# self.connect(self.filter, self.avg) +# self.connect(self.filter, (self.preamble, 0)) +# self.connect(self.avg, (self.preamble, 1)) +# self.connect(self.filter, (self.framer, 0)) +# self.connect(self.preamble, (self.framer, 1)) +# self.connect(self.filter, (self.slicer, 0)) +# self.connect(self.framer, (self.slicer, 1)) #use this flowgraph instead to omit the filter -# self.connect(self.u, self.demod) -# self.connect(self.demod, self.avg) -# self.connect(self.demod, (self.preamble, 0)) -# self.connect(self.avg, (self.preamble, 1)) -# self.connect(self.demod, (self.framer, 0)) -# self.connect(self.preamble, (self.framer, 1)) -# self.connect(self.demod, (self.slicer, 0)) -# self.connect(self.framer, (self.slicer, 1)) + self.connect(self.u, self.demod) + self.connect(self.demod, self.avg) + self.connect(self.demod, (self.preamble, 0)) + self.connect(self.avg, (self.preamble, 1)) + self.connect(self.demod, (self.framer, 0)) + self.connect(self.preamble, (self.framer, 1)) + self.connect(self.demod, (self.slicer, 0)) + self.connect(self.framer, (self.slicer, 1)) def tune(self, freq): - result = self.u.set_center_freq(freq) + result = self.u.set_center_freq(freq, 0) return result if __name__ == '__main__': From 72248eaecf63d028da1bd14bc6c27bd132c74514 Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Fri, 19 Nov 2010 14:39:56 -0800 Subject: [PATCH 02/11] Timestamps work. --- src/lib/air_modes_slicer.cc | 35 +++++++++++++++++------------------ src/lib/air_modes_slicer.h | 1 + src/python/modes_print.py | 7 +++---- 3 files changed, 21 insertions(+), 22 deletions(-) diff --git a/src/lib/air_modes_slicer.cc b/src/lib/air_modes_slicer.cc index 942869e..d83ecbb 100644 --- a/src/lib/air_modes_slicer.cc +++ b/src/lib/air_modes_slicer.cc @@ -55,6 +55,7 @@ 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 / channel_rate; set_output_multiple(1+d_check_width * 2); //how do you specify buffer size for sinks? } @@ -158,29 +159,30 @@ int air_modes_slicer::work(int noutput_items, } /******************** BEGIN TIMESTAMP BS ******************/ + rx_packet.timestamp_secs = 0; + rx_packet.timestamp_frac = 0; + uint64_t abs_sample_cnt = nitems_read(0); std::vector tags; + uint64_t timestamp_secs, timestamp_sample, timestamp_delta; + double timestamp_frac; - //printf("nitems_read: %i", abs_sample_cnt); pmt::pmt_t timestamp = pmt::mp(pmt::mp(0), pmt::mp(0)); //so we don't barf if there isn't one - get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + i); + get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + i, pmt::pmt_string_to_symbol("packet_time_stamp")); //tags.back() is the most recent timestamp, then. if(tags.size() > 0) { - std::sort(tags.begin(), tags.end(), pmtcompare); - - do { - timestamp = tags.back(); - tags.pop_back(); - } while(!pmt::pmt_eqv(gr_tags::get_key(timestamp), pmt::pmt_string_to_symbol("time"))); //only interested in timestamps + //if nobody but the USRP is producing timestamps this isn't necessary + //std::sort(tags.begin(), tags.end(), pmtcompare); + timestamp = tags.back(); - uint64_t timestamp_secs = pmt_to_uint64(pmt_tuple_ref(timestamp, 0)); - double timestamp_frac = pmt_to_double(pmt_tuple_ref(timestamp, 1)); - uint64_t timestamp_sample = gr_tags::get_nitems(timestamp); + timestamp_secs = pmt_to_uint64(pmt_tuple_ref(gr_tags::get_value(timestamp), 0)); + timestamp_frac = pmt_to_double(pmt_tuple_ref(gr_tags::get_value(timestamp), 1)); + timestamp_sample = gr_tags::get_nitems(timestamp); //now we have to offset the timestamp based on the current sample number - uint64_t timestamp_delta = (abs_sample_cnt + i) - timestamp_sample; + timestamp_delta = (abs_sample_cnt + i) - timestamp_sample; - timestamp_frac += timestamp_delta * (1.0 / (d_samples_per_chip * d_chip_rate)); + timestamp_frac += timestamp_delta * d_secs_per_sample; if(timestamp_frac > 1.0) { timestamp_frac -= 1.0; timestamp_secs++; @@ -189,10 +191,6 @@ int air_modes_slicer::work(int noutput_items, rx_packet.timestamp_secs = timestamp_secs; rx_packet.timestamp_frac = timestamp_frac; } - else { - rx_packet.timestamp_secs = 0; - rx_packet.timestamp_frac = 0; - } /******************* END TIMESTAMP BS *********************/ @@ -287,7 +285,8 @@ int air_modes_slicer::work(int noutput_items, } d_payload << " " << std::setw(6) << rx_packet.parity << " " << std::dec << rx_packet.reference_level - << " " << rx_packet.timestamp_secs << " " << rx_packet.timestamp_frac; + << " " << rx_packet.timestamp_secs + << " " << std::setprecision(10) << std::setw(10) << rx_packet.timestamp_frac; gr_message_sptr msg = gr_make_message_from_string(std::string(d_payload.str())); d_queue->handle(msg); diff --git a/src/lib/air_modes_slicer.h b/src/lib/air_modes_slicer.h index 423b4a3..bf60d9c 100644 --- a/src/lib/air_modes_slicer.h +++ b/src/lib/air_modes_slicer.h @@ -45,6 +45,7 @@ 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; diff --git a/src/python/modes_print.py b/src/python/modes_print.py index 3d3d6ea..38a3c3a 100644 --- a/src/python/modes_print.py +++ b/src/python/modes_print.py @@ -31,14 +31,13 @@ class modes_output_print(modes_parse.modes_parse): def parse(self, message): [msgtype, shortdata, longdata, parity, ecc, reference, time_secs, time_frac] = message.split() - time_secs = long(time_secs) - time_frac = float(time_frac) - shortdata = long(shortdata, 16) longdata = long(longdata, 16) parity = long(parity, 16) ecc = long(ecc, 16) reference = float(reference) + time_secs = long(time_secs) + time_frac = float(time_frac) msgtype = int(msgtype) @@ -58,7 +57,7 @@ class modes_output_print(modes_parse.modes_parse): output = "No handler for message type " + str(msgtype) + " from %x" % ecc if reference == 0.0: - refdb = 0.0 + refdb = -150.0 else: refdb = 10.0*math.log10(reference) From d173f9e256e809b5222740c8c288ec52e9373a45 Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Fri, 19 Nov 2010 16:38:32 -0800 Subject: [PATCH 03/11] Preamble detector cleanup. Better for >2 samps/symbol. --- src/lib/air_modes_preamble.cc | 67 ++++++++++++++--------------------- src/lib/air_modes_slicer.cc | 2 +- 2 files changed, 27 insertions(+), 42 deletions(-) diff --git a/src/lib/air_modes_preamble.cc b/src/lib/air_modes_preamble.cc index 994aa93..4ef3c8c 100644 --- a/src/lib/air_modes_preamble.cc +++ b/src/lib/air_modes_preamble.cc @@ -85,15 +85,15 @@ int air_modes_preamble::work(int noutput_items, //the packets are so short we choose not to do any sort of closed-loop synchronization after this simple gating. if we get a good center sample, the drift should be negligible. - pulse_offsets[0] = i; - pulse_offsets[1] = i+int(1.0 * d_samples_per_symbol); - pulse_offsets[2] = i+int(3.5 * d_samples_per_symbol); - pulse_offsets[3] = i+int(4.5 * d_samples_per_symbol); + pulse_offsets[0] = 0; + pulse_offsets[1] = int(1.0 * d_samples_per_symbol); + pulse_offsets[2] = int(3.5 * d_samples_per_symbol); + pulse_offsets[3] = int(4.5 * d_samples_per_symbol); - bit_energies[0] = bit_energy(&inraw[pulse_offsets[0]], d_samples_per_chip); - bit_energies[1] = bit_energy(&inraw[pulse_offsets[1]], d_samples_per_chip); - bit_energies[2] = bit_energy(&inraw[pulse_offsets[2]], d_samples_per_chip); - bit_energies[3] = bit_energy(&inraw[pulse_offsets[3]], d_samples_per_chip); + bit_energies[0] = bit_energy(&inraw[i+pulse_offsets[0]], d_samples_per_chip); + bit_energies[1] = bit_energy(&inraw[i+pulse_offsets[1]], d_samples_per_chip); + bit_energies[2] = bit_energy(&inraw[i+pulse_offsets[2]], d_samples_per_chip); + bit_energies[3] = bit_energy(&inraw[i+pulse_offsets[3]], d_samples_per_chip); //search for the rest of the pulses at their expected positions if( bit_energies[1] < pulse_threshold) continue; @@ -128,40 +128,25 @@ int air_modes_preamble::work(int noutput_items, //just for kicks, after validating a preamble, you might want to use all four peaks to form a more accurate "average" center sample time, so that if noise corrupts the first leading edge //sample, you don't mis-sample the entire packet. - //this could also be done in a separate packet, although it probably saves CPU to do it here - //for the 2 samples per chip case, you can just add up the peaks at the expected peak times, then do the same for +1, and -1. + //this could also be done in a separate block, although it probably saves CPU to do it here + //for the 2 samples per chip case, you can just add up the peaks at the expected peak times, then do the same for +1. if(valid_preamble) { - - gate_sum_now = bit_energies[0] + bit_energies[1] + bit_energies[2] + bit_energies[3]; -// gate_sum_early = bit_energy(&inraw[pulse_offsets[0]-1], d_samples_per_chip) -// + bit_energy(&inraw[pulse_offsets[1]-1], d_samples_per_chip) -// + bit_energy(&inraw[pulse_offsets[2]-1], d_samples_per_chip) -// + bit_energy(&inraw[pulse_offsets[3]-1], d_samples_per_chip); - gate_sum_late = bit_energy(&inraw[pulse_offsets[0]+1], d_samples_per_chip) - + bit_energy(&inraw[pulse_offsets[1]+1], d_samples_per_chip) - + bit_energy(&inraw[pulse_offsets[2]+1], d_samples_per_chip) - + bit_energy(&inraw[pulse_offsets[3]+1], d_samples_per_chip); -/* - if(d_samples_per_chip <= 2) { - gate_sum_now = inraw[pulse_offsets[0]+0] + inraw[pulse_offsets[1]+0] + inraw[pulse_offsets[2]+0] + inraw[pulse_offsets[3]+0]; - gate_sum_early = inraw[pulse_offsets[0]-1] + inraw[pulse_offsets[1]-1] + inraw[pulse_offsets[2]-1] + inraw[pulse_offsets[3]-1]; - gate_sum_late = inraw[pulse_offsets[0]+1] + inraw[pulse_offsets[1]+1] + inraw[pulse_offsets[2]+1] + inraw[pulse_offsets[3]+1]; - } else { - for(int j = 1-d_samples_per_chip/2; j < d_samples_per_chip/2; j++) { - gate_sum_now += inraw[j+pulse_offsets[0]+0] + inraw[j+pulse_offsets[1]+0] + inraw[j+pulse_offsets[2]+0] + inraw[j+pulse_offsets[3]+0]; - gate_sum_early += inraw[j+pulse_offsets[0]-1] + inraw[j+pulse_offsets[1]-1] + inraw[j+pulse_offsets[2]-1] + inraw[j+pulse_offsets[3]-1]; - gate_sum_late += inraw[j+pulse_offsets[0]+1] + inraw[j+pulse_offsets[1]+1] + inraw[j+pulse_offsets[2]+1] + inraw[j+pulse_offsets[3]+1]; - } - } -*/ -// if(gate_sum_early > gate_sum_now) { //i think this is redundant -// outattrib[i-1] = 1; -// } - /*else*/ if(gate_sum_late > gate_sum_now) { - outattrib[i+1] = 1; - i+=1; //so we skip the next one and don't overwrite it - } - else outattrib[i] = 1; + i--; + do { + i++; + gate_sum_now = bit_energy(&inraw[i+pulse_offsets[0]], d_samples_per_chip) + + bit_energy(&inraw[i+pulse_offsets[1]], d_samples_per_chip) + + bit_energy(&inraw[i+pulse_offsets[2]], d_samples_per_chip) + + bit_energy(&inraw[i+pulse_offsets[3]], d_samples_per_chip); + + + gate_sum_late = bit_energy(&inraw[i+pulse_offsets[0]+1], d_samples_per_chip) + + bit_energy(&inraw[i+pulse_offsets[1]+1], d_samples_per_chip) + + bit_energy(&inraw[i+pulse_offsets[2]+1], d_samples_per_chip) + + bit_energy(&inraw[i+pulse_offsets[3]+1], d_samples_per_chip); + } while(gate_sum_late > gate_sum_now); + + outattrib[i] = 1; } else outattrib[i] = 0; } diff --git a/src/lib/air_modes_slicer.cc b/src/lib/air_modes_slicer.cc index d83ecbb..3402ce1 100644 --- a/src/lib/air_modes_slicer.cc +++ b/src/lib/air_modes_slicer.cc @@ -167,7 +167,7 @@ int air_modes_slicer::work(int noutput_items, uint64_t timestamp_secs, timestamp_sample, timestamp_delta; double timestamp_frac; - pmt::pmt_t timestamp = pmt::mp(pmt::mp(0), pmt::mp(0)); //so we don't barf if there isn't one + pmt::pmt_t timestamp; get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + i, pmt::pmt_string_to_symbol("packet_time_stamp")); //tags.back() is the most recent timestamp, then. From 6af11394d1260c158bf7535d07e9f63a27f864c2 Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Sat, 20 Nov 2010 15:09:23 -0800 Subject: [PATCH 04/11] Removed gain print which bombs for file input --- src/python/uhd_modes.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/python/uhd_modes.py b/src/python/uhd_modes.py index f569a56..d59f520 100755 --- a/src/python/uhd_modes.py +++ b/src/python/uhd_modes.py @@ -79,7 +79,6 @@ class adsb_rx_block (gr.top_block): self.u = gr.file_source(gr.sizeof_gr_complex, options.filename) print "Rate is %i" % (rate,) - print "Gain is %i" % (self.u.get_gain(),) pass_all = 0 if options.output_all : pass_all = 1 From cd394e1863de6a943369b04b686ff42c801e750b Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Sat, 20 Nov 2010 16:34:44 -0800 Subject: [PATCH 05/11] Preamble now passes tags to framer. Note: Using set_history cocks up the nitems_written() call. --- src/lib/air_modes_framer.cc | 77 ++++++++++++++++--------------- src/lib/air_modes_preamble.cc | 86 +++++++++++++++++++---------------- src/lib/air_modes_preamble.h | 1 + src/lib/air_modes_slicer.cc | 2 - src/python/uhd_modes.py | 5 +- 5 files changed, 88 insertions(+), 83 deletions(-) diff --git a/src/lib/air_modes_framer.cc b/src/lib/air_modes_framer.cc index 2b59058..565ceea 100644 --- a/src/lib/air_modes_framer.cc +++ b/src/lib/air_modes_framer.cc @@ -28,6 +28,9 @@ #include #include #include +#include +#include +#include air_modes_framer_sptr air_make_modes_framer(int channel_rate) { @@ -36,16 +39,16 @@ air_modes_framer_sptr air_make_modes_framer(int channel_rate) air_modes_framer::air_modes_framer(int channel_rate) : gr_sync_block ("modes_framer", - gr_make_io_signature2 (2, 2, sizeof(float), sizeof(unsigned char)), //stream 0 is received data, stream 1 is binary preamble detector output + gr_make_io_signature (1, 1, sizeof(float)), //stream 0 is received data gr_make_io_signature (1, 1, sizeof(unsigned char))) //output is [0, 1, 2]: [no frame, short frame, long frame] { //initialize private data here d_chip_rate = 2000000; //2Mchips per second d_samples_per_chip = channel_rate / d_chip_rate; //must be integer number of samples per chip to work d_samples_per_symbol = d_samples_per_chip * 2; - d_check_width = 120 * d_samples_per_symbol; //gotta be able to look at two long frame lengths at a time in the event that FRUIT occurs near the end of the first frame - - set_output_multiple(1+d_check_width*2); + d_check_width = 120 * d_samples_per_symbol; //gotta be able to look at two long frame lengths at a time + //in the event that FRUIT occurs near the end of the first frame + //set_history(d_check_width*2); } int air_modes_framer::work(int noutput_items, @@ -54,60 +57,56 @@ int air_modes_framer::work(int noutput_items, { //do things! const float *inraw = (const float *) input_items[0]; - const unsigned char *inattrib = (const unsigned char *) input_items[1]; - //float *outraw = (float *) output_items[0]; unsigned char *outattrib = (unsigned char *) output_items[0]; - - int size = noutput_items - d_check_width; //need to be able to look ahead a full frame - - float reference_level = 0; + int size = noutput_items - d_check_width*2; + float reference_level; framer_packet_type packet_attrib; - - for(int i = 0; i < size; i++) { - - packet_attrib = No_Packet; - - if(!inattrib[i]) { - outattrib[i] = packet_attrib; - continue; //if there's no preamble marker, forget it, move on - } - + 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; + + memset(outattrib, 0x00, size * sizeof(unsigned char)); + + for(tag_iter = tags.begin(); tag_iter != tags.end(); tag_iter++) { + uint64_t i = gr_tags::get_nitems(*tag_iter) - abs_sample_cnt; //first, assume we have a long packet packet_attrib = Long_Packet; //let's use the preamble marker to get a reference level for the packet reference_level = (bit_energy(&inraw[i], d_samples_per_chip) - + bit_energy(&inraw[i+int(1.0*d_samples_per_symbol)], d_samples_per_chip) - + bit_energy(&inraw[i+int(3.5*d_samples_per_symbol)], d_samples_per_chip) - + bit_energy(&inraw[i+int(4.5*d_samples_per_symbol)], d_samples_per_chip)) / 4; - + + bit_energy(&inraw[i+int(1.0*d_samples_per_symbol)], d_samples_per_chip) + + bit_energy(&inraw[i+int(3.5*d_samples_per_symbol)], d_samples_per_chip) + + bit_energy(&inraw[i+int(4.5*d_samples_per_symbol)], d_samples_per_chip)) / 4; + //armed with our reference level, let's look for marks within 3dB of the reference level in bits 57-62 (65-70, see above) //if bits 57-62 have marks in either chip, we've got a long packet //otherwise we have a short packet - //NOTE: you can change the default here to be short packet, and then check for a long packet. don't know which way is better. - for(int j = (65 * d_samples_per_symbol); j < (70 * d_samples_per_symbol); j += d_samples_per_symbol) { - float t_max = (bit_energy(&inraw[i+j], d_samples_per_chip) > bit_energy(&inraw[i+j+d_samples_per_chip], d_samples_per_chip)) ? bit_energy(&inraw[i+j], d_samples_per_chip) : bit_energy(&inraw[i+j+d_samples_per_chip], d_samples_per_chip); - if(t_max < (reference_level / 2)) packet_attrib = Short_Packet; + float t_max = std::max(bit_energy(&inraw[i+j], d_samples_per_chip), + bit_energy(&inraw[i+j+d_samples_per_chip], d_samples_per_chip) + ); + if(t_max < (reference_level / 2.0)) packet_attrib = Short_Packet; } - - //BUT: we must also loop through the entire packet to make sure it is clear of additional preamble markers! if it has another preamble marker, it's been FRUITed, and we must only + + //BUT: we must also loop through the entire packet to make sure it is clear of additional preamble markers! + //if it has another preamble marker, it's been FRUITed, and we must only //mark the new packet (i.e., just continue). int lookahead; - if(packet_attrib == Long_Packet) lookahead = 112; - else lookahead = 56; - - for(int j = i+1; j < i+(lookahead * d_samples_per_symbol); j++) { - if(inattrib[j]) packet_attrib = Fruited_Packet; //FRUITed by mode S! in this case, we drop this first packet - //if(inraw[j] > (reference_level * 2)) packet_attrib = Fruited_Packet; //catches strong Mode A/C fruit inside the packet - //but good error correction should cope with that - } + if(packet_attrib == Long_Packet) lookahead = 112 * d_samples_per_symbol; + else lookahead = 56 * d_samples_per_symbol; + //ok we have to re-do lookahead for this tagged version. + //we can do this by looking for tags that fall within that window + std::vector fruit_tags; + get_tags_in_range(fruit_tags, 0, abs_sample_cnt+i+1, abs_sample_cnt+i+lookahead, pmt::pmt_string_to_symbol("preamble_found")); + if(fruit_tags.size() > 0) packet_attrib = Fruited_Packet; + outattrib[i] = packet_attrib; - } return size; diff --git a/src/lib/air_modes_preamble.cc b/src/lib/air_modes_preamble.cc index 4ef3c8c..97d65ba 100644 --- a/src/lib/air_modes_preamble.cc +++ b/src/lib/air_modes_preamble.cc @@ -27,6 +27,8 @@ #include #include #include +#include +#include air_modes_preamble_sptr air_make_modes_preamble(int channel_rate, float threshold_db) { @@ -36,9 +38,8 @@ air_modes_preamble_sptr air_make_modes_preamble(int channel_rate, float threshol air_modes_preamble::air_modes_preamble(int channel_rate, float threshold_db) : gr_sync_block ("modes_preamble", gr_make_io_signature2 (2, 2, sizeof(float), sizeof(float)), //stream 0 is received data, stream 1 is moving average for reference - gr_make_io_signature (1, 1, sizeof(unsigned char))) + gr_make_io_signature (1, 1, sizeof(float))) //the original data. we pass it out in order to use tags. { - //initialize private data here d_chip_rate = 2000000; //2Mchips per second d_samples_per_chip = channel_rate / d_chip_rate; //must be integer number of samples per chip to work d_samples_per_symbol = d_samples_per_chip * 2; @@ -46,6 +47,11 @@ air_modes_preamble::air_modes_preamble(int channel_rate, float threshold_db) : 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 set_output_multiple(1+d_check_width*2); + std::stringstream str; + str << name() << unique_id(); + d_me = pmt::pmt_string_to_symbol(str.str()); + d_key = pmt::pmt_string_to_symbol("preamble_found"); + //set_history(d_check_width); } int air_modes_preamble::work(int noutput_items, @@ -56,12 +62,15 @@ int air_modes_preamble::work(int noutput_items, const float *inraw = (const float *) input_items[0]; const float *inavg = (const float *) input_items[1]; - //float *outraw = (float *) output_items[0]; - unsigned char *outattrib = (unsigned char *) output_items[0]; + float *outraw = (float *) output_items[0]; int size = noutput_items - d_check_width; int pulse_offsets[4]; float bit_energies[4]; + + memcpy(outraw, inraw, size * sizeof(float)); + + uint64_t abs_out_sample_cnt = nitems_written(0); for(int i = d_samples_per_chip; i < size; i++) { float pulse_threshold = bit_energy(&inavg[i], d_samples_per_chip) * d_threshold; @@ -69,22 +78,10 @@ int air_modes_preamble::work(int noutput_items, float gate_sum_now = 0, gate_sum_early = 0, gate_sum_late = 0; if(bit_energy(&inraw[i], d_samples_per_chip) > pulse_threshold) { //if the sample is greater than the reference level by the specified amount - //while(inraw[i+1] > inraw[i]) i++; - - //if(inraw[i+1] > inraw[i]) continue; //we're still coming up on the pulse peak, so let's just fall out and look at it next time around - //if(inraw[i-1] > inraw[i]) continue; //we're past the peak, so it's no longer a valid pulse - - //a note on the above. this simple early/late gate system works for decim = 16, but doesn't work so great for decim = 8. the extra samples, subject to noise, - //mean the peak is not necessarily the center of the bit, and you get fooled into sampling at strange bit edges. the solution is an area integral, a real early/late gate - //system, computing the area of a bit and maximizing so you sample at the center of the bit shape for any decimation. for decim = 16 it won't really matter since you only have - //one possible bit center. - int gate_sum = early_late(&inraw[i], d_samples_per_chip); //see modes_energy.cc if(gate_sum != 0) continue; //if either the early gate or the late gate had greater energy, keep moving. -// if(gate_sum_late > gate_sum_now) continue; - - //the packets are so short we choose not to do any sort of closed-loop synchronization after this simple gating. if we get a good center sample, the drift should be negligible. - + //the packets are so short we choose not to do any sort of closed-loop synchronization after this simple gating. + //if we get a good center sample, the drift should be negligible. pulse_offsets[0] = 0; pulse_offsets[1] = int(1.0 * d_samples_per_symbol); pulse_offsets[2] = int(3.5 * d_samples_per_symbol); @@ -112,43 +109,52 @@ int air_modes_preamble::work(int noutput_items, for(int j = 5 * d_samples_per_symbol; j <= 7.5 * d_samples_per_symbol; j+=d_samples_per_chip) if(bit_energy(&inraw[i+j], d_samples_per_chip) > space_threshold) valid_preamble = false; - //make sure all four peaks are within 2dB of each other - - - float minpeak = avgpeak * 0.631; //-2db - float maxpeak = avgpeak * 1.585; //2db + //make sure all four peaks are within 3dB of each other + float minpeak = avgpeak * 0.5;//-3db, was 0.631; //-2db + float maxpeak = avgpeak * 2.0;//3db, was 1.585; //2db if(bit_energies[0] < minpeak || bit_energies[0] > maxpeak) continue; if(bit_energies[1] < minpeak || bit_energies[1] > maxpeak) continue; if(bit_energies[2] < minpeak || bit_energies[2] > maxpeak) continue; if(bit_energies[3] < minpeak || bit_energies[3] > maxpeak) continue; - } - //just for kicks, after validating a preamble, you might want to use all four peaks to form a more accurate "average" center sample time, so that if noise corrupts the first leading edge - //sample, you don't mis-sample the entire packet. - - //this could also be done in a separate block, although it probably saves CPU to do it here - //for the 2 samples per chip case, you can just add up the peaks at the expected peak times, then do the same for +1. if(valid_preamble) { - i--; + //get a more accurate chip center by finding the energy peak across all four preamble peaks + //there's some weirdness in the early part, so i ripped it out. + bool early, late; do { - i++; + early = late = false; + gate_sum_early= bit_energy(&inraw[i+pulse_offsets[0]-1], d_samples_per_chip) + + bit_energy(&inraw[i+pulse_offsets[1]-1], d_samples_per_chip) + + bit_energy(&inraw[i+pulse_offsets[2]-1], d_samples_per_chip) + + bit_energy(&inraw[i+pulse_offsets[3]-1], d_samples_per_chip); + gate_sum_now = bit_energy(&inraw[i+pulse_offsets[0]], d_samples_per_chip) + bit_energy(&inraw[i+pulse_offsets[1]], d_samples_per_chip) + bit_energy(&inraw[i+pulse_offsets[2]], d_samples_per_chip) + bit_energy(&inraw[i+pulse_offsets[3]], d_samples_per_chip); - - gate_sum_late = bit_energy(&inraw[i+pulse_offsets[0]+1], d_samples_per_chip) - + bit_energy(&inraw[i+pulse_offsets[1]+1], d_samples_per_chip) - + bit_energy(&inraw[i+pulse_offsets[2]+1], d_samples_per_chip) - + bit_energy(&inraw[i+pulse_offsets[3]+1], d_samples_per_chip); - } while(gate_sum_late > gate_sum_now); - - outattrib[i] = 1; + gate_sum_late = bit_energy(&inraw[i+pulse_offsets[0]+1], d_samples_per_chip) + + bit_energy(&inraw[i+pulse_offsets[1]+1], d_samples_per_chip) + + bit_energy(&inraw[i+pulse_offsets[2]+1], d_samples_per_chip) + + bit_energy(&inraw[i+pulse_offsets[3]+1], d_samples_per_chip); - } else outattrib[i] = 0; + early = (gate_sum_early > gate_sum_now); + late = (gate_sum_late > gate_sum_now); + if(late) i++; + //else if(early) i--; + //if(early && late) early = late = false; + } while(late); + + //finally after all this, let's post the preamble! + add_item_tag(0, //stream ID + nitems_written(0)+i, //sample + d_key, //preamble_found + pmt::PMT_T, //meaningless for us + d_me //block src id + ); + } } return size; } diff --git a/src/lib/air_modes_preamble.h b/src/lib/air_modes_preamble.h index 0334410..a537cc2 100644 --- a/src/lib/air_modes_preamble.h +++ b/src/lib/air_modes_preamble.h @@ -47,6 +47,7 @@ private: int d_samples_per_symbol; float d_threshold_db; float d_threshold; + pmt::pmt_t d_me, d_key; public: int work (int noutput_items, diff --git a/src/lib/air_modes_slicer.cc b/src/lib/air_modes_slicer.cc index 3402ce1..6f1a8bb 100644 --- a/src/lib/air_modes_slicer.cc +++ b/src/lib/air_modes_slicer.cc @@ -86,8 +86,6 @@ int air_modes_slicer::work(int noutput_items, int packet_length = 112; if(inattrib[i] == framer_packet_type(Short_Packet)) packet_length = 56; - //printf("Packet received from framer w/length %i\n", packet_length); - rx_packet.type = framer_packet_type(inattrib[i]); memset(&rx_packet.data, 0x00, 14 * sizeof(unsigned char)); memset(&rx_packet.lowconfbits, 0x00, 24 * sizeof(unsigned char)); diff --git a/src/python/uhd_modes.py b/src/python/uhd_modes.py index d59f520..cfb5d40 100755 --- a/src/python/uhd_modes.py +++ b/src/python/uhd_modes.py @@ -73,12 +73,14 @@ class adsb_rx_block (gr.top_block): print "Setting gain to %i" % (options.gain,) self.u.set_gain(options.gain) + print "Gain is %i" % (self.u.get_gain(),) else: rate = options.rate self.u = gr.file_source(gr.sizeof_gr_complex, options.filename) print "Rate is %i" % (rate,) + pass_all = 0 if options.output_all : pass_all = 1 @@ -113,8 +115,7 @@ class adsb_rx_block (gr.top_block): self.connect(self.demod, self.avg) self.connect(self.demod, (self.preamble, 0)) self.connect(self.avg, (self.preamble, 1)) - self.connect(self.demod, (self.framer, 0)) - self.connect(self.preamble, (self.framer, 1)) + self.connect((self.preamble, 0), (self.framer, 0)) self.connect(self.demod, (self.slicer, 0)) self.connect(self.framer, (self.slicer, 1)) From f8966f0d105b59a3c526201747d2ca9d934e060f Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Sat, 20 Nov 2010 17:06:48 -0800 Subject: [PATCH 06/11] All tags, all the time. Still some weirdness with set_history() that requires using the check widths in the framer & slicer. --- src/lib/air_modes_framer.cc | 21 +- src/lib/air_modes_framer.h | 1 + src/lib/air_modes_preamble.cc | 4 +- src/lib/air_modes_slicer.cc | 366 +++++++++++++++++----------------- src/python/uhd_modes.py | 17 +- 5 files changed, 203 insertions(+), 206 deletions(-) diff --git a/src/lib/air_modes_framer.cc b/src/lib/air_modes_framer.cc index 565ceea..a167a31 100644 --- a/src/lib/air_modes_framer.cc +++ b/src/lib/air_modes_framer.cc @@ -40,7 +40,7 @@ air_modes_framer_sptr air_make_modes_framer(int channel_rate) air_modes_framer::air_modes_framer(int channel_rate) : gr_sync_block ("modes_framer", gr_make_io_signature (1, 1, sizeof(float)), //stream 0 is received data - gr_make_io_signature (1, 1, sizeof(unsigned char))) //output is [0, 1, 2]: [no frame, short frame, long frame] + gr_make_io_signature (1, 1, sizeof(float))) //raw samples passed back out { //initialize private data here d_chip_rate = 2000000; //2Mchips per second @@ -49,6 +49,11 @@ air_modes_framer::air_modes_framer(int channel_rate) : d_check_width = 120 * d_samples_per_symbol; //gotta be able to look at two long frame lengths at a time //in the event that FRUIT occurs near the end of the first frame //set_history(d_check_width*2); + + std::stringstream str; + str << name() << unique_id(); + d_me = pmt::pmt_string_to_symbol(str.str()); + d_key = pmt::pmt_string_to_symbol("frame_info"); } int air_modes_framer::work(int noutput_items, @@ -57,8 +62,8 @@ int air_modes_framer::work(int noutput_items, { //do things! const float *inraw = (const float *) input_items[0]; - //float *outraw = (float *) output_items[0]; - unsigned char *outattrib = (unsigned char *) output_items[0]; + float *outraw = (float *) output_items[0]; + //unsigned char *outattrib = (unsigned char *) output_items[0]; int size = noutput_items - d_check_width*2; float reference_level; framer_packet_type packet_attrib; @@ -68,7 +73,7 @@ int air_modes_framer::work(int noutput_items, 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; - memset(outattrib, 0x00, size * sizeof(unsigned char)); + memcpy(outraw, inraw, size * sizeof(float)); for(tag_iter = tags.begin(); tag_iter != tags.end(); tag_iter++) { uint64_t i = gr_tags::get_nitems(*tag_iter) - abs_sample_cnt; @@ -106,7 +111,13 @@ int air_modes_framer::work(int noutput_items, get_tags_in_range(fruit_tags, 0, abs_sample_cnt+i+1, abs_sample_cnt+i+lookahead, pmt::pmt_string_to_symbol("preamble_found")); if(fruit_tags.size() > 0) packet_attrib = Fruited_Packet; - outattrib[i] = packet_attrib; + //insert tag here + add_item_tag(0, //stream ID + nitems_written(0)+i, //sample + d_key, //preamble_found + pmt::pmt_from_long((long)packet_attrib), + d_me //block src id + ); } return size; diff --git a/src/lib/air_modes_framer.h b/src/lib/air_modes_framer.h index f5214c9..067a09c 100644 --- a/src/lib/air_modes_framer.h +++ b/src/lib/air_modes_framer.h @@ -44,6 +44,7 @@ private: int d_chip_rate; int d_samples_per_chip; int d_samples_per_symbol; + pmt::pmt_t d_me, d_key; public: int work (int noutput_items, diff --git a/src/lib/air_modes_preamble.cc b/src/lib/air_modes_preamble.cc index 97d65ba..b7593b9 100644 --- a/src/lib/air_modes_preamble.cc +++ b/src/lib/air_modes_preamble.cc @@ -51,7 +51,7 @@ air_modes_preamble::air_modes_preamble(int channel_rate, float threshold_db) : str << name() << unique_id(); d_me = pmt::pmt_string_to_symbol(str.str()); d_key = pmt::pmt_string_to_symbol("preamble_found"); - //set_history(d_check_width); + set_history(d_check_width); } int air_modes_preamble::work(int noutput_items, @@ -64,7 +64,7 @@ int air_modes_preamble::work(int noutput_items, float *outraw = (float *) output_items[0]; - int size = noutput_items - d_check_width; + int size = noutput_items;// - d_check_width; int pulse_offsets[4]; float bit_energies[4]; diff --git a/src/lib/air_modes_slicer.cc b/src/lib/air_modes_slicer.cc index 6f1a8bb..7913414 100644 --- a/src/lib/air_modes_slicer.cc +++ b/src/lib/air_modes_slicer.cc @@ -46,7 +46,7 @@ air_modes_slicer_sptr air_make_modes_slicer(int channel_rate, gr_msg_queue_sptr air_modes_slicer::air_modes_slicer(int channel_rate, gr_msg_queue_sptr queue) : gr_sync_block ("modes_slicer", - gr_make_io_signature2 (2, 2, sizeof(float), sizeof(unsigned char)), //stream 0 is received data, stream 1 is binary preamble detector output + gr_make_io_signature (1, 1, sizeof(float)), //stream 0 is received data, stream 1 is binary preamble detector output gr_make_io_signature (0, 0, 0) ) { //initialize private data here @@ -74,222 +74,222 @@ int air_modes_slicer::work(int noutput_items, { //do things! const float *inraw = (const float *) input_items[0]; - const unsigned char *inattrib = (const unsigned char *) input_items[1]; 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("frame_info")); + 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; + framer_packet_type packet_type = framer_packet_type(pmt::pmt_to_long(gr_tags::get_value(*tag_iter))); - for(i = 0; i < size; i++) { - if(inattrib[i] == framer_packet_type(Short_Packet) || inattrib[i] == framer_packet_type(Long_Packet)) { //if there's a packet starting here.... - modes_packet rx_packet; + int packet_length; + packet_length = packet_type == framer_packet_type(Short_Packet) ? 56 : 112; - int packet_length = 112; - if(inattrib[i] == framer_packet_type(Short_Packet)) packet_length = 56; - - rx_packet.type = framer_packet_type(inattrib[i]); - memset(&rx_packet.data, 0x00, 14 * sizeof(unsigned char)); - memset(&rx_packet.lowconfbits, 0x00, 24 * sizeof(unsigned char)); - rx_packet.numlowconf = 0; + rx_packet.type = packet_type; + memset(&rx_packet.data, 0x00, 14 * sizeof(unsigned char)); + memset(&rx_packet.lowconfbits, 0x00, 24 * sizeof(unsigned char)); + rx_packet.numlowconf = 0; //let's use the preamble marker to get a reference level for the packet - rx_packet.reference_level = (bit_energy(&inraw[i], d_samples_per_chip) - + bit_energy(&inraw[i+int(1.0*d_samples_per_symbol)], d_samples_per_chip) - + bit_energy(&inraw[i+int(3.5*d_samples_per_symbol)], d_samples_per_chip) - + bit_energy(&inraw[i+int(4.5*d_samples_per_symbol)], d_samples_per_chip)) / 4; + rx_packet.reference_level = (bit_energy(&inraw[i], d_samples_per_chip) + + bit_energy(&inraw[i+int(1.0*d_samples_per_symbol)], d_samples_per_chip) + + bit_energy(&inraw[i+int(3.5*d_samples_per_symbol)], d_samples_per_chip) + + bit_energy(&inraw[i+int(4.5*d_samples_per_symbol)], d_samples_per_chip)) / 4; - i += 8 * d_samples_per_symbol; //move to the center of the first bit of the data + i += 8 * d_samples_per_symbol; //move to the center of the first bit of the data - //here we calculate the total energy contained in each chip of the symbol - for(int j = 0; j < packet_length; j++) { - int firstchip = i+j*d_samples_per_symbol; - int secondchip = firstchip + d_samples_per_chip; - bool slice, confidence; - float firstchip_energy=0, secondchip_energy=0; + //here we calculate the total energy contained in each chip of the symbol + for(int j = 0; j < packet_length; j++) { + int firstchip = i+j*d_samples_per_symbol; + int secondchip = firstchip + d_samples_per_chip; + bool slice, confidence; + float firstchip_energy=0, secondchip_energy=0; - firstchip_energy = bit_energy(&inraw[firstchip], d_samples_per_chip); - secondchip_energy = bit_energy(&inraw[secondchip], d_samples_per_chip); + firstchip_energy = bit_energy(&inraw[firstchip], d_samples_per_chip); + secondchip_energy = bit_energy(&inraw[secondchip], d_samples_per_chip); - //3dB limits for bit slicing and confidence measurement - float highlimit=rx_packet.reference_level*2; - float lowlimit=rx_packet.reference_level*0.5; - bool firstchip_inref = ((firstchip_energy > lowlimit) && (firstchip_energy < highlimit)); - bool secondchip_inref = ((secondchip_energy > lowlimit) && (secondchip_energy < highlimit)); + //3dB limits for bit slicing and confidence measurement + float highlimit=rx_packet.reference_level*2; + float lowlimit=rx_packet.reference_level*0.5; + bool firstchip_inref = ((firstchip_energy > lowlimit) && (firstchip_energy < highlimit)); + bool secondchip_inref = ((secondchip_energy > lowlimit) && (secondchip_energy < highlimit)); - //these two lines for a super simple naive slicer. -// slice = firstchip_energy > secondchip_energy; -// confidence = bool(int(firstchip_inref) + int(secondchip_inref)); //one and only one chip in the reference zone + //these two lines for a super simple naive slicer. +// slice = firstchip_energy > secondchip_energy; +// confidence = bool(int(firstchip_inref) + int(secondchip_inref)); //one and only one chip in the reference zone - //below is the Lincoln Labs slicer. it may produce greater bit errors. supposedly it is more resistant to mode A/C FRUIT. + //below is the Lincoln Labs slicer. it may produce greater bit errors. supposedly it is more resistant to mode A/C FRUIT. //see http://adsb.tc.faa.gov/WG3_Meetings/Meeting8/Squitter-Lon.pdf - if(firstchip_inref && !secondchip_inref) { - slice = 1; - confidence = 1; - } - else if(secondchip_inref && !firstchip_inref) { - slice = 0; - confidence = 1; - } - else if(firstchip_inref && secondchip_inref) { - slice = firstchip_energy > secondchip_energy; - confidence = 0; - } - else if(!firstchip_inref && !secondchip_inref) { //in this case, we determine the bit by whichever is larger, and we determine high confidence if the low chip is 6dB below reference. - slice = firstchip_energy > secondchip_energy; - if(slice) { - if(secondchip_energy < lowlimit * 0.5) confidence = 1; - else confidence = 0; - } else { - if(firstchip_energy < lowlimit * 0.5) confidence = 1; - else confidence = 0; - } - } - - //put the data into the packet + if(firstchip_inref && !secondchip_inref) { + slice = 1; + confidence = 1; + } + else if(secondchip_inref && !firstchip_inref) { + slice = 0; + confidence = 1; + } + else if(firstchip_inref && secondchip_inref) { + slice = firstchip_energy > secondchip_energy; + confidence = 0; + } + else if(!firstchip_inref && !secondchip_inref) { //in this case, we determine the bit by whichever is larger, and we determine high confidence if the low chip is 6dB below reference. + slice = firstchip_energy > secondchip_energy; if(slice) { - rx_packet.data[j/8] += 1 << (7-(j%8)); - } - //put the confidence decision into the packet - if(confidence) { - //rx_packet.confidence[j/8] += 1 << (7-(j%8)); + if(secondchip_energy < lowlimit * 0.5) confidence = 1; + else confidence = 0; } else { - if(rx_packet.numlowconf < 24) rx_packet.lowconfbits[rx_packet.numlowconf++] = j; + if(firstchip_energy < lowlimit * 0.5) confidence = 1; + else confidence = 0; } } - - /******************** BEGIN TIMESTAMP BS ******************/ - rx_packet.timestamp_secs = 0; - rx_packet.timestamp_frac = 0; - - uint64_t abs_sample_cnt = nitems_read(0); - std::vector tags; - uint64_t timestamp_secs, timestamp_sample, timestamp_delta; - double timestamp_frac; - - pmt::pmt_t timestamp; - - get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + i, pmt::pmt_string_to_symbol("packet_time_stamp")); - //tags.back() is the most recent timestamp, then. - if(tags.size() > 0) { - //if nobody but the USRP is producing timestamps this isn't necessary - //std::sort(tags.begin(), tags.end(), pmtcompare); - timestamp = tags.back(); - - timestamp_secs = pmt_to_uint64(pmt_tuple_ref(gr_tags::get_value(timestamp), 0)); - timestamp_frac = pmt_to_double(pmt_tuple_ref(gr_tags::get_value(timestamp), 1)); - timestamp_sample = gr_tags::get_nitems(timestamp); - //now we have to offset the timestamp based on the current sample number - timestamp_delta = (abs_sample_cnt + i) - timestamp_sample; - - timestamp_frac += timestamp_delta * d_secs_per_sample; - if(timestamp_frac > 1.0) { - timestamp_frac -= 1.0; - timestamp_secs++; - } - - rx_packet.timestamp_secs = timestamp_secs; - rx_packet.timestamp_frac = timestamp_frac; + //put the data into the packet + if(slice) { + rx_packet.data[j/8] += 1 << (7-(j%8)); } - - /******************* END TIMESTAMP BS *********************/ - - //increment for the next round - i += packet_length * d_samples_per_symbol; - - //here you might want to traverse the whole packet and if you find all 0's, just toss it. don't know why these packets turn up, but they pass ECC. - bool zeroes = 1; - for(int m = 0; m < 14; m++) { - if(rx_packet.data[m]) zeroes = 0; + //put the confidence decision into the packet + if(confidence) { + //rx_packet.confidence[j/8] += 1 << (7-(j%8)); + } else { + if(rx_packet.numlowconf < 24) rx_packet.lowconfbits[rx_packet.numlowconf++] = j; } - if(zeroes) continue; //toss it - - rx_packet.message_type = (rx_packet.data[0] >> 3) & 0x1F; //get the message type for the parser to conveniently use, and to make decisions on ECC methods - - //we note that short packets other than type 11 CANNOT be reliably decoded, since the a/c address is encoded with the parity bits. - //mode S in production ATC use relies on the fact that these short packets are reply squitters to transponder requests, - //and so the radar should already know the expected a/c reply address. so, error-correction makes no sense on short packets (other than type 11) - //this means two things: first, we will DROP short packets (other than type 11) with ANY low-confidence bits, since we can't be confident that we're seeing real data - //second, we will only perform error correction on LONG type S packets. - - //the limitation on short packets means in practice a short packet has to be at least 6dB above the noise floor in order to be output. long packets can theoretically - //be decoded at the 3dB SNR point. below that and the preamble detector won't fire. + } - //in practice, this limitation causes you to see a HUGE number of type 11 packets which pass CRC through random luck. - //these packets necessarily have large numbers of low-confidence bits, so we toss them with an arbitrary limit of 10. - //that's a pretty dang low threshold so i don't think we'll drop many legit packets - - if(rx_packet.type == Short_Packet && rx_packet.message_type != 11 && rx_packet.numlowconf != 0) continue; - if(rx_packet.type == Short_Packet && rx_packet.message_type == 11 && rx_packet.numlowconf >= 10) continue; + /******************** BEGIN TIMESTAMP BS ******************/ + rx_packet.timestamp_secs = 0; + rx_packet.timestamp_frac = 0; + + uint64_t abs_sample_cnt = nitems_read(0); + std::vector tags; + uint64_t timestamp_secs, timestamp_sample, timestamp_delta; + double timestamp_frac; + pmt::pmt_t timestamp; + + get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + i, pmt::pmt_string_to_symbol("packet_time_stamp")); + //tags.back() is the most recent timestamp, then. + if(tags.size() > 0) { + //if nobody but the USRP is producing timestamps this isn't necessary + //std::sort(tags.begin(), tags.end(), pmtcompare); + timestamp = tags.back(); + + timestamp_secs = pmt_to_uint64(pmt_tuple_ref(gr_tags::get_value(timestamp), 0)); + timestamp_frac = pmt_to_double(pmt_tuple_ref(gr_tags::get_value(timestamp), 1)); + timestamp_sample = gr_tags::get_nitems(timestamp); + //now we have to offset the timestamp based on the current sample number + timestamp_delta = (abs_sample_cnt + i) - timestamp_sample; - //if(rx_packet.numlowconf >= 24) continue; //don't even try, this is the maximum number of errors ECC could possibly correct - //the above line should be part of ECC, and only checked if the message has parity errors - - rx_packet.parity = modes_check_parity(rx_packet.data, packet_length); - - if(rx_packet.parity && rx_packet.type == Long_Packet) { -// long before = rx_packet.parity; - bruteResultTypeDef bruteResult = modes_ec_brute(rx_packet); - - if(bruteResult == No_Solution) { - //printf("No solution!\n"); - continue; - } else if(bruteResult == Multiple_Solutions) { -// printf("Multiple solutions!\n"); - continue; - } else if(bruteResult == Too_Many_LCBs) { - //printf("Too many LCBs (%i)!\n", rx_packet.numlowconf); - continue; - } else if(bruteResult == No_Error) { -// printf("No error!\n"); - } else if(bruteResult == Solution_Found) { -// printf("Solution found for %i LCBs!\n", rx_packet.numlowconf); - } -// rx_packet.parity = modes_check_parity(rx_packet.data, packet_length); -// if(rx_packet.parity) printf("Error: packet fails parity check after correction, was %x, now %x\n", before, rx_packet.parity); + timestamp_frac += timestamp_delta * d_secs_per_sample; + if(timestamp_frac > 1.0) { + timestamp_frac -= 1.0; + timestamp_secs++; } + + rx_packet.timestamp_secs = timestamp_secs; + rx_packet.timestamp_frac = timestamp_frac; + } -// if(rx_packet.parity && rx_packet.type == Long_Packet) printf("Error! Bad packet forwarded to the queue.\n"); - //now we have a complete packet with confidence data, let's print it to the message queue - //here, rather than send the entire packet, since we've already done parity checking and ECC in C++, we'll - //send just the data (no confidence bits), separated into fields for easier parsing. + /******************* END TIMESTAMP BS *********************/ + + //increment for the next round - //we'll replicate some data by sending the message type as the first field, followed by the first 8+24=32 bits of the packet, followed by - //56 long packet data bits if applicable (zero-padded if not), followed by parity + //here you might want to traverse the whole packet and if you find all 0's, just toss it. don't know why these packets turn up, but they pass ECC. + bool zeroes = 1; + for(int m = 0; m < 14; m++) { + if(rx_packet.data[m]) zeroes = 0; + } + if(zeroes) continue; //toss it - d_payload.str(""); + rx_packet.message_type = (rx_packet.data[0] >> 3) & 0x1F; //get the message type for the parser to conveniently use, and to make decisions on ECC methods - d_payload << std::dec << std::setw(2) << std::setfill('0') << rx_packet.message_type << std::hex << " "; + //we note that short packets other than type 11 CANNOT be reliably decoded, since the a/c address is encoded with the parity bits. + //mode S in production ATC use relies on the fact that these short packets are reply squitters to transponder requests, + //and so the radar should already know the expected a/c reply address. so, error-correction makes no sense on short packets (other than type 11) + //this means two things: first, we will DROP short packets (other than type 11) with ANY low-confidence bits, since we can't be confident that we're seeing real data + //second, we will only perform error correction on LONG type S packets. - for(int m = 0; m < 4; m++) { + //the limitation on short packets means in practice a short packet has to be at least 6dB above the noise floor in order to be output. long packets can theoretically + //be decoded at the 3dB SNR point. below that and the preamble detector won't fire. + + //in practice, this limitation causes you to see a HUGE number of type 11 packets which pass CRC through random luck. + //these packets necessarily have large numbers of low-confidence bits, so we toss them with an arbitrary limit of 10. + //that's a pretty dang low threshold so i don't think we'll drop many legit packets + + if(rx_packet.type == Short_Packet && rx_packet.message_type != 11 && rx_packet.numlowconf != 0) continue; + if(rx_packet.type == Short_Packet && rx_packet.message_type == 11 && rx_packet.numlowconf >= 10) continue; + + + //if(rx_packet.numlowconf >= 24) continue; //don't even try, this is the maximum number of errors ECC could possibly correct + //the above line should be part of ECC, and only checked if the message has parity errors + + rx_packet.parity = modes_check_parity(rx_packet.data, packet_length); + + if(rx_packet.parity && rx_packet.type == Long_Packet) { +// long before = rx_packet.parity; + bruteResultTypeDef bruteResult = modes_ec_brute(rx_packet); + + if(bruteResult == No_Solution) { + //printf("No solution!\n"); + continue; + } else if(bruteResult == Multiple_Solutions) { +// printf("Multiple solutions!\n"); + continue; + } else if(bruteResult == Too_Many_LCBs) { + //printf("Too many LCBs (%i)!\n", rx_packet.numlowconf); + continue; + } else if(bruteResult == No_Error) { +// printf("No error!\n"); + } else if(bruteResult == Solution_Found) { +// printf("Solution found for %i LCBs!\n", rx_packet.numlowconf); + } +// rx_packet.parity = modes_check_parity(rx_packet.data, packet_length); +// if(rx_packet.parity) printf("Error: packet fails parity check after correction, was %x, now %x\n", before, rx_packet.parity); + } + +// if(rx_packet.parity && rx_packet.type == Long_Packet) printf("Error! Bad packet forwarded to the queue.\n"); + //now we have a complete packet with confidence data, let's print it to the message queue + //here, rather than send the entire packet, since we've already done parity checking and ECC in C++, we'll + //send just the data (no confidence bits), separated into fields for easier parsing. + //we'll replicate some data by sending the message type as the first field, followed by the first 8+24=32 bits of the packet, followed by + //56 long packet data bits if applicable (zero-padded if not), followed by parity + + d_payload.str(""); + + d_payload << std::dec << std::setw(2) << std::setfill('0') << rx_packet.message_type << std::hex << " "; + + for(int m = 0; m < 4; m++) { + d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]); + } + d_payload << " "; + if(packet_length == 112) { + for(int m = 4; m < 11; m++) { d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]); } d_payload << " "; - if(packet_length == 112) { - for(int m = 4; m < 11; m++) { - d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]); - } - d_payload << " "; - for(int m = 11; m < 14; m++) { - d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]); - } - } else { - for(int m = 4; m < 11; m++) { - d_payload << std::setw(2) << std::setfill('0') << unsigned(0); - } - d_payload << " "; - for(int m = 4; m < 7; m++) { - d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]); - } + for(int m = 11; m < 14; m++) { + d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]); + } + } else { + for(int m = 4; m < 11; m++) { + d_payload << std::setw(2) << std::setfill('0') << unsigned(0); + } + d_payload << " "; + for(int m = 4; m < 7; m++) { + d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]); } - - d_payload << " " << std::setw(6) << rx_packet.parity << " " << std::dec << rx_packet.reference_level - << " " << rx_packet.timestamp_secs - << " " << std::setprecision(10) << std::setw(10) << rx_packet.timestamp_frac; - - gr_message_sptr msg = gr_make_message_from_string(std::string(d_payload.str())); - d_queue->handle(msg); - } + + d_payload << " " << std::setw(6) << rx_packet.parity << " " << std::dec << rx_packet.reference_level + << " " << rx_packet.timestamp_secs + << " " << std::setprecision(10) << std::setw(10) << rx_packet.timestamp_frac; + gr_message_sptr msg = gr_make_message_from_string(std::string(d_payload.str())); + d_queue->handle(msg); + } return size; diff --git a/src/python/uhd_modes.py b/src/python/uhd_modes.py index cfb5d40..20bcad3 100755 --- a/src/python/uhd_modes.py +++ b/src/python/uhd_modes.py @@ -96,28 +96,13 @@ class adsb_rx_block (gr.top_block): self.preamble = air.modes_preamble(rate, options.threshold) self.framer = air.modes_framer(rate) self.slicer = air.modes_slicer(rate, queue) - - #self.nullsink = gr.file_sink(gr.sizeof_gr_complex, "/dev/null") - - #self.connect(self.u, self.nullsink) - -# self.connect(self.u, self.demod, self.filter) -# self.connect(self.filter, self.avg) -# self.connect(self.filter, (self.preamble, 0)) -# self.connect(self.avg, (self.preamble, 1)) -# self.connect(self.filter, (self.framer, 0)) -# self.connect(self.preamble, (self.framer, 1)) -# self.connect(self.filter, (self.slicer, 0)) -# self.connect(self.framer, (self.slicer, 1)) -#use this flowgraph instead to omit the filter self.connect(self.u, self.demod) self.connect(self.demod, self.avg) self.connect(self.demod, (self.preamble, 0)) self.connect(self.avg, (self.preamble, 1)) self.connect((self.preamble, 0), (self.framer, 0)) - self.connect(self.demod, (self.slicer, 0)) - self.connect(self.framer, (self.slicer, 1)) + self.connect(self.framer, self.slicer) def tune(self, freq): result = self.u.set_center_freq(freq, 0) From 00bcc041b2c0937a449c82eb5f29fdbb5a7efe7f Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Sun, 28 Nov 2010 17:07:09 -0800 Subject: [PATCH 07/11] Cosmetics --- src/lib/air_modes_preamble.cc | 10 +++++----- src/lib/air_modes_slicer.cc | 13 ------------- 2 files changed, 5 insertions(+), 18 deletions(-) diff --git a/src/lib/air_modes_preamble.cc b/src/lib/air_modes_preamble.cc index b7593b9..05a969e 100644 --- a/src/lib/air_modes_preamble.cc +++ b/src/lib/air_modes_preamble.cc @@ -125,10 +125,10 @@ int air_modes_preamble::work(int noutput_items, bool early, late; do { early = late = false; - gate_sum_early= bit_energy(&inraw[i+pulse_offsets[0]-1], d_samples_per_chip) - + bit_energy(&inraw[i+pulse_offsets[1]-1], d_samples_per_chip) - + bit_energy(&inraw[i+pulse_offsets[2]-1], d_samples_per_chip) - + bit_energy(&inraw[i+pulse_offsets[3]-1], d_samples_per_chip); + //gate_sum_early= bit_energy(&inraw[i+pulse_offsets[0]-1], d_samples_per_chip) + // + bit_energy(&inraw[i+pulse_offsets[1]-1], d_samples_per_chip) + // + bit_energy(&inraw[i+pulse_offsets[2]-1], d_samples_per_chip) + // + bit_energy(&inraw[i+pulse_offsets[3]-1], d_samples_per_chip); gate_sum_now = bit_energy(&inraw[i+pulse_offsets[0]], d_samples_per_chip) + bit_energy(&inraw[i+pulse_offsets[1]], d_samples_per_chip) @@ -146,7 +146,7 @@ int air_modes_preamble::work(int noutput_items, //else if(early) i--; //if(early && late) early = late = false; } while(late); - + //finally after all this, let's post the preamble! add_item_tag(0, //stream ID nitems_written(0)+i, //sample diff --git a/src/lib/air_modes_slicer.cc b/src/lib/air_modes_slicer.cc index 7913414..21a79e8 100644 --- a/src/lib/air_modes_slicer.cc +++ b/src/lib/air_modes_slicer.cc @@ -230,38 +230,25 @@ int air_modes_slicer::work(int noutput_items, rx_packet.parity = modes_check_parity(rx_packet.data, packet_length); if(rx_packet.parity && rx_packet.type == Long_Packet) { -// long before = rx_packet.parity; bruteResultTypeDef bruteResult = modes_ec_brute(rx_packet); if(bruteResult == No_Solution) { - //printf("No solution!\n"); continue; } else if(bruteResult == Multiple_Solutions) { -// printf("Multiple solutions!\n"); continue; } else if(bruteResult == Too_Many_LCBs) { - //printf("Too many LCBs (%i)!\n", rx_packet.numlowconf); continue; } else if(bruteResult == No_Error) { -// printf("No error!\n"); } else if(bruteResult == Solution_Found) { // printf("Solution found for %i LCBs!\n", rx_packet.numlowconf); } -// rx_packet.parity = modes_check_parity(rx_packet.data, packet_length); -// if(rx_packet.parity) printf("Error: packet fails parity check after correction, was %x, now %x\n", before, rx_packet.parity); } -// if(rx_packet.parity && rx_packet.type == Long_Packet) printf("Error! Bad packet forwarded to the queue.\n"); - //now we have a complete packet with confidence data, let's print it to the message queue - //here, rather than send the entire packet, since we've already done parity checking and ECC in C++, we'll - //send just the data (no confidence bits), separated into fields for easier parsing. //we'll replicate some data by sending the message type as the first field, followed by the first 8+24=32 bits of the packet, followed by //56 long packet data bits if applicable (zero-padded if not), followed by parity d_payload.str(""); - d_payload << std::dec << std::setw(2) << std::setfill('0') << rx_packet.message_type << std::hex << " "; - for(int m = 0; m < 4; m++) { d_payload << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]); } From 1acea7c9fd55361b2273846a34c040cc9bf67556 Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Sun, 5 Dec 2010 17:43:13 -0800 Subject: [PATCH 08/11] Added multilateration library mlat.py and test program mlat-test.py. No integration into the application yet. --- src/python/mlat-test.py | 7 ++ src/python/mlat.py | 218 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 225 insertions(+) create mode 100755 src/python/mlat-test.py create mode 100755 src/python/mlat.py diff --git a/src/python/mlat-test.py b/src/python/mlat-test.py new file mode 100755 index 0000000..3bb5e73 --- /dev/null +++ b/src/python/mlat-test.py @@ -0,0 +1,7 @@ +#!/usr/bin/python +import mlat +import numpy + +ans = mlat.mlat(mlat.teststations, mlat.teststamps, mlat.testalt) +error = numpy.linalg.norm(numpy.array(mlat.llh2ecef(ans))-numpy.array(mlat.testplane)) +print error diff --git a/src/python/mlat.py b/src/python/mlat.py new file mode 100755 index 0000000..1ce063c --- /dev/null +++ b/src/python/mlat.py @@ -0,0 +1,218 @@ +#!/usr/bin/python +import math +import numpy +from scipy.ndimage import map_coordinates + +#functions for multilateration. + +#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####################################### + + +#this is a 10x10-degree WGS84 geoid datum, in meters relative to the WGS84 reference ellipsoid. given the maximum slope, you should probably interpolate. +#NIMA suggests a 2x2 interpolation using four neighbors. we'll go cubic spline JUST BECAUSE WE CAN +wgs84_geoid = numpy.array([[13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13], #90N + [3,1,-2,-3,-3,-3,-1,3,1,5,9,11,19,27,31,34,33,34,33,34,28,23,17,13,9,4,4,1,-2,-2,0,2,3,2,1,1], #80N + [2,2,1,-1,-3,-7,-14,-24,-27,-25,-19,3,24,37,47,60,61,58,51,43,29,20,12,5,-2,-10,-14,-12,-10,-14,-12,-6,-2,3,6,4], #70N + [2,9,17,10,13,1,-14,-30,-39,-46,-42,-21,6,29,49,65,60,57,47,41,21,18,14,7,-3,-22,-29,-32,-32,-26,-15,-2,13,17,19,6], #60N + [-8,8,8,1,-11,-19,-16,-18,-22,-35,-40,-26,-12,24,45,63,62,59,47,48,42,28,12,-10,-19,-33,-43,-42,-43,-29,-2,17,23,22,6,2], #50N + [-12,-10,-13,-20,-31,-34,-21,-16,-26,-34,-33,-35,-26,2,33,59,52,51,52,48,35,40,33,-9,-28,-39,-48,-59,-50,-28,3,23,37,18,-1,-11], #40N + [-7,-5,-8,-15,-28,-40,-42,-29,-22,-26,-32,-51,-40,-17,17,31,34,44,36,28,29,17,12,-20,-15,-40,-33,-34,-34,-28,7,29,43,20,4,-6], #30N + [5,10,7,-7,-23,-39,-47,-34,-9,-10,-20,-45,-48,-32,-9,17,25,31,31,26,15,6,1,-29,-44,-61,-67,-59,-36,-11,21,39,49,39,22,10], #20N + [13,12,11,2,-11,-28,-38,-29,-10,3,1,-11,-41,-42,-16,3,17,33,22,23,2,-3,-7,-36,-59,-90,-95,-63,-24,12,53,60,58,46,36,26], #10N + [22,16,17,13,1,-12,-23,-20,-14,-3,14,10,-15,-27,-18,3,12,20,18,12,-13,-9,-28,-49,-62,-89,-102,-63,-9,33,58,73,74,63,50,32], #0 + [36,22,11,6,-1,-8,-10,-8,-11,-9,1,32,4,-18,-13,-9,4,14,12,13,-2,-14,-25,-32,-38,-60,-75,-63,-26,0,35,52,68,76,64,52], #10S + [51,27,10,0,-9,-11,-5,-2,-3,-1,9,35,20,-5,-6,-5,0,13,17,23,21,8,-9,-10,-11,-20,-40,-47,-45,-25,5,23,45,58,57,63], #20S + [46,22,5,-2,-8,-13,-10,-7,-4,1,9,32,16,4,-8,4,12,15,22,27,34,29,14,15,15,7,-9,-25,-37,-39,-23,-14,15,33,34,45], #30S + [21,6,1,-7,-12,-12,-12,-10,-7,-1,8,23,15,-2,-6,6,21,24,18,26,31,33,39,41,30,24,13,-2,-20,-32,-33,-27,-14,-2,5,20], #40S + [-15,-18,-18,-16,-17,-15,-10,-10,-8,-2,6,14,13,3,3,10,20,27,25,26,34,39,45,45,38,39,28,13,-1,-15,-22,-22,-18,-15,-14,-10], #50S + [-45,-43,-37,-32,-30,-26,-23,-22,-16,-10,-2,10,20,20,21,24,22,17,16,19,25,30,35,35,33,30,27,10,-2,-14,-23,-30,-33,-29,-35,-43], #60S + [-61,-60,-61,-55,-49,-44,-38,-31,-25,-16,-6,1,4,5,4,2,6,12,16,16,17,21,20,26,26,22,16,10,-1,-16,-29,-36,-46,-55,-54,-59], #70S + [-53,-54,-55,-52,-48,-42,-38,-38,-29,-26,-26,-24,-23,-21,-19,-16,-12,-8,-4,-1,1,4,4,6,5,4,2,-6,-15,-24,-33,-40,-48,-50,-53,-52], #80S + [-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30]], #90S + dtype=numpy.float) + +#ok this calculates the geoid offset from the reference ellipsoid +#combined with LLH->ECEF this gets you XYZ for a ground-referenced point +def wgs84_height(lat, lon): + yi = numpy.array([9-lat/10.0]) + xi = numpy.array([18+lon/10.0]) + return float(map_coordinates(wgs84_geoid, [yi, xi])) + +#WGS84 reference ellipsoid constants +wgs84_a = 6378137.0 +wgs84_b = 6356752.314245 +wgs84_e2 = 0.0066943799901975848 +wgs84_a2 = wgs84_a**2 #to speed things up a bit +wgs84_b2 = wgs84_b**2 + +#convert ECEF to lat/lon/alt without geoid correction +#returns alt in meters +def ecef2llh((x,y,z)): + ep = math.sqrt((wgs84_a2 - wgs84_b2) / wgs84_b2) + p = math.sqrt(x**2+y**2) + th = math.atan2(wgs84_a*z, wgs84_b*p) + lon = math.atan2(y, x) + lat = math.atan2(z+ep**2*wgs84_b*math.sin(th)**3, p-wgs84_e2*wgs84_a*math.cos(th)**3) + N = wgs84_a / math.sqrt(1-wgs84_e2*math.sin(lat)**2) + alt = p / math.cos(lat) - N + + lon *= (180. / math.pi) + lat *= (180. / math.pi) + + return [lat, lon, alt] + +#convert lat/lon/alt coords to ECEF without geoid correction, WGS84 model +#remember that alt is in meters +def llh2ecef((lat, lon, alt)): + lat *= (math.pi / 180.0) + lon *= (math.pi / 180.0) + + n = lambda x: wgs84_a / math.sqrt(1 - wgs84_e2*(math.sin(x)**2)) + + x = (n(lat) + alt)*math.cos(lat)*math.cos(lon) + y = (n(lat) + alt)*math.cos(lat)*math.sin(lon) + z = (n(lat)*(1-wgs84_e2)+alt)*math.sin(lat) + + return [x,y,z] + +#do both of the above to get a geoid-corrected x,y,z position +def llh2geoid((lat, lon, alt)): + (x,y,z) = llh2ecef((lat, lon, alt + wgs84_height(lat, lon))) + return [x,y,z] + + +c = 299792458 / 1.0003 #modified for refractive index of air, why not + +#here's some test data to validate the algorithm +teststations = [[37.76225, -122.44254, 100], [37.409044, -122.077748, 100], [37.585085, -121.986395, 100]] +testalt = 8000 +testplane = numpy.array(llh2ecef([37.617175,-122.380843, testalt])) +testme = llh2geoid(teststations[0]) +teststamps = [10, + 10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[1]))) / c, + 10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[2]))) / c, + ] + +#this function is the iterative solver core of the mlat function below +def mlat_iter(rel_stations, prange_obs, xguess = [0,0,0], numrounds = 10): + for i in range(0,numrounds): + prange_est = [] + for station in rel_stations: + prange_est.append([numpy.linalg.norm(station - xguess)]) + dphat = prange_obs - prange_est + H = [] + for row in range(0,len(rel_stations)): + H.append((numpy.array(-rel_stations[row,:])-xguess) / prange_est[row]) + H = numpy.array(H) + #now we have H, the Jacobian, and can solve for residual error + xerr = numpy.dot(numpy.linalg.solve(numpy.dot(H.T,H), H.T), dphat).flatten() + xguess += xerr + return xguess + +#func mlat: +#uses a modified GPS pseudorange solver to locate aircraft by multilateration. +#stations is a list of listening station positions in X,Y,Z ECEF format, geoid corrected +#timestamps is a list of times at which the correlated squitters were heard +#altitude is the barometric altitude of the aircraft as returned by the aircraft +#returns the estimated position of the aircraft in (lat, lon, alt) geoid-corrected WGS84. +def mlat(stations, timestamps, altitude): + if len(timestamps) != len(stations): + raise Exception("Must have x timestamps for x stations reporting!") + + me_llh = stations[0] + me = llh2geoid(stations[0]) + + rel_stations = [] #list of stations in XYZ relative to me + for station in stations[1:]: + rel_stations.append(numpy.array(llh2geoid(station)) - numpy.array(me)) + rel_stations.append([0,0,0]-numpy.array(me)) #arne saknussemm, reporting in + rel_stations = numpy.array(rel_stations) #convert list of arrays to 2d array + + tdoa = [] + for stamp in timestamps[1:]: + tdoa.append(stamp - timestamps[0]) + + prange_obs = [] + for stamp in tdoa: + prange_obs.append([c * stamp]) + + #so here we calc the estimated pseudorange to the center of the earth, using station[0] as a reference point for the geoid + #this is a necessary approximation since we don't know the location of the aircraft yet + #if the dang earth were actually round this wouldn't be an issue + prange_obs.append( [numpy.linalg.norm(llh2ecef((me_llh[0], me_llh[1], altitude)))] ) #use ECEF not geoid since alt is MSL not GPS + #prange_obs.append( [numpy.linalg.norm(testplane)]) #test for error + prange_obs = numpy.array(prange_obs) + + xyzpos = mlat_iter(rel_stations, prange_obs) + llhpos = ecef2llh(xyzpos+me) + + #now, we could return llhpos right now and be done with it. + #but the assumption we made above, namely that the aircraft is directly above the + #nearest station, results in significant error due to the oblateness of the Earth's geometry. + #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) + + return llhpos From 35ca3c886912a909fa1035e74e68b815e71a11f8 Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Mon, 6 Dec 2010 10:28:44 -0800 Subject: [PATCH 09/11] Modified mlat solver to solve to a threshold and quit. Also sorts incoming timestamps. --- src/python/mlat-test.py | 10 ++++++++-- src/python/mlat.py | 31 ++++++++++++++++++++++++------- 2 files changed, 32 insertions(+), 9 deletions(-) diff --git a/src/python/mlat-test.py b/src/python/mlat-test.py index 3bb5e73..4ccea51 100755 --- a/src/python/mlat-test.py +++ b/src/python/mlat-test.py @@ -2,6 +2,12 @@ import mlat import numpy -ans = mlat.mlat(mlat.teststations, mlat.teststamps, mlat.testalt) +replies = [] +for i in range(0, len(mlat.teststations)): + replies.append((mlat.teststations[i], mlat.teststamps[i])) + +ans = mlat.mlat(replies, mlat.testalt) error = numpy.linalg.norm(numpy.array(mlat.llh2ecef(ans))-numpy.array(mlat.testplane)) -print error +range = numpy.linalg.norm(mlat.llh2geoid(ans)-numpy.array(mlat.llh2geoid(mlat.teststations[0]))) +print "Error: %.2fm" % (error) +print "Range: %.2fkm (from first station in list)" % (range/1000) diff --git a/src/python/mlat.py b/src/python/mlat.py index 1ce063c..d8b368a 100755 --- a/src/python/mlat.py +++ b/src/python/mlat.py @@ -153,8 +153,15 @@ teststamps = [10, ] #this function is the iterative solver core of the mlat function below -def mlat_iter(rel_stations, prange_obs, xguess = [0,0,0], numrounds = 10): - for i in range(0,numrounds): +#we use limit as a goal to stop solving when we get "close enough" (error magnitude in meters for that iteration) +#basically 20 meters is way less than the anticipated error of the system so it doesn't make sense to continue +#it's possible this could fail in situations where the solution converges slowly +#because the change in ERROR is not necessarily the error itself +#still, it should converge quickly when close, so it SHOULDN'T give more than 2-3x the precision in total error +def mlat_iter(rel_stations, prange_obs, xguess = [0,0,0], limit = 20, maxrounds = 50): + xerr = [1e9, 1e9, 1e9] + rounds = 0 + while numpy.linalg.norm(xerr) > limit: prange_est = [] for station in rel_stations: prange_est.append([numpy.linalg.norm(station - xguess)]) @@ -166,18 +173,28 @@ def mlat_iter(rel_stations, prange_obs, xguess = [0,0,0], numrounds = 10): #now we have H, the Jacobian, and can solve for residual error xerr = numpy.dot(numpy.linalg.solve(numpy.dot(H.T,H), H.T), dphat).flatten() xguess += xerr + rounds += 1 + if rounds > maxrounds: + raise Exception("Failed to converge!") + break return xguess #func mlat: #uses a modified GPS pseudorange solver to locate aircraft by multilateration. -#stations is a list of listening station positions in X,Y,Z ECEF format, geoid corrected -#timestamps is a list of times at which the correlated squitters were heard +#replies is a list of reports, in ([lat, lon, alt], timestamp) format #altitude is the barometric altitude of the aircraft as returned by the aircraft #returns the estimated position of the aircraft in (lat, lon, alt) geoid-corrected WGS84. -def mlat(stations, timestamps, altitude): - if len(timestamps) != len(stations): - raise Exception("Must have x timestamps for x stations reporting!") +#let's make it take a list of tuples so we can sort by them +def mlat(replies, altitude): + sorted_replies = sorted(replies, key=lambda time: time[1]) + stations = [] + timestamps = [] + #i really couldn't figure out how to unpack this, sorry + for i in range(0, len(replies)): + stations.append( sorted_replies[i][0] ) + timestamps.append( sorted_replies[i][1] ) + me_llh = stations[0] me = llh2geoid(stations[0]) From 756ccc8548625fe35513879c811130ece4d51cea Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Mon, 21 Mar 2011 15:18:08 -0700 Subject: [PATCH 10/11] Merge in UHD change to master --- src/python/uhd_modes.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/python/uhd_modes.py b/src/python/uhd_modes.py index 20bcad3..5ce65e9 100755 --- a/src/python/uhd_modes.py +++ b/src/python/uhd_modes.py @@ -54,7 +54,7 @@ class adsb_rx_block (gr.top_block): self.args = args if options.filename is None: - self.u = uhd.single_usrp_source("", uhd.io_type_t.COMPLEX_FLOAT32) + self.u = uhd.single_usrp_source("", uhd.io_type_t.COMPLEX_FLOAT32, 1) #if(options.rx_subdev_spec is None): # options.rx_subdev_spec = "" From f0b913fe3531403fc47c0709d0005279aa786aed Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Fri, 6 May 2011 10:39:43 +0200 Subject: [PATCH 11/11] make parser invocations match new prototype --- src/python/modes_sbs1.py | 2 +- src/python/modes_sql.py | 2 +- src/python/uhd_modes.py | 3 ++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/python/modes_sbs1.py b/src/python/modes_sbs1.py index cd8eb93..c75508c 100644 --- a/src/python/modes_sbs1.py +++ b/src/python/modes_sbs1.py @@ -59,7 +59,7 @@ class modes_output_sbs1(modes_parse.modes_parse): #assembles a SBS-1-style output string from the received message #this version ignores anything that isn't Type 17 for now, because we just don't care - [msgtype, shortdata, longdata, parity, ecc, reference] = message.split() + [msgtype, shortdata, longdata, parity, ecc, reference, time_secs, time_frac] = message.split() shortdata = long(shortdata, 16) longdata = long(longdata, 16) diff --git a/src/python/modes_sql.py b/src/python/modes_sql.py index 64accf1..6e188b9 100644 --- a/src/python/modes_sql.py +++ b/src/python/modes_sql.py @@ -68,7 +68,7 @@ class modes_output_sql(modes_parse.modes_parse): def make_insert_query(self, message): #assembles a SQL query tailored to our database #this version ignores anything that isn't Type 17 for now, because we just don't care - [msgtype, shortdata, longdata, parity, ecc, reference] = message.split() + [msgtype, shortdata, longdata, parity, ecc, reference, time_secs, time_frac] = message.split() shortdata = long(shortdata, 16) longdata = long(longdata, 16) diff --git a/src/python/uhd_modes.py b/src/python/uhd_modes.py index 5ce65e9..fd0299c 100755 --- a/src/python/uhd_modes.py +++ b/src/python/uhd_modes.py @@ -19,7 +19,8 @@ # Boston, MA 02110-1301, USA. # -my_position = [37.76225, -122.44254] +#my_position = [37.76225, -122.44254] +my_position = [37.409066,-122.077836] from gnuradio import gr, gru, optfir, eng_notation, blks2, air from gnuradio import uhd