From 90116417d9a2eb1e54d1e940a2d00a746761bae0 Mon Sep 17 00:00:00 2001 From: nzkarit Date: Sun, 25 Aug 2019 15:50:14 +1200 Subject: [PATCH] Working of velcity, heading and vertical speed TODO There is something up with the math and rounding or something need work on this more and understand the math actually going on TODO Velocity, Heading and vertical speed as argument --- ADSB_Encoder.py | 7 ++++--- ModeS.py | 53 ++++++++++++++++++++++++++++++++++--------------- 2 files changed, 41 insertions(+), 19 deletions(-) diff --git a/ADSB_Encoder.py b/ADSB_Encoder.py index 4ef2f4c..0517832 100755 --- a/ADSB_Encoder.py +++ b/ADSB_Encoder.py @@ -63,7 +63,8 @@ def argParser(): parser.add_argument('--csv', '--csvfile', '--in', '--input', action='store', type=str, default=cfg.get('general', 'csvfile'), dest='csvfile', help='Import a CSV file with the plane data in it. Default: %(default)s') parser.add_argument('--intermessagegap', action='store', type=int, default=cfg.get('general', 'intermessagegap'), dest='intermessagegap', help='When repeating or reading a CSV the number of microseconds between messages. Default: %(default)s') parser.add_argument('--realtime', action='store', default=cfg.getboolean('general', 'realtime'), type=auto_bool, dest='realtime', help='When running a CSV which has a timestamp column whether to run in realtime following the timestamp or if just follow intermessagegap. If realtime is set it will override intermessagegap. Default: %(default)s') - # TODO Make it so it can do a static checksum + # TODO Make it so it can do a static checksum or one/two bit error + # TODO Velocity, Heading and vertical speed as argument return parser.parse_args() def singlePlane(arguments): @@ -74,7 +75,7 @@ def singlePlane(arguments): modes = ModeS() (df17_pos_even, df17_pos_odd) = modes.df17_pos_rep_encode(arguments.capability, arguments.icao, arguments.typecode, arguments.surveillancestatus, arguments.nicsupplementb, arguments.altitude, arguments.time, arguments.latitude, arguments.longitude, arguments.surface) - df17_velocity = modes.vel_heading_encode(arguments.capability, arguments.icao) + df17_velocity = modes.vel_heading_encode(arguments.capability, arguments.icao, 450, 200, -1000) ppm = PPM() df17_array_position = ppm.frame_1090es_ppm_modulate(df17_pos_even, df17_pos_odd) @@ -142,7 +143,7 @@ def manyPlanes(arguments): modes = ModeS() (df17_pos_even, df17_pos_odd) = modes.df17_pos_rep_encode(row['capability'], row['icao'], row['typecode'], row['surveillancestatus'], row['nicsupplementb'], row['altitude'], row['time'], row['latitude'], row['longitude'], row['surface']) - df17_velocity = modes.vel_heading_encode(row['capability'], row['icao']) + df17_velocity = modes.vel_heading_encode(row['capability'], row['icao'], 450, 200, -1000) ppm = PPM() df17_array_position = ppm.frame_1090es_ppm_modulate(df17_pos_even, df17_pos_odd) diff --git a/ModeS.py b/ModeS.py index 4b4a1ab..2270abb 100644 --- a/ModeS.py +++ b/ModeS.py @@ -1,5 +1,6 @@ from ModeSLocation import ModeSLocation import math +import numpy ############################################################### # Further work on fork # Copyright (C) 2017 David Robinson @@ -73,37 +74,62 @@ class ModeS: return (df17_even_bytes, df17_odd_bytes) #From https://github.com/jaywilhelm/ADSB-Out_Python on 2019-08-18 - def vel_heading_encode(self, ca, icao): + # TODO There is something up with the math and rounding or something need work on this more and understand the math actually going on + def vel_heading_encode(self, ca, icao, in_velocity, in_heading_deg, vertical_rate): #(ca,icao,ew_dir,ew_vel,ns_dir,ns_vel) df = 17 #ca = 5 - #1-5 downlink format #6-8 CA capability #9-32 ICAO #33-88 DATA -> 33-87 w/ 33-37 TC #89-112 Parity + in_heading_rad = numpy.deg2rad(in_heading_deg) + V_EW = abs(int(in_velocity*numpy.sin(in_heading_rad))) + V_NS = abs(int(in_velocity*numpy.cos(in_heading_rad))) + + quadrant = numpy.floor(in_heading_deg / 90) + + if(quadrant == 0): + S_EW = 1 + S_NS = 1 + elif(quadrant == 1): + S_EW = 0 + S_NS = 1 + elif(quadrant == 2): + S_EW = 0 + S_NS = 0 + else: + S_EW = 1 + S_NS = 0 + + S_Vr = 1 + Vr = int(vertical_rate) + + if(vertical_rate < 0): + Vr = -Vr + S_Vr = 0 tc = 19 #33-37 1-5 type code st = 0x01 #38-40 6-8 subtype, 3 air, 1 ground speed ic = 0 # #41 9 intent change flag resv_a = 0#1 #42 10 NAC = 2#0 #43-45 11-13 velocity uncertainty - S_EW = 1#1 #46 14 - V_EW = 97#9 #47-56 15-24 - S_NS = 0#1 #57 25 north-south sign - V_NS = 378#0xA0 #58-67 26-35 160 north-south vel + #S_EW = 1#1 #46 14 + #V_EW = 97#9 #47-56 15-24 + #S_NS = 0#1 #57 25 north-south sign + #V_NS = 379#0xA0 #58-67 26-35 160 north-south vel VrSrc = 1#0 #68 36 vertical rate source - S_Vr = 1#1 #69 37 vertical rate sign - Vr = 40#0x0E #70-78 38-46 14 vertical rate + #S_Vr = 1#1 #69 37 vertical rate sign + #Vr = 41#0x0E #70-78 38-46 14 vertical rate RESV_B = 0 #79-80 47-48 S_Dif = 0 #81 49 diff from baro alt, sign Dif = 0x1c#0x17 #82-88 50-66 23 diff from baro alt - #ca = 5 + ca = 5 #icao = 0xabcdef#0xa06703 #0x485020 # - + dfvel = [] dfvel.append((df << 3) | ca) dfvel.append((icao >> 16) & 0xff) @@ -117,16 +143,11 @@ class ModeS: dfvel.append(((V_NS << 5) & 0xE0) | (VrSrc << 4) | (S_Vr << 3) | ((Vr >> 6) & 0x03)) dfvel.append(((Vr << 2) & 0xFC) | (RESV_B)) dfvel.append((S_Dif << 7) | (Dif)) - dfvel_str = "{0:02x} {1:02x} {2:02x} {3:02x} {4:02x} {5:02x} {6:02x} {7:02x} {8:02x} {9:02x} {10:02x}".format( *dfvel[0:11]) - #print(dfvel_str) dfvel_str2 = "{0:02x}{1:02x}{2:02x}{3:02x}{4:02x}{5:02x}{6:02x}{7:02x}{8:02x}{9:02x}{10:02x}".format( *dfvel[0:11]) crc_str = "%X" % self.bin2int(self.modes_crc(dfvel_str2+"000000", encode=True)) - #print(crc_str) - #print(dfvel_str), " %X" % +"000000", encode=True)) - #, "%X" % get_parity(hex2bin(dfvel_str+"000000"), extended=True)) dfvel_crc = self.bin2int(self.modes_crc(dfvel_str2 + "000000", encode=True)) dfvel.append((dfvel_crc >> 16) & 0xff) dfvel.append((dfvel_crc >> 8) & 0xff) @@ -164,7 +185,7 @@ class ModeS: """ GENERATOR = "1111111111111010000001001" # Currently don't know what is magic about this number - + msgbin = list(self.hex2bin(msg)) if encode: