Skip to content
Find file
Fetching contributors…
Cannot retrieve contributors at this time
executable file 215 lines (186 sloc) 8.94 KB
#!/usr/bin/env python
#flightgear interface to
#outputs UDP data to add traffic to FGFS
import struct
import socket
import air_modes
from air_modes import mlat
import sqlite3
import string, threading, math, time
from air_modes.sql import output_sql
from Quaternion import Quat
import numpy
from air_modes.exceptions import *
class output_flightgear(air_modes.parse):
def __init__(self, localpos, hostname, port):
air_modes.parse.__init__(self, localpos)
self.hostname = hostname
self.port = port
self.localpos = localpos
self.positions = {}
self.velocities = {}
self.callsigns = {}
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.sock.connect((self.hostname, self.port))
def output(self, message):
[data, ecc, reference, timestamp] = message.split()
data = air_modes.modes_reply(long(data, 16))
msgtype = data["df"]
if msgtype == 17: #ADS-B report
icao24 = data["aa"]
bdsreg = data["me"].get_type()
if bdsreg == 0x08: #ident packet
(ident, actype) = self.parseBDS08(data)
#select model based on actype
self.callsigns[icao24] = [ident, actype]
elif bdsreg == 0x06: #BDS0,6 pos
[ground_track, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS06(data)
self.positions[icao24] = [decoded_lat, decoded_lon, 0]
elif bdsreg == 0x05: #BDS0,5 pos
[altitude, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS05(data)
self.positions[icao24] = [decoded_lat, decoded_lon, altitude]
elif bdsreg == 0x09: #velocity
subtype = data["bds09"].get_type()
if subtype == 0:
[velocity, heading, vert_spd, turnrate] = self.parseBDS09_0(data)
elif subtype == 1:
[velocity, heading, vert_spd] = self.parseBDS09_1(data)
turnrate = 0
self.velocities[icao24] = [velocity, heading, vert_spd, turnrate]
except ADSBError:
def update(self, icao24):
#check to see that ICAO24 appears in all three records and that the data looks valid
complete = (icao24 in self.positions)\
and (icao24 in self.velocities)\
and (icao24 in self.callsigns)
if complete:
print "FG update: %s" % (self.callsigns[icao24][0])
msg = fg_posmsg(self.callsigns[icao24][0],
class fg_header:
def __init__(self):
self.magic = "FGFS"
self.proto = 0x00010001
self.msgid = 0
self.msglen = 0 #in bytes, though they swear it isn't
self.replyaddr = 0 #unused
self.replyport = 0 #unused
self.callsign = "UNKNOWN" = None
hdrfmt = '!4sLLLLL8s0L'
def pack(self):
self.msglen = 32 + len(
packed = struct.pack(self.hdrfmt, self.magic, self.proto, self.msgid, self.msglen, self.replyaddr, self.replyport, self.callsign)
return packed
#so this appears to work, but FGFS doesn't display it in flight for some reason. not in the chat window either. oh well.
class fg_chatmsg(fg_header):
def __init__(self, msg):
self.chatmsg = msg
self.msgid = 1
def pack(self):
self.chatfmt = '!' + str(len(self.chatmsg)) + 's'
#print "Packing with strlen %i " % len(self.chatmsg) = struct.pack(self.chatfmt, self.chatmsg)
return fg_header.pack(self) +
modelmap = { None: 'Aircraft/777-200/Models/777-200ER.xml',
"NO INFO": 'Aircraft/777-200/Models/777-200ER.xml',
"LIGHT": 'Aircraft/c172p/Models/c172p.xml',
"SMALL": 'Aircraft/CitationX/Models/Citation-X.xml',
"LARGE": 'Aircraft/CRJ700-family/Models/CRJ700.xml',
"LARGE HIGH VORTEX": 'Aircraft/757-200/Models/757-200.xml',
"HEAVY": 'Aircraft/747-200/Models/boeing747-200.xml',
"HIGH PERFORMANCE": 'Aircraft/SR71-BlackBird/Models/Blackbird-SR71B.xml', #yeah i know
"ROTORCRAFT": 'Aircraft/ec130/Models/ec130b4.xml',
"GLIDER": 'Aircraft/ASK21-MI/Models/ask21mi.xml',
"BALLOON/BLIMP": 'Aircraft/ZLT-NT/Models/ZLT-NT.xml',
"ULTRALIGHT": 'Aircraft/cri-cri/Models/MC-15.xml',
"UAV": 'Aircraft/YardStik/Models/yardstik.xml', #hahahaha
"SPACECRAFT": 'Aircraft/SpaceShip-One/Models/spaceshipone.xml',
"SURFACE EMERGENCY VEHICLE": 'Aircraft/followme/Models/follow_me.xml', #not the best
"SURFACE SERVICE VEHICLE": 'Aircraft/pushback/Models/Pushback.xml'
class fg_posmsg(fg_header):
def __init__(self, callsign, modelname, lat, lon, alt, hdg, vel, vs, turnrate):
#from the above, calculate valid FGFS mp vals
#this is the translation layer between ADS-B and FGFS
self.callsign = callsign
if self.callsign is None:
self.callsign = "UNKNOWN"
self.modelname = modelname
if self.modelname not in modelmap:
#this should keep people on their toes when strange aircraft types are seen
self.model = 'Aircraft/santa/Models/santa.xml'
self.model = modelmap[self.modelname] = lat
self.lon = lon
self.alt = alt
self.hdg = hdg
self.vel = vel
self.vs = vs
self.turnrate = turnrate
self.msgid = 7
self.time = time.time()
self.lag = 0
def pack(self):
#this is, in order:
#model, time (time.time() is fine), lag, position, orientation, linear vel, angular vel, linear accel, angular accel (accels unused), 0
#position is in ECEF format -- same as mlat uses. what luck!
pos = mlat.llh2ecef([, self.lon, self.alt * 0.3048]) #alt is in meters!
#get the rotation quaternion to rotate to local reference frame from lat/lon
rotquat = Quat([, self.lon])
#get the quaternion corresponding to aircraft orientation
acquat = Quat([self.hdg, 0, 0])
#rotate aircraft into ECEF frame
ecefquat = rotquat * acquat
#get it in angle/axis representation
(angle, axis) = ecefquat._get_angle_axis()
orientation = angle * axis
kts_to_ms = 0.514444444 #convert kts to m/s
vel_ms = self.vel * kts_to_ms
velvec = (vel_ms,0,0) #velocity vector in m/s -- is this in the local frame? looks like [0] is fwd vel,
#we'll pretend the a/c is always moving the dir it's pointing
turnvec = (0,0,self.turnrate * (math.pi / 180.) ) #turn rates in rad/s [roll, pitch, yaw]
accelvec = (0,0,0)
turnaccelvec = (0,0,0)
self.posfmt = '!96s' + 'd' + 'd' + '3d' + '3f' + '3f' + '3f' + '3f' + '3f' + 'I' = struct.pack(self.posfmt,
pos[0], pos[1], pos[2],
orientation[0], orientation[1], orientation[2],
velvec[0], velvec[1], velvec[2],
turnvec[0], turnvec[1], turnvec[2],
accelvec[0], accelvec[1], accelvec[2],
turnaccelvec[0], turnaccelvec[1], turnaccelvec[2],
return fg_header.pack(self) +
if __name__ == '__main__':
timeoffset = time.time()
iof = open('27augrudi3.txt')
localpos = [37.409066,-122.077836]
hostname = "localhost"
port = 5000
fgout = output_flightgear(localpos, hostname, port)
for line in iof:
timetosend = float(line.split()[6])
while (time.time() - timeoffset) < timetosend:
Jump to Line
Something went wrong with that request. Please try again.