From c3839d861c3364c4f38d78ee129d9a3772b5b9c2 Mon Sep 17 00:00:00 2001 From: Xavier Olive Date: Wed, 28 Dec 2022 00:07:13 +0100 Subject: [PATCH] minimal attempt for 2.4M demodulation --- .gitignore | 6 + build.py | 20 +-- pyModeS/extra/demod2400/__init__.py | 3 + pyModeS/extra/demod2400/core.pyx | 49 +++++ pyModeS/extra/demod2400/demod2400.c | 256 +++++++++++++++++++++++++++ pyModeS/extra/demod2400/demod2400.h | 11 ++ pyModeS/extra/demod2400/rtlreader.py | 133 ++++++++++++++ pyModeS/streamer/modeslive.py | 6 +- pyModeS/streamer/source.py | 45 +++++ 9 files changed, 516 insertions(+), 13 deletions(-) create mode 100644 pyModeS/extra/demod2400/__init__.py create mode 100644 pyModeS/extra/demod2400/core.pyx create mode 100644 pyModeS/extra/demod2400/demod2400.c create mode 100644 pyModeS/extra/demod2400/demod2400.h create mode 100644 pyModeS/extra/demod2400/rtlreader.py diff --git a/.gitignore b/.gitignore index 453dfd0..2b969dd 100644 --- a/.gitignore +++ b/.gitignore @@ -5,6 +5,11 @@ __pycache__/ *.py[cod] .pytest_cache/ +# Cython +pyModeS/decoder/flarm/decode.c +pyModeS/extra/demod2400/core.c +pyModeS/c_common.c + # C extensions *.so @@ -64,3 +69,4 @@ target/ .venv env/ venv/ + diff --git a/build.py b/build.py index 5608858..9f72dc9 100644 --- a/build.py +++ b/build.py @@ -37,16 +37,16 @@ def build() -> None: extra_compile_args=compile_args, include_dirs=["pyModeS/decoder/flarm"], ), - # Extension( - # "pyModeS.extra.demod2400.core", - # [ - # "pyModeS/extra/demod2400/core.pyx", - # "pyModeS/extra/demod2400/demod2400.c", - # ], - # extra_compile_args=compile_args, - # include_dirs=["pyModeS/extra/demod2400"], - # libraries=["m"], - # ), + Extension( + "pyModeS.extra.demod2400.core", + [ + "pyModeS/extra/demod2400/core.pyx", + "pyModeS/extra/demod2400/demod2400.c", + ], + extra_compile_args=compile_args, + include_dirs=["pyModeS/extra/demod2400"], + libraries=["m"], + ), ] ext_modules = cythonize( diff --git a/pyModeS/extra/demod2400/__init__.py b/pyModeS/extra/demod2400/__init__.py new file mode 100644 index 0000000..4d58490 --- /dev/null +++ b/pyModeS/extra/demod2400/__init__.py @@ -0,0 +1,3 @@ +from .core import demod2400 + +__all__ = ["demod2400"] diff --git a/pyModeS/extra/demod2400/core.pyx b/pyModeS/extra/demod2400/core.pyx new file mode 100644 index 0000000..1eb02cf --- /dev/null +++ b/pyModeS/extra/demod2400/core.pyx @@ -0,0 +1,49 @@ +from libc.stdint cimport uint16_t, uint8_t +import numpy as np + +from ...c_common cimport crc, df + +cdef extern from "demod2400.h": + int demodulate2400(uint16_t *data, uint8_t *msg, int len_data, int* len_msg) + + +def demod2400(uint16_t[:] data, float timestamp): + cdef uint8_t[:] msg_bin + cdef int i = 0, j, length, crc_msg = 1 + cdef long size = data.shape[0] + + msg_bin = np.zeros(14, dtype=np.uint8) + + while i < size: + j = demodulate2400(&data[i], &msg_bin[0], size-i, &length) + if j == 0: + yield dict( + # 1 sample data = 2 IQ samples (hence 2*) + timestamp=timestamp + 2.*i/2400000., + payload=None, + crc=None, + index=i, + ) + return + i += j + msg_clip = np.asarray(msg_bin)[:length] + msg = "".join(f"{elt:02X}" for elt in msg_clip) + crc_msg = crc(msg) + # if df(msg) != 17 or crc_msg == 0: + if crc_msg == 0: + yield dict( + # 1 sample data = 2 IQ samples (hence 2*) + timestamp=timestamp + 2.*i/2400000., + payload=msg, + crc=crc_msg, + index=i, + ) + + yield dict( + # 1 sample data = 2 IQ samples (hence 2*) + timestamp=timestamp + 2.*i/2400000., + payload=None, + crc=None, + index=i, + ) + return \ No newline at end of file diff --git a/pyModeS/extra/demod2400/demod2400.c b/pyModeS/extra/demod2400/demod2400.c new file mode 100644 index 0000000..9fdd3e3 --- /dev/null +++ b/pyModeS/extra/demod2400/demod2400.c @@ -0,0 +1,256 @@ +#include "demod2400.h" + +static inline int slice_phase0(uint16_t *m) +{ + return 5 * m[0] - 3 * m[1] - 2 * m[2]; +} +static inline int slice_phase1(uint16_t *m) +{ + return 4 * m[0] - m[1] - 3 * m[2]; +} +static inline int slice_phase2(uint16_t *m) +{ + return 3 * m[0] + m[1] - 4 * m[2]; +} +static inline int slice_phase3(uint16_t *m) +{ + return 2 * m[0] + 3 * m[1] - 5 * m[2]; +} +static inline int slice_phase4(uint16_t *m) +{ + return m[0] + 5 * m[1] - 5 * m[2] - m[3]; +} + +int demodulate2400(uint16_t *mag, uint8_t *msg, int len_mag, int *len_msg) +{ + + uint32_t j; + for (j = 0; j < len_mag / 2 - 300; j++) + { // SALE + + uint16_t *preamble = &mag[j]; + int high; + uint32_t base_signal, base_noise; + + // quick check: we must have a rising edge 0->1 and a falling edge 12->13 + if (!(preamble[0] < preamble[1] && preamble[12] > preamble[13])) + continue; + + if (preamble[1] > preamble[2] && // 1 + preamble[2] < preamble[3] && preamble[3] > preamble[4] && // 3 + preamble[8] < preamble[9] && preamble[9] > preamble[10] && // 9 + preamble[10] < preamble[11]) + { // 11-12 + // peaks at 1,3,9,11-12: phase 3 + high = (preamble[1] + preamble[3] + preamble[9] + preamble[11] + preamble[12]) / 4; + base_signal = preamble[1] + preamble[3] + preamble[9]; + base_noise = preamble[5] + preamble[6] + preamble[7]; + } + else if (preamble[1] > preamble[2] && // 1 + preamble[2] < preamble[3] && preamble[3] > preamble[4] && // 3 + preamble[8] < preamble[9] && preamble[9] > preamble[10] && // 9 + preamble[11] < preamble[12]) + { // 12 + // peaks at 1,3,9,12: phase 4 + high = (preamble[1] + preamble[3] + preamble[9] + preamble[12]) / 4; + base_signal = preamble[1] + preamble[3] + preamble[9] + preamble[12]; + base_noise = preamble[5] + preamble[6] + preamble[7] + preamble[8]; + } + else if (preamble[1] > preamble[2] && // 1 + preamble[2] < preamble[3] && preamble[4] > preamble[5] && // 3-4 + preamble[8] < preamble[9] && preamble[10] > preamble[11] && // 9-10 + preamble[11] < preamble[12]) + { // 12 + // peaks at 1,3-4,9-10,12: phase 5 + high = (preamble[1] + preamble[3] + preamble[4] + preamble[9] + preamble[10] + preamble[12]) / 4; + base_signal = preamble[1] + preamble[12]; + base_noise = preamble[6] + preamble[7]; + } + else if (preamble[1] > preamble[2] && // 1 + preamble[3] < preamble[4] && preamble[4] > preamble[5] && // 4 + preamble[9] < preamble[10] && preamble[10] > preamble[11] && // 10 + preamble[11] < preamble[12]) + { // 12 + // peaks at 1,4,10,12: phase 6 + high = (preamble[1] + preamble[4] + preamble[10] + preamble[12]) / 4; + base_signal = preamble[1] + preamble[4] + preamble[10] + preamble[12]; + base_noise = preamble[5] + preamble[6] + preamble[7] + preamble[8]; + } + else if (preamble[2] > preamble[3] && // 1-2 + preamble[3] < preamble[4] && preamble[4] > preamble[5] && // 4 + preamble[9] < preamble[10] && preamble[10] > preamble[11] && // 10 + preamble[11] < preamble[12]) + { // 12 + // peaks at 1-2,4,10,12: phase 7 + high = (preamble[1] + preamble[2] + preamble[4] + preamble[10] + preamble[12]) / 4; + base_signal = preamble[4] + preamble[10] + preamble[12]; + base_noise = preamble[6] + preamble[7] + preamble[8]; + } + else + { + // no suitable peaks + continue; + } + + // Check for enough signal + if (base_signal * 2 < 3 * base_noise) // about 3.5dB SNR + continue; + + // Check that the "quiet" bits 6,7,15,16,17 are actually quiet + if (preamble[5] >= high || + preamble[6] >= high || + preamble[7] >= high || + preamble[8] >= high || + preamble[14] >= high || + preamble[15] >= high || + preamble[16] >= high || + preamble[17] >= high || + preamble[18] >= high) + { + continue; + } + + // // try all phases + // Modes.stats_current.demod_preambles++; + // bestmsg = NULL; bestscore = -2; bestphase = -1; + for (int try_phase = 4; try_phase <= 8; ++try_phase) + { + uint16_t *pPtr; + int phase, i, bytelen; + + // Decode all the next 112 bits, regardless of the actual message + // size. We'll check the actual message type later + + pPtr = &mag[j + 19] + (try_phase / 5); + phase = try_phase % 5; + + bytelen = MODES_LONG_MSG_BYTES; + for (i = 0; i < bytelen; ++i) + { + uint8_t theByte = 0; + + switch (phase) + { + case 0: + theByte = + (slice_phase0(pPtr) > 0 ? 0x80 : 0) | + (slice_phase2(pPtr + 2) > 0 ? 0x40 : 0) | + (slice_phase4(pPtr + 4) > 0 ? 0x20 : 0) | + (slice_phase1(pPtr + 7) > 0 ? 0x10 : 0) | + (slice_phase3(pPtr + 9) > 0 ? 0x08 : 0) | + (slice_phase0(pPtr + 12) > 0 ? 0x04 : 0) | + (slice_phase2(pPtr + 14) > 0 ? 0x02 : 0) | + (slice_phase4(pPtr + 16) > 0 ? 0x01 : 0); + + phase = 1; + pPtr += 19; + break; + + case 1: + theByte = + (slice_phase1(pPtr) > 0 ? 0x80 : 0) | + (slice_phase3(pPtr + 2) > 0 ? 0x40 : 0) | + (slice_phase0(pPtr + 5) > 0 ? 0x20 : 0) | + (slice_phase2(pPtr + 7) > 0 ? 0x10 : 0) | + (slice_phase4(pPtr + 9) > 0 ? 0x08 : 0) | + (slice_phase1(pPtr + 12) > 0 ? 0x04 : 0) | + (slice_phase3(pPtr + 14) > 0 ? 0x02 : 0) | + (slice_phase0(pPtr + 17) > 0 ? 0x01 : 0); + + phase = 2; + pPtr += 19; + break; + + case 2: + theByte = + (slice_phase2(pPtr) > 0 ? 0x80 : 0) | + (slice_phase4(pPtr + 2) > 0 ? 0x40 : 0) | + (slice_phase1(pPtr + 5) > 0 ? 0x20 : 0) | + (slice_phase3(pPtr + 7) > 0 ? 0x10 : 0) | + (slice_phase0(pPtr + 10) > 0 ? 0x08 : 0) | + (slice_phase2(pPtr + 12) > 0 ? 0x04 : 0) | + (slice_phase4(pPtr + 14) > 0 ? 0x02 : 0) | + (slice_phase1(pPtr + 17) > 0 ? 0x01 : 0); + + phase = 3; + pPtr += 19; + break; + + case 3: + theByte = + (slice_phase3(pPtr) > 0 ? 0x80 : 0) | + (slice_phase0(pPtr + 3) > 0 ? 0x40 : 0) | + (slice_phase2(pPtr + 5) > 0 ? 0x20 : 0) | + (slice_phase4(pPtr + 7) > 0 ? 0x10 : 0) | + (slice_phase1(pPtr + 10) > 0 ? 0x08 : 0) | + (slice_phase3(pPtr + 12) > 0 ? 0x04 : 0) | + (slice_phase0(pPtr + 15) > 0 ? 0x02 : 0) | + (slice_phase2(pPtr + 17) > 0 ? 0x01 : 0); + + phase = 4; + pPtr += 19; + break; + + case 4: + theByte = + (slice_phase4(pPtr) > 0 ? 0x80 : 0) | + (slice_phase1(pPtr + 3) > 0 ? 0x40 : 0) | + (slice_phase3(pPtr + 5) > 0 ? 0x20 : 0) | + (slice_phase0(pPtr + 8) > 0 ? 0x10 : 0) | + (slice_phase2(pPtr + 10) > 0 ? 0x08 : 0) | + (slice_phase4(pPtr + 12) > 0 ? 0x04 : 0) | + (slice_phase1(pPtr + 15) > 0 ? 0x02 : 0) | + (slice_phase3(pPtr + 17) > 0 ? 0x01 : 0); + + phase = 0; + pPtr += 20; + break; + } + + msg[i] = theByte; + if (i == 0) + { + switch (msg[0] >> 3) + { + case 0: + case 4: + case 5: + case 11: + bytelen = MODES_SHORT_MSG_BYTES; + *len_msg = MODES_SHORT_MSG_BYTES; + break; + + case 16: + case 17: + case 18: + case 20: + case 21: + case 24: + *len_msg = MODES_LONG_MSG_BYTES; + break; + + default: + bytelen = 1; // unknown DF, give up immediately + break; + } + } + } + + return j + 1; + } + + // Score the mode S message and see if it's any good. + // score = scoreModesMessage(msg, i*8); + // if (score > bestscore) { + // // new high score! + // bestmsg = msg; + // bestscore = score; + // bestphase = try_phase; + // // swap to using the other buffer so we don't clobber our demodulated data + // // (if we find a better result then we'll swap back, but that's OK because + // // we no longer need this copy if we found a better one) + // msg = (msg == msg1) ? msg2 : msg1; + // } + } + return 0; +} \ No newline at end of file diff --git a/pyModeS/extra/demod2400/demod2400.h b/pyModeS/extra/demod2400/demod2400.h new file mode 100644 index 0000000..00bd42f --- /dev/null +++ b/pyModeS/extra/demod2400/demod2400.h @@ -0,0 +1,11 @@ +#ifndef __DEMOD_2400_H__ +#define __DEMOD_2400_H__ + +#define MODES_LONG_MSG_BYTES 14 +#define MODES_SHORT_MSG_BYTES 7 + +#include + +int demodulate2400(uint16_t *mag, uint8_t *msg, int len_mag, int* len_msg); + +#endif \ No newline at end of file diff --git a/pyModeS/extra/demod2400/rtlreader.py b/pyModeS/extra/demod2400/rtlreader.py new file mode 100644 index 0000000..a20e8b8 --- /dev/null +++ b/pyModeS/extra/demod2400/rtlreader.py @@ -0,0 +1,133 @@ +import time +import traceback +import numpy as np +import pyModeS as pms +from pyModeS.extra.demod2400 import demod2400 + + +try: + import rtlsdr # type: ignore +except ImportError: + print( + "------------------------------------------------------------------------" + ) + print( + "! Warning: pyrtlsdr not installed (required for using RTL-SDR devices) !" + ) + print( + "------------------------------------------------------------------------" + ) + +modes_frequency = 1090e6 +sampling_rate = 2.4e6 +buffer_size = 16 * 16384 +read_size = buffer_size / 2 + + +class RtlReader(object): + def __init__(self, **kwargs): + super(RtlReader, self).__init__() + self.signal_buffer = [] # amplitude of the sample only + self.sdr = rtlsdr.RtlSdr() + self.sdr.sample_rate = sampling_rate + self.sdr.center_freq = modes_frequency + self.sdr.gain = "auto" + + self.debug = kwargs.get("debug", False) + self.raw_pipe_in = None + self.stop_flag = False + self.exception_queue = None + + def _process_buffer(self): + """process raw IQ data in the buffer""" + + # Mode S messages + messages = [] + + data = (np.array(self.signal_buffer) * 65535).astype(np.uint16) + + for s in demod2400(data, self.timestamp): + if s["payload"] is None: + idx = s["index"] + # reset the buffer + self.signal_buffer = self.signal_buffer[idx:] + self.timestamp = s["timestamp"] + break + if self._check_msg(s["payload"]): + messages.append([s["payload"], time.time()]) # s["timestamp"]]) + if self.debug: + self._debug_msg(s["payload"]) + + self.timestamp = s["timestamp"] + return messages + + def _check_msg(self, msg): + df = pms.df(msg) + msglen = len(msg) + if df == 17 and msglen == 28: + if pms.crc(msg) == 0: + return True + elif df in [20, 21] and msglen == 28: + return True + elif df in [4, 5, 11] and msglen == 14: + return True + + def _debug_msg(self, msg): + df = pms.df(msg) + msglen = len(msg) + if df == 17 and msglen == 28: + print(msg, pms.icao(msg), df, pms.crc(msg)) + print(pms.tell(msg)) + elif df in [20, 21] and msglen == 28: + print(msg, pms.icao(msg), df) + elif df in [4, 5, 11] and msglen == 14: + print(msg, pms.icao(msg), df) + else: + # print("[*]", msg) + pass + + def _read_callback(self, data, rtlsdr_obj): + amp = np.absolute(data) + self.signal_buffer.extend(amp.tolist()) + + if len(self.signal_buffer) >= buffer_size: + messages = self._process_buffer() + self.handle_messages(messages) + + def handle_messages(self, messages): + """re-implement this method to handle the messages""" + for msg, t in messages: + # print("%15.9f %s" % (t, msg)) + pass + + def stop(self, *args, **kwargs): + self.sdr.close() + + def run(self, raw_pipe_in=None, stop_flag=None, exception_queue=None): + self.raw_pipe_in = raw_pipe_in + self.exception_queue = exception_queue + self.stop_flag = stop_flag + + try: + # raise RuntimeError("test exception") + self.timestamp = time.time() + + while True: + data = self.sdr.read_samples(read_size) + self._read_callback(data, None) + + except Exception as e: + tb = traceback.format_exc() + if self.exception_queue is not None: + self.exception_queue.put(tb) + raise e + + +if __name__ == "__main__": + import signal + + rtl = RtlReader() + signal.signal(signal.SIGINT, rtl.stop) + + rtl.debug = True + rtl.run() diff --git a/pyModeS/streamer/modeslive.py b/pyModeS/streamer/modeslive.py index 77be514..574ad6b 100755 --- a/pyModeS/streamer/modeslive.py +++ b/pyModeS/streamer/modeslive.py @@ -9,7 +9,7 @@ import signal import multiprocessing from pyModeS.streamer.decode import Decode from pyModeS.streamer.screen import Screen -from pyModeS.streamer.source import NetSource, RtlSdrSource # , RtlSdrSource24 +from pyModeS.streamer.source import NetSource, RtlSdrSource, RtlSdrSource24 def main(): @@ -100,8 +100,8 @@ def main(): source = NetSource(host=SERVER, port=PORT, rawtype=DATATYPE) elif SOURCE == "rtlsdr": source = RtlSdrSource() - # elif SOURCE == "rtlsdr24": - # source = RtlSdrSource24() + elif SOURCE == "rtlsdr24": + source = RtlSdrSource24() recv_process = multiprocessing.Process( target=source.run, args=(raw_pipe_in, stop_flag, exception_queue) diff --git a/pyModeS/streamer/source.py b/pyModeS/streamer/source.py index c8273e1..53f8704 100644 --- a/pyModeS/streamer/source.py +++ b/pyModeS/streamer/source.py @@ -1,6 +1,7 @@ import pyModeS as pms from pyModeS.extra.tcpclient import TcpClient from pyModeS.extra.rtlreader import RtlReader +from pyModeS.extra.demod2400.rtlreader import RtlReader as RtlReader24 class NetSource(TcpClient): @@ -89,3 +90,47 @@ class RtlSdrSource(RtlReader): } ) self.reset_local_buffer() + + +class RtlSdrSource24(RtlReader24): + def __init__(self): + super(RtlSdrSource24, self).__init__() + self.reset_local_buffer() + + def reset_local_buffer(self): + self.local_buffer_adsb_msg = [] + self.local_buffer_adsb_ts = [] + self.local_buffer_commb_msg = [] + self.local_buffer_commb_ts = [] + + def handle_messages(self, messages): + + if self.stop_flag.value is True: + self.stop() + return + + for msg, t in messages: + if len(msg) < 28: # only process long messages + continue + + df = pms.df(msg) + + if df == 17 or df == 18: + self.local_buffer_adsb_msg.append(msg) + self.local_buffer_adsb_ts.append(t) + elif df == 20 or df == 21: + self.local_buffer_commb_msg.append(msg) + self.local_buffer_commb_ts.append(t) + else: + continue + + if len(self.local_buffer_adsb_msg) > 1: + self.raw_pipe_in.send( + { + "adsb_ts": self.local_buffer_adsb_ts, + "adsb_msg": self.local_buffer_adsb_msg, + "commb_ts": self.local_buffer_commb_ts, + "commb_msg": self.local_buffer_commb_msg, + } + ) + self.reset_local_buffer()