Temp commit while adding a modes_report class using named tuples. Mlat server definitely broken as a result.

If all your stations lie in a plane (they do), there is an ambiguity about that plane such that there are two solutions.
This means you need to provide an initial guess on the correct side of the plane (i.e., above ground).
In addition, using a very reasonable solution guarantees that the system is as linear as possible.
I'm using the nearest station as a guess; I would have assumed it would work with any station as a guess but this
doesn't appear to be the case.
The Arne Saknussemm is still broken.

apps/CMakeLists.txt

@@ -21,6 +21,7 @@ include(GrPython)
 
 GR_PYTHON_INSTALL(
     PROGRAMS
+    mlat_server
     modes_rx
     modes_gui
     uhd_modes.py

apps/mlat_server

@@ -0,0 +1,265 @@
+#!/usr/bin/env python
+#
+# Copyright 2012 Nick Foster
+# 
+# This file is part of gr-air-modes
+# 
+# gr-air-modes is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 3, or (at your option)
+# any later version.
+# 
+# gr-air-modes is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+# 
+# You should have received a copy of the GNU General Public License
+# along with gr-air-modes; see the file COPYING.  If not, write to
+# the Free Software Foundation, Inc., 51 Franklin Street,
+# Boston, MA 02110-1301, USA.
+#
+
+#simple multilateration server app.
+#looks at received messages and then attempts to multilaterate positions
+#will not attempt clock synchronization yet; it's up to clients to present
+#accurate timestamps. later on we can do clock sync based on ADS-B packets.
+
+#SECURITY NOTE: THIS SERVER WAS WRITTEN WITH NO REGARD TOWARD SECURITY.
+#NO REAL ATTEMPT AT DATA VALIDATION, MUCH LESS SECURITY, HAS BEEN MADE.
+#USE THIS PROGRAM AT YOUR OWN RISK AND WITH THE UNDERSTANDING THAT THE
+#INTERNET IS A SCARY PLACE FULL OF MEAN PEOPLE.
+
+import time, os, sys, socket, struct
+from string import split, join
+from datetime import *
+import air_modes
+import pickle
+import time
+import bisect
+
+#change this to 0 for ASCII format for debugging. use HIGHEST_PROTOCOL
+#for actual use to keep the pickle size down.
+pickle_prot = 0
+#pickle_prot = pickle.HIGHEST_PROTOCOL
+
+def ordered_insert(a, item):
+    a.insert(bisect.bisect_right(a, item), item)
+
+class client_info:
+    def __init__(self):
+        self.name = ""
+        self.position = []
+        self.offset_secs = 0
+        self.offset_frac_secs = 0.0
+        self.time_source = None
+
+class connection:
+    def __init__(self, addr, sock, clientinfo):
+        self.addr = addr
+        self.sock = sock
+        self.clientinfo = clientinfo
+    
+class mlat_server:
+    def __init__(self, mypos, port):
+        self._s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self._s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        self._s.bind(('', port))
+        self._s.listen(1)
+        self._s.setblocking(0) #nonblocking
+        self._conns = [] #list of active connections
+        self._reports = {} #hashed by data
+        self._lastreport = 0 #used for pruning
+        self._parser = air_modes.parse(None)
+        self._remnant = None #for piecing together messages across packets
+
+    def __del__(self):
+        self._s.close()
+
+    def get_messages(self):
+        num=0
+        for conn in self._conns:
+            pkt = None
+            try:
+                pkt = conn.sock.recv(16384)
+            except socket.error:
+                print "%s disconnected" % conn.clientinfo.name
+                self._conns.remove(conn)
+            if not pkt: break
+            try:
+                for msg in pkt.splitlines(True):
+                    if msg.endswith("\n"):
+                        if self._remnant:
+                            msg = self._remnant + msg
+                            self._remnant = None
+                        [data, ecc, reference, timestamp_int, timestamp_frac] = msg.split()
+                        st = stamp(conn.clientinfo, int(timestamp_int), float(timestamp_frac))
+                        if data not in self._reports:
+                            self._reports[data] = []
+
+                        #ordered insert
+                        ordered_insert(self._reports[data], st)
+                        if st.tofloat() > self._lastreport:
+                            self._lastreport = st.tofloat()
+                        num += 1
+                    else:
+                        if self._remnant is not None:
+                            raise Exception("Malformed data: " + msg)
+                        else:
+                            self._remnant = msg
+
+            except Exception as e:
+                print "Invalid message from %s: %s..." % (conn.addr, pkt[:64])
+                print e
+
+        #self.prune()
+        return num
+
+    #prune should delete all reports in self._reports older than 5s.
+    #not really tested well
+    def prune(self):
+        for data, stamps in self._reports.iteritems():
+            #check the last stamp first so we don't iterate over
+            #the whole list if we don't have to
+            if self._lastreport - stamps[-1].tofloat() > 5:
+                del self._reports[data]
+            else:
+                for i,st in enumerate(stamps):
+                    if self._lastreport - st.tofloat() > 5:
+                        del self._reports[data][i]
+                if len(self._reports[data]) == 0:
+                    del self._reports[data]
+
+    #return a list of eligible messages for multilateration
+    #eligible reports are:
+    #1. bit-identical
+    #2. from distinct stations (at least 3)
+    #3. within 0.003 seconds of each other
+    #traverse the reports for each data pkt (hash) looking for >3 reports
+    #within 0.003s, then check for unique IPs (this should pass 99% of the time)
+    #let's break a record for most nested loops. this one goes four deep.
+    #it's loop-ception!
+    def get_eligible_reports(self):
+        groups = []
+        for data,stamps in self._reports.iteritems():
+            if len(stamps) >= 4: #quick check before we do a set()
+                stations = set([st.clientinfo for st in stamps])
+                if len(stations) >= 4:
+                    i=0
+                    #it's O(n) since the list is already sorted
+                    #can probably be cleaner and more concise
+                    while(i < len(stamps)):
+                        refstamp = stamps[i].tofloat()
+                        reps = []
+                        while (i<len(stamps)) and (stamps[i].tofloat() < (refstamp + 0.003)):
+                            reps.append(stamps[i])
+                            i+=1
+                        deduped = []
+                        for cinfo in stations:
+                            for st in reps[::-1]:
+                                if st.clientinfo == cinfo:
+                                    deduped.append(st)
+                                    break
+                        if len(deduped) >= 4:
+                            groups.append({"data": data, "stamps": deduped})
+
+        #TODO: pop the entries so you don't issue duplicate reports
+        if len(groups) > 0:
+            return groups
+        return None
+
+    #issue multilaterated positions
+    def output(self, msg):
+        if msg is not None:
+            try:
+                for conn in self._conns[:]: #iterate over a copy of the list
+                    conn.sock.send(msg)
+            except socket.error:
+                print "%s disconnected" % conn.clientinfo.name
+                self._conns.remove(conn)
+                print "Connections: ", len(self._conns)
+
+    #add a new connection to the list
+    def add_pending_conns(self):
+        try:
+            conn, addr = self._s.accept()
+            conn.send("HELO") #yeah it's like that
+            msg = conn.recv(1024)
+            if not msg:
+                return
+            try:
+                clientinfo = pickle.loads(msg)
+            except:
+                print "Invalid pickle received from client"
+                return
+    
+            if clientinfo.__class__.__name__ != "client_info":
+                print "Invalid datatype received from client"
+                return
+
+            self._conns.append(connection(addr[0], conn, clientinfo))
+            print "New connection from %s: %s" % (addr[0], clientinfo.name)
+        except socket.error:
+            pass
+            
+#retrieve altitude from a mode S packet or None if not available
+#returns alt in meters
+def get_modes_altitude(data):
+    df = data["df"] #reply type
+    f2m = 0.3048
+    if df == 0 or df == 4:
+        return air_modes.altitude.decode_alt(data["ac"], True)
+    elif df == 17:
+        bds = data["me"].get_type()
+        if bds == 0x05:
+            return f2m*air_modes.altitude.decode_alt(data["me"]["alt"], False)
+    else:
+        return None
+
+position = [37.76, -117.443, 100]
+
+if __name__=="__main__":
+    srv = mlat_server(position, 19005)
+    while 1:
+        nummsgs = srv.get_messages()
+        if len(srv._conns) > 0:
+            print "Received %i messages" % nummsgs
+        srv.add_pending_conns()
+        reps = srv.get_eligible_reports()
+        if reps:
+            for rep in reps:
+                print "Report with data %x" % rep["data"]
+                for st in rep["stamps"]:
+                    print "Stamp from %s: %f" % (st.clientinfo.name, st.tofloat())
+        srv.prune()
+
+        #now format the reports to get them ready for multilateration
+        #it's expecting a list of tuples [(station[], timestamp)...]
+        #also have to parse the data to pull altitude out of the mix
+        if reps:
+            for rep in reps:
+                alt = get_modes_altitude(air_modes.modes_reply(rep["data"]))
+                if (alt is None and len(rep["stamps"]) > 3) or alt is not None:
+                    mlat_list = [(st.clientinfo.position, st.tofloat()) for st in rep["stamps"]]
+                    print mlat_list
+                    #multilaterate!
+                    try:
+                        pos, senttime = air_modes.mlat.mlat(mlat_list, alt)
+                        if pos is not None:
+                            print "Resolved position: ", pos
+                            #send a report!
+                            msg = "%x %i %i %f %f %f %f %f %f\n" % \
+                                  (rep["data"], len(mlat_list), int(senttime), \
+                                   float(senttime) - int(senttime), \
+                                   pos[0], pos[1], pos[2], 0.0, 0.0)
+                            srv.output(msg)
+
+                    except air_modes.exceptions.MlatNonConvergeError as e:
+                        print e
+
+#DEBUG
+#        else:
+#            msg = "0921354 4 2 0.12345 36.671234 -112.342515 9028.1243 0.0 0.0\n"
+#            srv.output(msg)
+            
+        time.sleep(0.3)

apps/mlat_test.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+
+#test stuff for mlat server
+
+import socket, pickle, time, random, sys, math, numpy
+import air_modes
+
+class rx_data:
+    secs = 0
+    frac_secs = 0.0
+    data = None
+
+class client_info:
+    def __init__(self):
+        self.name = ""
+        self.position = []
+        self.offset_secs = 0
+        self.offset_frac_secs = 0.0
+
+info = client_info()
+info.name = sys.argv[1]
+info.position = [float(sys.argv[2]), float(sys.argv[3]), 100]
+info.offset_secs = 0
+info.offset_frac_secs = 0.0
+
+data1 = rx_data()
+data1.secs = 0
+data1.data = int("0x8da81f875857f10eb65b10cb66f3", 16)
+
+ac_starting_pos = [37.617175, -122.400843, 8000]
+ac_hdg = 130.
+ac_spd = 0.00008
+def get_pos(time):
+    return [ac_starting_pos[0] + ac_spd * time * math.cos(ac_hdg*math.pi/180.), \
+            ac_starting_pos[1] + ac_spd * time * math.sin(ac_hdg*math.pi/180.), \
+            ac_starting_pos[2]]
+
+def get_simulated_timestamp(time, position):
+    prange = numpy.linalg.norm(numpy.array(air_modes.mlat.llh2ecef(position))-numpy.array(air_modes.mlat.llh2geoid(info.position))) / air_modes.mlat.c
+    return time + prange + random.random()*60e-9 - 30e-9
+
+sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+sock.setblocking(1)
+sock.connect(("localhost", 19005))
+sock.send(pickle.dumps(info))
+print sock.recv(1024)
+sock.setblocking(0)
+ts = 0.0
+while 1:
+    pos = get_pos(ts)
+    stamp = get_simulated_timestamp(ts, pos)
+    print "Timestamp: %.10f" % (stamp)
+    print "Position: ", pos
+    data1.secs = int(stamp)
+    data1.frac_secs = float(stamp)
+    data1.frac_secs -= int(data1.frac_secs)
+    sock.send(pickle.dumps([data1]))
+    try:
+        positions = sock.recv(1024)
+    except:
+        pass
+    if positions: print positions
+    ts+=1
+    time.sleep(1)
+
+sock.close()
+sock = None

apps/modes_rx

@@ -27,9 +27,12 @@ from optparse import OptionParser
 import time, os, sys, threading
 from string import split, join
 import air_modes
+from air_modes.types import modes_report, stamp
+from air_modes.parse import modes_reply
 import gnuradio.gr.gr_threading as _threading
 import csv
 from air_modes.exceptions import *
+import pickle
 
 class top_block_runner(_threading.Thread):
     def __init__(self, tb):
@@ -51,17 +54,22 @@ class adsb_rx_block (gr.top_block):
     self.args = args
     rate = int(options.rate)
     use_resampler = False
+    self.time_source = None
 
     if options.filename is None and options.udp is None and not options.rtlsdr:
       #UHD source by default
       from gnuradio import uhd
       self.u = uhd.single_usrp_source(options.args, uhd.io_type_t.COMPLEX_FLOAT32, 1)
-      time_spec = uhd.time_spec(0.0)
-      self.u.set_time_now(time_spec)
 
-      #if(options.rx_subdev_spec is None):
-      #  options.rx_subdev_spec = ""
-      #self.u.set_subdev_spec(options.rx_subdev_spec)
+      #check for GPSDO
+      #if you have a GPSDO, UHD will automatically set the timestamp to UTC time
+      #as well as automatically set the clock to lock to GPSDO.
+      if self.u.get_time_source(0) == 'gpsdo':
+        self.time_source = 'gpsdo'
+      else:
+        self.time_source = None
+        self.u.set_time_now(uhd.time_spec(0.0))
+
       if not options.antenna is None:
         self.u.set_antenna(options.antenna)
 
@@ -126,6 +134,9 @@ class adsb_rx_block (gr.top_block):
 def printraw(msg):
     print msg
 
+def printmlat(msg):
+    print "Mlat report: %s" % msg
+
 if __name__ == '__main__':
   usage = "%prog: [options] output filename"
   parser = OptionParser(option_class=eng_option, usage=usage)
@@ -165,6 +176,8 @@ if __name__ == '__main__':
                       help="Use RTLSDR dongle instead of UHD source")
   parser.add_option("-p","--pmf", action="store_true", default=False,
                       help="Use pulse matched filtering")
