add bds05

pyModeS/c_decoder/bds/bds05.pyx

@@ -0,0 +1,192 @@
+# Copyright (C) 2018 Junzi Sun (TU Delft)
+
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+
+# ------------------------------------------
+#   BDS 0,5
+#   ADS-B TC=9-18
+#   Airborn position
+# ------------------------------------------
+
+# cython: language_level=3
+
+cimport cython
+
+from .. cimport common
+from libc.math cimport NAN as nan, round as c_round
+
+
+@cython.cdivision(True)
+def airborne_position(bytes msg0 not None, bytes msg1 not None, long t0, long t1):
+    """Decode airborn position from a pair of even and odd position message
+
+    Args:
+        msg0 (string): even message (28 bytes hexadecimal string)
+        msg1 (string): odd message (28 bytes hexadecimal string)
+        t0 (int): timestamps for the even message
+        t1 (int): timestamps for the odd message
+
+    Returns:
+        (float, float): (latitude, longitude) of the aircraft
+    """
+
+    cdef bytearray mb0 = common.hex2bin(msg0)[32:]
+    cdef bytearray mb1 = common.hex2bin(msg1)[32:]
+
+    cdef unsigned char oe0 = common.char_to_int(mb0[21])
+    cdef unsigned char oe1 = common.char_to_int(mb1[21])
+
+    if oe0 == 0 and oe1 == 1:
+        pass
+    elif oe0 == 1 and oe1 == 0:
+        mb0, mb1 = mb1, mb0
+        t0, t1 = t1, t0
+    else:
+        raise RuntimeError("Both even and odd CPR frames are required.")
+
+    # 131072 is 2^17, since CPR lat and lon are 17 bits each.
+    cdef double cprlat_even = common.bin2int(mb0[22:39]) / 131072.0
+    cdef double cprlon_even = common.bin2int(mb0[39:56]) / 131072.0
+    cdef double cprlat_odd = common.bin2int(mb1[22:39]) / 131072.0
+    cdef double cprlon_odd = common.bin2int(mb1[39:56]) / 131072.0
+
+    cdef double air_d_lat_even = 360.0 / 60
+    cdef double air_d_lat_odd = 360.0 / 59
+
+    # compute latitude index 'j'
+    cdef long j = common.floor(59 * cprlat_even - 60 * cprlat_odd + 0.5)
+
+    cdef double lat_even = (air_d_lat_even * (j % 60 + cprlat_even))
+    cdef double lat_odd = (air_d_lat_odd * (j % 59 + cprlat_odd))
+
+    if lat_even >= 270:
+        lat_even = lat_even - 360
+
+    if lat_odd >= 270:
+        lat_odd = lat_odd - 360
+
+    # check if both are in the same latidude zone, exit if not
+    if common.cprNL(lat_even) != common.cprNL(lat_odd):
+        return nan
+
+    cdef int nl, ni, m
+    cdef double lat, lon
+
+    # compute ni, longitude index m, and longitude
+    if t0 > t1:
+        lat = lat_even
+        nl = common.cprNL(lat)
+        # ni = max(common.cprNL(lat) - 0, 1)
+        ni = common.cprNL(lat)
+        if ni < 1:
+            ni = 1
+        m = common.floor(cprlon_even * (nl - 1) - cprlon_odd * nl + 0.5)
+        lon = (360.0 / ni) * (m % ni + cprlon_even)
+    else:
+        lat = lat_odd
+        nl = common.cprNL(lat)
+        # ni = max(common.cprNL(lat) - 1, 1)
+        ni = common.cprNL(lat) - 1
+        if ni < 1:
+            ni = 1
+        m = common.floor(cprlon_even * (nl - 1) - cprlon_odd * nl + 0.5)
+        lon = (360.0 / ni) * (m % ni + cprlon_odd)
+
+    if lon > 180:
+        lon = lon - 360
+
+    return round(lat, 5), round(lon, 5)
+
+
+@cython.cdivision(True)
+def airborne_position_with_ref(bytes msg not None, double lat_ref, double lon_ref):
+    """Decode airborne position with only one message,
+    knowing reference nearby location, such as previously calculated location,
+    ground station, or airport location, etc. The reference position shall
+    be with in 180NM of the true position.
+
+    Args:
+        msg (string): even message (28 bytes hexadecimal string)
+        lat_ref: previous known latitude
+        lon_ref: previous known longitude
+
+    Returns:
+        (float, float): (latitude, longitude) of the aircraft
+    """
+
+    cdef bytearray mb = common.hex2bin(msg)[32:]
+
+    cdef double cprlat = common.bin2int(mb[22:39]) / 131072.0
+    cdef double cprlon = common.bin2int(mb[39:56]) / 131072.0
+
+    cdef unsigned char i = common.char_to_int(mb[21])
+    cdef double d_lat = 360.0 / 59 if i else 360.0 / 60
+
+    cdef long j = common.floor(lat_ref / d_lat) + common.floor(
+        0.5 + ((lat_ref % d_lat) / d_lat) - cprlat
+    )
+
+    cdef double lat = d_lat * (j + cprlat)
+    cdef double d_lon, lon
+
+    cdef int ni = common.cprNL(lat) - i
+
+    if ni > 0:
+        d_lon = 360.0 / ni
+    else:
+        d_lon = 360.0
+
+    cdef int m = common.floor(lon_ref / d_lon) + common.floor(
+        0.5 + ((lon_ref % d_lon) / d_lon) - cprlon
+    )
+
+    lon = d_lon * (m + cprlon)
+
+    return round(lat, 5), round(lon, 5)
+
+
+cpdef double altitude(bytes msg):
+    """Decode aircraft altitude
+
+    Args:
+        msg (string): 28 bytes hexadecimal message string
+
+    Returns:
+        int: altitude in feet
+    """
+
+    cdef int tc = common.typecode(msg)
+
+    if tc < 9 or tc == 19 or tc > 22:
+        raise RuntimeError("%s: Not a airborn position message" % msg)
+
+    cdef bytearray mb = common.hex2bin(msg)[32:]
+    cdef unsigned char q
+    cdef int n
+    cdef double alt
+
+    if tc < 19:
+        # barometric altitude
+        q = mb[15]
+        if q:
+            n = common.bin2int(mb[8:15] + mb[16:20])
+            alt = n * 25 - 1000
+        else:
+            alt = nan
+    else:
+        # GNSS altitude, meters -> feet
+        alt = common.bin2int(mb[8:20]) * 3.28084
+
+    return alt

