Merge pull request #134 from junzis/pr_flarm

FLARM decoder

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

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);
+  }
+}

pyModeS/decoder/flarm/core.h

@@ -0,0 +1,13 @@
+#ifndef __CORE_H__
+#define __CORE_H__
+
+#include <stdint.h>
+
+#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

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*)

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: ...

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
+    )

setup.py

@@ -11,6 +11,8 @@ Steps for deploying a new version:
 4. twine upload dist/*
 """
 
+import sys
+
 # Always prefer setuptools over distutils
 from setuptools import setup, find_packages
 
@@ -46,17 +48,54 @@ details = dict(
     # 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"]
 
-    setup(**dict(details, ext_modules=cythonize(extensions)))
+    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,
+                include_path=include_dirs,
+                compiler_directives={"binding": True, "language_level": 3},
+            ),
+        )
+    )
 
 except ImportError:
     setup(**details)