separate cleanly cython and python, bds05, bds06, modulo issues

This commit is contained in:
Xavier Olive 2019-10-29 16:37:44 +01:00
parent d48caed7e6
commit c804cd876c
5 changed files with 417 additions and 3 deletions

View File

@ -25,7 +25,7 @@
cimport cython
from .. cimport common
from libc.math cimport NAN as nan, round as c_round
from libc.math cimport NAN as nan, remainder
@cython.cdivision(True)
@ -134,8 +134,10 @@ def airborne_position_with_ref(bytes msg not None, double lat_ref, double lon_re
cdef unsigned char i = common.char_to_int(mb[21])
cdef double d_lat = 360.0 / 59 if i else 360.0 / 60
# https://docs.python.org/3/library/math.html#math.fmod
cdef double mod_lat = lat_ref % d_lat if lat_ref >= 0 else (lat_ref % d_lat + d_lat)
cdef long j = common.floor(lat_ref / d_lat) + common.floor(
0.5 + ((lat_ref % d_lat) / d_lat) - cprlat
0.5 + (mod_lat / d_lat) - cprlat
)
cdef double lat = d_lat * (j + cprlat)
@ -148,8 +150,10 @@ def airborne_position_with_ref(bytes msg not None, double lat_ref, double lon_re
else:
d_lon = 360.0
# https://docs.python.org/3/library/math.html#math.fmod
cdef double mod_lon = lon_ref % d_lon if lon_ref >= 0 else (lon_ref % d_lon + d_lon)
cdef int m = common.floor(lon_ref / d_lon) + common.floor(
0.5 + ((lon_ref % d_lon) / d_lon) - cprlon
0.5 + (mod_lon / d_lon) - cprlon
)
lon = d_lon * (m + cprlon)

View File

@ -0,0 +1,252 @@
# 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,6
# ADS-B TC=5-8
# Surface position
# ------------------------------------------
# cython: language_level=3
cimport cython
from .. cimport common
from cpython cimport array
from libc.math cimport NAN as nan, remainder
import math
@cython.cdivision(True)
def surface_position(bytes msg0 not None, bytes msg1 not None, long t0, long t1, double lat_ref, double lon_ref):
"""Decode surface position from a pair of even and odd position message,
the lat/lon of receiver must be provided to yield the correct solution.
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
lat_ref (float): latitude of the receiver
lon_ref (float): longitude of the receiver
Returns:
(float, float): (latitude, longitude) of the aircraft
"""
cdef bytearray msgbin0 = common.hex2bin(msg0)
cdef bytearray msgbin1 = common.hex2bin(msg1)
# 131072 is 2^17, since CPR lat and lon are 17 bits each.
cdef double cprlat_even = common.bin2int(msgbin0[54:71]) / 131072.0
cdef double cprlon_even = common.bin2int(msgbin0[71:88]) / 131072.0
cdef double cprlat_odd = common.bin2int(msgbin1[54:71]) / 131072.0
cdef double cprlon_odd = common.bin2int(msgbin1[71:88]) / 131072.0
cdef double air_d_lat_even = 90.0 / 60
cdef double air_d_lat_odd = 90.0 / 59
# compute latitude index 'j'
cdef long j = common.floor(59 * cprlat_even - 60 * cprlat_odd + 0.5)
# solution for north hemisphere
cdef int j_mod_60 = j % 60 if j > 0 else (j % 60) + 60
cdef int j_mod_59 = j % 59 if j > 0 else (j % 59) + 59
cdef double lat_even_n = (air_d_lat_even * ((j_mod_60) + cprlat_even))
cdef double lat_odd_n = (air_d_lat_odd * ((j_mod_59) + cprlat_odd))
# solution for north hemisphere
cdef double lat_even_s = lat_even_n - 90.0
cdef double lat_odd_s = lat_odd_n - 90.0
# chose which solution corrispondes to receiver location
cdef double lat_even = lat_even_n if lat_ref > 0 else lat_even_s
cdef double lat_odd = lat_odd_n if lat_ref > 0 else lat_odd_s
# check if both are in the same latidude zone, rare but possible
if common.cprNL(lat_even) != common.cprNL(lat_odd):
return nan
cdef int nl, ni, m, m_mod_ni
cdef double lat, lon
# compute ni, longitude index m, and longitude
if t0 > t1:
lat = lat_even
nl = common.cprNL(lat_even)
# ni = max(common.cprNL(lat_even) - 0, 1)
ni = common.cprNL(lat_even)
if ni < 1:
ni = 1
m = common.floor(cprlon_even * (nl - 1) - cprlon_odd * nl + 0.5)
# https://docs.python.org/3/library/math.html#math.fmod
m_mod_ni = m % ni if ni > 0 else (m % ni) + ni
lon = (90.0 / ni) * (m_mod_ni + cprlon_even)
else:
lat = lat_odd
nl = common.cprNL(lat_odd)
# ni = max(common.cprNL(lat_odd) - 1, 1)
ni = common.cprNL(lat_odd) - 1
if ni < 1:
ni = 1
m = common.floor(cprlon_even * (nl - 1) - cprlon_odd * nl + 0.5)
# https://docs.python.org/3/library/math.html#math.fmod
m_mod_ni = m % ni if ni > 0 else (m % ni) + ni
lon = (90.0 / ni) * (m_mod_ni + cprlon_odd)
# four possible longitude solutions
# lons = [lon, lon + 90.0, lon + 180.0, lon + 270.0]
cdef array.array _lons = array.array(
'd', [lon, lon + 90.0, lon + 180.0, lon + 270.0]
)
cdef double[4] lons = _lons
# make sure lons are between -180 and 180
# lons = [(l + 180) % 360 - 180 for l in lons]
cdef int idxmin = 0
cdef float d_, delta = abs(lons[0] - lon_ref)
for i in range(1, 4):
lons[i] = (lons[i] + 180) % 360 - 180
d_ = abs(lons[i] - lon_ref)
if d_ < delta:
idxmin = i
delta = d_
# the closest solution to receiver is the correct one
# dls = [abs(lon_ref - l) for l in lons]
# imin = min(range(4), key=dls.__getitem__)
# lon = lons[imin]
lon = lons[idxmin]
return round(lat, 5), round(lon, 5)
@cython.cdivision(True)
def surface_position_with_ref(bytes msg not None, double lat_ref, double lon_ref):
"""Decode surface 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 45NM 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 = 90.0 / 59 if i else 90.0 / 60
# https://docs.python.org/3/library/math.html#math.fmod
cdef double mod_lat = lat_ref % d_lat if lat_ref >= 0 else (lat_ref % d_lat + d_lat)
cdef long j = common.floor(lat_ref / d_lat) + common.floor(
0.5 + (mod_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 = 90.0 / ni
else:
d_lon = 90.0
# https://docs.python.org/3/library/math.html#math.fmod
cdef double mod_lon = lon_ref % d_lon if lon_ref >= 0 else (lon_ref % d_lon + d_lon)
cdef int m = common.floor(lon_ref / d_lon) + common.floor(
0.5 + (mod_lon / d_lon) - cprlon
)
lon = d_lon * (m + cprlon)
return round(lat, 5), round(lon, 5)
@cython.cdivision(True)
def surface_velocity(bytes msg, bint rtn_sources=False):
"""Decode surface velocity from from a surface position message
Args:
msg (string): 28 bytes hexadecimal message string
rtn_source (boolean): If the function will return
the sources for direction of travel and vertical
rate. This will change the return value from a four
element array to a six element array.
Returns:
(int, float, int, string, string, None): speed (kt),
ground track (degree), None for rate of climb/descend (ft/min),
and speed type ('GS' for ground speed), direction source
('true_north' for ground track / true north as reference),
None rate of climb/descent source.
"""
if common.typecode(msg) < 5 or common.typecode(msg) > 8:
raise RuntimeError("%s: Not a surface message, expecting 5<TC<8" % msg)
cdef bytearray mb = common.hex2bin(msg)[32:]
cdef double trk
# ground track
cdef unsigned char trk_status = common.char_to_int(mb[12])
if trk_status == 1:
trk = common.bin2int(mb[13:20]) * 360.0 / 128.0
trk = round(trk, 1)
else:
trk = nan
# ground movment / speed
cdef long mov = common.bin2int(mb[5:12])
cdef double spd, step
cdef array.array _movs, _kts
cdef double[7] movs, kts
cdef Py_ssize_t i = 0
if mov == 0 or mov > 124:
spd = nan
elif mov == 1:
spd = 0
elif mov == 124:
spd = 175
else:
_movs = array.array('d', [2, 9, 13, 39, 94, 109, 124])
_kts = array.array('d', [0.125, 1, 2, 15, 70, 100, 175])
movs = _movs
kts = _kts
# i = next(m[0] for m in enumerate(movs) if m[1] > mov)
for i in range(7):
if movs[i] > mov:
break
step = (kts[i] - kts[i - 1]) * 1.0 / (movs[i] - movs[i - 1])
spd = kts[i - 1] + (mov - movs[i - 1]) * step
spd = round(spd, 2)
if rtn_sources:
return spd, trk, 0, "GS", "true_north", None
else:
return spd, trk, 0, "GS"

View File

@ -21,7 +21,9 @@ from Cython.Build import cythonize
extensions = [
Extension("pyModeS.c_decoder.common", ["pyModeS/c_decoder/common.pyx"]),
Extension("pyModeS.c_decoder.adsb", ["pyModeS/c_decoder/adsb.pyx"]),
Extension("pyModeS.c_decoder.bds.bds05", ["pyModeS/c_decoder/bds/bds05.pyx"]),
Extension("pyModeS.c_decoder.bds.bds06", ["pyModeS/c_decoder/bds/bds06.pyx"]),
]

95
tests/test_c_adsb.py Normal file
View File

@ -0,0 +1,95 @@
from pyModeS.c_decoder import adsb
# === TEST ADS-B package ===
def test_adsb_icao():
assert adsb.icao(b"8D406B902015A678D4D220AA4BDA") == "406B90"
def test_adsb_category():
assert adsb.category(b"8D406B902015A678D4D220AA4BDA") == 0
def test_adsb_callsign():
assert adsb.callsign(b"8D406B902015A678D4D220AA4BDA") == "EZY85MH_"
def test_adsb_position():
pos = adsb.position(
b"8D40058B58C901375147EFD09357",
b"8D40058B58C904A87F402D3B8C59",
1446332400,
1446332405,
)
assert pos == (49.81755, 6.08442)
def test_adsb_position_swap_odd_even():
pos = adsb.position(
b"8D40058B58C904A87F402D3B8C59",
b"8D40058B58C901375147EFD09357",
1446332405,
1446332400,
)
assert pos == (49.81755, 6.08442)
def test_adsb_position_with_ref():
pos = adsb.position_with_ref(b"8D40058B58C901375147EFD09357", 49.0, 6.0)
assert pos == (49.82410, 6.06785)
pos = adsb.position_with_ref(b"8FC8200A3AB8F5F893096B000000", -43.5, 172.5)
assert pos == (-43.48564, 172.53942)
def test_adsb_airborne_position_with_ref():
pos = adsb.airborne_position_with_ref(b"8D40058B58C901375147EFD09357", 49.0, 6.0)
assert pos == (49.82410, 6.06785)
pos = adsb.airborne_position_with_ref(b"8D40058B58C904A87F402D3B8C59", 49.0, 6.0)
assert pos == (49.81755, 6.08442)
def test_adsb_surface_position_with_ref():
pos = adsb.surface_position_with_ref(b"8FC8200A3AB8F5F893096B000000", -43.5, 172.5)
assert pos == (-43.48564, 172.53942)
def test_adsb_surface_position():
pos = adsb.surface_position(
b"8CC8200A3AC8F009BCDEF2000000",
b"8FC8200A3AB8F5F893096B000000",
0,
2,
-43.496,
172.558,
)
assert pos == (-43.48564, 172.53942)
def test_adsb_alt():
assert adsb.altitude(b"8D40058B58C901375147EFD09357") == 39000
def test_adsb_velocity():
vgs = adsb.velocity(b"8D485020994409940838175B284F")
vas = adsb.velocity(b"8DA05F219B06B6AF189400CBC33F")
vgs_surface = adsb.velocity(b"8FC8200A3AB8F5F893096B000000")
assert vgs == (159, 182.88, -832, "GS")
assert vas == (375, 243.98, -2304, "TAS")
assert vgs_surface == (19.0, 42.2, 0, "GS")
assert adsb.altitude_diff(b"8D485020994409940838175B284F") == 550
# def test_nic():
# assert adsb.nic('8D3C70A390AB11F55B8C57F65FE6') == 0
# assert adsb.nic('8DE1C9738A4A430B427D219C8225') == 1
# assert adsb.nic('8D44058880B50006B1773DC2A7E9') == 2
# assert adsb.nic('8D44058881B50006B1773DC2A7E9') == 3
# assert adsb.nic('8D4AB42A78000640000000FA0D0A') == 4
# assert adsb.nic('8D4405887099F5D9772F37F86CB6') == 5
# assert adsb.nic('8D4841A86841528E72D9B472DAC2') == 6
# assert adsb.nic('8D44057560B9760C0B840A51C89F') == 7
# assert adsb.nic('8D40621D58C382D690C8AC2863A7') == 8
# assert adsb.nic('8F48511C598D04F12CCF82451642') == 9
# assert adsb.nic('8DA4D53A50DBF8C6330F3B35458F') == 10
# assert adsb.nic('8D3C4ACF4859F1736F8E8ADF4D67') == 11

61
tests/test_c_common.py Normal file
View File

@ -0,0 +1,61 @@
from pyModeS.c_decoder import common
def test_conversions():
assert common.hex2bin(b"6E406B") == bytearray(b"011011100100000001101011")
def test_crc_decode():
assert common.crc(b"8D406B902015A678D4D220AA4BDA") == 0
assert common.crc(b"8d8960ed58bf053cf11bc5932b7d") == 0
assert common.crc(b"8d45cab390c39509496ca9a32912") == 0
assert common.crc(b"8d49d3d4e1089d00000000744c3b") == 0
assert common.crc(b"8d74802958c904e6ef4ba0184d5c") == 0
assert common.crc(b"8d4400cd9b0000b4f87000e71a10") == 0
assert common.crc(b"8d4065de58a1054a7ef0218e226a") == 0
assert common.crc(b"c80b2dca34aa21dd821a04cb64d4") == 10719924
assert common.crc(b"a800089d8094e33a6004e4b8a522") == 4805588
assert common.crc(b"a8000614a50b6d32bed000bbe0ed") == 5659991
assert common.crc(b"a0000410bc900010a40000f5f477") == 11727682
assert common.crc(b"8d4ca251204994b1c36e60a5343d") == 16
assert common.crc(b"b0001718c65632b0a82040715b65") == 353333
def test_crc_encode():
parity = common.crc(b"8D406B902015A678D4D220AA4BDA", encode=True)
assert parity == 11160538
def test_icao():
assert common.icao(b"8D406B902015A678D4D220AA4BDA") == "406B90"
assert common.icao(b"A0001839CA3800315800007448D9") == "400940"
assert common.icao(b"A000139381951536E024D4CCF6B5") == "3C4DD2"
assert common.icao(b"A000029CFFBAA11E2004727281F1") == "4243D0"
def test_modes_altcode():
assert common.altcode(b"A02014B400000000000000F9D514") == 32300
def test_modes_idcode():
assert common.idcode(b"A800292DFFBBA9383FFCEB903D01") == "1346"
def test_graycode_to_altitude():
assert common.gray2alt(bytearray(b"00000000010")) == -1000
assert common.gray2alt(bytearray(b"00000001010")) == -500
assert common.gray2alt(bytearray(b"00000011011")) == -100
assert common.gray2alt(bytearray(b"00000011010")) == 0
assert common.gray2alt(bytearray(b"00000011110")) == 100
assert common.gray2alt(bytearray(b"00000010011")) == 600
assert common.gray2alt(bytearray(b"00000110010")) == 1000
assert common.gray2alt(bytearray(b"00001001001")) == 5800
assert common.gray2alt(bytearray(b"00011100100")) == 10300
assert common.gray2alt(bytearray(b"01100011010")) == 32000
assert common.gray2alt(bytearray(b"01110000100")) == 46300
assert common.gray2alt(bytearray(b"01010101100")) == 50200
assert common.gray2alt(bytearray(b"11011110100")) == 73200
assert common.gray2alt(bytearray(b"10000000011")) == 126600
assert common.gray2alt(bytearray(b"10000000001")) == 126700