Context Navigation

adsb2nmea.py @ 294

Last change on this file since 294 was 276, checked in by dominic, 11 years ago
Bugfix PFLAU sentence?
File size: 15.0 KB

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

Note: See TracBrowser for help on using the repository browser.

Context Navigation

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

Download in other formats: