[271] | 1 | #!/usr/bin/env python |
---|
| 2 | # -*- coding: utf-8 -*- |
---|
| 3 | # |
---|
[272] | 4 | # This script reads dump1090 input from port, converts it in FLARM specific NMEA sentences and writes these sentences to a local named pipe |
---|
| 5 | # More information about the ADS-B message format can be found under : http://www.homepages.mcb.net/bones/SBS/Article/Barebones42_Socket_Data.htm |
---|
| 6 | # |
---|
[271] | 7 | |
---|
| 8 | from math import * |
---|
| 9 | import datetime |
---|
| 10 | import re |
---|
| 11 | import os |
---|
| 12 | from socket import socket, gethostbyname, getaddrinfo, error, \ |
---|
| 13 | AF_INET, SOCK_STREAM, SOCK_DGRAM |
---|
| 14 | from sys import argv, stderr |
---|
| 15 | from time import time |
---|
| 16 | |
---|
| 17 | NaN = float('nan') |
---|
[272] | 18 | Hex = '' |
---|
[271] | 19 | |
---|
| 20 | TypeADSB = 1 |
---|
| 21 | traffic_data = {} # dictionary for all traffic |
---|
| 22 | |
---|
| 23 | class aircraft: |
---|
| 24 | def __init__(self): |
---|
| 25 | self.id = "" # ICAO Id |
---|
| 26 | self.latitude = self.longitude = 0.0 |
---|
| 27 | self.altitude = NaN # Feet |
---|
| 28 | |
---|
| 29 | class adsb2udp(): |
---|
| 30 | "SBS client interface to ADS-B." |
---|
| 31 | def __init__(self, src_host='192.168.178.21', src_port=30003, device='', \ |
---|
| 32 | my_Id='TEST', dst_host='192.168.2.108', dst_port=10110, \ |
---|
| 33 | base_lat=48.715247, base_long=11.533155, base_alt=400.0): |
---|
| 34 | print 'adsb2udp: src host =' , src_host , ', src_port =' , src_port , \ |
---|
| 35 | ', dst_host =' , dst_host , ', dst_port =' , dst_port , ', base_lat =' , \ |
---|
| 36 | base_lat , ', base_long =' , base_long , ', base_alt =' , base_alt |
---|
| 37 | self.verbose = 0 |
---|
| 38 | self.linebuffer = "" |
---|
| 39 | self.response = "" |
---|
| 40 | self.sock = None # in case we blow up in connect |
---|
| 41 | self.pos = aircraft() |
---|
| 42 | self.device = device |
---|
| 43 | self.src_host = gethostbyname(src_host) |
---|
| 44 | self.src_port = src_port |
---|
| 45 | self.dst_host = gethostbyname(dst_host) |
---|
| 46 | self.dst_port = dst_port |
---|
| 47 | self.base_lat = base_lat |
---|
| 48 | self.base_long = base_long |
---|
| 49 | self.base_alt = base_alt |
---|
| 50 | if src_host != None: |
---|
| 51 | self.connect(src_host, src_port) |
---|
| 52 | self.d = socket(AF_INET, SOCK_DGRAM) |
---|
| 53 | |
---|
| 54 | def connect(self, host, port): |
---|
| 55 | """Connect to a host on a given port.""" |
---|
| 56 | msg = "getaddrinfo returns an empty list" |
---|
| 57 | self.sock = None |
---|
| 58 | for res in getaddrinfo(host, port, 0, SOCK_STREAM): |
---|
| 59 | af, socktype, proto, canonname, sa = res |
---|
| 60 | try: |
---|
| 61 | self.sock = socket(af, socktype, proto) |
---|
| 62 | #if self.debuglevel > 0: print 'connect:', (host, port) |
---|
| 63 | self.sock.connect(sa) |
---|
| 64 | except error, msg: |
---|
| 65 | #if self.debuglevel > 0: print 'connect fail:', (host, port) |
---|
| 66 | self.close() |
---|
| 67 | continue |
---|
| 68 | break |
---|
| 69 | if not self.sock: |
---|
| 70 | raise error, msg |
---|
| 71 | |
---|
| 72 | FIFO_PATH = '/tmp/my_fifo' |
---|
| 73 | if os.path.exists(FIFO_PATH): |
---|
| 74 | os.unlink(FIFO_PATH) |
---|
| 75 | |
---|
| 76 | if not os.path.exists(FIFO_PATH): |
---|
| 77 | os.mkfifo(FIFO_PATH) |
---|
| 78 | self.my_fifo = open(FIFO_PATH, 'w+') |
---|
| 79 | print "my_fifo:",self.my_fifo |
---|
| 80 | |
---|
| 81 | def close(self): |
---|
| 82 | if self.sock: |
---|
| 83 | self.sock.close() |
---|
| 84 | self.sock = None |
---|
| 85 | |
---|
| 86 | def __del__(self): |
---|
| 87 | self.close() |
---|
| 88 | |
---|
| 89 | def read(self): |
---|
| 90 | "Wait for and read data being streamed from the daemon." |
---|
| 91 | if self.verbose > 1: |
---|
| 92 | stderr.write("poll: reading from daemon...\n") |
---|
| 93 | eol = self.linebuffer.find('\n') |
---|
| 94 | if eol == -1: |
---|
| 95 | |
---|
| 96 | frag = self.sock.recv(4096) |
---|
| 97 | self.linebuffer += frag |
---|
| 98 | if self.verbose > 1: |
---|
| 99 | stderr.write("poll: read complete.\n") |
---|
| 100 | if not self.linebuffer: |
---|
| 101 | if self.verbose > 1: |
---|
| 102 | stderr.write("poll: returning -1.\n") |
---|
| 103 | # Read failed |
---|
| 104 | return -1 |
---|
| 105 | eol = self.linebuffer.find('\n') |
---|
| 106 | if eol == -1: |
---|
| 107 | if self.verbose > 1: |
---|
| 108 | stderr.write("poll: returning 0.\n") |
---|
| 109 | # Read succeeded, but only got a fragment |
---|
| 110 | return 0 |
---|
| 111 | else: |
---|
| 112 | if self.verbose > 1: |
---|
| 113 | stderr.write("poll: fetching from buffer.\n") |
---|
| 114 | |
---|
| 115 | # We got a line |
---|
| 116 | eol += 1 |
---|
| 117 | self.response = self.linebuffer[:eol] |
---|
| 118 | self.linebuffer = self.linebuffer[eol:] |
---|
| 119 | |
---|
| 120 | # Can happen if daemon terminates while we're reading. |
---|
| 121 | if not self.response: |
---|
| 122 | return -1 |
---|
| 123 | if self.verbose: |
---|
| 124 | stderr.write("poll: data is %s\n" % repr(self.response)) |
---|
| 125 | self.received = time() |
---|
| 126 | # We got a \n-terminated line |
---|
| 127 | return len(self.response) |
---|
| 128 | |
---|
| 129 | def unpack(self, buf): |
---|
[272] | 130 | global Hex |
---|
| 131 | Hex = '' |
---|
[271] | 132 | fields = buf.strip().split(",") |
---|
| 133 | # print fields |
---|
| 134 | if fields[0] == "MSG" and fields[1] == "3": |
---|
| 135 | # print fields |
---|
| 136 | self.pos.id = fields[4] |
---|
| 137 | self.latitude = self.longitude = 0.0 |
---|
| 138 | self.altitude = NaN # Feet |
---|
| 139 | if fields[11] != '': |
---|
| 140 | self.pos.altitude = float(fields[11]) |
---|
| 141 | if fields[14] != '': |
---|
| 142 | self.pos.latitude = float(fields[14]) |
---|
| 143 | if fields[15] != '': |
---|
| 144 | self.pos.longitude = float(fields[15]) |
---|
[272] | 145 | if (traffic_data.has_key((fields[4],'CS'))): |
---|
| 146 | traffic_data[fields[4],'Lat'] = float(fields[14]) |
---|
| 147 | traffic_data[fields[4],'Lon'] = float(fields[15]) |
---|
| 148 | traffic_data[fields[4],'Alt'] = float(fields[11]) |
---|
| 149 | traffic_data[fields[4],'Tim'] = fields[7] |
---|
| 150 | if traffic_data.has_key((fields[4],'Trk')): |
---|
| 151 | Hex = fields[4] |
---|
| 152 | # print CS |
---|
| 153 | |
---|
[271] | 154 | |
---|
[272] | 155 | |
---|
[271] | 156 | if fields[0] == "MSG" and fields[1] == "1": |
---|
[272] | 157 | # Message 1: MT,TT,SID,AID,Hex,FID,DMG,TMG,DML,TML,CS |
---|
| 158 | # print "HexID: " + fields[4] |
---|
| 159 | # print "Callsign: " + fields[10] |
---|
| 160 | if not (traffic_data.has_key((fields[4],'CS'))): |
---|
| 161 | # Callsign not known yet ---> store in dict |
---|
| 162 | traffic_data[fields[4],'CS'] = fields[10] |
---|
| 163 | traffic_data[fields[4],'Tim'] = fields[7] |
---|
| 164 | # print traffic_data |
---|
| 165 | |
---|
| 166 | |
---|
| 167 | if fields[0] == "MSG" and fields[1] == "4": |
---|
| 168 | # Message 4: MT,TT,SID,AID,Hex,FID,DMG,TMG,DML,TML,,,GS,Trk,,,VR |
---|
| 169 | if (traffic_data.has_key((fields[4],'CS'))): |
---|
| 170 | # Callsign known, we can store data to it |
---|
[273] | 171 | try: |
---|
| 172 | # Sometimes GS returns non floats |
---|
| 173 | traffic_data[fields[4],'GS'] = float(fields[12])*0.514444 |
---|
| 174 | except ValueError: |
---|
| 175 | # TODO: Check if GS field is already defined and keep old value? |
---|
| 176 | print "GS conversion failure: " + fields[12] |
---|
| 177 | traffic_data[fields[4],'GS'] = float(999) |
---|
| 178 | |
---|
| 179 | try: |
---|
| 180 | traffic_data[fields[4],'Trk'] = int(fields[13]) |
---|
| 181 | except ValueError: |
---|
| 182 | print "Trk conversion failure: " + fields[13] |
---|
| 183 | traffic_data[fields[4],'Trk'] = int(180) |
---|
| 184 | |
---|
| 185 | try: |
---|
| 186 | traffic_data[fields[4],'VR'] = float(fields[16])/3.28084/60 |
---|
| 187 | except ValueError: |
---|
| 188 | print "Vertical rate conversion failure" + fields[16] |
---|
| 189 | traffic_data[fields[4],'VR'] = float(0) |
---|
| 190 | |
---|
[272] | 191 | traffic_data[fields[4],'Tim'] = fields[7] |
---|
| 192 | |
---|
[271] | 193 | |
---|
| 194 | def checksum(self, sentence): |
---|
| 195 | |
---|
| 196 | """ Remove leading $ """ |
---|
| 197 | sentence = sentence.lstrip('$') |
---|
| 198 | |
---|
| 199 | nmeadata,cksum = re.split('\*', sentence) |
---|
| 200 | #print nmeadata |
---|
| 201 | calc_cksum = 0 |
---|
| 202 | for s in nmeadata: |
---|
| 203 | calc_cksum ^= ord(s) |
---|
| 204 | |
---|
| 205 | return calc_cksum |
---|
| 206 | |
---|
| 207 | |
---|
| 208 | def process(self): |
---|
| 209 | "Process one incoming ADS-B message into one outgoing UDP packet." |
---|
| 210 | |
---|
| 211 | try: |
---|
| 212 | while True: |
---|
| 213 | |
---|
| 214 | self.read() |
---|
| 215 | |
---|
| 216 | # stderr.write("poll: data is %s\n" % repr(self.response)) |
---|
[272] | 217 | if self.response.startswith("MSG,1") : |
---|
[271] | 218 | # print self.response |
---|
[272] | 219 | self.unpack(self.response) |
---|
| 220 | |
---|
| 221 | if self.response.startswith("MSG,4") : |
---|
| 222 | self.unpack(self.response) |
---|
| 223 | |
---|
| 224 | if self.response.startswith("MSG,2") : |
---|
| 225 | # MSG 2 triggered by ground switch - we don't process it here |
---|
| 226 | print 'Alt, GS, Trk, Lat, Lng, GND' |
---|
| 227 | print self.response |
---|
| 228 | |
---|
[271] | 229 | if self.response.startswith("MSG,3") : |
---|
| 230 | self.unpack(self.response) |
---|
[272] | 231 | # Only treat MSG3, if CS is already known |
---|
| 232 | if Hex != '': |
---|
| 233 | #print '----------------------------------------' |
---|
| 234 | #print ' ADS-B reading' |
---|
| 235 | #print '----------------------------------------' |
---|
| 236 | #print 'id ' , self.pos.id |
---|
| 237 | #print 'latitude ' , self.pos.latitude |
---|
| 238 | #print 'longitude ' , self.pos.longitude |
---|
| 239 | #print 'altitude ' , self.pos.altitude |
---|
[271] | 240 | |
---|
[272] | 241 | buf = "%s %s %f %f %.1f %.1f %.1f %u" % ( 0, self.pos.id, \ |
---|
| 242 | self.pos.latitude, self.pos.longitude, self.pos.altitude / 3.2808, \ |
---|
| 243 | 0, 0 , TypeADSB ) |
---|
[271] | 244 | |
---|
[272] | 245 | lon1, lat1, lon2, lat2 = map(radians, [self.base_long, self.base_lat, self.pos.longitude, self.pos.latitude]) |
---|
[271] | 246 | |
---|
[272] | 247 | dlon = lon2 - lon1 |
---|
| 248 | dlat = lat2 - lat1 |
---|
| 249 | a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2 |
---|
| 250 | c = 2 * atan2(sqrt(a), sqrt(1-a)) |
---|
| 251 | Distance = 6371000 * c |
---|
[271] | 252 | |
---|
[272] | 253 | Bearing = atan2(sin(lon2-lon1)*cos(lat2), cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)) |
---|
| 254 | relBearing = degrees(Bearing) |
---|
| 255 | absBearing = (relBearing + 360) % 360 |
---|
| 256 | #stderr.write("Bearing %f ° " % Bearing) |
---|
| 257 | #stderr.write("Distance %f km \n" % Distance) |
---|
[271] | 258 | |
---|
[272] | 259 | relNorth = cos(radians(absBearing))*Distance |
---|
| 260 | relEast = sin(radians(absBearing))*Distance |
---|
| 261 | relVert = self.pos.altitude/ 3.2808 - self.base_alt |
---|
[271] | 262 | |
---|
[272] | 263 | #tip: it is necessary to supply both $PFLAU and $PFLAA Flarm(R) NMEA messages to XCSoar, otherwise it does not not show the traffic. |
---|
| 264 | #PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>,<IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType> |
---|
| 265 | #PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>,<RelativeVertical>,<RelativeDistance>(,<ID>) |
---|
[271] | 266 | |
---|
[272] | 267 | str1 = "$PFLAU,,,,0,%d,0,%d,%u,%s*" % \ |
---|
[273] | 268 | ( relBearing, int(relVert), int(Distance), traffic_data[Hex,'CS'].replace(" ","") ) |
---|
[272] | 269 | csum = self.checksum(str1) |
---|
| 270 | str1 += "%02x\r\n" % csum |
---|
[273] | 271 | # stderr.write(str1) |
---|
[272] | 272 | # XCSoar ignores warning data from PFLAU |
---|
| 273 | # $PFLAA,0,1671,186,410,2,DDAA30,252,,21,12.4,1*62 |
---|
| 274 | # print traffic_data |
---|
| 275 | # $PFLAA,0,24,-5,3,1,123456,0,,0,0.1,1*36 |
---|
| 276 | # $PFLAA,0,16,2,2,1,123457,0,,0,0.1,1*6A |
---|
| 277 | # $PFLAA,0,155,-1069,418,1,ABCDEF,221,,37,2.8,1*0B |
---|
| 278 | # $PFLAA,0,24,-5,3,1,123456,0,,0,0.1,1*36" |
---|
| 279 | |
---|
| 280 | |
---|
| 281 | str2 = "$PFLAA,0,%d,%d,%d,1,%s,%i,,%.1f,%.1f,8*" % \ |
---|
| 282 | ( int(relNorth), int(relEast), int(relVert), traffic_data[Hex,'CS'].replace(" ",""),int(traffic_data[Hex,'Trk']),traffic_data[Hex,'GS'],traffic_data[Hex,'VR']) |
---|
| 283 | csum = self.checksum(str2) |
---|
| 284 | str2 += "%02x\r\n" % csum |
---|
| 285 | # stderr.write(str2) |
---|
[271] | 286 | |
---|
[272] | 287 | if (self.base_lat < 0) : |
---|
| 288 | Latstring = "S" |
---|
| 289 | else : |
---|
| 290 | Latstring = "N" |
---|
| 291 | if (self.base_long < 0) : |
---|
| 292 | Longstring = "W" |
---|
| 293 | else : |
---|
| 294 | Longstring = "E" |
---|
| 295 | latdeg = int(abs(floor(self.base_lat))) |
---|
| 296 | latmin = abs(60*(self.base_lat-latdeg)) |
---|
| 297 | longdeg = int(abs(floor(self.base_long))) |
---|
| 298 | longmin = abs(60*(self.base_long-longdeg)) |
---|
| 299 | #--GLL Latitude,N/S,Longitude,E/W,UTC,Status (A=Valid, V=Invalid),Checksum |
---|
| 300 | #GPRMC,225446,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68 |
---|
| 301 | now = datetime.datetime.utcnow() |
---|
| 302 | str3 = "$GPRMC,%02i%02i%02i,A,%s%.2f,%s,%s%.2f,%s,,,%02i%02i%02i,001.0,W*" % \ |
---|
| 303 | (now.hour, now.minute, now.second, latdeg, latmin, Latstring, longdeg, longmin, Longstring, now.day, now.month, now.year ) |
---|
| 304 | csum = self.checksum(str3) |
---|
| 305 | str3 += "%02x\r\n" % csum |
---|
[271] | 306 | |
---|
[272] | 307 | #--- GPGGA sentences - required for flarmradar.ch |
---|
| 308 | # $GPGGA,173431.00,4934.90002,N,00924.13332,E,7,5,0.96,415.1,M,48.0,M,,*60 |
---|
| 309 | str4 = "$GPGGA,%02i%02i%02i.00,%02d%.5f,%s,%03d%.5f,%s,7,5,0.96,415.1,M,48.0,M,,*" % \ |
---|
| 310 | (now.hour, now.minute, now.second, latdeg, latmin, Latstring, longdeg, longmin, Longstring) |
---|
| 311 | csum = self.checksum(str4) |
---|
| 312 | str4 += "%02x\r\n" % csum |
---|
| 313 | # print str4 |
---|
[271] | 314 | |
---|
[272] | 315 | buf = str1 + str2 + str3 + str4 |
---|
| 316 | # stderr.write(buf) |
---|
| 317 | # Write NMEA sentences to pipe for IPC to adsbclient.pl (Flarmradar) |
---|
| 318 | self.my_fifo.write(buf) |
---|
[271] | 319 | |
---|
| 320 | |
---|
[272] | 321 | # self.d.sendto(buf, (self.dst_host, self.dst_port)) |
---|
[271] | 322 | |
---|
| 323 | except KeyboardInterrupt: |
---|
| 324 | # Avoid garble on ^C |
---|
| 325 | print "" |
---|
| 326 | |
---|
| 327 | if __name__ == '__main__': |
---|
| 328 | |
---|
| 329 | opts = { } |
---|
| 330 | |
---|
| 331 | argc = len(argv) |
---|
| 332 | if argc > 1: |
---|
| 333 | opts["src_host"] = argv[1] |
---|
| 334 | |
---|
| 335 | if argc > 2: |
---|
| 336 | opts["src_port"] = int(argv[2]) |
---|
| 337 | |
---|
| 338 | if argc > 3: |
---|
| 339 | opts["dst_host"] = argv[3] |
---|
| 340 | |
---|
| 341 | if argc > 4: |
---|
| 342 | opts["dst_port"] = int(argv[4]) |
---|
| 343 | |
---|
| 344 | session = adsb2udp(**opts) |
---|
| 345 | session.process() |
---|
| 346 | |
---|
| 347 | # driver.py ends here |
---|