fleg
/
OpenRTX
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Implement M17 demodulator logic 

Implemented frame wrapping logic and demodulator loop.
This commit is contained in:
Niccolò Izzo 2021-12-28 11:59:51 +01:00 committed by Silvano Seva
parent 963fbdc141
commit 1a9b3c4168
2 changed files with 74 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
openrtx/include/protocols/M17/M17Demodulator.h
View File

@ -110,12 +110,15 @@ private:
static constexpr float conv_stats_alpha = 0.0001f; static constexpr float conv_stats_alpha = 0.0001f;
static constexpr float conv_threshold_factor = 3.70; static constexpr float conv_threshold_factor = 3.70;
static constexpr float qnt_maxmin_alpha = 0.999f; static constexpr float qnt_maxmin_alpha = 0.999f;
static constexpr size_t M17_BRIDGE_SIZE = M17_SAMPLES_PER_SYMBOL *
M17_SYNCWORD_SYMBOLS;
/** /**
* M17 syncwords; * M17 syncwords;
*/ */
int8_t lsf_syncword[M17_SYNCWORD_SYMBOLS] = { +3, +3, +3, +3, -3, -3, +3, -3 }; 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 }; 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 dataBuffer_t = std::array< int16_t, M17_FRAME_SAMPLES_24K >;
using dataFrame_t = std::array< uint8_t, M17_FRAME_BYTES >; using dataFrame_t = std::array< uint8_t, M17_FRAME_BYTES >;
@ -127,11 +130,13 @@ private:
int16_t *baseband_buffer; ///< Buffer for baseband audio handling. int16_t *baseband_buffer; ///< Buffer for baseband audio handling.
dataBlock_t baseband; ///< Half buffer, free to be processed. dataBlock_t baseband; ///< Half buffer, free to be processed.
uint16_t *rawFrame; ///< Analog values to be quantized. 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 *activeFrame; ///< Half frame, in demodulation.
dataFrame_t *idleFrame; ///< Half frame, free to be processed. dataFrame_t *idleFrame; ///< Half frame, free to be processed.
bool isLSF; ///< Indicates that we demodualated an LSF. bool isLSF; ///< Indicates that we demodualated an LSF.
bool locked; ///< A syncword was detected. 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 * State variables

92
openrtx/src/protocols/M17/M17Demodulator.cpp
View File

@ -23,6 +23,7 @@
#include <M17/M17Utils.h> #include <M17/M17Utils.h>
#include <interfaces/audio_stream.h> #include <interfaces/audio_stream.h>
#include <math.h> #include <math.h>
#include <cstring>
namespace M17 namespace M17
{ {
@ -50,6 +51,8 @@ void M17Demodulator::init()
activeFrame = new dataFrame_t; activeFrame = new dataFrame_t;
rawFrame = new uint16_t[M17_FRAME_SYMBOLS]; rawFrame = new uint16_t[M17_FRAME_SYMBOLS];
idleFrame = new dataFrame_t; idleFrame = new dataFrame_t;
frameIndex = 0;
phase = 0;
} }
void M17Demodulator::terminate() void M17Demodulator::terminate()
@ -144,8 +147,14 @@ int32_t M17Demodulator::convolution(size_t offset,
int32_t conv = 0; int32_t conv = 0;
for(uint32_t i = 0; i < target_size; i++) for(uint32_t i = 0; i < target_size; i++)
{ {
conv += (int32_t) target[i] * int16_t sample_index = offset + i * M17_SAMPLES_PER_SYMBOL;
(int32_t) this->baseband.data[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; return conv;
} }
@ -178,11 +187,16 @@ sync_t M17Demodulator::nextFrameSync(uint32_t offset)
int8_t M17Demodulator::quantize(int32_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; return +3;
else if (baseband.data[offset] < getQuantizationMin() * 2 / 3) else if (sample < getQuantizationMin() * 2 / 3)
return -3; return -3;
else if (baseband.data[offset] > 0) else if (sample > 0)
return +1; return +1;
else else
return -1; return -1;
@ -200,6 +214,9 @@ bool M17Demodulator::isFrameLSF()
void M17Demodulator::update() 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 // Read samples from the ADC
baseband = inputStream_getData(basebandId); baseband = inputStream_getData(basebandId);
@ -211,41 +228,58 @@ void M17Demodulator::update()
float elem = static_cast< float >(baseband.data[i]); float elem = static_cast< float >(baseband.data[i]);
baseband.data[i] = static_cast< int16_t >(M17::rrc(elem)); baseband.data[i] = static_cast< int16_t >(M17::rrc(elem));
} }
// If we are not locked search for a syncword
// If we locked a syncword just demodulate samples if (!locked)
if (locked)
{ {
syncword = nextFrameSync(-M17_SYNCWORD_SYMBOLS * M17_SAMPLES_PER_SYMBOL);
} if (syncword.index != -1) // Lock was just acquired
else // Otherwise find next syncword
{
M17::sync_t syncword = { -1, false };
uint16_t offset = 0;
syncword = nextFrameSync(offset);
if (syncword.index != -1)
{ {
locked = true; locked = true;
// Set a flag to mark LSF
isLSF = syncword.lsf; isLSF = syncword.lsf;
// Next syncword does not overlap with current syncword offset = syncword.index + 2;
offset = syncword.index + M17_SAMPLES_PER_SYMBOL; // DEBUG: prints
// Slice the input buffer to extract a frame and quantize if (isLSF)
for(uint16_t i = 0; i < M17_FRAME_SYMBOLS; i++) 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)
{ {
// Quantize // Slice the input buffer to extract a frame and quantize
uint16_t symbol_index = syncword.index + 2 + uint16_t symbol_index = offset + M17_SAMPLES_PER_SYMBOL * decoded_syms;
M17_SAMPLES_PER_SYMBOL * i;
updateQuantizationStats(baseband.data[symbol_index]); updateQuantizationStats(baseband.data[symbol_index]);
int8_t symbol = quantize(symbol_index); int8_t symbol = quantize(symbol_index);
setSymbol<M17_FRAME_BYTES>(*activeFrame, i, symbol); setSymbol<M17_FRAME_BYTES>(*activeFrame, frameIndex, symbol);
frameIndex++;
// If the frame buffer is full switch active and idle frame // If the frame buffer is full switch active and idle frame
if (rawFrameIndex == M17_FRAME_SYMBOLS) if (frameIndex == M17_FRAME_SYMBOLS)
{
std::swap(activeFrame, idleFrame); std::swap(activeFrame, idleFrame);
} frameIndex = 0;
// If we have some samples left, try to decode the syncword // DEBUG: print idleFrame bytes
// If decoding fails, signal lock is lost 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);
} }
} }