From d48caed7e6bbdea28d4d534eb3507d4da7d2312a Mon Sep 17 00:00:00 2001 From: Xavier Olive Date: Tue, 29 Oct 2019 11:41:00 +0100 Subject: [PATCH] add bds05 --- pyModeS/c_decoder/bds/__init__.py | 0 pyModeS/c_decoder/bds/bds05.pyx | 192 ++++++++++++++++++++++++++++++ pyModeS/c_decoder/common.pxd | 23 ++++ pyModeS/c_decoder/common.pyx | 15 ++- setup.py | 3 +- 5 files changed, 224 insertions(+), 9 deletions(-) create mode 100644 pyModeS/c_decoder/bds/__init__.py create mode 100644 pyModeS/c_decoder/bds/bds05.pyx create mode 100644 pyModeS/c_decoder/common.pxd diff --git a/pyModeS/c_decoder/bds/__init__.py b/pyModeS/c_decoder/bds/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyModeS/c_decoder/bds/bds05.pyx b/pyModeS/c_decoder/bds/bds05.pyx new file mode 100644 index 0000000..09d12e2 --- /dev/null +++ b/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 . + + +# ------------------------------------------ +# 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 diff --git a/pyModeS/c_decoder/common.pxd b/pyModeS/c_decoder/common.pxd new file mode 100644 index 0000000..36e165c --- /dev/null +++ b/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) \ No newline at end of file diff --git a/pyModeS/c_decoder/common.pyx b/pyModeS/c_decoder/common.pyx index 8074ab4..e1809e6 100644 --- a/pyModeS/c_decoder/common.pyx +++ b/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 c_floor(x) + return 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) diff --git a/setup.py b/setup.py index c3055d6..fa6eb55 100644 --- a/setup.py +++ b/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"]), ]