commit
5a703a0d3c
25
pyModeS/decoder/flarm/__init__.py
Normal file
25
pyModeS/decoder/flarm/__init__.py
Normal 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
|
124
pyModeS/decoder/flarm/core.c
Normal file
124
pyModeS/decoder/flarm/core.c
Normal file
@ -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);
|
||||
}
|
||||
}
|
13
pyModeS/decoder/flarm/core.h
Normal file
13
pyModeS/decoder/flarm/core.h
Normal 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
|
4
pyModeS/decoder/flarm/core.pxd
Normal file
4
pyModeS/decoder/flarm/core.pxd
Normal file
@ -0,0 +1,4 @@
|
||||
|
||||
cdef extern from "core.h":
|
||||
void make_key(int*, long time, long address)
|
||||
void btea(int*, int, int*)
|
14
pyModeS/decoder/flarm/decode.pyi
Normal file
14
pyModeS/decoder/flarm/decode.pyi
Normal 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: ...
|
145
pyModeS/decoder/flarm/decode.pyx
Normal file
145
pyModeS/decoder/flarm/decode.pyx
Normal 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
|
||||
)
|
47
setup.py
47
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)
|
||||
|
Loading…
Reference in New Issue
Block a user