diff --git a/apps/mlat_server.py b/apps/mlat_server.py new file mode 100755 index 0000000..8a758bb --- /dev/null +++ b/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: { : [{ "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 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) diff --git a/apps/mlat_test.py b/apps/mlat_test.py new file mode 100755 index 0000000..5639196 --- /dev/null +++ b/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