| 1 | #!/usr/bin/env python |
|---|
| 2 | # -*- coding: utf-8 -*- |
|---|
| 3 | # |
|---|
| 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 | # |
|---|
| 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') |
|---|
| 18 | Hex = '' |
|---|
| 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): |
|---|
| 130 | global Hex |
|---|
| 131 | error = 0 |
|---|
| 132 | Hex = '' |
|---|
| 133 | fields = buf.strip().split(",") |
|---|
| 134 | # print fields |
|---|
| 135 | if fields[0] == "MSG" and fields[1] == "3": |
|---|
| 136 | # print fields |
|---|
| 137 | self.pos.id = fields[4] |
|---|
| 138 | self.latitude = self.longitude = 0.0 |
|---|
| 139 | self.altitude = NaN # Feet |
|---|
| 140 | if fields[11] != '': |
|---|
| 141 | self.pos.altitude = float(fields[11]) |
|---|
| 142 | if fields[14] != '': |
|---|
| 143 | self.pos.latitude = float(fields[14]) |
|---|
| 144 | if fields[15] != '': |
|---|
| 145 | self.pos.longitude = float(fields[15]) |
|---|
| 146 | if (traffic_data.has_key((fields[4],'CS'))): |
|---|
| 147 | try: |
|---|
| 148 | traffic_data[fields[4],'Lat'] = float(fields[14]) |
|---|
| 149 | except ValueError: |
|---|
| 150 | print "Conversion Error Lat: " + fields[14] |
|---|
| 151 | traffic_data[fields[4],'Lat'] = float(0) |
|---|
| 152 | error += 1 |
|---|
| 153 | |
|---|
| 154 | try: |
|---|
| 155 | traffic_data[fields[4],'Lon'] = float(fields[15]) |
|---|
| 156 | except: |
|---|
| 157 | print "Conversion Error Lon: " + fields[15] |
|---|
| 158 | traffic_data[fields[4],'Lon'] = float(0) |
|---|
| 159 | error += 1 |
|---|
| 160 | |
|---|
| 161 | try: |
|---|
| 162 | traffic_data[fields[4],'Alt'] = float(fields[11]) |
|---|
| 163 | except ValueError: |
|---|
| 164 | print "Conversion Error Alt: " + fields[11] |
|---|
| 165 | traffic_data[fields[4],'Alt'] = float(0) |
|---|
| 166 | error += 1 |
|---|
| 167 | |
|---|
| 168 | traffic_data[fields[4],'Tim'] = fields[7] |
|---|
| 169 | if (traffic_data.has_key((fields[4],'Trk')) and error == 0): |
|---|
| 170 | # Setting Hex triggers NMEA data output |
|---|
| 171 | Hex = fields[4] |
|---|
| 172 | # print CS |
|---|
| 173 | |
|---|
| 174 | |
|---|
| 175 | |
|---|
| 176 | if fields[0] == "MSG" and fields[1] == "1": |
|---|
| 177 | # Message 1: MT,TT,SID,AID,Hex,FID,DMG,TMG,DML,TML,CS |
|---|
| 178 | # print "HexID: " + fields[4] |
|---|
| 179 | # print "Callsign: " + fields[10] |
|---|
| 180 | if not (traffic_data.has_key((fields[4],'CS'))): |
|---|
| 181 | # Callsign not known yet ---> store in dict |
|---|
| 182 | traffic_data[fields[4],'CS'] = fields[10] |
|---|
| 183 | traffic_data[fields[4],'Tim'] = fields[7] |
|---|
| 184 | # print traffic_data |
|---|
| 185 | |
|---|
| 186 | |
|---|
| 187 | if fields[0] == "MSG" and fields[1] == "4": |
|---|
| 188 | # Message 4: MT,TT,SID,AID,Hex,FID,DMG,TMG,DML,TML,,,GS,Trk,,,VR |
|---|
| 189 | if (traffic_data.has_key((fields[4],'CS'))): |
|---|
| 190 | # Callsign known, we can store data to it |
|---|
| 191 | try: |
|---|
| 192 | # Sometimes GS returns non floats |
|---|
| 193 | traffic_data[fields[4],'GS'] = float(fields[12])*0.514444 |
|---|
| 194 | except ValueError: |
|---|
| 195 | # TODO: Check if GS field is already defined and keep old value? |
|---|
| 196 | print "GS conversion failure: " + fields[12] |
|---|
| 197 | traffic_data[fields[4],'GS'] = float(999) |
|---|
| 198 | |
|---|
| 199 | try: |
|---|
| 200 | traffic_data[fields[4],'Trk'] = int(fields[13]) |
|---|
| 201 | except ValueError: |
|---|
| 202 | print "Trk conversion failure: " + fields[13] |
|---|
| 203 | traffic_data[fields[4],'Trk'] = int(180) |
|---|
| 204 | |
|---|
| 205 | try: |
|---|
| 206 | traffic_data[fields[4],'VR'] = float(fields[16])/3.28084/60 |
|---|
| 207 | except ValueError: |
|---|
| 208 | print "Vertical rate conversion failure" + fields[16] |
|---|
| 209 | traffic_data[fields[4],'VR'] = float(0) |
|---|
| 210 | |
|---|
| 211 | traffic_data[fields[4],'Tim'] = fields[7] |
|---|
| 212 | |
|---|
| 213 | |
|---|
| 214 | def checksum(self, sentence): |
|---|
| 215 | |
|---|
| 216 | """ Remove leading $ """ |
|---|
| 217 | sentence = sentence.lstrip('$') |
|---|
| 218 | |
|---|
| 219 | nmeadata,cksum = re.split('\*', sentence) |
|---|
| 220 | #print nmeadata |
|---|
| 221 | calc_cksum = 0 |
|---|
| 222 | for s in nmeadata: |
|---|
| 223 | calc_cksum ^= ord(s) |
|---|
| 224 | |
|---|
| 225 | return calc_cksum |
|---|
| 226 | |
|---|
| 227 | |
|---|
| 228 | def process(self): |
|---|
| 229 | "Process one incoming ADS-B message into one outgoing UDP packet." |
|---|
| 230 | |
|---|
| 231 | try: |
|---|
| 232 | while True: |
|---|
| 233 | |
|---|
| 234 | self.read() |
|---|
| 235 | |
|---|
| 236 | # stderr.write("poll: data is %s\n" % repr(self.response)) |
|---|
| 237 | if self.response.startswith("MSG,1") : |
|---|
| 238 | # print self.response |
|---|
| 239 | self.unpack(self.response) |
|---|
| 240 | |
|---|
| 241 | if self.response.startswith("MSG,4") : |
|---|
| 242 | self.unpack(self.response) |
|---|
| 243 | |
|---|
| 244 | if self.response.startswith("MSG,2") : |
|---|
| 245 | # MSG 2 triggered by ground switch - we don't process it here |
|---|
| 246 | print 'Alt, GS, Trk, Lat, Lng, GND' |
|---|
| 247 | print self.response |
|---|
| 248 | |
|---|
| 249 | if self.response.startswith("MSG,3") : |
|---|
| 250 | self.unpack(self.response) |
|---|
| 251 | # Only treat MSG3, if CS is already known |
|---|
| 252 | if Hex != '': |
|---|
| 253 | #print '----------------------------------------' |
|---|
| 254 | #print ' ADS-B reading' |
|---|
| 255 | #print '----------------------------------------' |
|---|
| 256 | #print 'id ' , self.pos.id |
|---|
| 257 | #print 'latitude ' , self.pos.latitude |
|---|
| 258 | #print 'longitude ' , self.pos.longitude |
|---|
| 259 | #print 'altitude ' , self.pos.altitude |
|---|
| 260 | |
|---|
| 261 | buf = "%s %s %f %f %.1f %.1f %.1f %u" % ( 0, self.pos.id, \ |
|---|
| 262 | self.pos.latitude, self.pos.longitude, self.pos.altitude / 3.2808, \ |
|---|
| 263 | 0, 0 , TypeADSB ) |
|---|
| 264 | |
|---|
| 265 | lon1, lat1, lon2, lat2 = map(radians, [self.base_long, self.base_lat, self.pos.longitude, self.pos.latitude]) |
|---|
| 266 | |
|---|
| 267 | dlon = lon2 - lon1 |
|---|
| 268 | dlat = lat2 - lat1 |
|---|
| 269 | a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2 |
|---|
| 270 | c = 2 * atan2(sqrt(a), sqrt(1-a)) |
|---|
| 271 | Distance = 6371000 * c |
|---|
| 272 | |
|---|
| 273 | Bearing = atan2(sin(lon2-lon1)*cos(lat2), cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)) |
|---|
| 274 | relBearing = degrees(Bearing) |
|---|
| 275 | absBearing = (relBearing + 360) % 360 |
|---|
| 276 | #stderr.write("Bearing %f ° " % Bearing) |
|---|
| 277 | #stderr.write("Distance %f km \n" % Distance) |
|---|
| 278 | |
|---|
| 279 | relNorth = cos(radians(absBearing))*Distance |
|---|
| 280 | relEast = sin(radians(absBearing))*Distance |
|---|
| 281 | relVert = self.pos.altitude/ 3.2808 - self.base_alt |
|---|
| 282 | |
|---|
| 283 | #tip: it is necessary to supply both $PFLAU and $PFLAA Flarm(R) NMEA messages to XCSoar, otherwise it does not not show the traffic. |
|---|
| 284 | #PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>,<IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType> |
|---|
| 285 | #PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>,<RelativeVertical>,<RelativeDistance>(,<ID>) |
|---|
| 286 | |
|---|
| 287 | str1 = "$PFLAU,,,,0,%d,0,%d,%u,%s*" % \ |
|---|
| 288 | ( relBearing, int(relVert), int(Distance), traffic_data[Hex,'CS'].replace(" ","") ) |
|---|
| 289 | csum = self.checksum(str1) |
|---|
| 290 | str1 += "%02x\r\n" % csum |
|---|
| 291 | # stderr.write(str1) |
|---|
| 292 | # XCSoar ignores warning data from PFLAU |
|---|
| 293 | # $PFLAA,0,1671,186,410,2,DDAA30,252,,21,12.4,1*62 |
|---|
| 294 | # print traffic_data |
|---|
| 295 | # $PFLAA,0,24,-5,3,1,123456,0,,0,0.1,1*36 |
|---|
| 296 | # $PFLAA,0,16,2,2,1,123457,0,,0,0.1,1*6A |
|---|
| 297 | # $PFLAA,0,155,-1069,418,1,ABCDEF,221,,37,2.8,1*0B |
|---|
| 298 | # $PFLAA,0,24,-5,3,1,123456,0,,0,0.1,1*36" |
|---|
| 299 | |
|---|
| 300 | |
|---|
| 301 | str2 = "$PFLAA,0,%d,%d,%d,1,%s,%i,,%.1f,%.1f,8*" % \ |
|---|
| 302 | ( int(relNorth), int(relEast), int(relVert), traffic_data[Hex,'CS'].replace(" ",""),int(traffic_data[Hex,'Trk']),traffic_data[Hex,'GS'],traffic_data[Hex,'VR']) |
|---|
| 303 | csum = self.checksum(str2) |
|---|
| 304 | str2 += "%02x\r\n" % csum |
|---|
| 305 | # stderr.write(str2) |
|---|
| 306 | |
|---|
| 307 | if (self.base_lat < 0) : |
|---|
| 308 | Latstring = "S" |
|---|
| 309 | else : |
|---|
| 310 | Latstring = "N" |
|---|
| 311 | if (self.base_long < 0) : |
|---|
| 312 | Longstring = "W" |
|---|
| 313 | else : |
|---|
| 314 | Longstring = "E" |
|---|
| 315 | latdeg = int(abs(floor(self.base_lat))) |
|---|
| 316 | latmin = abs(60*(self.base_lat-latdeg)) |
|---|
| 317 | longdeg = int(abs(floor(self.base_long))) |
|---|
| 318 | longmin = abs(60*(self.base_long-longdeg)) |
|---|
| 319 | #--GLL Latitude,N/S,Longitude,E/W,UTC,Status (A=Valid, V=Invalid),Checksum |
|---|
| 320 | #GPRMC,225446,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68 |
|---|
| 321 | now = datetime.datetime.utcnow() |
|---|
| 322 | str3 = "$GPRMC,%02i%02i%02i,A,%s%.2f,%s,%s%.2f,%s,,,%02i%02i%02i,001.0,W*" % \ |
|---|
| 323 | (now.hour, now.minute, now.second, latdeg, latmin, Latstring, longdeg, longmin, Longstring, now.day, now.month, now.year ) |
|---|
| 324 | csum = self.checksum(str3) |
|---|
| 325 | str3 += "%02x\r\n" % csum |
|---|
| 326 | |
|---|
| 327 | #--- GPGGA sentences - required for flarmradar.ch |
|---|
| 328 | # $GPGGA,173431.00,4934.90002,N,00924.13332,E,7,5,0.96,415.1,M,48.0,M,,*60 |
|---|
| 329 | str4 = "$GPGGA,%02i%02i%02i.00,%02d%.5f,%s,%03d%.5f,%s,7,5,0.96,415.1,M,48.0,M,,*" % \ |
|---|
| 330 | (now.hour, now.minute, now.second, latdeg, latmin, Latstring, longdeg, longmin, Longstring) |
|---|
| 331 | csum = self.checksum(str4) |
|---|
| 332 | str4 += "%02x\r\n" % csum |
|---|
| 333 | # print str4 |
|---|
| 334 | |
|---|
| 335 | buf = str1 + str2 + str3 + str4 |
|---|
| 336 | # stderr.write(buf) |
|---|
| 337 | # Write NMEA sentences to pipe for IPC to adsbclient.pl (Flarmradar) |
|---|
| 338 | self.my_fifo.write(buf) |
|---|
| 339 | |
|---|
| 340 | |
|---|
| 341 | # self.d.sendto(buf, (self.dst_host, self.dst_port)) |
|---|
| 342 | |
|---|
| 343 | except KeyboardInterrupt: |
|---|
| 344 | # Avoid garble on ^C |
|---|
| 345 | print "" |
|---|
| 346 | |
|---|
| 347 | if __name__ == '__main__': |
|---|
| 348 | |
|---|
| 349 | opts = { } |
|---|
| 350 | |
|---|
| 351 | argc = len(argv) |
|---|
| 352 | if argc > 1: |
|---|
| 353 | opts["src_host"] = argv[1] |
|---|
| 354 | |
|---|
| 355 | if argc > 2: |
|---|
| 356 | opts["src_port"] = int(argv[2]) |
|---|
| 357 | |
|---|
| 358 | if argc > 3: |
|---|
| 359 | opts["dst_host"] = argv[3] |
|---|
| 360 | |
|---|
| 361 | if argc > 4: |
|---|
| 362 | opts["dst_port"] = int(argv[4]) |
|---|
| 363 | |
|---|
| 364 | session = adsb2udp(**opts) |
|---|
| 365 | session.process() |
|---|
| 366 | |
|---|
| 367 | # driver.py ends here |
|---|
![(please configure the [header_logo] section in trac.ini)](/trac/flarmradar/chrome/site/your_project_logo.png){kind=logo}
