From 1a9b3c416868e9261ed64c17341bd39ee1ab0173 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Izzo?= Date: Tue, 28 Dec 2021 11:59:51 +0100 Subject: [PATCH] Implement M17 demodulator logic Implemented frame wrapping logic and demodulator loop. --- .../include/protocols/M17/M17Demodulator.h | 7 +- openrtx/src/protocols/M17/M17Demodulator.cpp | 102 ++++++++++++------ 2 files changed, 74 insertions(+), 35 deletions(-) diff --git a/openrtx/include/protocols/M17/M17Demodulator.h b/openrtx/include/protocols/M17/M17Demodulator.h index d6e75ecf..82100c95 100644 --- a/openrtx/include/protocols/M17/M17Demodulator.h +++ b/openrtx/include/protocols/M17/M17Demodulator.h @@ -110,12 +110,15 @@ private: static constexpr float conv_stats_alpha = 0.0001f; static constexpr float conv_threshold_factor = 3.70; static constexpr float qnt_maxmin_alpha = 0.999f; + static constexpr size_t M17_BRIDGE_SIZE = M17_SAMPLES_PER_SYMBOL * + M17_SYNCWORD_SYMBOLS; /** * M17 syncwords; */ int8_t lsf_syncword[M17_SYNCWORD_SYMBOLS] = { +3, +3, +3, +3, -3, -3, +3, -3 }; int8_t stream_syncword[M17_SYNCWORD_SYMBOLS] = { -3, -3, -3, -3, +3, +3, -3, +3 }; + uint8_t stream_syncword_bytes[2] = {0xff, 0x5d}; using dataBuffer_t = std::array< int16_t, M17_FRAME_SAMPLES_24K >; using dataFrame_t = std::array< uint8_t, M17_FRAME_BYTES >; @@ -127,11 +130,13 @@ private: int16_t *baseband_buffer; ///< Buffer for baseband audio handling. dataBlock_t baseband; ///< Half buffer, free to be processed. uint16_t *rawFrame; ///< Analog values to be quantized. - uint16_t rawFrameIndex; ///< Index for filling the raw frame. + uint16_t frameIndex; ///< Index for filling the raw frame. dataFrame_t *activeFrame; ///< Half frame, in demodulation. dataFrame_t *idleFrame; ///< Half frame, free to be processed. bool isLSF; ///< Indicates that we demodualated an LSF. bool locked; ///< A syncword was detected. + int16_t basebandBridge[M17_BRIDGE_SIZE] = { 0 }; ///< Bridge buffer + uint16_t phase; ///< Phase of the signal w.r.t. sampling /* * State variables diff --git a/openrtx/src/protocols/M17/M17Demodulator.cpp b/openrtx/src/protocols/M17/M17Demodulator.cpp index d494b6e6..ebac85b5 100644 --- a/openrtx/src/protocols/M17/M17Demodulator.cpp +++ b/openrtx/src/protocols/M17/M17Demodulator.cpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace M17 { @@ -50,6 +51,8 @@ void M17Demodulator::init() activeFrame = new dataFrame_t; rawFrame = new uint16_t[M17_FRAME_SYMBOLS]; idleFrame = new dataFrame_t; + frameIndex = 0; + phase = 0; } void M17Demodulator::terminate() @@ -144,8 +147,14 @@ int32_t M17Demodulator::convolution(size_t offset, int32_t conv = 0; for(uint32_t i = 0; i < target_size; i++) { - conv += (int32_t) target[i] * - (int32_t) this->baseband.data[offset + i * M17_SAMPLES_PER_SYMBOL]; + int16_t sample_index = offset + i * M17_SAMPLES_PER_SYMBOL; + int16_t sample = 0; + // When we are at negative indices use bridge buffer + if (sample_index < 0) + sample = basebandBridge[M17_BRIDGE_SIZE + sample_index]; + else + sample = baseband.data[sample_index]; + conv += (int32_t) target[i] * (int32_t) sample; } return conv; } @@ -178,11 +187,16 @@ sync_t M17Demodulator::nextFrameSync(uint32_t offset) int8_t M17Demodulator::quantize(int32_t offset) { - if (baseband.data[offset] > getQuantizationMax() * 2 / 3) + int16_t sample = 0; + if (offset < 0) // When we are at negative offsets use bridge buffer + sample = basebandBridge[M17_BRIDGE_SIZE + offset]; + else // Otherwise use regular data buffer + sample = baseband.data[offset]; + if (sample > getQuantizationMax() * 2 / 3) return +3; - else if (baseband.data[offset] < getQuantizationMin() * 2 / 3) + else if (sample < getQuantizationMin() * 2 / 3) return -3; - else if (baseband.data[offset] > 0) + else if (sample > 0) return +1; else return -1; @@ -200,6 +214,9 @@ bool M17Demodulator::isFrameLSF() void M17Demodulator::update() { + M17::sync_t syncword = { -1, false }; + int16_t offset = -(int16_t) M17_BRIDGE_SIZE; + uint16_t decoded_syms = 0; // Read samples from the ADC baseband = inputStream_getData(basebandId); @@ -211,41 +228,58 @@ void M17Demodulator::update() float elem = static_cast< float >(baseband.data[i]); baseband.data[i] = static_cast< int16_t >(M17::rrc(elem)); } - - // If we locked a syncword just demodulate samples - if (locked) + // If we are not locked search for a syncword + if (!locked) { - - } - else // Otherwise find next syncword - { - M17::sync_t syncword = { -1, false }; - uint16_t offset = 0; - syncword = nextFrameSync(offset); - if (syncword.index != -1) + syncword = nextFrameSync(-M17_SYNCWORD_SYMBOLS * M17_SAMPLES_PER_SYMBOL); + if (syncword.index != -1) // Lock was just acquired { locked = true; - // Set a flag to mark LSF isLSF = syncword.lsf; - // Next syncword does not overlap with current syncword - offset = syncword.index + M17_SAMPLES_PER_SYMBOL; - // Slice the input buffer to extract a frame and quantize - for(uint16_t i = 0; i < M17_FRAME_SYMBOLS; i++) - { - // Quantize - uint16_t symbol_index = syncword.index + 2 + - M17_SAMPLES_PER_SYMBOL * i; - updateQuantizationStats(baseband.data[symbol_index]); - int8_t symbol = quantize(symbol_index); - setSymbol(*activeFrame, i, symbol); - // If the frame buffer is full switch active and idle frame - if (rawFrameIndex == M17_FRAME_SYMBOLS) - std::swap(activeFrame, idleFrame); - } - // If we have some samples left, try to decode the syncword - // If decoding fails, signal lock is lost + offset = syncword.index + 2; + // DEBUG: prints + if (isLSF) + printf("Found LSF!\n"); + else + printf("Found SYNC!\n"); } } + else // Lock was inherited from previous frame + offset = phase; + // While we are locked, demodulate available samples + while (locked && offset < (int16_t) M17_FRAME_SAMPLES_24K) + { + // Slice the input buffer to extract a frame and quantize + uint16_t symbol_index = offset + M17_SAMPLES_PER_SYMBOL * decoded_syms; + updateQuantizationStats(baseband.data[symbol_index]); + int8_t symbol = quantize(symbol_index); + setSymbol(*activeFrame, frameIndex, symbol); + frameIndex++; + // If the frame buffer is full switch active and idle frame + if (frameIndex == M17_FRAME_SYMBOLS) + { + std::swap(activeFrame, idleFrame); + frameIndex = 0; + // DEBUG: print idleFrame bytes + for(size_t i = 0; i < idleFrame->size(); i+=2) + { + if (i % 16 == 14) + printf("\n"); + printf(" %02X%02X", (*idleFrame)[i], (*idleFrame)[i+1]); + } + } + if (frameIndex == 1 && + ((*activeFrame)[0] != stream_syncword_bytes[0] || + (*activeFrame)[1] != stream_syncword_bytes[1])) // Lock is lost + locked = false; + } + // Compute phase of next buffer + phase = offset % M17_SAMPLES_PER_SYMBOL + + (M17_INPUT_BUF_SIZE % M17_SAMPLES_PER_SYMBOL); + // copy N samples to bridge buffer + memcpy(basebandBridge, + baseband.data + M17_FRAME_SAMPLES_24K - M17_BRIDGE_SIZE, + sizeof(int16_t) * M17_BRIDGE_SIZE); } }