diff --git a/pyModeS/decoder/flarm/__init__.py b/pyModeS/decoder/flarm/__init__.py new file mode 100644 index 0000000..513b0ed --- /dev/null +++ b/pyModeS/decoder/flarm/__init__.py @@ -0,0 +1,25 @@ +from typing import TypedDict + +from .decode import flarm as flarm_decode + +__all__ = ["DecodedMessage", "flarm"] + + +class DecodedMessage(TypedDict): + timestamp: int + icao24: str + latitude: float + longitude: float + altitude: int + vertical_speed: float + groundspeed: int + track: int + type: str + sensorLatitude: float + sensorLongitude: float + isIcao24: bool + noTrack: bool + stealth: bool + + +flarm = flarm_decode diff --git a/pyModeS/decoder/flarm/core.c b/pyModeS/decoder/flarm/core.c new file mode 100644 index 0000000..f3d9089 --- /dev/null +++ b/pyModeS/decoder/flarm/core.c @@ -0,0 +1,124 @@ +#include "core.h" + +/* + * + * https://pastebin.com/YK2f8bfm + * + * NEW ENCRYPTION + * + * Swiss glider anti-colission system moved to a new encryption scheme: XXTEA + * The algorithm encrypts all the packet after the header: total 20 bytes or 5 long int words of data + * + * XXTEA description and code are found here: http://en.wikipedia.org/wiki/XXTEA + * The system uses 6 iterations of the main loop. + * + * The system version 6 sends two type of packets: position and ... some unknown data + * The difference is made by bit 0 of byte 3 of the packet: for position data this bit is zero. + * + * For position data the key used depends on the time and transmitting device address. + * The key is as well obscured by a weird algorithm. + * The code to generate the key is: + * + * */ + +void make_key(int *key, long time, long address) +{ + const long key1[4] = {0xe43276df, 0xdca83759, 0x9802b8ac, 0x4675a56b}; + const long key1b[4] = {0xfc78ea65, 0x804b90ea, 0xb76542cd, 0x329dfa32}; + const long *table = ((((time >> 23) & 255) & 0x01) != 0) ? key1b : key1; + + for (int i = 0; i < 4; i++) + { + key[i] = obscure(table[i] ^ ((time >> 6) ^ address), 0x045D9F3B) ^ 0x87B562F4; + } +} + +long obscure(long key, unsigned long seed) +{ + unsigned int m1 = seed * (key ^ (key >> 16)); + unsigned int m2 = seed * (m1 ^ (m1 >> 16)); + return m2 ^ (m2 >> 16); +} + +/* + * NEW PACKET FORMAT: + * + * Byte Bits + * 0 AAAA AAAA device address + * 1 AAAA AAAA + * 2 AAAA AAAA + * 3 00aa 0000 aa = 10 or 01 + * + * 4 vvvv vvvv vertical speed + * 5 xxxx xxvv + * 6 gggg gggg GPS status + * 7 tttt gggg plane type + * + * 8 LLLL LLLL Latitude + * 9 LLLL LLLL + * 10 aaaa aLLL + * 11 aaaa aaaa Altitude + * + * 12 NNNN NNNN Longitude + * 13 NNNN NNNN + * 14 xxxx NNNN + * 15 FFxx xxxx multiplying factor + * + * 16 SSSS SSSS as in version 4 + * 17 ssss ssss + * 18 KKKK KKKK + * 19 kkkk kkkk + * + * 20 EEEE EEEE + * 21 eeee eeee + * 22 PPPP PPPP + * 24 pppp pppp + * */ + +/* + * https://en.wikipedia.org/wiki/XXTEA + */ + +void btea(uint32_t *v, int n, uint32_t const key[4]) +{ + uint32_t y, z, sum; + unsigned p, rounds, e; + if (n > 1) + { /* Coding Part */ + /* Unused, should remove? */ + rounds = 6 + 52 / n; + sum = 0; + z = v[n - 1]; + do + { + sum += DELTA; + e = (sum >> 2) & 3; + for (p = 0; p < (unsigned)n - 1; p++) + { + y = v[p + 1]; + z = v[p] += MX; + } + y = v[0]; + z = v[n - 1] += MX; + } while (--rounds); + } + else if (n < -1) + { /* Decoding Part */ + n = -n; + rounds = 6; // + 52 / n; + sum = rounds * DELTA; + y = v[0]; + do + { + e = (sum >> 2) & 3; + for (p = n - 1; p > 0; p--) + { + z = v[p - 1]; + y = v[p] -= MX; + } + z = v[n - 1]; + y = v[0] -= MX; + sum -= DELTA; + } while (--rounds); + } +} \ No newline at end of file diff --git a/pyModeS/decoder/flarm/core.h b/pyModeS/decoder/flarm/core.h new file mode 100644 index 0000000..a8e7b0e --- /dev/null +++ b/pyModeS/decoder/flarm/core.h @@ -0,0 +1,13 @@ +#ifndef __CORE_H__ +#define __CORE_H__ + +#include + +#define DELTA 0x9e3779b9 +#define MX (((z >> 5 ^ y << 2) + (y >> 3 ^ z << 4)) ^ ((sum ^ y) + (key[(p & 3) ^ e] ^ z))) + +void make_key(int *key, long time, long address); +long obscure(long key, unsigned long seed); +void btea(uint32_t *v, int n, uint32_t const key[4]); + +#endif diff --git a/pyModeS/decoder/flarm/core.pxd b/pyModeS/decoder/flarm/core.pxd new file mode 100644 index 0000000..e1ad1fd --- /dev/null +++ b/pyModeS/decoder/flarm/core.pxd @@ -0,0 +1,4 @@ + +cdef extern from "core.h": + void make_key(int*, long time, long address) + void btea(int*, int, int*) \ No newline at end of file diff --git a/pyModeS/decoder/flarm/decode.pyi b/pyModeS/decoder/flarm/decode.pyi new file mode 100644 index 0000000..3b9cbec --- /dev/null +++ b/pyModeS/decoder/flarm/decode.pyi @@ -0,0 +1,14 @@ +from typing import Any + +from . import DecodedMessage + +AIRCRAFT_TYPES: list[str] + + +def flarm( + timestamp: int, + msg: str, + refLat: float, + refLon: float, + **kwargs: Any, +) -> DecodedMessage: ... diff --git a/pyModeS/decoder/flarm/decode.pyx b/pyModeS/decoder/flarm/decode.pyx new file mode 100644 index 0000000..816463d --- /dev/null +++ b/pyModeS/decoder/flarm/decode.pyx @@ -0,0 +1,145 @@ +from core cimport make_key as c_make_key, btea as c_btea +from cpython cimport array + +import array +import math +from ctypes import c_byte +from textwrap import wrap + +AIRCRAFT_TYPES = [ + "Unknown", # 0 + "Glider", # 1 + "Tow-Plane", # 2 + "Helicopter", # 3 + "Parachute", # 4 + "Parachute Drop-Plane", # 5 + "Hangglider", # 6 + "Paraglider", # 7 + "Aircraft", # 8 + "Jet", # 9 + "UFO", # 10 + "Balloon", # 11 + "Airship", # 12 + "UAV", # 13 + "Reserved", # 14 + "Static Obstacle", # 15 +] + +cdef long bytearray2int(str icao24): + return ( + (int(icao24[4:6], 16) & 0xFF) + | ((int(icao24[2:4], 16) & 0xFF) << 8) + | ((int(icao24[:2], 16) & 0xFF) << 16) + ) + +cpdef array.array make_key(long timestamp, str icao24): + cdef long addr = bytearray2int(icao24) + cdef array.array a = array.array('i', [0, 0, 0, 0]) + c_make_key(a.data.as_ints, timestamp, (addr << 8) & 0xffffff) + return a + +cpdef array.array btea(long timestamp, str msg): + cdef int p + cdef str icao24 = msg[4:6] + msg[2:4] + msg[:2] + cdef array.array key = make_key(timestamp, icao24) + + pieces = wrap(msg[8:], 8) + cdef array.array toDecode = array.array('i', len(pieces) * [0]) + for i, piece in enumerate(pieces): + p = 0 + for elt in wrap(piece, 2)[::-1]: + p = (p << 8) + int(elt, 16) + toDecode[i] = p + + c_btea(toDecode.data.as_ints, -5, key.data.as_ints) + return toDecode + +cdef float velocity(int ns, int ew): + return math.hypot(ew / 4, ns / 4) + +def heading(ns, ew, velocity): + if velocity < 1e-6: + velocity = 1 + return (math.atan2(ew / velocity / 4, ns / velocity / 4) / 0.01745) % 360 + +def turningRate(a1, a2): + return ((((a2 - a1)) + 540) % 360) - 180 + +def flarm(long timestamp, str msg, float refLat, float refLon, **kwargs): + """Decode a FLARM message. + + Args: + timestamp (int) + msg (str) + refLat (float): the receiver's location + refLon (float): the receiver's location + + Returns: + a dictionary with all decoded fields. Any extra keyword argument passed + is included in the output dictionary. + """ + cdef str icao24 = msg[4:6] + msg[2:4] + msg[:2] + cdef int magic = int(msg[6:8], 16) + + if magic != 0x10 and magic != 0x20: + return None + + cdef array.array decoded = btea(timestamp, msg) + + cdef int aircraft_type = (decoded[0] >> 28) & 0xF + cdef int gps = (decoded[0] >> 16) & 0xFFF + cdef int raw_vs = c_byte(decoded[0] & 0x3FF).value + + noTrack = ((decoded[0] >> 14) & 0x1) == 1 + stealth = ((decoded[0] >> 13) & 0x1) == 1 + + cdef int altitude = (decoded[1] >> 19) & 0x1FFF + + cdef int lat = decoded[1] & 0x7FFFF + + cdef int mult_factor = 1 << ((decoded[2] >> 30) & 0x3) + cdef int lon = decoded[2] & 0xFFFFF + + ns = list( + c_byte((decoded[3] >> (i * 8)) & 0xFF).value * mult_factor + for i in range(4) + ) + ew = list( + c_byte((decoded[4] >> (i * 8)) & 0xFF).value * mult_factor + for i in range(4) + ) + + cdef int roundLat = int(refLat * 1e7) >> 7 + lat = (lat - roundLat) % 0x080000 + if lat >= 0x040000: + lat -= 0x080000 + lat = (((lat + roundLat) << 7) + 0x40) + + roundLon = int(refLon * 1e7) >> 7 + lon = (lon - roundLon) % 0x100000 + if lon >= 0x080000: + lon -= 0x100000 + lon = (((lon + roundLon) << 7) + 0x40) + + speed = sum(velocity(n, e) for n, e in zip(ns, ew)) / 4 + + heading4 = heading(ns[0], ew[0], speed) + heading8 = heading(ns[1], ew[1], speed) + + return dict( + timestamp=timestamp, + icao24=icao24, + latitude=round(lat * 1e-7, 6), + longitude=round(lon * 1e-7, 6), + geoaltitude=altitude, + vertical_speed=raw_vs * mult_factor / 10, + groundspeed=round(speed), + track=round(heading4 - 4 * turningRate(heading4, heading8) / 4), + type=AIRCRAFT_TYPES[aircraft_type], + sensorLatitude=refLat, + sensorLongitude=refLon, + isIcao24=magic==0x10, + noTrack=noTrack, + stealth=stealth, + **kwargs + ) \ No newline at end of file diff --git a/setup.py b/setup.py index 172cc0b..8c1ddc3 100644 --- a/setup.py +++ b/setup.py @@ -11,6 +11,8 @@ 4. twine upload dist/* """ +import sys + # Always prefer setuptools over distutils from setuptools import setup, find_packages @@ -46,17 +48,54 @@ # typing_extensions are no longer necessary after Python 3.8 (TypedDict) install_requires=["numpy", "pyzmq", "typing_extensions"], extras_require={"fast": ["Cython"]}, - package_data={"pyModeS": ["*.pyx", "*.pxd", "py.typed"]}, + package_data={ + "pyModeS": ["*.pyx", "*.pxd", "py.typed"], + "pyModeS.decoder.flarm": ["*.pyx", "*.pxd", "*.pyi"], + }, scripts=["pyModeS/streamer/modeslive"], ) try: - from setuptools.extension import Extension + from distutils.core import Extension from Cython.Build import cythonize - extensions = [Extension("pyModeS.c_common", ["pyModeS/c_common.pyx"])] + compile_args = [] + include_dirs = ["pyModeS/decoder/flarm"] + + if sys.platform == "linux": + compile_args += [ + "-march=native", + "-O3", + "-msse", + "-msse2", + "-mfma", + "-mfpmath=sse", + "-Wno-pointer-sign", + ] + + extensions = [ + Extension("pyModeS.c_common", ["pyModeS/c_common.pyx"]), + Extension( + "pyModeS.decoder.flarm.decode", + [ + "pyModeS/decoder/flarm/decode.pyx", + "pyModeS/decoder/flarm/core.c", + ], + extra_compile_args=compile_args, + include_dirs=include_dirs, + ), + ] - setup(**dict(details, ext_modules=cythonize(extensions))) + setup( + **dict( + details, + ext_modules=cythonize( + extensions, + include_path=include_dirs, + compiler_directives={"binding": True, "language_level": 3}, + ), + ) + ) except ImportError: setup(**details)