From 15f2833aee74f86e295bc03453d8b348cca65af5 Mon Sep 17 00:00:00 2001
From: junzis <junzis@users.noreply.github.com>
Date: Tue, 7 Mar 2017 13:41:07 +0100
Subject: [PATCH] add surface velocity decoding from msg TC:5-8

---
 README.rst         |  8 +++--
 pyModeS/adsb.py    | 77 ++++++++++++++++++++++++++++++++++++++++++----
 tests/test_adsb.py |  2 ++
 3 files changed, 79 insertions(+), 8 deletions(-)

diff --git a/README.rst b/README.rst
index cc1dcca..a0e823f 100644
--- a/README.rst
+++ b/README.rst
@@ -72,8 +72,12 @@ Core functions for ADS-B decoding:
     pms.adsb.surface_position_with_ref(msg, lat_ref, lon_ref)
 
     pms.adsb.altitude(msg)
-    pms.adsb.velocity(msg)
-    pms.adsb.speed_heading(msg)
+
+    pms.adsb.velocity(msg)          # handles both surface & airborne messages
+    pms.adsb.speed_heading(msg)     # handles both surface & airborne messages
+    pms.adsb.surface_velocity(msg)
+    pms.adsb.airborne_velocity(msg)
+
 
 **Hint: When you have a fix position of the aircraft, it is convenient to
 use `position_with_ref()` method to decode with only one position message
diff --git a/pyModeS/adsb.py b/pyModeS/adsb.py
index 04aa9dc..1391ffa 100644
--- a/pyModeS/adsb.py
+++ b/pyModeS/adsb.py
@@ -534,6 +534,42 @@ def nic(msg):
 
 def velocity(msg):
     """Calculate the speed, heading, and vertical rate
+    (handles both airborne or surface message)
+
+    Args:
+        msg (string): 28 bytes hexadecimal message string
+
+    Returns:
+        (int, float, int, string): speed (kt), heading (degree),
+            rate of climb/descend (ft/min), and speed type
+            ('GS' for ground speed, 'AS' for airspeed)
+    """
+
+    if 5 <= typecode(msg) <= 8:
+        return surface_velocity(msg)
+
+    elif typecode(msg) == 19:
+        return airborne_velocity(msg)
+
+    else:
+        raise RuntimeError("incorrect or inconsistant message types")
+
+def speed_heading(msg):
+    """Get speed and heading only from the velocity message
+    (handles both airborne or surface message)
+
+    Args:
+        msg (string): 28 bytes hexadecimal message string
+
+    Returns:
+        (int, float): speed (kt), heading (degree)
+    """
+    spd, hdg, rocd, tag = velocity(msg)
+    return spd, hdg
+
+
+def airborne_velocity(msg):
+    """Calculate the speed, heading, and vertical rate
 
     Args:
         msg (string): 28 bytes hexadecimal message string
@@ -582,14 +618,43 @@ def velocity(msg):
     return int(spd), round(hdg, 1), int(rocd), tag
 
 
-def speed_heading(msg):
-    """Get speed and heading only from the velocity message
-
+def surface_velocity(msg):
+    """Decode surface velocity from from a surface position message
     Args:
         msg (string): 28 bytes hexadecimal message string
 
     Returns:
-        (int, float): speed (kt), heading (degree)
+        (int, float, int, string): speed (kt), heading (degree),
+            rate of climb/descend (ft/min), and speed type
+            ('GS' for ground speed, 'AS' for airspeed)
     """
-    spd, hdg, rocd, tag = velocity(msg)
-    return spd, hdg
+
+    if typecode(msg) < 5 or typecode(msg) > 8:
+        raise RuntimeError("%s: Not a surface message" % msg)
+
+    msgbin = util.hex2bin(msg)
+
+    # heading
+    hdg_status = int(msgbin[44])
+    if hdg_status == 1:
+        hdg = util.bin2int(msgbin[45:52]) * 360.0 / 128.0
+    else:
+        hdg = None
+
+    # ground movment / speed
+    mov = util.bin2int(msgbin[37:44])
+
+    if mov == 0 or mov > 124:
+        spd = None
+    elif mov == 1:
+        spd = 0
+    elif mov == 124:
+        spd = 175
+    else:
+        movs = [2, 9, 13, 39, 94, 109, 124]
+        kts = [0.125, 1, 2, 15, 70, 100, 175]
+        i = next(m[0] for m in enumerate(movs) if m[1] > mov)
+        step = (kts[i] - kts[i-1]) * 1.0 / (movs[i]-movs[i-1])
+        spd = kts[i-1] + (mov-movs[i-1]) * step
+
+        return round(spd, 2), round(hdg, 1), 0, 'GS'
diff --git a/tests/test_adsb.py b/tests/test_adsb.py
index 50b1d61..22bbb81 100644
--- a/tests/test_adsb.py
+++ b/tests/test_adsb.py
@@ -58,8 +58,10 @@ def test_adsb_alt():
 def test_adsb_velocity():
     vgs = adsb.velocity("8D485020994409940838175B284F")
     vas = adsb.velocity("8DA05F219B06B6AF189400CBC33F")
+    vgs_surface = adsb.velocity("8FC8200A3AB8F5F893096B000000")
     assert vgs == (159, 182.9, -832, 'GS')
     assert vas == (376, 244.0, -2304, 'AS')
+    assert vgs_surface == (19.0, 42.2, 0 , 'GS')
 
 
 def test_nic():