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

M17: deeply restructured the demodulator code 

Restructured the M17Demodulator class and rewritten the
demodulator logic to have a more solid lock on the
baseband stream. This fixes the long standing bug of the
demodulator causing random losses of lock even when
receiving a solid baseband stream.
This commit is contained in:
Silvano Seva 2024-01-27 14:03:02 +01:00
parent ddf889177c
commit 598f1c4523
3 changed files with 232 additions and 478 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

204
openrtx/include/protocols/M17/M17Demodulator.hpp
View File

@ -1,5 +1,5 @@
/*************************************************************************** /***************************************************************************
* Copyright (C) 2021 - 2023 by Federico Amedeo Izzo IU2NUO, * * Copyright (C) 2021 - 2024 by Federico Amedeo Izzo IU2NUO, *
* Niccolò Izzo IU2KIN * * Niccolò Izzo IU2KIN *
* Wojciech Kaczmarski SP5WWP * * Wojciech Kaczmarski SP5WWP *
* Frederik Saraci IU2NRO * * Frederik Saraci IU2NRO *
@ -27,6 +27,7 @@
#error This header is C++ only! #error This header is C++ only!
#endif #endif
#include <iir.hpp>
#include <cstdint> #include <cstdint>
#include <cstddef> #include <cstddef>
#include <memory> #include <memory>
@ -37,17 +38,12 @@
#include <audio_stream.h> #include <audio_stream.h>
#include <M17/M17Datatypes.hpp> #include <M17/M17Datatypes.hpp>
#include <M17/M17Constants.hpp> #include <M17/M17Constants.hpp>
#include <M17/Correlator.hpp>
#include <M17/Synchronizer.hpp>
namespace M17 namespace M17
{ {
typedef struct
{
int32_t index;
bool lsf;
}
sync_t;
class M17Demodulator class M17Demodulator
{ {
public: public:
@ -90,169 +86,87 @@ public:
*/ */
const frame_t& getFrame(); const frame_t& getFrame();
/**
* @return true if the last decoded frame is an LSF.
*/
bool isFrameLSF();
/** /**
* Demodulates data from the ADC and fills the idle frame. * Demodulates data from the ADC and fills the idle frame.
* Everytime this function is called a whole ADC buffer is consumed. * Everytime this function is called a whole ADC buffer is consumed.
* *
* @param invertPhase: invert the phase of the baseband signal before decoding.
* @return true if a new frame has been fully decoded. * @return true if a new frame has been fully decoded.
*/ */
bool update(); bool update(const bool invertPhase = false);
/** /**
* @return true if a demodulator is locked on an M17 stream. * @return true if a demodulator is locked on an M17 stream.
*/ */
bool isLocked(); bool isLocked();
/**
* Invert baseband signal phase before decoding.
*
* @param status: if set to true signal phase is inverted.
*/
void invertPhase(const bool status);
private: private:
/**
* Quantize a given sample to its corresponding symbol and append it to the
* ongoing frame. When a frame is complete, it swaps the pointers and updates
* newFrame variable.
*
* @param sample: baseband sample.
* @return quantized symbol.
*/
int8_t updateFrame(const int16_t sample);
/**
* Reset the demodulator state.
*/
void reset();
/** /**
* M17 baseband signal sampled at 24kHz, half of an M17 frame is processed * M17 baseband signal sampled at 24kHz, half of an M17 frame is processed
* at each update of the demodulator. * at each update of the demodulator.
*/ */
static constexpr size_t M17_RX_SAMPLE_RATE = 24000; static constexpr size_t RX_SAMPLE_RATE = 24000;
static constexpr size_t SAMPLES_PER_SYMBOL = RX_SAMPLE_RATE / M17_SYMBOL_RATE;
static constexpr size_t FRAME_SAMPLES = M17_FRAME_SYMBOLS * SAMPLES_PER_SYMBOL;
static constexpr size_t M17_SAMPLES_PER_SYMBOL = M17_RX_SAMPLE_RATE / M17_SYMBOL_RATE; static constexpr size_t SAMPLE_BUF_SIZE = FRAME_SAMPLES / 2;
static constexpr size_t M17_FRAME_SAMPLES = M17_FRAME_SYMBOLS * M17_SAMPLES_PER_SYMBOL; static constexpr size_t SYNCWORD_SAMPLES = SAMPLES_PER_SYMBOL * M17_SYNCWORD_SYMBOLS;
static constexpr size_t M17_SAMPLE_BUF_SIZE = M17_FRAME_SAMPLES / 2;
static constexpr size_t M17_SYNCWORD_SAMPLES = M17_SAMPLES_PER_SYMBOL * M17_SYNCWORD_SYMBOLS;
static constexpr int8_t SYNC_SWEEP_WIDTH = 10;
static constexpr int8_t SYNC_SWEEP_OFFSET = ceil(SYNC_SWEEP_WIDTH / M17_SAMPLES_PER_SYMBOL);
static constexpr int16_t M17_BRIDGE_SIZE = M17_SYNCWORD_SAMPLES + 2 * SYNC_SWEEP_WIDTH;
static constexpr float CONV_STATS_ALPHA = 0.005f;
static constexpr float CONV_THRESHOLD_FACTOR = 3.40;
static constexpr int16_t QNT_SMA_WINDOW = 8;
/** /**
* M17 syncwords; * Internal state of the demodulator.
*/ */
int8_t lsf_syncword[M17_SYNCWORD_SYMBOLS] = { +3, +3, +3, +3, -3, -3, +3, -3 }; enum class DemodState
int8_t stream_syncword[M17_SYNCWORD_SYMBOLS] = { -3, -3, -3, -3, +3, +3, -3, +3 }; {
INIT, ///< Initializing
/* UNLOCKED, ///< Not locked
* Buffers SYNCED, ///< Synchronized, validate syncword
*/ LOCKED, ///< Locked
std::unique_ptr< int16_t[] > baseband_buffer; ///< Buffer for baseband audio handling. SYNC_UPDATE ///< Updating the sampling point
streamId basebandId; ///< Id of the baseband input stream. };
pathId basebandPath; ///< Id of the baseband input path.
dataBlock_t baseband; ///< Data block with samples to be processed.
uint16_t frame_index; ///< Index for filling the raw frame.
std::unique_ptr<frame_t > demodFrame; ///< Frame being demodulated.
std::unique_ptr<frame_t > readyFrame; ///< Fully demodulated frame to be returned.
bool syncDetected; ///< A syncword was detected.
bool locked; ///< A syncword was correctly demodulated.
bool newFrame; ///< A new frame has been fully decoded.
int16_t basebandBridge[M17_BRIDGE_SIZE] = { 0 }; ///< Bridge buffer
int16_t phase; ///< Phase of the signal w.r.t. sampling
bool invPhase; ///< Invert signal phase
/*
* State variables
*/
bool m17RxEnabled; ///< M17 Reception Enabled
/*
* Convolution statistics computation
*/
float conv_emvar = 0.0f;
/*
* Quantization statistics computation
*/
int8_t qnt_pos_cnt; ///< Number of received positive samples
int8_t qnt_neg_cnt; ///< Number of received negative samples
int32_t qnt_pos_acc; ///< Accumulator for quantization average
int32_t qnt_neg_acc; ///< Accumulator for quantization average
float qnt_pos_avg = 0.0f; ///< Rolling average of positive samples
float qnt_neg_avg = 0.0f; ///< Rolling average of negative samples
/*
* DSP filter state
*/
filter_state_t dsp_state;
/** /**
* Resets the exponential mean and variance/stddev computation. * Cofficients of the sample filter
*/ */
void resetCorrelationStats(); static constexpr std::array < float, 3 > sfNum = {4.24433681e-05f, 8.48867363e-05f, 4.24433681e-05f};
static constexpr std::array < float, 3 > sfDen = {1.0f, -1.98148851f, 0.98165828f};
/** DemodState demodState; ///< Demodulator state
* Updates the mean and variance with the given correlation value. std::unique_ptr< int16_t[] > baseband_buffer; ///< Buffer for baseband audio handling.
* streamId basebandId; ///< Id of the baseband input stream.
* @param value: value to be added to the exponential moving pathId basebandPath; ///< Id of the baseband input path.
* average/variance computation std::unique_ptr<frame_t > demodFrame; ///< Frame being demodulated.
*/ std::unique_ptr<frame_t > readyFrame; ///< Fully demodulated frame to be returned.
void updateCorrelationStats(int32_t value); bool locked; ///< A syncword was correctly demodulated.
bool newFrame; ///< A new frame has been fully decoded.
uint16_t frameIndex; ///< Index for filling the raw frame.
uint32_t sampleIndex; ///< Sample index, from 0 to (SAMPLES_PER_SYMBOL - 1)
uint32_t samplingPoint; ///< Symbol sampling point
uint32_t sampleCount; ///< Free-running sample counter
uint8_t missedSyncs; ///< Counter of missed synchronizations
uint32_t initCount; ///< Downcounter for initialization
uint32_t syncCount; ///< Downcounter for resynchronization
std::pair < int32_t, int32_t > outerDeviation; ///< Deviation of outer symbols
float corrThreshold; ///< Correlation threshold
filter_state_t dcrState; ///< State of the DC removal filter
/** Correlator < M17_SYNCWORD_SYMBOLS, SAMPLES_PER_SYMBOL > correlator;
* Returns the standard deviation from all the correlation values. Synchronizer < M17_SYNCWORD_SYMBOLS, SAMPLES_PER_SYMBOL > streamSync{{ -3, -3, -3, -3, +3, +3, -3, +3 }};
* Iir < 3 > sampleFilter{sfNum, sfDen};
* @returns float numerical value of the standard deviation
*/
float getCorrelationStddev();
/**
* Resets the quantization max, min and ema computation.
*/
void resetQuantizationStats();
/**
* Updates the max, min and ema for the received samples.
*
* @param offset: index value to be added to the exponential moving
* average/variance computation
*/
void updateQuantizationStats(int32_t frame_index, int32_t symbol_index);
/**
* Computes the convolution between a stride of samples starting from
* a given offset and a target waveform.
*
* @param offset: the offset in the active buffer where to start the stride
* @param target: a buffer containing the target waveform to be convoluted
* @param target_size: the number of symbols of the target waveform
* @return uint16_t numerical value of the convolution
*/
int32_t convolution(int32_t offset, int8_t *target, size_t target_size);
/**
* Finds the index of the next frame syncword in the baseband stream.
*
* @param baseband: buffer containing the sampled baseband signal
* @param offset: offset of the buffer after which syncword are searched
* @return uint16_t index of the first syncword in the buffer after the offset
*/
sync_t nextFrameSync(int32_t offset);
/**
* Takes the value from the input baseband at a given offsets and quantizes
* it leveraging the quantization max and min hold statistics.
*
* @param offset: the offset in the input baseband
* @return int8_t quantized symbol
*/
int8_t quantize(int32_t offset);
/**
* Perform a limited search for a syncword using correlation
*
* @param offset: sample index right after a syncword
* @return int32_t sample of the beginning of a syncword
*/
int32_t syncwordSweep(int32_t offset);
}; };
} /* M17 */ } /* M17 */

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

