From 2e3ceed0b00cd4ad22e1d6576fd517fa4ca8e365 Mon Sep 17 00:00:00 2001
From: Junzi Sun <junzi.sun@gmail.com>
Date: Wed, 6 Jan 2016 18:19:35 +0100
Subject: [PATCH] add subtype 3/4 velocity decoding

---
 decoder.py  | 59 +++++++++++++++++++++++++++++++++++------------------
 run_test.py | 42 +++++++++++++++++++++++---------------
 2 files changed, 64 insertions(+), 37 deletions(-)

diff --git a/decoder.py b/decoder.py
index 5a0f6d1..f025167 100644
--- a/decoder.py
+++ b/decoder.py
@@ -214,45 +214,64 @@ def cpr2position(cprlat0, cprlat1, cprlon0, cprlon1, t0, t1):
     # compute ni, longitude index m, and longitude
     if (t0 > t1):
         ni = cprN(lat_even, 0)
-        m = math.floor(cprlon_even * (cprNL(lat_even)-1)
-                       - cprlon_odd * cprNL(lat_even) + 0.5)
+        m = math.floor(cprlon_even * (cprNL(lat_even)-1) -
+                       cprlon_odd * cprNL(lat_even) + 0.5)
         lon = (360.0 / ni) * (m % ni + cprlon_even)
         lat = lat_even
     else:
         ni = cprN(lat_odd, 1)
-        m = math.floor(cprlon_even * (cprNL(lat_odd)-1)
-                       - cprlon_odd * cprNL(lat_odd) + 0.5)
+        m = math.floor(cprlon_even * (cprNL(lat_odd)-1) -
+                       cprlon_odd * cprNL(lat_odd) + 0.5)
         lon = (360.0 / ni) * (m % ni + cprlon_odd)
         lat = lat_odd
 
     if lon > 180:
         lon = lon - 360
 
-    return [lat, lon]
+    return lat, lon
 
+def get_velocity(msg):
+    """Calculate the speed, heading, and vertical rate"""
 
-def get_speed_heading(msg):
-    """Calculate the speed and heading."""
     msgbin = hex2bin(msg)
 
-    v_ew_dir = bin2int(msgbin[45])
-    v_ew = bin2int(msgbin[46:56])       # east-west velocity
+    subtype = bin2int(msgbin[37:40])
+
+    if subtype in (1, 2):
+        v_ew_sign = bin2int(msgbin[45])
+        v_ew = bin2int(msgbin[46:56]) - 1       # east-west velocity
+
+        v_ns_sign = bin2int(msgbin[56])
+        v_ns = bin2int(msgbin[57:67]) - 1       # north-south velocity
+
+        v_we = -1*v_ew if v_ew_sign else v_ew
+        v_sn = -1*v_ns if v_ns_sign else v_ns
+
+        spd = math.sqrt(v_sn*v_sn + v_we*v_we)  # unit in kts
 
-    v_ns_dir = bin2int(msgbin[56])
-    v_ns = bin2int(msgbin[57:67])       # north-south velocity
+        hdg = math.atan2(v_we, v_sn)
+        hdg = math.degrees(hdg)                 # convert to degrees
+        hdg = hdg if hdg >= 0 else hdg + 360    # no negative val
 
-    v_ew = -1*v_ew if v_ew_dir else v_ew
-    v_ns = -1*v_ns if v_ns_dir else v_ns
+        tag = 'GS'
 
-    # vr       = bin2int(msgbin[68:77])       # vertical rate
-    # vr_dir   = bin2int(msgbin[77])
+    else:
+        hdg = bin2int(msgbin[46:56]) / 1024.0 * 360.0
+        spd = bin2int(msgbin[57:67])
+
+        tag = 'AS'
+
+    vr_sign = bin2int(msgbin[68])
+    vr = bin2int(msgbin[68:77])             # vertical rate
+    rocd = -1*vr if vr_sign else vr         # rate of climb/descend
 
-    speed = math.sqrt(v_ns*v_ns + v_ew*v_ew)    # unit in kts
+    return int(spd), hdg, int(rocd), tag
 
-    heading = math.atan2(v_ew, v_ns)
-    heading = heading * 360.0 / (2 * math.pi)   # convert to degrees
-    heading = heading if heading >= 0 else heading + 360     # no negative val
-    return [speed, heading]
+
+def get_speed_heading(msg):
+    """Calculate the speed and heading."""
+    spd, hdg, rocd, tag = get_velocity(msg)
+    return spd, hdg
 
 
 def get_callsign(msg):
diff --git a/run_test.py b/run_test.py
index 3893a73..a39381d 100644
--- a/run_test.py
+++ b/run_test.py
@@ -1,38 +1,46 @@
 import decoder
 
+print '*************************'
 print 'Testing the ADS-B decoder'
-print '---------------------------'
+print '*************************'
 print
 
 # decode call sign test
-msg = '8D51004E20092578DB782072C825'
-cs = decoder.get_callsign(msg)
-print 'Message:', msg
+print "------- Test Callsign -------"
+msg_cs = '8D51004E20092578DB782072C825'
+cs = decoder.get_callsign(msg_cs)
+print 'Message:', msg_cs
 print 'Call sign:', cs
 print
 
 # decode position
-msg0 = '8D40058B58C901375147EFD09357'
-msg1 = '8D40058B58C904A87F402D3B8C59'
+print "------- Test Postiions -------"
+msg_pos_0 = '8D40058B58C901375147EFD09357'
+msg_pos_1 = '8D40058B58C904A87F402D3B8C59'
 t0 = 1446332400
 t1 = 1446332405
-pos = decoder.get_position(msg0, msg1, t0, t1)
-print 'Message E:', msg0
-print 'Message O:', msg1
+pos = decoder.get_position(msg_pos_0, msg_pos_1, t0, t1)
+print 'Message E:', msg_pos_0
+print 'Message O:', msg_pos_1
 print 'Position:', pos
 print
 
 # decode velocity
-msg = '8D51004E99850702685C00E582E4'
-sh = decoder.get_speed_heading(msg)
-print 'Message:', msg
-print 'Speed and heading:', sh
+print "------- Test Velocity -------"
+msg_v_s1 = '8D485020994409940838175B284F'   # subtype 1
+msg_v_s3 = '8DA05F219B06B6AF189400CBC33F'   # subtype 3
+v1 = decoder.get_velocity(msg_v_s1)
+v2 = decoder.get_velocity(msg_v_s3)
+print 'Message:', msg_v_s1
+print 'velocity:', v1
+print 'Message:', msg_v_s3
+print 'velocity:', v2
 print
 
 # test NIC
-# decode position
-msg = '8D40058B58C901375147EFD09357'
-nic = decoder.get_nic(msg1)
-print 'Message:', msg
+print "------- Test NIC -------"
+msg_nic = '8D40621D58C382D690C8AC2863A7'
+nic = decoder.get_nic(msg_nic)
+print 'Message:', msg_nic
 print 'NIC:', nic
 print