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

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#

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')

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 traffic:
    def __init__(self):
        self.id = id                # ICAO Id
        self.latitude = self.longitude = 0.0
        self.altitude = NaN         # Feet
        self.GS = 0                 # Ground speed
        self.VR = 0                 # vertical rate
        self.CS = ""                # call sign
        self.TRK = 0                # Track
        self.time = ""              # Time

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):
      fields = buf.strip().split(",")
      # print fields
      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 fields[0] == "MSG" and fields[1] == "1":
        # Call sign
        # fields[1] = traffic(fields[4])
        print fields
                
  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 'Callsign'
            # print self.response
            # self.unpack(self.response)
        # if self.response.startswith("MSG,4") :
            # print 'GS,TRK,VR'
            # print self.response
            # self.unpack(self.response)
        # if self.response.startswith("MSG,2") :
            # print 'Alt, GS, Trk, Lat, Lng, GND'
            # print self.response
        if self.response.startswith("MSG,3") :
            self.unpack(self.response)
            #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,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>,<IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType>
            #PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>,<RelativeVertical>,<RelativeDistance>(,<ID>)
            
            str1 = "$PFLAU,,,,0,%d,0,%d,%u,%s*" % \
            ( relBearing, int(relVert), int(Distance), self.pos.id )
            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
            str2 = "$PFLAA,0,%d,%d,%d,1,%s,180,0,999,0,8*" % \
            ( int(relNorth), int(relEast), int(relVert), self.pos.id )
            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