@ -1,5 +1,5 @@
/*************************************************************************** /***************************************************************************
* Copyright (C) 2021 - 2023 by Federico Amedeo Izzo IU2NUO, * * Copyright (C) 2021 - 2024 by Federico Amedeo Izzo IU2NUO, *
* Niccolò Izzo IU2KIN * * Niccolò Izzo IU2KIN *
* Wojciech Kaczmarski SP5WWP * * Wojciech Kaczmarski SP5WWP *
* Frederik Saraci IU2NRO * * Frederik Saraci IU2NRO *
@ -166,15 +166,11 @@ void M17Demodulator::init()
* placement new. * placement new.
*/ */
baseband_buffer = std::make_unique< int16_t[] >(2 * M17_SAMPLE_BUF_SIZE); baseband_buffer = std::make_unique< int16_t[] >(2 * SAMPLE_BUF_SIZE);
demodFrame = std::make_unique< frame_t >(); demodFrame = std::make_unique< frame_t >();
readyFrame = std::make_unique< frame_t >(); readyFrame = std::make_unique< frame_t >();
baseband = { nullptr, 0 };
frame_index = 0; reset();
phase = 0;
syncDetected = false;
locked = false;
newFrame = false;
#ifdef ENABLE_DEMOD_LOG #ifdef ENABLE_DEMOD_LOG
logRunning = true; logRunning = true;
@ -206,165 +202,19 @@ void M17Demodulator::startBasebandSampling()
{ {
basebandPath = audioPath_request(SOURCE_RTX, SINK_MCU, PRIO_RX); basebandPath = audioPath_request(SOURCE_RTX, SINK_MCU, PRIO_RX);
basebandId = audioStream_start(basebandPath, baseband_buffer.get(), basebandId = audioStream_start(basebandPath, baseband_buffer.get(),
2 * M17_SAMPLE_BUF_SIZE, M17_RX_SAMPLE_RATE, 2 * SAMPLE_BUF_SIZE, RX_SAMPLE_RATE,
STREAM_INPUT | BUF_CIRC_DOUBLE); STREAM_INPUT | BUF_CIRC_DOUBLE);
// Clean start of the demodulation statistics reset();
resetCorrelationStats();
resetQuantizationStats();
// DC removal filter reset
dsp_resetFilterState(&dsp_state);
} }
void M17Demodulator::stopBasebandSampling() void M17Demodulator::stopBasebandSampling()
{ {
audioStream_terminate(basebandId); audioStream_terminate(basebandId);
audioPath_release(basebandPath); audioPath_release(basebandPath);
phase = 0;
syncDetected = false;
locked = false; locked = false;
} }
void M17Demodulator::resetCorrelationStats()
{
conv_emvar = 40000000.0f;
}
/**
* Algorithms taken from
* https://fanf2.user.srcf.net/hermes/doc/antiforgery/stats.pdf
*/
void M17Demodulator::updateCorrelationStats(int32_t value)
{
float incr = CONV_STATS_ALPHA * static_cast<float>(value);
conv_emvar = (1.0f - CONV_STATS_ALPHA) * (conv_emvar + static_cast<float>(value) * incr);
}
float M17Demodulator::getCorrelationStddev()
{
return sqrt(conv_emvar);
}
void M17Demodulator::resetQuantizationStats()
{
qnt_pos_avg = 0.0f;
qnt_neg_avg = 0.0f;
}
void M17Demodulator::updateQuantizationStats(int32_t frame_index,
int32_t symbol_index)
{
int16_t sample = 0;
// When we are at negative indices use bridge buffer
if (symbol_index < 0)
sample = basebandBridge[M17_BRIDGE_SIZE + symbol_index];
else
sample = baseband.data[symbol_index];
if (sample > 0)
{
qnt_pos_acc += sample;
qnt_pos_cnt++;
}
else
{
qnt_neg_acc += sample;
qnt_neg_cnt++;
}
// If we reached end of the syncword, compute average and reset queue
if(frame_index == M17_SYNCWORD_SYMBOLS - 1)
{
qnt_pos_avg = qnt_pos_acc / static_cast<float>(qnt_pos_cnt);
qnt_neg_avg = qnt_neg_acc / static_cast<float>(qnt_neg_cnt);
qnt_pos_acc = 0;
qnt_neg_acc = 0;
qnt_pos_cnt = 0;
qnt_neg_cnt = 0;
}
}
int32_t M17Demodulator::convolution(int32_t offset,
int8_t *target,
size_t target_size)
{
// Compute convolution
int32_t conv = 0;
for(uint32_t i = 0; i < target_size; i++)
{
int32_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;
}
sync_t M17Demodulator::nextFrameSync(int32_t offset)
{
sync_t syncword = { -1, false };
// Find peaks in the correlation between the baseband and the frame syncword
// Leverage the fact LSF syncword is the opposite of the frame syncword
// to detect both syncwords at once. Stop early because convolution needs
// access samples ahead of the starting offset.
int32_t maxLen = static_cast < int32_t >(baseband.len - M17_SYNCWORD_SAMPLES);
for(int32_t i = offset; (syncword.index == -1) && (i < maxLen); i++)
{
int32_t conv = convolution(i, stream_syncword, M17_SYNCWORD_SYMBOLS);
updateCorrelationStats(conv);
#ifdef ENABLE_DEMOD_LOG
log_entry_t log;
log.sample = (i < 0) ? basebandBridge[M17_BRIDGE_SIZE + i] : baseband.data[i];
log.conv = conv;
log.conv_th = CONV_THRESHOLD_FACTOR * getCorrelationStddev();
log.sample_index = i;
log.qnt_pos_avg = 0.0;
log.qnt_neg_avg = 0.0;
log.symbol = 0;
log.frame_index = 0;
log.flags = 1;
pushLog(log);
#endif
// Positive correlation peak -> frame syncword
if (conv > (getCorrelationStddev() * CONV_THRESHOLD_FACTOR))
{
syncword.lsf = false;
syncword.index = i;
}
// Negative correlation peak -> LSF syncword
else if (conv < -(getCorrelationStddev() * CONV_THRESHOLD_FACTOR))
{
syncword.lsf = true;
syncword.index = i;
}
}
return syncword;
}
int8_t M17Demodulator::quantize(int32_t offset)
{
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 > static_cast< int16_t >(qnt_pos_avg / 1.5f))
return +3;
else if (sample < static_cast< int16_t >(qnt_neg_avg / 1.5f))
return -3;
else if (sample > 0)
return +1;
else
return -1;
}
const frame_t& M17Demodulator::getFrame() const frame_t& M17Demodulator::getFrame()
{ {
// When a frame is read is not new anymore // When a frame is read is not new anymore
@ -377,220 +227,211 @@ bool M17Demodulator::isLocked()
return locked; return locked;
} }
int32_t M17Demodulator::syncwordSweep(int32_t offset) bool M17Demodulator::update(const bool invertPhase)
{ {
int32_t max_conv = 0, max_index = 0; // Audio path closed, nothing to do
// Start from 5 samples behind, end 5 samples after if(audioPath_getStatus(basebandPath) != PATH_OPEN)
for(int i = -SYNC_SWEEP_WIDTH; i <= SYNC_SWEEP_WIDTH; i++) return false;
{
// TODO: Extend for LSF and BER syncwords
int32_t conv = convolution(offset + i,
stream_syncword,
M17_SYNCWORD_SYMBOLS);
#ifdef ENABLE_DEMOD_LOG
int16_t sample;
if (offset + i < 0)
sample = basebandBridge[M17_BRIDGE_SIZE + offset + i];
else
sample = baseband.data[offset + i];
log_entry_t log;
log.sample = sample;
log.conv = conv;
log.conv_th = 0.0;
log.sample_index = offset + i;
log.qnt_pos_avg = 0.0;
log.qnt_neg_avg = 0.0;
log.symbol = 0;
log.frame_index = 0;
log.flags = 2;
pushLog(log);
#endif
if (conv > max_conv)
{
max_conv = conv;
max_index = i;
}
}
return max_index;
}
bool M17Demodulator::update()
{
sync_t syncword = { 0, false };
phase = (syncDetected) ? phase % M17_SAMPLES_PER_SYMBOL : -M17_BRIDGE_SIZE;
uint16_t decoded_syms = 0;
// Read samples from the ADC // Read samples from the ADC
if(audioPath_getStatus(basebandPath) != PATH_OPEN) return false; dataBlock_t baseband = inputStream_getData(basebandId);
baseband = inputStream_getData(basebandId);
if(baseband.data != NULL) if(baseband.data != NULL)
{ {
// Apply DC removal filter // Apply DC removal filter
dsp_dcRemoval(&dsp_state, baseband.data, baseband.len); dsp_dcRemoval(&dcrState, baseband.data, baseband.len);
// Apply RRC on the baseband buffer // Process samples
for(size_t i = 0; i < baseband.len; i++) for(size_t i = 0; i < baseband.len; i++)
{ {
float elem = static_cast< float >(baseband.data[i]); // Apply RRC on the baseband sample
if(invPhase) elem = 0.0f - elem; float elem = static_cast< float >(baseband.data[i]);
baseband.data[i] = static_cast< int16_t >(M17::rrc_24k(elem)); if(invertPhase) elem = 0.0f - elem;
} int16_t sample = static_cast< int16_t >(M17::rrc_24k(elem));
// Process the buffer // Update correlator and sample filter for correlation thresholds
while(syncword.index != -1) correlator.sample(sample);
{ corrThreshold = sampleFilter(std::abs(sample));
// If we are not demodulating a syncword, search for one switch(demodState)
if (syncDetected == false)
{ {
syncword = nextFrameSync(phase); case DemodState::INIT:
if (syncword.index != -1) // Valid syncword found
{ {
phase = syncword.index + 1; initCount -= 1;
syncDetected = true; if(initCount == 0)
frame_index = 0; demodState = DemodState::UNLOCKED;
decoded_syms = 0;
} }
}
// While we detected a syncword, demodulate available samples
else
{
// Slice the input buffer to extract a frame and quantize
int32_t symbol_index = phase
+ (M17_SAMPLES_PER_SYMBOL * decoded_syms);
if (symbol_index >= static_cast<int32_t>(baseband.len))
break; break;
// Update quantization stats only on syncwords
if (frame_index < M17_SYNCWORD_SYMBOLS)
updateQuantizationStats(frame_index, symbol_index);
int8_t symbol = quantize(symbol_index);
#ifdef ENABLE_DEMOD_LOG case DemodState::UNLOCKED:
// Log quantization
for (int i = -2; i <= 2; i++)
{ {
if ((symbol_index + i) >= 0 && int32_t syncThresh = static_cast< int32_t >(corrThreshold * 33.0f);
(symbol_index + i) < static_cast<int32_t> (baseband.len)) int8_t syncStatus = streamSync.update(correlator, syncThresh, -syncThresh);
{
log_entry_t log;
log.sample = baseband.data[symbol_index + i];
log.conv = phase;
log.conv_th = 0.0;
log.sample_index = symbol_index + i;
log.qnt_pos_avg = qnt_pos_avg / 1.5f;
log.qnt_neg_avg = qnt_neg_avg / 1.5f;
log.symbol = symbol;
log.frame_index = frame_index;
log.flags = 3;
if(i == 0) log.flags += 8;
pushLog(log); if(syncStatus != 0)
} demodState = DemodState::SYNCED;
} }
#endif break;
setSymbol(*demodFrame, frame_index, symbol); case DemodState::SYNCED:
decoded_syms++;
frame_index++;
if (frame_index == M17_SYNCWORD_SYMBOLS)
{ {
/* // Set sampling point and deviation, zero frame symbol count
* Check for valid syncword using hamming distance. samplingPoint = streamSync.samplingIndex();
* The demodulator switches to locked state only if there outerDeviation = correlator.maxDeviation(samplingPoint);
* is an exact syncword match, this avoids continuous false frameIndex = 0;
* detections in absence of an M17 signal.
*/
uint8_t maxHamming = 2;
if(locked == false) maxHamming = 0;
uint8_t hammingSync = hammingDistance((*demodFrame)[0], // Quantize the syncword taking data from the correlator
STREAM_SYNC_WORD[0]) // memory.
+ hammingDistance((*demodFrame)[1], for(size_t i = 0; i < SYNCWORD_SAMPLES; i++)
STREAM_SYNC_WORD[1]);
uint8_t hammingLsf = hammingDistance((*demodFrame)[0],
LSF_SYNC_WORD[0])
+ hammingDistance((*demodFrame)[1],
LSF_SYNC_WORD[1]);
if ((hammingSync > maxHamming) && (hammingLsf > maxHamming))
{ {
// Lock lost, reset demodulator alignment (phase) only size_t pos = (correlator.index() + i) % SYNCWORD_SAMPLES;
// if we were locked on a valid signal. int16_t val = correlator.data()[pos];
// This to avoid, in case of absence of carrier, to fall
// in a loop where the demodulator continues to search
// for the syncword in the same block of samples, causing
// the update function to take more than 20ms to complete.
if(locked) phase = 0;
syncDetected = false;
locked = false;
#ifdef ENABLE_DEMOD_LOG if((pos % SAMPLES_PER_SYMBOL) == samplingPoint)
// Pre-arm the log trigger. updateFrame(val);
trigEnable = true; }
#endif
uint8_t hd = hammingDistance((*demodFrame)[0], STREAM_SYNC_WORD[0]);
hd += hammingDistance((*demodFrame)[1], STREAM_SYNC_WORD[1]);
if(hd == 0)
{
locked = true;
demodState = DemodState::LOCKED;
} }
else else
{ {
// Correct syncword found demodState = DemodState::UNLOCKED;
locked = true;
#ifdef ENABLE_DEMOD_LOG
// Trigger a data dump when lock is re-acquired.
if((dumpData == false) && (trigEnable == true))
{
trigEnable = false;
triggered = true;
}
#endif
} }
} }
break;
// Locate syncword to correct clock skew between Tx and Rx case DemodState::LOCKED:
if (frame_index == M17_SYNCWORD_SYMBOLS + SYNC_SWEEP_OFFSET)
{ {
// Find index (possibly negative) of the syncword // Quantize and update frame at each sampling point
int32_t expected_sync = if(sampleIndex == samplingPoint)
phase + {
M17_SAMPLES_PER_SYMBOL * decoded_syms - updateFrame(sample);
M17_SYNCWORD_SAMPLES -
SYNC_SWEEP_OFFSET * M17_SAMPLES_PER_SYMBOL;
int32_t sync_skew = syncwordSweep(expected_sync);
phase += sync_skew;
}
// If the frame buffer is full switch demod and ready frame // When we have reached almost the end of a frame, switch
if (frame_index == M17_FRAME_SYMBOLS) // to syncpoint update.
{ if(frameIndex == (M17_FRAME_SYMBOLS - M17_SYNCWORD_SYMBOLS/2))
demodFrame.swap(readyFrame); {
frame_index = 0; demodState = DemodState::SYNC_UPDATE;
newFrame = true; syncCount = SYNCWORD_SAMPLES * 2;
}
}
} }
break;
case DemodState::SYNC_UPDATE:
{
// Keep filling the ongoing frame!
if(sampleIndex == samplingPoint)
updateFrame(sample);
// Find the new correlation peak
int32_t syncThresh = static_cast< int32_t >(corrThreshold * 33.0f);
int8_t syncStatus = streamSync.update(correlator, syncThresh, -syncThresh);
if(syncStatus != 0)
{
// Correlation has to coincide with a syncword!
if(frameIndex == M17_SYNCWORD_SYMBOLS)
{
uint8_t hd = hammingDistance((*demodFrame)[0], STREAM_SYNC_WORD[0]);
hd += hammingDistance((*demodFrame)[1], STREAM_SYNC_WORD[1]);
// Valid sync found: update deviation and sample
// point, then go back to locked state
if(hd <= 1)
{
outerDeviation = correlator.maxDeviation(samplingPoint);
samplingPoint = streamSync.samplingIndex();
missedSyncs = 0;
demodState = DemodState::LOCKED;
break;
}
}
}
// No syncword found within the window, increase the count
// of missed syncs and choose where to go. The lock is lost
// after four consecutive sync misses.
if(syncCount == 0)
{
if(missedSyncs >= 4)
{
demodState = DemodState::UNLOCKED;
locked = false;
}
else
{
demodState = DemodState::LOCKED;
}
missedSyncs += 1;
}
syncCount -= 1;
}
break;
} }
sampleCount += 1;
sampleIndex = (sampleIndex + 1) % SAMPLES_PER_SYMBOL;
} }
// Copy last N samples to bridge buffer
memcpy(basebandBridge,
baseband.data + (baseband.len - M17_BRIDGE_SIZE),
sizeof(int16_t) * M17_BRIDGE_SIZE);
} }
#if defined(PLATFORM_LINUX) && defined(ENABLE_DEMOD_LOG)
if (baseband.data == NULL)
dumpData = true;
#endif
return newFrame; return newFrame;
} }
void M17Demodulator::invertPhase(const bool status) int8_t M17Demodulator::updateFrame(stream_sample_t sample)
{ {
invPhase = status; int8_t symbol;
if(sample > (2 * outerDeviation.first)/3)
{
symbol = +3;
}
else if(sample < (2 * outerDeviation.second)/3)
{
symbol = -3;
}
else if(sample > 0)
{
symbol = +1;
}
else
{
symbol = -1;
}
setSymbol(*demodFrame, frameIndex, symbol);
frameIndex += 1;
if(frameIndex >= M17_FRAME_SYMBOLS)
{
std::swap(readyFrame, demodFrame);
frameIndex = 0;
newFrame = true;
}
return symbol;
} }
void M17Demodulator::reset()
{
sampleIndex = 0;
frameIndex = 0;
sampleCount = 0;
newFrame = false;
locked = false;
demodState = DemodState::INIT;
initCount = RX_SAMPLE_RATE / 50; // 50ms of init time
dsp_resetFilterState(&dcrState);
}
constexpr std::array < float, 3 > M17Demodulator::sfNum;
constexpr std::array < float, 3 > M17Demodulator::sfDen;

3
openrtx/src/rtx/OpMode_M17.cpp
View File

@ -169,14 +169,13 @@ void OpMode_M17::rxState(rtxStatus_t *const status)
if(startRx) if(startRx)
{ {
demodulator.startBasebandSampling(); demodulator.startBasebandSampling();
demodulator.invertPhase(invertRxPhase);
radio_enableRx(); radio_enableRx();
startRx = false; startRx = false;
} }
bool newData = demodulator.update(); bool newData = demodulator.update(invertRxPhase);
bool lock = demodulator.isLocked(); bool lock = demodulator.isLocked();
// Reset frame decoder when transitioning from unlocked to locked state. // Reset frame decoder when transitioning from unlocked to locked state.