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

Demodulator bugfix 

Fix several bugs in the demodulator code, tipycally sign errors.
This commit is contained in:
Niccolò Izzo 2021-12-30 23:07:08 +01:00 committed by Silvano Seva
parent 1a9b3c4168
commit 7a9bbfcaf2
2 changed files with 98 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -177,7 +177,7 @@ private:
void resetQuantizationStats(); void resetQuantizationStats();
void updateQuantizationStats(uint32_t offset); void updateQuantizationStats(int32_t offset);
float getQuantizationMax(); float getQuantizationMax();
@ -192,7 +192,7 @@ private:
* @param target_size: the number of symbols of the target waveform * @param target_size: the number of symbols of the target waveform
* @return uint16_t numerical value of the convolution * @return uint16_t numerical value of the convolution
*/ */
int32_t convolution(size_t offset, int8_t *target, size_t target_size); 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. * Finds the index of the next frame syncword in the baseband stream.
@ -201,7 +201,7 @@ private:
* @param offset: offset of the buffer after which syncword are searched * @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 * @return uint16_t index of the first syncword in the buffer after the offset
*/ */
sync_t nextFrameSync(uint32_t offset); sync_t nextFrameSync(int32_t offset);
/** /**
* Takes the value from the input baseband at a given offsets and quantizes * Takes the value from the input baseband at a given offsets and quantizes

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

@ -22,6 +22,7 @@
#include <M17/M17DSP.h> #include <M17/M17DSP.h>
#include <M17/M17Utils.h> #include <M17/M17Utils.h>
#include <interfaces/audio_stream.h> #include <interfaces/audio_stream.h>
#include <interfaces/gpio.h>
#include <math.h> #include <math.h>
#include <cstring> #include <cstring>
@ -46,13 +47,14 @@ void M17Demodulator::init()
* placement new. * placement new.
*/ */
baseband_buffer = new int16_t[M17_FRAME_SAMPLES_24K]; baseband_buffer = new int16_t[2 * M17_INPUT_BUF_SIZE];
baseband = { nullptr, 0 }; baseband = { nullptr, 0 };
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; frameIndex = 0;
phase = 0; phase = 0;
locked = false;
} }
void M17Demodulator::terminate() void M17Demodulator::terminate()
@ -66,7 +68,6 @@ void M17Demodulator::terminate()
void M17Demodulator::startBasebandSampling() void M17Demodulator::startBasebandSampling()
{ {
basebandId = inputStream_start(SOURCE_RTX, PRIO_RX, basebandId = inputStream_start(SOURCE_RTX, PRIO_RX,
baseband_buffer, baseband_buffer,
M17_INPUT_BUF_SIZE, M17_INPUT_BUF_SIZE,
@ -107,21 +108,26 @@ void M17Demodulator::resetQuantizationStats()
qnt_max = 0.0f; qnt_max = 0.0f;
} }
void M17Demodulator::updateQuantizationStats(uint32_t offset) void M17Demodulator::updateQuantizationStats(int32_t offset)
{ {
auto value = baseband.data[offset]; // If offset is negative use bridge buffer
if (value > qnt_max) 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 > qnt_max)
{ {
qnt_max = value; qnt_max = sample;
} }
else else
{ {
qnt_max *= qnt_maxmin_alpha; qnt_max *= qnt_maxmin_alpha;
} }
if (value < qnt_min) if (sample < qnt_min)
{ {
qnt_min = value; qnt_min = sample;
} }
else else
{ {
@ -139,7 +145,7 @@ float M17Demodulator::getQuantizationMin()
return qnt_min; return qnt_min;
} }
int32_t M17Demodulator::convolution(size_t offset, int32_t M17Demodulator::convolution(int32_t offset,
int8_t *target, int8_t *target,
size_t target_size) size_t target_size)
{ {
@ -147,7 +153,7 @@ 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++)
{ {
int16_t sample_index = offset + i * M17_SAMPLES_PER_SYMBOL; int32_t sample_index = offset + i * M17_SAMPLES_PER_SYMBOL;
int16_t sample = 0; int16_t sample = 0;
// When we are at negative indices use bridge buffer // When we are at negative indices use bridge buffer
if (sample_index < 0) if (sample_index < 0)
@ -159,15 +165,19 @@ int32_t M17Demodulator::convolution(size_t offset,
return conv; return conv;
} }
sync_t M17Demodulator::nextFrameSync(uint32_t offset) sync_t M17Demodulator::nextFrameSync(int32_t offset)
{ {
sync_t syncword = { -1, false }; sync_t syncword = { -1, false };
// Find peaks in the correlation between the baseband and the frame syncword // Find peaks in the correlation between the baseband and the frame syncword
// Leverage the fact LSF syncword is the opposite of the frame syncword // Leverage the fact LSF syncword is the opposite of the frame syncword
// to detect both syncwords at once. // to detect both syncwords at once.
for(uint32_t i = offset; syncword.index == -1 && i < baseband.len; i++) for(int32_t i = offset; syncword.index == -1 && i < (int32_t) baseband.len; i++)
{ {
// If we are not locked search for a syncword
int32_t conv = convolution(i, stream_syncword, M17_SYNCWORD_SYMBOLS); int32_t conv = convolution(i, stream_syncword, M17_SYNCWORD_SYMBOLS);
//printf("%d, ", conv);
//if (i % 8 == 0)
// printf("\r\n");
updateCorrelationStats(conv); updateCorrelationStats(conv);
// Positive correlation peak -> frame syncword // Positive correlation peak -> frame syncword
if (conv > getCorrelationStddev() * conv_threshold_factor) if (conv > getCorrelationStddev() * conv_threshold_factor)
@ -188,6 +198,8 @@ sync_t M17Demodulator::nextFrameSync(uint32_t offset)
int8_t M17Demodulator::quantize(int32_t offset) int8_t M17Demodulator::quantize(int32_t offset)
{ {
int16_t sample = 0; int16_t sample = 0;
int16_t zero = getQuantizationMin() +
(getQuantizationMax() - getQuantizationMin()) / 2;
if (offset < 0) // When we are at negative offsets use bridge buffer if (offset < 0) // When we are at negative offsets use bridge buffer
sample = basebandBridge[M17_BRIDGE_SIZE + offset]; sample = basebandBridge[M17_BRIDGE_SIZE + offset];
else // Otherwise use regular data buffer else // Otherwise use regular data buffer
@ -196,7 +208,7 @@ int8_t M17Demodulator::quantize(int32_t offset)
return +3; return +3;
else if (sample < getQuantizationMin() * 2 / 3) else if (sample < getQuantizationMin() * 2 / 3)
return -3; return -3;
else if (sample > 0) else if (sample > zero)
return +1; return +1;
else else
return -1; return -1;
@ -214,8 +226,9 @@ bool M17Demodulator::isFrameLSF()
void M17Demodulator::update() void M17Demodulator::update()
{ {
M17::sync_t syncword = { -1, false }; M17::sync_t syncword = { 0, false };
int16_t offset = -(int16_t) M17_BRIDGE_SIZE; int32_t offset = -(int32_t) M17_BRIDGE_SIZE;
int32_t phase = 0;
uint16_t decoded_syms = 0; uint16_t decoded_syms = 0;
// Read samples from the ADC // Read samples from the ADC
baseband = inputStream_getData(basebandId); baseband = inputStream_getData(basebandId);
@ -228,59 +241,82 @@ 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 // Process the buffer
if (!locked) while(syncword.index != -1 && (offset + 1 + phase +
(int32_t) M17_SAMPLES_PER_SYMBOL * decoded_syms <
(int32_t) baseband.len))
{ {
syncword = nextFrameSync(-M17_SYNCWORD_SYMBOLS * M17_SAMPLES_PER_SYMBOL); // If we are not locked search for a syncword
if (syncword.index != -1) // Lock was just acquired if (!locked)
{ {
locked = true; //printf("\nSearching at offset: %d\n", offset);
isLSF = syncword.lsf; syncword = nextFrameSync(offset);
offset = syncword.index + 2; if (syncword.index != -1) // Lock was just acquired
// 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<M17_FRAME_BYTES>(*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) locked = true;
printf("\n"); #ifdef PLATFORM_MOD17
printf(" %02X%02X", (*idleFrame)[i], (*idleFrame)[i+1]); gpio_setPin(SYNC_LED);
#endif // PLATFORM_MOD17
isLSF = syncword.lsf;
offset = syncword.index + 1;
// DEBUG: prints
//if (isLSF)
// printf("\nFound LSF at position %d!\r\n", syncword.index);
//else
// printf("\nFound SYNC at position %d!\r\n", syncword.index);
}
}
// While we are locked, demodulate available samples
while (locked)
{
// Slice the input buffer to extract a frame and quantize
int32_t symbol_index = offset + 1 + phase + M17_SAMPLES_PER_SYMBOL * decoded_syms;
updateQuantizationStats(baseband.data[symbol_index]);
int8_t symbol = quantize(symbol_index);
setSymbol<M17_FRAME_BYTES>(*activeFrame, frameIndex, symbol);
decoded_syms++;
frameIndex++;
//printf("%2d ", symbol);
//if (decoded_syms % 16 == 0)
// printf("\n");
// 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("\r\n");
// printf(" %02X%02X", (*idleFrame)[i], (*idleFrame)[i+1]);
//}
}
if (frameIndex == M17_SYNCWORD_SYMBOLS &&
((*activeFrame)[0] != stream_syncword_bytes[0] ||
(*activeFrame)[1] != stream_syncword_bytes[1])) // Lock is lost
{
locked = false;
std::swap(activeFrame, idleFrame);
frameIndex = 0;
#ifdef PLATFORM_MOD17
gpio_clearPin(SYNC_LED);
#endif // PLATFORM_MOD17
} }
} }
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 // We are at the end of the buffer
phase = offset % M17_SAMPLES_PER_SYMBOL + if (locked) {
(M17_INPUT_BUF_SIZE % M17_SAMPLES_PER_SYMBOL); // Compute phase of next buffer
// copy N samples to bridge buffer phase = offset % M17_SAMPLES_PER_SYMBOL +
(M17_INPUT_BUF_SIZE % M17_SAMPLES_PER_SYMBOL);
}
// Copy last N samples to bridge buffer
memcpy(basebandBridge, memcpy(basebandBridge,
baseband.data + M17_FRAME_SAMPLES_24K - M17_BRIDGE_SIZE, baseband.data + baseband.len - M17_BRIDGE_SIZE,
sizeof(int16_t) * M17_BRIDGE_SIZE); sizeof(int16_t) * M17_BRIDGE_SIZE);
} }
//printf("END\n");
} }
} /* M17 */ } /* M17 */