+  parser.add_option("-s","--mlat-server", type="string", default=None,
+                      help="Use a multilateration server to track non-ADS-B aircraft")
                       
   (options, args) = parser.parse_args()
 
@@ -173,8 +186,10 @@ if __name__ == '__main__':
     my_position = reader.next()
 
   queue = gr.msg_queue()
+  mlat_queue = None
   
   outputs = [] #registry of plugin output functions
+  mlat_outputs = [] #registry of plugin mlat handling functions
   updates = [] #registry of plugin update functions
 
   if options.raw is True:
@@ -205,13 +220,21 @@ if __name__ == '__main__':
     outputs.append(fgout.output)
 
   fg = adsb_rx_block(options, args, queue)
+
+  if options.mlat_server is not None:
+    mlat_queue = gr.msg_queue()
+    mlat_client = air_modes.mlat_client(mlat_queue, my_position, options.mlat_server, fg.time_source)
+    outputs.append(mlat_client.output) #output to server when we get a report
+    updates.append(mlat_client.get_mlat_positions) #check for replies from the server
+    #note: this means that get_mlat_positions and printmlat execute in the same thread.
+    #you could just have mlat_client spawn a thread to check its socket. might make more sense to do this.
+    mlat_outputs.append(printmlat)
+  
   runner = top_block_runner(fg)
 
   while 1:
     try:
-      #the update registry is really for the SBS1 and raw server plugins -- we're looking for new TCP connections.
-      #i think we have to do this here rather than in the output handler because otherwise connections will stack up
-      #until the next output arrives
+      #handle the once-per-loop updates (check for mlat responses, add TCP conns to SBS-1 plugin, etc.)
       for update in updates:
         update()
       
@@ -219,14 +242,26 @@ if __name__ == '__main__':
       if not queue.empty_p() :
         while not queue.empty_p() :
           msg = queue.delete_head() #blocking read
-
+          [data, ecc, reference, timestamp_int, timestamp_frac] = msg.to_string().split()
+          #error handling? creating a modes_reply can throw NoHandlerError, but you really want that to be handled by the output plugin in question.
+          msg_tuple = modes_report(modes_reply(long(data, 16)), long(ecc, 16), float(reference), stamp(int(timestamp_int), float(timestamp_frac)))
           for out in outputs:
             try:
-              out(msg.to_string())
+              out(msg_tuple)
             except air_modes.ADSBError:
               pass
 
-      elif runner.done:
+      if mlat_queue:
+        if not mlat_queue.empty_p():
+          while not mlat_queue.empty_p():
+            msg = mlat_queue.delete_head()
+            for out in mlat_outputs:
+              try:
+                out(msg.to_string())
+              except air_modes.ADSBError:
+                pass
+
+      if runner.done:
         raise KeyboardInterrupt
       else:
         time.sleep(0.1)

