#!/usr/bin/env python # -*- coding: utf-8 -*- # # This script reads dump1090 input from port, converts it in FLARM specific NMEA sentences and writes these sentences to a local named pipe # More information about the ADS-B message format can be found under : http://www.homepages.mcb.net/bones/SBS/Article/Barebones42_Socket_Data.htm # from math import * import datetime import re import os from socket import socket, gethostbyname, getaddrinfo, error, \ AF_INET, SOCK_STREAM, SOCK_DGRAM from sys import argv, stderr from time import time NaN = float('nan') Hex = '' TypeADSB = 1 traffic_data = {} # dictionary for all traffic class aircraft: def __init__(self): self.id = "" # ICAO Id self.latitude = self.longitude = 0.0 self.altitude = NaN # Feet class adsb2udp(): "SBS client interface to ADS-B." def __init__(self, src_host='192.168.178.21', src_port=30003, device='', \ my_Id='TEST', dst_host='192.168.2.108', dst_port=10110, \ base_lat=48.715247, base_long=11.533155, base_alt=400.0): print 'adsb2udp: src host =' , src_host , ', src_port =' , src_port , \ ', dst_host =' , dst_host , ', dst_port =' , dst_port , ', base_lat =' , \ base_lat , ', base_long =' , base_long , ', base_alt =' , base_alt self.verbose = 0 self.linebuffer = "" self.response = "" self.sock = None # in case we blow up in connect self.pos = aircraft() self.device = device self.src_host = gethostbyname(src_host) self.src_port = src_port self.dst_host = gethostbyname(dst_host) self.dst_port = dst_port self.base_lat = base_lat self.base_long = base_long self.base_alt = base_alt if src_host != None: self.connect(src_host, src_port) self.d = socket(AF_INET, SOCK_DGRAM) def connect(self, host, port): """Connect to a host on a given port.""" msg = "getaddrinfo returns an empty list" self.sock = None for res in getaddrinfo(host, port, 0, SOCK_STREAM): af, socktype, proto, canonname, sa = res try: self.sock = socket(af, socktype, proto) #if self.debuglevel > 0: print 'connect:', (host, port) self.sock.connect(sa) except error, msg: #if self.debuglevel > 0: print 'connect fail:', (host, port) self.close() continue break if not self.sock: raise error, msg FIFO_PATH = '/tmp/my_fifo' if os.path.exists(FIFO_PATH): os.unlink(FIFO_PATH) if not os.path.exists(FIFO_PATH): os.mkfifo(FIFO_PATH) self.my_fifo = open(FIFO_PATH, 'w+') print "my_fifo:",self.my_fifo def close(self): if self.sock: self.sock.close() self.sock = None def __del__(self): self.close() def read(self): "Wait for and read data being streamed from the daemon." if self.verbose > 1: stderr.write("poll: reading from daemon...\n") eol = self.linebuffer.find('\n') if eol == -1: frag = self.sock.recv(4096) self.linebuffer += frag if self.verbose > 1: stderr.write("poll: read complete.\n") if not self.linebuffer: if self.verbose > 1: stderr.write("poll: returning -1.\n") # Read failed return -1 eol = self.linebuffer.find('\n') if eol == -1: if self.verbose > 1: stderr.write("poll: returning 0.\n") # Read succeeded, but only got a fragment return 0 else: if self.verbose > 1: stderr.write("poll: fetching from buffer.\n") # We got a line eol += 1 self.response = self.linebuffer[:eol] self.linebuffer = self.linebuffer[eol:] # Can happen if daemon terminates while we're reading. if not self.response: return -1 if self.verbose: stderr.write("poll: data is %s\n" % repr(self.response)) self.received = time() # We got a \n-terminated line return len(self.response) def unpack(self, buf): global Hex error = 0 Hex = '' fields = buf.strip().split(",") # print fields # ============== MSG 3 ========================= if fields[0] == "MSG" and fields[1] == "3": # print fields self.pos.id = fields[4] self.latitude = self.longitude = 0.0 self.altitude = NaN # Feet if fields[11] != '': self.pos.altitude = float(fields[11]) if fields[14] != '': self.pos.latitude = float(fields[14]) if fields[15] != '': self.pos.longitude = float(fields[15]) if (traffic_data.has_key((fields[4],'CS'))): try: traffic_data[fields[4],'Lat'] = float(fields[14]) except ValueError: print "Conversion Error Lat: " + fields[14] traffic_data[fields[4],'Lat'] = float(0) error += 1 try: traffic_data[fields[4],'Lon'] = float(fields[15]) except: print "Conversion Error Lon: " + fields[15] traffic_data[fields[4],'Lon'] = float(0) error += 1 try: traffic_data[fields[4],'Alt'] = float(fields[11]) except ValueError: print "Conversion Error Alt: " + fields[11] traffic_data[fields[4],'Alt'] = float(0) error += 1 traffic_data[fields[4],'Tim'] = datetime.datetime(int(fields[6].split('/')[0]),int(fields[6].split('/')[1]),int(fields[6].split('/')[2]),int(fields[7].split(":")[0]),int(fields[7].split(":")[1]),int(fields[7].split(":")[2].split(".")[0]),int(fields[7].split(":")[2].split(".")[1])*1000) if (traffic_data.has_key((fields[4],'Trk')) and error == 0): # Setting Hex triggers NMEA data output Hex = fields[4] # print traffic_data.keys() # print datetime.datetime.now(),"now" for i,j in traffic_data: if j == "Tim": # Sync issue between CPU time and GPS time from ADSB input may generate neg. time values # print traffic_data[i,j],(datetime.datetime.now() - traffic_data[i,j]).seconds if ((datetime.datetime.now() - traffic_data[i,j]).seconds < 80000) and (((datetime.datetime.now() - traffic_data[i,j]).seconds) >= 60): if (traffic_data.has_key((i,'Tim'))): del traffic_data[i,'Tim'] if (traffic_data.has_key((i,'GS'))): del traffic_data[i,'GS'] if (traffic_data.has_key((i,'Trk'))): del traffic_data[i,'Trk'] if (traffic_data.has_key((i,'Lat'))): del traffic_data[i,'Lat'] if (traffic_data.has_key((i,'Lon'))): del traffic_data[i,'Lon'] if (traffic_data.has_key((i,'Alt'))): del traffic_data[i,'Alt'] if (traffic_data.has_key((i,'CS'))): del traffic_data[i,'CS'] if (traffic_data.has_key((i,'VR'))): del traffic_data[i,'VR'] # print "KILLER" break # breaks i or j loop or both??? # print "=====" # print CS # print traffic_data # print (traffic_data[Hex,'Tim'] - datetime.datetime.now()).seconds # print traffic_data[Hex,'Tim'] # ============== MSG 1 ========================= if fields[0] == "MSG" and fields[1] == "1": # Message 1: MT,TT,SID,AID,Hex,FID,DMG,TMG,DML,TML,CS # print "HexID: " + fields[4] # print "Callsign: " + fields[10] if not (traffic_data.has_key((fields[4],'CS'))): # Callsign not known yet ---> store in dict traffic_data[fields[4],'CS'] = fields[10] traffic_data[fields[4],'Tim'] = datetime.datetime(int(fields[6].split('/')[0]),int(fields[6].split('/')[1]),int(fields[6].split('/')[2]),int(fields[7].split(":")[0]),int(fields[7].split(":")[1]),int(fields[7].split(":")[2].split(".")[0]),int(fields[7].split(":")[2].split(".")[1])*1000) # ============== MSG 4 ========================= if fields[0] == "MSG" and fields[1] == "4": # Message 4: MT,TT,SID,AID,Hex,FID,DMG,TMG,DML,TML,,,GS,Trk,,,VR if (traffic_data.has_key((fields[4],'CS'))): # Callsign known, we can store data to it try: # Sometimes GS returns non floats traffic_data[fields[4],'GS'] = float(fields[12])*0.514444 except ValueError: # TODO: Check if GS field is already defined and keep old value? print "GS conversion failure: " + fields[12] traffic_data[fields[4],'GS'] = float(999) try: traffic_data[fields[4],'Trk'] = int(fields[13]) except ValueError: print "Trk conversion failure: " + fields[13] traffic_data[fields[4],'Trk'] = int(180) try: traffic_data[fields[4],'VR'] = float(fields[16])/3.28084/60 except ValueError: print "Vertical rate conversion failure" + fields[16] traffic_data[fields[4],'VR'] = float(0) traffic_data[fields[4],'Tim'] = datetime.datetime(int(fields[6].split('/')[0]),int(fields[6].split('/')[1]),int(fields[6].split('/')[2]),int(fields[7].split(":")[0]),int(fields[7].split(":")[1]),int(fields[7].split(":")[2].split(".")[0]),int(fields[7].split(":")[2].split(".")[1])*1000) def checksum(self, sentence): """ Remove leading $ """ sentence = sentence.lstrip('$') nmeadata,cksum = re.split('\*', sentence) #print nmeadata calc_cksum = 0 for s in nmeadata: calc_cksum ^= ord(s) return calc_cksum def process(self): "Process one incoming ADS-B message into one outgoing UDP packet." try: while True: self.read() # stderr.write("poll: data is %s\n" % repr(self.response)) if self.response.startswith("MSG,1") : # print self.response self.unpack(self.response) if self.response.startswith("MSG,4") : self.unpack(self.response) if self.response.startswith("MSG,2") : # MSG 2 triggered by ground switch - we don't process it here print 'Alt, GS, Trk, Lat, Lng, GND' print self.response if self.response.startswith("MSG,3") : self.unpack(self.response) # Only treat MSG3, if CS is already known if Hex != '': #print '----------------------------------------' #print ' ADS-B reading' #print '----------------------------------------' #print 'id ' , self.pos.id #print 'latitude ' , self.pos.latitude #print 'longitude ' , self.pos.longitude #print 'altitude ' , self.pos.altitude buf = "%s %s %f %f %.1f %.1f %.1f %u" % ( 0, self.pos.id, \ self.pos.latitude, self.pos.longitude, self.pos.altitude / 3.2808, \ 0, 0 , TypeADSB ) lon1, lat1, lon2, lat2 = map(radians, [self.base_long, self.base_lat, self.pos.longitude, self.pos.latitude]) dlon = lon2 - lon1 dlat = lat2 - lat1 a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2 c = 2 * atan2(sqrt(a), sqrt(1-a)) Distance = 6371000 * c Bearing = atan2(sin(lon2-lon1)*cos(lat2), cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)) relBearing = degrees(Bearing) absBearing = (relBearing + 360) % 360 #stderr.write("Bearing %f ° " % Bearing) #stderr.write("Distance %f km \n" % Distance) relNorth = cos(radians(absBearing))*Distance relEast = sin(radians(absBearing))*Distance relVert = self.pos.altitude/ 3.2808 - self.base_alt #tip: it is necessary to supply both $PFLAU and $PFLAA Flarm(R) NMEA messages to XCSoar, otherwise it does not not show the traffic. #PFLAA,,,,,,,,,,, #PFLAU,,,,,,,,,(,) str1 = "$PFLAU,,,,0,%d,0,%d,%u,%s*" % \ ( relBearing, int(relVert), int(Distance), traffic_data[Hex,'CS'].replace(" ","") ) csum = self.checksum(str1) str1 += "%02x\r\n" % csum # stderr.write(str1) # XCSoar ignores warning data from PFLAU # $PFLAA,0,1671,186,410,2,DDAA30,252,,21,12.4,1*62 # print traffic_data # $PFLAA,0,24,-5,3,1,123456,0,,0,0.1,1*36 # $PFLAA,0,16,2,2,1,123457,0,,0,0.1,1*6A # $PFLAA,0,155,-1069,418,1,ABCDEF,221,,37,2.8,1*0B # $PFLAA,0,24,-5,3,1,123456,0,,0,0.1,1*36" str2 = "$PFLAA,0,%d,%d,%d,1,%s,%i,,%.1f,%.1f,8*" % \ ( int(relNorth), int(relEast), int(relVert), traffic_data[Hex,'CS'].replace(" ",""),int(traffic_data[Hex,'Trk']),traffic_data[Hex,'GS'],traffic_data[Hex,'VR']) csum = self.checksum(str2) str2 += "%02x\r\n" % csum # stderr.write(str2) if (self.base_lat < 0) : Latstring = "S" else : Latstring = "N" if (self.base_long < 0) : Longstring = "W" else : Longstring = "E" latdeg = int(abs(floor(self.base_lat))) latmin = abs(60*(self.base_lat-latdeg)) longdeg = int(abs(floor(self.base_long))) longmin = abs(60*(self.base_long-longdeg)) #--GLL Latitude,N/S,Longitude,E/W,UTC,Status (A=Valid, V=Invalid),Checksum #GPRMC,225446,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68 now = datetime.datetime.utcnow() str3 = "$GPRMC,%02i%02i%02i,A,%s%.2f,%s,%s%.2f,%s,,,%02i%02i%02i,001.0,W*" % \ (now.hour, now.minute, now.second, latdeg, latmin, Latstring, longdeg, longmin, Longstring, now.day, now.month, now.year ) csum = self.checksum(str3) str3 += "%02x\r\n" % csum #--- GPGGA sentences - required for flarmradar.ch # $GPGGA,173431.00,4934.90002,N,00924.13332,E,7,5,0.96,415.1,M,48.0,M,,*60 str4 = "$GPGGA,%02i%02i%02i.00,%02d%.5f,%s,%03d%.5f,%s,7,5,0.96,415.1,M,48.0,M,,*" % \ (now.hour, now.minute, now.second, latdeg, latmin, Latstring, longdeg, longmin, Longstring) csum = self.checksum(str4) str4 += "%02x\r\n" % csum # print str4 buf = str1 + str2 + str3 + str4 # stderr.write(buf) # Write NMEA sentences to pipe for IPC to adsbclient.pl (Flarmradar) self.my_fifo.write(buf) # self.d.sendto(buf, (self.dst_host, self.dst_port)) except KeyboardInterrupt: # Avoid garble on ^C print "" if __name__ == '__main__': opts = { } argc = len(argv) if argc > 1: opts["src_host"] = argv[1] if argc > 2: opts["src_port"] = int(argv[2]) if argc > 3: opts["dst_host"] = argv[3] if argc > 4: opts["dst_port"] = int(argv[4]) session = adsb2udp(**opts) session.process() # driver.py ends here