def export_nmea(emu, icao, trlat, trlgt, tralt):
distance = EarthDistance((trlat, trlgt), (emu.mylat, emu.mylon))
alt_diff = tralt - emu.refalt
if (distance < DISTANCE_FAR
and alt_diff < ALTITUDE_FAR): # Filter out unrealistic readings
(dx, dy) = MeterOffset((trlat, trlgt), (emu.mylat, emu.mylon))
math_angle = Rad2Deg(atan2(dy, dx))
azim = 90 - math_angle
if emu.NorthUp is False:
azim = azim - emu.mytrk
azim = (azim + 360) % 360
bearing = int(azim)
if bearing > 180:
bearing = bearing - 360
#print distance , azimuth
if (distance < DISTANCE_CLOSE):
alarm_level = ALARM_LEVEL_URGENT
else:
if (distance < DISTANCE_NEAR):
alarm_level = ALARM_LEVEL_IMPORTANT
else:
alarm_level = ALARM_LEVEL_LOW
str1 = "$PFLAU,%d,1,2,1,%d,%d,2,%d,%u*" % \
( 1, alarm_level, bearing, int(alt_diff), int(distance) )
csum = checksum(str1)
str1 += "%02x\r\n" % csum
#print str1
#self.tty.write(str1)
emu.x.sendto(bytearray(str1), (emu.xcsoar_host, emu.xcsoar_port))
# XCSoar ignores warning data from PFLAU
azim_rad = radians(azim)
str2 = "$PFLAA,%d,%d,%d,%d,2,%s,,,,,1*" % \
( alarm_level, int(distance * cos(azim_rad)), \
int(distance * sin(azim_rad)), int(alt_diff), icao )
csum = checksum(str2)
str2 += "%02x\r\n" % csum
#print str2
#self.tty.write(str2)
emu.x.sendto(bytearray(str2), (emu.xcsoar_host, emu.xcsoar_port))
def Proximity(dlist, dlock, display_sock, display_host, display_port, MyId,
NorthUp):
"Create traffic proximity table for my aircraft."
display = Display(display_sock, display_host, display_port)
while True:
OtherTrafiic = []
Iaminlist = False
dlock.acquire()
for element in dlist:
#print element.id
if element.id == MyId:
Me = element
Iaminlist = True
else:
OtherTrafiic.append(element)
dlock.release()
if Iaminlist:
ProximityList = []
for element in OtherTrafiic:
mylat = float(Me.lat)
mylgt = float(Me.lgt)
myalt = float(Me.alt)
mytrk = float(Me.track)
trlat = float(element.lat)
trlgt = float(element.lgt)
tralt = float(element.alt)
dist = EarthDistance((trlat, trlgt), (mylat, mylgt))
(dx, dy) = MeterOffset((trlat, trlgt), (mylat, mylgt))
math_angle = Rad2Deg(atan2(dy, dx))
azim = 90 - math_angle
if NorthUp is False:
azim = azim - mytrk
azim = (azim + 360) % 360
alt_diff = tralt - myalt
id = element.id
track = float(element.track)
speed = float(element.speed)
traffic = Traffic(dist, azim, alt_diff, id, track, speed)
ProximityList.append(traffic)
#for element in ProximityList:
# print element.dist, element.azim, element.alt_diff, element.id
ProximityList.sort(key=sortByDist)
if len(ProximityList) > 0:
display.clear()
for element in ProximityList:
print element.dist, element.azim, element.alt_diff, element.id
display.mark(element.dist, element.azim, element.alt_diff,
element.id)
if len(ProximityList) > 0:
print
display.draw()
time.sleep(PROXIMITY_CYCLE)