include/air_modes_slicer.h

@@ -47,7 +47,8 @@ private:
     int d_samples_per_chip;
     int d_samples_per_symbol;
     gr_msg_queue_sptr d_queue;
-    std::ostringstream d_payload;
+    unsigned char d_lowconfbits[24];
+    unsigned char d_data[14];
 
 public:
     int work (int noutput_items,

include/air_modes_types.h

@@ -24,20 +24,6 @@
 #define AIR_MODES_TYPES_H
 
 typedef enum { No_Packet = 0, Short_Packet = 1, Fruited_Packet = 2, Long_Packet = 3 } framer_packet_type;
-typedef enum { No_Error = 0, Solution_Found, Too_Many_LCBs, No_Solution, Multiple_Solutions } bruteResultTypeDef;
-
-struct modes_packet {
-	unsigned char data[14];
-//	unsigned char confidence[14]; //112 bits of boolean high/low confidence data for each bit
-	unsigned char lowconfbits[24]; //positions of low confidence bits within the packet
-
-	unsigned long crc;
-	unsigned int numlowconf;
-	framer_packet_type type; //what length packet are we
-	unsigned int message_type;
-	float reference_level;
-	double timestamp;
-};
 
 struct slice_result_t {
 	bool decision;

include/modes_crc.h

@@ -24,7 +24,4 @@
 #define INCLUDED_MODES_CRC_H
 extern const unsigned int modes_crc_table[112];
 int modes_check_crc(unsigned char data[], int length);
-bruteResultTypeDef modes_ec_brute(modes_packet &err_packet);
-unsigned next_set_of_n_elements(unsigned x);
-
 #endif

lib/air_modes_preamble.cc

@@ -78,20 +78,31 @@ static double correlate_preamble(const float *in, int samples_per_chip) {
 	return corr;
 }
 
-//todo: make it return a pair of some kind, otherwise you can lose precision
-static double tag_to_timestamp(gr_tag_t tstamp, uint64_t abs_sample_cnt, double secs_per_sample) {
+//takes an rx tag and issues a tag offset appropriately for the preamble
+static const pmt::pmt_t offset_stamp(const gr_tag_t &tstamp, const uint64_t abs_sample_cnt, const double secs_per_sample) {
 	uint64_t ts_sample, last_whole_stamp;
 	double last_frac_stamp;
 
-	if(tstamp.key == NULL || pmt::pmt_symbol_to_string(tstamp.key) != "rx_time") return 0;
-
-	last_whole_stamp = pmt::pmt_to_uint64(pmt::pmt_tuple_ref(tstamp.value, 0));
-	last_frac_stamp = pmt::pmt_to_double(pmt::pmt_tuple_ref(tstamp.value, 1));
-	ts_sample = tstamp.offset;
+	if(tstamp.key == NULL || pmt::pmt_symbol_to_string(tstamp.key) != "rx_time") {
+		last_whole_stamp = 0;
+		last_frac_stamp = 0;
+		ts_sample = 0;
+	} else {
+		last_whole_stamp = pmt::pmt_to_uint64(pmt::pmt_tuple_ref(tstamp.value, 0));
+		last_frac_stamp = pmt::pmt_to_double(pmt::pmt_tuple_ref(tstamp.value, 1));
+		ts_sample = tstamp.offset;
+	}
 	
-	double tstime = double(abs_sample_cnt * secs_per_sample) + last_whole_stamp + last_frac_stamp;
-	if(0) std::cout << "HEY WE GOT A STAMP AT " << tstime << " TICKS AT SAMPLE " << ts_sample << " ABS SAMPLE CNT IS " << abs_sample_cnt << std::endl;
-	return tstime;
+	double fractime = double(abs_sample_cnt * secs_per_sample) + last_frac_stamp;
+	last_whole_stamp += int(fractime);
+	fractime -= int(fractime);
+
+	const pmt::pmt_t newval = pmt::pmt_make_tuple(
+		pmt::pmt_from_uint64(last_whole_stamp),
+		pmt::pmt_from_double(fractime)
+	);
+
+	return newval;
 }
 
 int air_modes_preamble::general_work(int noutput_items,
@@ -186,16 +197,14 @@ 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 = tag_to_timestamp(d_timestamp, abs_sample_cnt + i, d_secs_per_sample);
-
+			const pmt::pmt_t new_pmt = offset_stamp(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(tstamp),
-			         d_me        //block src id
-			        );
+					nitems_written(0), //sample
+					d_key,      //frame_info
+					new_pmt,
+					d_me        //block src id
+					);
 					 
 			//std::cout << "PREAMBLE" << std::endl;
 			

lib/air_modes_slicer.cc

@@ -112,84 +112,79 @@ int air_modes_slicer::work(int noutput_items,
 
 	for(tag_iter = tags.begin(); tag_iter != tags.end(); tag_iter++) {
 		uint64_t i = tag_iter->offset - abs_sample_cnt;
-		modes_packet rx_packet;
 
-		memset(&rx_packet.data, 0x00, 14 * sizeof(unsigned char));
-		memset(&rx_packet.lowconfbits, 0x00, 24 * sizeof(unsigned char));
-		rx_packet.numlowconf = 0;
+		memset(&d_data, 0x00, 14 * sizeof(unsigned char));
+		memset(&d_lowconfbits, 0x00, 24 * sizeof(unsigned char));
+		unsigned int numlowconf = 0;
 
 		//let's use the preamble to get a reference level for the packet
 		//fixme: a better thing to do is create a bi-level avg 1 and avg 0
 		//through simple statistics, then take the median for your slice level
 		//this won't improve decoding but will improve confidence
-		rx_packet.reference_level = (in[i]
-								   + in[i+2]
-								   + in[i+7]
-								   + in[i+9]) / 4.0;
+		double reference_level = (in[i]
+							    + in[i+2]
+							    + in[i+7]
+							    + in[i+9]) / 4.0;
 
 		i += 16; //move on up to the first bit of the packet data
 		//now let's slice the header so we can determine if it's a short pkt or a long pkt
 		unsigned char pkt_hdr = 0;
 		for(int j=0; j < 5; j++) {
-			slice_result_t slice_result = slicer(in[i+j*2], in[i+j*2+1], rx_packet.reference_level);
+			slice_result_t slice_result = slicer(in[i+j*2], in[i+j*2+1], reference_level);
 			if(slice_result.decision) pkt_hdr += 1 << (4-j);
 		}
-		if(pkt_hdr == 16 or pkt_hdr == 17 or pkt_hdr == 20 or pkt_hdr == 21) rx_packet.type = Long_Packet;
-		else rx_packet.type = Short_Packet;
-		int packet_length = (rx_packet.type == framer_packet_type(Short_Packet)) ? 56 : 112;
+		framer_packet_type type;
+		if(pkt_hdr == 16 or pkt_hdr == 17 or pkt_hdr == 20 or pkt_hdr == 21) type = Long_Packet;
+		else type = Short_Packet;
+		int packet_length = (type == Short_Packet) ? 56 : 112;
 
 		//it's slice time!
 		//TODO: don't repeat your work here, you already have the first 5 bits
+		slice_result_t slice_result;
 		for(int j = 0; j < packet_length; j++) {
-			slice_result_t slice_result = slicer(in[i+j*2], in[i+j*2+1], rx_packet.reference_level);
+			slice_result = slicer(in[i+j*2], in[i+j*2+1], reference_level);
 
 			//put the data into the packet
 			if(slice_result.decision) {
-				rx_packet.data[j/8] += 1 << (7-(j%8));
+				d_data[j/8] += 1 << (7-(j%8));
 			}
 			//put the confidence decision into the packet
 			if(slice_result.confidence) {
-				//rx_packet.confidence[j/8] += 1 << (7-(j%8));
+				//d_confidence[j/8] += 1 << (7-(j%8));
 			} else {
-				if(rx_packet.numlowconf < 24) rx_packet.lowconfbits[rx_packet.numlowconf++] = j;
+				if(numlowconf < 24) d_lowconfbits[numlowconf++] = j;
 			}
 		}
-			
-		/******************** BEGIN TIMESTAMP BS ******************/
-		rx_packet.timestamp = pmt_to_double(tag_iter->value);
-		/******************* END TIMESTAMP BS *********************/
-			
-		//increment for the next round
+
+		uint64_t timestamp_secs = pmt::pmt_to_uint64(pmt::pmt_tuple_ref(tag_iter->value, 0));
+		double timestamp_frac = pmt::pmt_to_double(pmt::pmt_tuple_ref(tag_iter->value, 1));
 
 		//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(d_data[m]) zeroes = 0;
 		}
 		if(zeroes) {continue;} //toss it
 
-		rx_packet.message_type = (rx_packet.data[0] >> 3) & 0x1F; //get the message type to make decisions on ECC methods
+		unsigned int message_type = (d_data[0] >> 3) & 0x1F; //get the message type to make decisions on ECC methods
 
-		if(rx_packet.type == Short_Packet && rx_packet.message_type != 11 && rx_packet.numlowconf > 0) {continue;}
-		if(rx_packet.message_type == 11 && rx_packet.numlowconf >= 10) {continue;}
+		if(type == Short_Packet && message_type != 11 && numlowconf > 0) {continue;}
+		if(message_type == 11 && numlowconf >= 10) {continue;}
 			
-		rx_packet.crc = modes_check_crc(rx_packet.data, packet_length);
+		unsigned long crc = modes_check_crc(d_data, packet_length);
 
 		//crc for packets that aren't type 11 or type 17 is encoded with the transponder ID, which we don't know
-		//therefore we toss 'em if there's syndrome
-		//crc for the other short packets is usually nonzero, so they can't really be trusted that far
-		if(rx_packet.crc && (rx_packet.message_type == 11 || rx_packet.message_type == 17)) {continue;}
-
-		d_payload.str("");
+		//we forward them if they have no low-confidence bits (see above), but this still lets some crap through.
+		if(crc && (message_type == 11 || message_type == 17)) {continue;}
+		std::ostringstream payload;
 		for(int m = 0; m < packet_length/8; m++) {
-			d_payload << std::hex << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]);
+			payload << std::hex << std::setw(2) << std::setfill('0') << unsigned(d_data[m]);
 		}
 
-		d_payload << " " << std::setw(6) << rx_packet.crc << " " << std::dec << rx_packet.reference_level
-		          << " " << std::setprecision(10) << std::setw(10) << rx_packet.timestamp;
-			gr_message_sptr msg = gr_make_message_from_string(std::string(d_payload.str()));
+		payload << " " << std::setw(6) << crc << " " << std::dec << reference_level
+		          << " " << timestamp_secs << " " << std::setprecision(20) << timestamp_frac;
+			gr_message_sptr msg = gr_make_message_from_string(std::string(payload.str()));
 		d_queue->handle(msg);
 	}
-	if(0) std::cout << "Slicer consumed " << size << ", returned " << size << std::endl;
 	return size;
 }

