FLARM decoder

This commit is contained in:
Xavier Olive 2022-12-04 00:04:55 +01:00
parent 0667c1b3c3
commit 87e6f50486
6 changed files with 244 additions and 4 deletions

View File

@ -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

View File

@ -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

View File

@ -0,0 +1,4 @@
cdef extern from "core.h":
void make_key(int*, long time, long address)
void btea(int*, int, int*)

View File

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

View File

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

View File

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