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

M17: Demodulator: split FSM states into functions 

Signed-off-by: Silvano Seva <silseva@fastwebnet.it>
This commit is contained in:
Silvano Seva 2025-10-30 22:20:18 +01:00
parent 7b633c5bee
commit a6561ef59b
2 changed files with 126 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -110,13 +110,37 @@ private:
* @param sample: baseband sample. * @param sample: baseband sample.
* @return quantized symbol. * @return quantized symbol.
*/ */
int8_t updateFrame(const int16_t sample); void quantize(const int16_t sample);
/** /**
* Reset the demodulator state. * Reset the demodulator state.
*/ */
void reset(); void reset();
/**
* State handler function for DemodState::UNLOCKED
*/
void unlockedState();
/**
* State handler function for DemodState::SYNCED
*/
void syncedState();
/**
* State handler function for DemodState::LOCKED
*
* @param sample: current baseband sample
*/
void lockedState(int16_t sample);
/**
* State handler function for DemodState::SYNC_UPDATE
*
* @param sample: current baseband sample
*/
void syncUpdateState(int16_t sample);
/** /**
* 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.
@ -151,7 +175,6 @@ private:
pathId basebandPath; ///< Id of the baseband input path. pathId basebandPath; ///< Id of the baseband input path.
std::unique_ptr<frame_t > demodFrame; ///< Frame being demodulated. std::unique_ptr<frame_t > demodFrame; ///< Frame being demodulated.
std::unique_ptr<frame_t > readyFrame; ///< Fully demodulated frame to be returned. std::unique_ptr<frame_t > readyFrame; ///< Fully demodulated frame to be returned.
bool locked; ///< A syncword was correctly demodulated.
bool newFrame; ///< A new frame has been fully decoded. bool newFrame; ///< A new frame has been fully decoded.
uint16_t frameIndex; ///< Index for filling the raw frame. uint16_t frameIndex; ///< Index for filling the raw frame.
uint32_t sampleIndex; ///< Sample index, from 0 to (SAMPLES_PER_SYMBOL - 1) uint32_t sampleIndex; ///< Sample index, from 0 to (SAMPLES_PER_SYMBOL - 1)

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

@ -212,7 +212,6 @@ void M17Demodulator::stopBasebandSampling()
{ {
audioStream_terminate(basebandId); audioStream_terminate(basebandId);
audioPath_release(basebandPath); audioPath_release(basebandPath);
locked = false;
} }
const frame_t& M17Demodulator::getFrame() const frame_t& M17Demodulator::getFrame()
@ -224,7 +223,8 @@ const frame_t& M17Demodulator::getFrame()
bool M17Demodulator::isLocked() bool M17Demodulator::isLocked()
{ {
return locked; return (demodState == DemodState::LOCKED)
|| (demodState == DemodState::SYNC_UPDATE);
} }
bool M17Demodulator::update(const bool invertPhase) bool M17Demodulator::update(const bool invertPhase)
@ -257,121 +257,27 @@ bool M17Demodulator::update(const bool invertPhase)
{ {
case DemodState::INIT: case DemodState::INIT:
{ {
initCount -= 1;
if(initCount == 0) if(initCount == 0)
demodState = DemodState::UNLOCKED; demodState = DemodState::UNLOCKED;
else
initCount -= 1;
} }
break; break;
case DemodState::UNLOCKED: case DemodState::UNLOCKED:
{ unlockedState();
int32_t syncThresh = static_cast< int32_t >(corrThreshold * 33.0f);
int8_t syncStatus = streamSync.update(correlator, syncThresh, -syncThresh);
if(syncStatus != 0)
demodState = DemodState::SYNCED;
}
break; break;
case DemodState::SYNCED: case DemodState::SYNCED:
{ syncedState();
// Set sampling point and deviation, zero frame symbol count
samplingPoint = streamSync.samplingIndex();
outerDeviation = correlator.maxDeviation(samplingPoint);
frameIndex = 0;
// Quantize the syncword taking data from the correlator
// memory.
for(size_t i = 0; i < SYNCWORD_SAMPLES; i++)
{
size_t pos = (correlator.index() + i) % SYNCWORD_SAMPLES;
int16_t val = correlator.data()[pos];
if((pos % SAMPLES_PER_SYMBOL) == samplingPoint)
updateFrame(val);
}
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
{
demodState = DemodState::UNLOCKED;
}
}
break; break;
case DemodState::LOCKED: case DemodState::LOCKED:
{ lockedState(sample);
if(sampleIndex != samplingPoint)
break;
// Quantize and update frame at each sampling point
updateFrame(sample);
// When we have reached almost the end of a frame, switch
// to syncpoint update.
if(frameIndex == (M17_FRAME_SYMBOLS - M17_SYNCWORD_SYMBOLS/2))
{
demodState = DemodState::SYNC_UPDATE;
syncCount = SYNCWORD_SAMPLES * 2;
}
}
break; break;
case DemodState::SYNC_UPDATE: case DemodState::SYNC_UPDATE:
{ syncUpdateState(sample);
// 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);
// Correlation has to coincide with a syncword!
if((syncStatus != 0) && (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; break;
} }
@ -382,7 +288,7 @@ bool M17Demodulator::update(const bool invertPhase)
return newFrame; return newFrame;
} }
int8_t M17Demodulator::updateFrame(stream_sample_t sample) void M17Demodulator::quantize(stream_sample_t sample)
{ {
int8_t symbol; int8_t symbol;
@ -412,8 +318,6 @@ int8_t M17Demodulator::updateFrame(stream_sample_t sample)
frameIndex = 0; frameIndex = 0;
newFrame = true; newFrame = true;
} }
return symbol;
} }
void M17Demodulator::reset() void M17Demodulator::reset()
@ -422,13 +326,104 @@ void M17Demodulator::reset()
frameIndex = 0; frameIndex = 0;
sampleCount = 0; sampleCount = 0;
newFrame = false; newFrame = false;
locked = false;
demodState = DemodState::INIT; demodState = DemodState::INIT;
initCount = RX_SAMPLE_RATE / 50; // 50ms of init time initCount = RX_SAMPLE_RATE / 50; // 50ms of init time
dsp_resetState(dcBlock); dsp_resetState(dcBlock);
} }
void M17Demodulator::unlockedState()
{
int32_t syncThresh = static_cast< int32_t >(corrThreshold * 33.0f);
int8_t syncStatus = streamSync.update(correlator, syncThresh, -syncThresh);
if(syncStatus != 0)
demodState = DemodState::SYNCED;
}
void M17Demodulator::syncedState()
{
// Set sampling point and deviation, zero frame symbol count
samplingPoint = streamSync.samplingIndex();
outerDeviation = correlator.maxDeviation(samplingPoint);
frameIndex = 0;
// Quantize the syncword taking data from the correlator
// memory.
for(size_t i = 0; i < SYNCWORD_SAMPLES; i++) {
size_t pos = (correlator.index() + i) % SYNCWORD_SAMPLES;
if((pos % SAMPLES_PER_SYMBOL) == samplingPoint) {
int16_t val = correlator.data()[pos];
quantize(val);
}
}
uint8_t hd = hammingDistance((*demodFrame)[0], STREAM_SYNC_WORD[0])
+ hammingDistance((*demodFrame)[1], STREAM_SYNC_WORD[1]);
if(hd == 0)
demodState = DemodState::LOCKED;
else
demodState = DemodState::UNLOCKED;
}
void M17Demodulator::lockedState(int16_t sample)
{
if(sampleIndex != samplingPoint)
return;
// Quantize and update frame at each sampling point
quantize(sample);
// When we have reached almost the end of a frame, switch
// to syncpoint update.
if(frameIndex == (M17_FRAME_SYMBOLS - M17_SYNCWORD_SYMBOLS/2)) {
demodState = DemodState::SYNC_UPDATE;
syncCount = SYNCWORD_SAMPLES * 2;
}
}
void M17Demodulator::syncUpdateState(int16_t sample)
{
// Keep filling the ongoing frame!
if(sampleIndex == samplingPoint)
quantize(sample);
// Find the new correlation peak
int32_t syncThresh = static_cast< int32_t >(corrThreshold * 33.0f);
int8_t syncStatus = streamSync.update(correlator, syncThresh, -syncThresh);
// Correlation has to coincide with a syncword!
if((syncStatus != 0) && (frameIndex == M17_SYNCWORD_SYMBOLS)) {
uint8_t hd = hammingDistance((*demodFrame)[0], STREAM_SYNC_WORD[0])
+ 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;
return;
}
}
// 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;
else
demodState = DemodState::LOCKED;
missedSyncs += 1;
}
syncCount -= 1;
}
constexpr std::array < float, 3 > M17Demodulator::sfNum; constexpr std::array < float, 3 > M17Demodulator::sfNum;
constexpr std::array < float, 3 > M17Demodulator::sfDen; constexpr std::array < float, 3 > M17Demodulator::sfDen;