python/CMakeLists.txt

@@ -35,6 +35,7 @@ GR_PYTHON_INSTALL(
     az_map.py
     cpr.py
     mlat.py
+    mlat_client.py
     exceptions.py
     flightgear.py
     gui_model.py
@@ -45,6 +46,7 @@ GR_PYTHON_INSTALL(
     rx_path.py
     sbs1.py
     sql.py
+    types.py
     Quaternion.py
     DESTINATION ${GR_PYTHON_DIR}/air_modes
 )

python/__init__.py

@@ -58,8 +58,10 @@ from sql import output_sql
 from sbs1 import output_sbs1
 from kml import output_kml
 from raw_server import raw_server
+from mlat_client import mlat_client
 from exceptions import *
 from az_map import *
+from types import *
 #this is try/excepted in case the user doesn't have numpy installed
 try:
     from flightgear import output_flightgear

python/exceptions.py

@@ -44,4 +44,3 @@ class CPRBoundaryStraddleError(CPRNoPositionError):
 class FieldNotInPacket(ParserError):
     def __init__(self, item):
         self.item = item
-

python/flightgear.py

@@ -27,36 +27,34 @@ class output_flightgear(air_modes.parse):
         self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
         self.sock.connect((self.hostname, self.port))
 
-    def output(self, message):
-        [data, ecc, reference, timestamp] = message.split()
-        data = air_modes.modes_reply(long(data, 16))
+    def output(self, msg):
         
         try:
-            msgtype = data["df"]
+            msgtype = msg.data["df"]
             if msgtype == 17: #ADS-B report
-                icao24 = data["aa"]
-                bdsreg = data["me"].get_type()
+                icao24 = msg.data["aa"]
+                bdsreg = msg.data["me"].get_type()
                 if bdsreg == 0x08: #ident packet
-                    (ident, actype) = self.parseBDS08(data)
+                    (ident, actype) = self.parseBDS08(msg.data)
                     #select model based on actype
                     self.callsigns[icao24] = [ident, actype]
                     
                 elif bdsreg == 0x06: #BDS0,6 pos
-                    [ground_track, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS06(data)
+                    [ground_track, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS06(msg.data)
                     self.positions[icao24] = [decoded_lat, decoded_lon, 0]
                     self.update(icao24)
 
                 elif bdsreg == 0x05: #BDS0,5 pos
-                    [altitude, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS05(data)
+                    [altitude, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS05(msg.data)
                     self.positions[icao24] = [decoded_lat, decoded_lon, altitude]
                     self.update(icao24)
 
                 elif bdsreg == 0x09: #velocity
                     subtype = data["bds09"].get_type()
                     if subtype == 0:
-                      [velocity, heading, vert_spd, turnrate] = self.parseBDS09_0(data)
+                      [velocity, heading, vert_spd, turnrate] = self.parseBDS09_0(msg.data)
                     elif subtype == 1:
-                      [velocity, heading, vert_spd] = self.parseBDS09_1(data)
+                      [velocity, heading, vert_spd] = self.parseBDS09_1(msg.data)
                       turnrate = 0
                     else:
                         return

python/mlat.py

@@ -95,28 +95,31 @@ c = 299792458 / 1.0003 #modified for refractive index of air, why not
 #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
-#TODO: this fails to converge for some seriously advantageous geometry
-def mlat_iter(rel_stations, prange_obs, xguess = [0,0,0], limit = 20, maxrounds = 100):
-    xerr = [1e9, 1e9, 1e9]
+#THIS WORKS PLEASE DON'T MESS WITH IT
+def mlat_iter(stations, prange_obs, guess = [0,0,0], limit = 20, maxrounds = 100):
+    xerr = [1e9, 1e9, 1e9, 1e9]
     rounds = 0
-    while numpy.linalg.norm(xerr) > limit:
-        prange_est = [[numpy.linalg.norm(station - xguess)] for station in rel_stations]
+    while numpy.linalg.norm(xerr[:3]) > limit:
+        #get p_i, the estimated pseudoranges based on the latest position guess
+        prange_est = [[numpy.linalg.norm(station - guess)] for station in stations]
+        #get the difference d_p^ between the observed and calculated pseudoranges
         dphat = prange_obs - prange_est
-        H = numpy.array([(numpy.array(-rel_stations[row,:])+xguess) / prange_est[row] for row in range(0,len(rel_stations))])
+        #create a matrix of partial differentials to find the slope of the error in X,Y,Z,t directions
+        H = numpy.array([(numpy.array(-stations[row,:])+guess) / prange_est[row] for row in range(len(stations))])
+        H = numpy.append(H, numpy.ones(len(prange_obs)).reshape(len(prange_obs),1)*c, axis=1)
         #now we have H, the Jacobian, and can solve for residual error
-        xerr = numpy.linalg.lstsq(H, dphat)[0].flatten()
-        xguess += xerr
-        #print xguess, xerr
+        solved = numpy.linalg.lstsq(H, dphat)
+        xerr = solved[0].flatten()
+        guess += xerr[:3] #we ignore the time error for xguess
         rounds += 1
         if rounds > maxrounds:
-            raise Exception("Failed to converge!")
-            break
-    return xguess
+            raise MlatNonConvergeError("Failed to converge!")
+    return (guess, xerr[3])
 
 #func mlat:
 #uses a modified GPS pseudorange solver to locate aircraft by multilateration.
 #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
+#altitude is the barometric altitude of the aircraft as returned by the aircraft, if any
 #returns the estimated position of the aircraft in (lat, lon, alt) geoid-corrected WGS84.
 #let's make it take a list of tuples so we can sort by them
 def mlat(replies, altitude):
@@ -124,76 +127,70 @@ def mlat(replies, altitude):
 
     stations = [sorted_reply[0] for sorted_reply in sorted_replies]
     timestamps = [sorted_reply[1] for sorted_reply in sorted_replies]
+    nearest_llh = stations[0]
+    stations_xyz = numpy.array([llh2geoid(station) for station in stations])
 
-    me_llh = stations[0]
-    me = llh2geoid(stations[0])
+    #the absolute time shouldn't matter at all except perhaps in
+    #maintaining floating-point precision; in other words you can
+    #add a constant here and it should fall out in the solver as time error
+    prange_obs = [[c*stamp] for stamp in timestamps]
 
-    
-    #list of stations in XYZ relative to me
-    rel_stations = [numpy.array(llh2geoid(station)) - numpy.array(me) for station in stations[1:]]
-    rel_stations.append([0,0,0] - numpy.array(me))
-    rel_stations = numpy.array(rel_stations) #convert list of arrays to 2d array
+    #if no alt, use a reasonably large number (in meters)
+    #since if all your stations lie in a plane (they basically will),
+    #there's a reasonable solution at negative altitude as well
+    if altitude is None:
+        altitude = 20000
+    firstguess = numpy.array(llh2ecef((nearest_llh[0], nearest_llh[1], altitude)))
 
-    #differentiate the timestamps to get TDOA, multiply by c to get pseudorange
-    prange_obs = [[c*(stamp-timestamps[0])] for stamp in timestamps[1:]]
-
-    #so here we calc the estimated pseudorange to the center of the earth, using station[0] as a reference point for the geoid
-    #in other words, we say "if the aircraft were directly overhead of station[0], this is the prange to the center of the earth"
-    #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 = numpy.array(prange_obs)
-
-    #xguess = llh2ecef([37.617175,-122.400843, 8000])-numpy.array(me)
-    #xguess = [0,0,0]
-    #start our guess directly overhead, who cares
-    xguess = numpy.array(llh2ecef([me_llh[0], me_llh[1], altitude])) - numpy.array(me)
+    xyzpos, time_offset = mlat_iter(stations_xyz, prange_obs, firstguess, maxrounds=100)
+    llhpos = ecef2llh(xyzpos)
     
-    xyzpos = mlat_iter(rel_stations, prange_obs, xguess)
-    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
-    #and <4cm errors with it, not that we'll get that close in reality but hey let's do it right
-    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, time_offset)
 
-    #and now, what the hell, let's try to get dilution of precision data
-    #avec is the unit vector of relative ranges to the aircraft from each of the stations
-#    for i in range(len(avec)):
-#        avec[i] = numpy.array(avec[i]) / numpy.linalg.norm(numpy.array(avec[i]))
-#    numpy.append(avec, [[-1],[-1],[-1],[-1]], 1) #must be # of stations
-#    doparray = numpy.linalg.inv(avec.T*avec)
-#the diagonal elements of doparray will be the x, y, z DOPs.
-    
-    return llhpos
+#tests the mlat_iter algorithm using sample data from Farrell & Barth (p.147)
+def farrell_barth_test():
+    pranges = numpy.array([[22228206.42],
+                           [24096139.11],
+                           [21729070.63],
+                           [21259581.09]])
+    svpos = numpy.array([[7766188.44, -21960535.34, 12522838.56],
+                         [-25922679.66, -6629461.28, 31864.37],
+                         [-5743774.02, -25828319.92, 1692757.72],
+                         [-2786005.69, -15900725.80, 21302003.49]])
 
+    #this is the "correct" resolved position, not the real receiver position
+    known_pos = numpy.array( [-2430745.0959362, -4702345.11359277, 3546568.70599656])
+
+    pos, time_offset = mlat_iter(svpos, pranges)
+    print "Position: ", pos
+    print "LLH: ", ecef2llh(pos)
+    error = numpy.linalg.norm(pos - known_pos)
+    print "Error: ", error
+    if error < 1e-3:
+        print "Farrell & Barth test OK"
+    else:
+        raise Exception("ERROR: Failed Farrell & Barth test")
 
 if __name__ == '__main__':
-    #here's some test data to validate the algorithm
+    #check to see that you haven't screwed up mlat_iter
+    farrell_barth_test()
+
+    #construct simulated returns from these stations
     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(llh2ecef([37.617175,-122.400843, 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,
-                    10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[3]))) / c,
-                ]
+    tx_time      = 10 #time offset to apply to timestamps
+    teststamps   = [tx_time+numpy.linalg.norm(testplane-numpy.array(llh2geoid(station))) / c for station in teststations]
 
-    print teststamps
+    print "Actual pranges: ", sorted([numpy.linalg.norm(testplane - numpy.array(llh2geoid(station))) for station in teststations])
 
-    replies = []
-    for i in range(0, len(teststations)):
-        replies.append((teststations[i], teststamps[i]))
-    ans = mlat(replies, testalt)
+    replies = [(station, stamp) for station,stamp in zip(teststations, teststamps)]
+
+    ans, offset = mlat(replies, testalt)
     error = numpy.linalg.norm(numpy.array(llh2ecef(ans))-numpy.array(testplane))
-    range = numpy.linalg.norm(llh2geoid(ans)-numpy.array(testme))
-    print testplane-testme
-    print ans
+    rng = numpy.linalg.norm(llh2geoid(ans)-numpy.array(llh2geoid(teststations[0])))
+    print "Resolved lat/lon/alt: ", ans
     print "Error: %.2fm" % (error)
-    print "Range: %.2fkm (from first station in list)" % (range/1000)
+    print "Range: %.2fkm (from first station in list)" % (rng/1000)
+    print "Aircraft-local transmit time: %.8fs" % (offset)

python/msprint.py

@@ -29,39 +29,33 @@ class output_print(air_modes.parse):
   def __init__(self, mypos):
       air_modes.parse.__init__(self, mypos)
       
-  def parse(self, message):
-    [data, ecc, reference, timestamp] = message.split()
+  def parse(self, msg):
 
-    ecc = long(ecc, 16)
-    reference = float(reference)
-    timestamp = float(timestamp)
-
-    if reference == 0.0:
+    if msg.reference == 0.0:
       refdb = -150.0
     else:
-      refdb = 20.0*math.log10(reference)
-    output = "(%.0f %.10f) " % (refdb, timestamp);
+      refdb = 20.0*math.log10(msg.reference)
+    output = "(%.0f %.10f) " % (refdb, float(msg.timestamp));
 
     try:
-      data = air_modes.modes_reply(long(data, 16))
-      msgtype = data["df"]
+      msgtype = msg.data["df"]
       if msgtype == 0:
-        output += self.print0(data, ecc)
+        output += self.print0(msg.data, msg.ecc)
       elif msgtype == 4:
-        output += self.print4(data, ecc)
+        output += self.print4(msg.data, msg.ecc)
       elif msgtype == 5:
-        output += self.print5(data, ecc)
+        output += self.print5(msg.data, msg.ecc)
       elif msgtype == 11:
-        output += self.print11(data, ecc)
+        output += self.print11(msg.data, msg.ecc)
       elif msgtype == 17:
-        output += self.print17(data)
+        output += self.print17(msg.data)
       elif msgtype == 20 or msgtype == 21 or msgtype == 16:
-        output += self.printTCAS(data, ecc)
+        output += self.printTCAS(msg.data, msg.ecc)
       else:
-        output += "No handler for message type %i from %x (but it's in modes_parse)" % (msgtype, ecc)
+        output += "No handler for message type %i from %x (but it's in modes_parse)" % (msgtype, msg.ecc)
       return output
     except NoHandlerError as e:
-      output += "No handler for message type %s from %x" % (e.msgtype, ecc)
+      output += "No handler for message type %s from %x" % (e.msgtype, msg.ecc)
       return output
     except MetricAltError:
       pass

python/sbs1.py

@@ -100,26 +100,21 @@ class output_sbs1(air_modes.parse):
     else:
       return ",,,"
 
-  def parse(self, message):
+  def parse(self, msg):
     #assembles a SBS-1-style output string from the received message
-
-    [data, ecc, reference, timestamp] = message.split()
-
-    data = air_modes.modes_reply(long(data, 16))
-    ecc = long(ecc, 16)
-    msgtype = data["df"]
+    msgtype = msg.data["df"]
     outmsg = None
 
     if msgtype == 0:
-      outmsg = self.pp0(data, ecc)
+      outmsg = self.pp0(msg.data, msg.ecc)
     elif msgtype == 4:
-      outmsg = self.pp4(data, ecc)
+      outmsg = self.pp4(msg.data, msg.ecc)
     elif msgtype == 5:
-      outmsg = self.pp5(data, ecc)
+      outmsg = self.pp5(msg.data, msg.ecc)
     elif msgtype == 11:
-      outmsg = self.pp11(data, ecc)
+      outmsg = self.pp11(msg.data, msg.ecc)
     elif msgtype == 17:
-      outmsg = self.pp17(data)
+      outmsg = self.pp17(msg.data)
     else:
       raise NoHandlerError(msgtype)
     return outmsg

python/sql.py

@@ -86,20 +86,13 @@ class output_sql(air_modes.parse):
       except ADSBError:
         pass
 
-  def make_insert_query(self, message):
+  def make_insert_query(self, msg):
     #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
-    [data, ecc, reference, timestamp] = message.split()
-
-    data = air_modes.modes_reply(long(data, 16))
-    ecc = long(ecc, 16)
-#   reference = float(reference)
-
-
     query = None
-    msgtype = data["df"]
+    msgtype = msg.data["df"]
     if msgtype == 17:
-      query = self.sql17(data)
+      query = self.sql17(msg.data)
 
     return query
 

python/types.py

@@ -0,0 +1,62 @@
+#
+# Copyright 2013 Nick Foster
+# 
+# This file is part of gr-air-modes
+# 
+# gr-air-modes is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 3, or (at your option)
+# any later version.
+# 
+# gr-air-modes is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+# 
+# You should have received a copy of the GNU General Public License
+# along with gr-air-modes; see the file COPYING.  If not, write to
+# the Free Software Foundation, Inc., 51 Franklin Street,
+# Boston, MA 02110-1301, USA.
+#
+
+from collections import namedtuple
+
+#this is a timestamp that preserves precision when used with UTC timestamps
+#ordinary double-precision timestamp would lose significant fractional precision
+#when the mantissa is as large as necessary for UTC timestamps.
+class stamp:
+    def __init__(self, secs, frac_secs):
+        self.secs = secs
+        self.frac_secs = frac_secs
+    def __lt__(self, other):
+        if self.secs == other.secs:
+            return self.frac_secs < other.frac_secs
+        else:
+            return self.secs < other.secs
+    def __gt__(self, other):
+        if self.secs == other.secs:
+            return self.frac_secs > other.frac_secs
+        else:
+            return self.secs > other.secs
+    def __eq__(self, other):
+        if isinstance(other, self.__class__):
+            return self.secs == other.secs and self.frac_secs == other.frac_secs
+        elif isinstance(other, float):
+            return float(self) == other
+        else:
+            raise TypeError
+    def __ne__(self, other):
+        return self.secs != other.secs or self.frac_secs != other.frac_secs
+    def __le__(self, other):
+        return (self == other) or (self < other)
+    def __ge__(self, other):
+        return (self == other) or (self > other)
+
+    #to ensure we don't hash by stamp
+    __hash__ = None
+    
+    #good to within ms for comparison
+    def __float__(self):
+        return self.secs + self.frac_secs
+
+modes_report = namedtuple('modes_report', ['data', 'ecc', 'reference', 'timestamp'])