pyModeS/c_decoder/common.pxd

@@ -0,0 +1,23 @@
+# cython: language_level=3
+
+cdef int char_to_int(unsigned char binstr)
+cdef unsigned char int_to_char(unsigned char i)
+
+cpdef bytearray hex2bin(bytes hexstr)
+cpdef long bin2int(bytearray binstr)
+cpdef long hex2int(bytearray binstr)
+
+cpdef unsigned char df(bytes msg)
+cpdef long crc(bytes msg, bint encode=*)
+
+cpdef long floor(double x)
+cpdef str icao(bytes msg)
+cpdef bint is_icao_assigned(bytes icao)
+
+cpdef int typecode(bytes msg)
+cpdef int cprNL(double lat)
+cpdef str idcode(bytes msg)
+cpdef int altcode(bytes msg)
+
+cdef bytes data(bytes msg)
+cpdef bint allzeros(bytes msg)

pyModeS/c_decoder/common.pyx

@@ -1,4 +1,3 @@
-
 # cython: language_level=3
 
 cimport cython
@@ -143,14 +142,14 @@ cpdef long crc(bytes msg, bint encode=False):
 
 
 
-cpdef int floor(float x):
+cpdef long floor(double x):
     """Mode-S floor function.
 
     Defined as the greatest integer value k, such that k <= x
     For example: floor(3.6) = 3 and floor(-3.6) = -4
 
     """
-    return <int> c_floor(x)
+    return <long> c_floor(x)
 
 cpdef str icao(bytes msg):
     """Calculate the ICAO address from an Mode-S message.
@@ -228,7 +227,7 @@ cpdef int typecode(bytes msg):
     return bin2int(tcbin[0:5])
 
 @cython.cdivision(True)
-cpdef int cprNL(int lat):
+cpdef int cprNL(double lat):
     """NL() function in CPR decoding."""
 
     if lat == 0:
@@ -241,9 +240,9 @@ cpdef int cprNL(int lat):
         return 1
 
     cdef int nz = 15
-    cdef float a = 1 - cos(pi / (2 * nz))
-    cdef float b = cos(pi / 180.0 * fabs(lat)) ** 2
-    cdef float nl = 2 * pi / (acos(1 - a / b))
+    cdef double a = 1 - cos(pi / (2 * nz))
+    cdef double b = cos(pi / 180.0 * fabs(lat)) ** 2
+    cdef double nl = 2 * pi / (acos(1 - a / b))
     NL = floor(nl)
     return NL
 
@@ -359,7 +358,7 @@ cpdef int altcode(bytes msg):
 
 
 
-cdef int gray2alt(bytearray codestr):
+cpdef int gray2alt(bytearray codestr):
     cdef bytearray gc500 = codestr[:8]
     cdef int n500 = gray2int(gc500)
 

setup.py

@@ -20,7 +20,8 @@ from setuptools.extension import Extension
 from Cython.Build import cythonize
 
 extensions = [
-    Extension("pyModeS.c_decoder.common", ["pyModeS/c_decoder/common.pyx"])
+    Extension("pyModeS.c_decoder.common", ["pyModeS/c_decoder/common.pyx"]),
+    Extension("pyModeS.c_decoder.bds.bds05", ["pyModeS/c_decoder/bds/bds05.pyx"]),
 ]