source: core/trunk/client/adsb2nmea.py @ 273

Last change on this file since 273 was 273, checked in by dominic, 11 years ago

Added error trapping for missing GS,Trk

File size: 12.1 KB
RevLine 
[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
8from math import *
9import datetime
10import re
11import os
12from socket import socket, gethostbyname, getaddrinfo, error, \
13  AF_INET, SOCK_STREAM, SOCK_DGRAM
14from sys import argv, stderr
15from time import time
16
17NaN = float('nan')
[272]18Hex = ''
[271]19
20TypeADSB       = 1
21traffic_data = {}       # dictionary for all traffic
22
23class aircraft:
24    def __init__(self):
25        self.id = ""                # ICAO Id
26        self.latitude = self.longitude = 0.0
27        self.altitude = NaN         # Feet
28
29class 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
327if __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
Note: See TracBrowser for help on using the repository browser.