First stab at multilateration server. No client, incomplete.

apps/mlat_server.py

@@ -0,0 +1,237 @@
+#!/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.
+#
+
+import time, os, sys, socket, struct
+from string import split, join
+from datetime import *
+import air_modes
+import pickle
+import time
+import bisect
+
+#simple multilateration server app.
+#accepts connections from clients (need a raw_client output thing)
+#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.
+
+
+#how to store records for quick retrieval?
+#the data is really a hash; we use it to find correlated records.
+#self._records should be a dict of replies
+#so: { <adsbdata>: [{ "addr": "192.168.10.1", "secs": 11, "frac_secs": 0.123456 }, {....}...] ... }
+#adsbdata should be an int.
+
+#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
+
+class rx_data:
+    def __init__(self):
+        self.secs = 0
+        self.frac_secs = 0.0
+        self.data = None
+
+class stamp:
+    def __init__(self, addr, secs, frac_secs):
+        self.addr = addr
+        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):
+        return self.secs == other.secs and self.frac_secs == other.frac_secs
+    def __ne__(self, other):
+        return self.secs != other.secs or self.frac_secs != other.frac_secs
+    #good to within ms for comparison
+    def tofloat(self):
+        return self.secs + self.frac_secs
+
+def ordered_insert(a, item):
+    a.insert(bisect.bisect_right(a, item), item)
+
+class client_info:
+    def __init__(self):
+        self.name = ""
+        self.latitude = 0.0
+        self.longitude = 0.0
+        self.altitude = 0.0
+        self.offset_secs = 0
+        self.offset_frac_secs = 0.0
+
+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)
+
+    def __del__(self):
+        self._s.close()
+
+    def get_messages(self):
+        for conn in self._conns:
+            pkt = None
+            try:
+                pkt = conn.sock.recv(1024)
+            except socket.error:
+                self._conns.remove(conn)
+            if not pkt: break
+            #try:
+            msglist = pickle.loads(pkt)
+            for msg in msglist:
+                #DEBUG: change conn.clientinfo.name to conn.addr for production
+                st = stamp(conn.clientinfo.name, msg.secs, msg.frac_secs)
+                if msg.data not in self._reports:
+                    self._reports[msg.data] = []
+
+                #ordered insert
+                ordered_insert(self._reports[msg.data], st)
+                self._lastreport = st.tofloat()
+#            for report in self._reports.values():
+#                for st in report:
+#                    print st.addr, st.secs, st.frac_secs
+
+            #except Exception as e:
+            #    print "Invalid message from %s: %s" % (conn.addr, pkt)
+            #    print e
+
+        #self.prune()
+
+    #prune should delete all reports in self._reports older than 5s.
+    #how do we get the appropriate time? we either trust the reporting
+    #stations, or we use UTC.
+    #if we assume all stations are using UTC, we can prune on UTC, but
+    #this computer has to be closely synchronized as well
+    def prune(self):
+        for report in self._reports:
+            if self._reports[report][-1].tofloat() - self._lastreport > 5:
+                self._reports.remove(report)
+
+    #return a list of eligible messages for multilateration
+    #eligible reports are:
+    #1. bit-identical
+    #2. from distinct stations (at least 3)
+    #3. within 0.001 seconds of each other
+    #traverse the reports for each data pkt (hash) looking for >3 reports
+    #within 0.001s, 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) > 2: #quick check before we do a set()
+                stations = set([st.addr for st in stamps])
+                if len(stations) > 2:
+                    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.001)):
+                            reps.append(stamps[i])
+                            i+=1
+                        deduped = []
+                        for addr in stations:
+                            for st in reps[::-1]:
+                                if st.addr == addr:
+                                    deduped.append(st)
+                                    break
+                        if len(deduped) > 2:
+                            groups.append({"data": data, "stamps": deduped})
+
+        if len(groups) > 0:
+            return groups
+        return None
+
+    #issue multilaterated positions
+    def output(self, msg):
+        #do something here to compose a message
+            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 "Client %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\n") #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
+
+            conn.send("OK")
+            self._conns.append(connection(addr[0], conn, clientinfo))
+            print "New connection from %s: %s" % (addr[0], clientinfo.name)
+        except socket.error:
+            pass
+
+if __name__=="__main__":
+    srv = mlat_server("nothin'", 31337)
+    while 1:
+        srv.output("Buttes")
+        srv.get_messages()
+        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.addr, st.tofloat())
+        srv.prune()
+        time.sleep(0.3)

apps/mlat_test.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python
+
+#test stuff for mlat server
+
+import socket, pickle, time, random, sys
+
+class rx_data:
+    secs = 0
+    frac_secs = 0.0
+    data = None
+
+class client_info:
+    def __init__(self):
+        self.name = ""
+        self.latitude = 0.0
+        self.longitude = 0.0
+        self.altitude = 0.0
+        self.offset_secs = 0
+        self.offset_frac_secs = 0.0
+
+info = client_info()
+info.name = sys.argv[1]
+info.latitude = float(sys.argv[2])
+info.longitude = float(sys.argv[3])
+info.altitude = 123
+info.offset_secs = 0
+info.offset_frac_secs = 0.0
+
+data1 = rx_data()
+data1.secs = 0
+data1.data = int("0x8da81f875857f10eb65b10cb66f3", 16)
+
+sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+sock.setblocking(1)
+sock.connect(("localhost", 31337))
+sock.send(pickle.dumps(info))
+print sock.recv(1024)
+while 1:
+    time.sleep(0.05)
+    data1.frac_secs = random.random()
+    sock.send(pickle.dumps([data1]))
+
+sock.close()
+sock = None