diff --git a/openrtx/include/protocols/M17/M17Demodulator.h b/openrtx/include/protocols/M17/M17Demodulator.h index 38a6d97c..7aebf972 100644 --- a/openrtx/include/protocols/M17/M17Demodulator.h +++ b/openrtx/include/protocols/M17/M17Demodulator.h @@ -122,7 +122,8 @@ private: static constexpr size_t M17_FRAME_BYTES = M17_FRAME_SYMBOLS / 4; static constexpr size_t M17_FRAME_SAMPLES = M17_FRAME_SYMBOLS * M17_SAMPLES_PER_SYMBOL; static constexpr size_t M17_SAMPLE_BUF_SIZE = M17_FRAME_SAMPLES / 2; - static constexpr size_t M17_BRIDGE_SIZE = M17_SAMPLES_PER_SYMBOL * M17_SYNCWORD_SYMBOLS; + static constexpr size_t M17_SYNCWORD_SAMPLES = M17_SAMPLES_PER_SYMBOL * M17_SYNCWORD_SYMBOLS; + static constexpr size_t M17_BRIDGE_SIZE = M17_SYNCWORD_SAMPLES; static constexpr float CONV_STATS_ALPHA = 0.005f; static constexpr float CONV_THRESHOLD_FACTOR = 3.40; @@ -239,9 +240,19 @@ private: /** * Compute Hamming Distance between two bytes + * + * @param x: first byte + * @param y: second byte */ uint8_t hammingDistance(uint8_t x, uint8_t y); + /** + * 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 */ diff --git a/openrtx/src/core/dsp.cpp b/openrtx/src/core/dsp.cpp index bb29a9b1..312cb316 100644 --- a/openrtx/src/core/dsp.cpp +++ b/openrtx/src/core/dsp.cpp @@ -81,24 +81,26 @@ void dsp_dcRemoval(filter_state_t *state, audio_sample_t *buffer, size_t length) if(length < 2) return; + static constexpr float alpha = 0.999f; + size_t pos = 0; + if(state->initialised == false) { state->u[1] = static_cast< float >(buffer[0]); state->initialised = true; + pos = 1; } - static constexpr float alpha = 0.999f; - - for(size_t i = 1; i < length; i++) + for(; pos < length; pos++) { - state->u[0] = static_cast< float >(buffer[i]); + state->u[0] = static_cast< float >(buffer[pos]); state->y[0] = (state->u[0]) - (state->u[1]) + alpha * (state->y[1]); state->u[1] = state->u[0]; state->y[1] = state->y[0]; - buffer[i] = static_cast< audio_sample_t >(state->y[0] + 0.5f); + buffer[pos] = static_cast< audio_sample_t >(state->y[0] + 0.5f); } } diff --git a/openrtx/src/protocols/M17/M17Demodulator.cpp b/openrtx/src/protocols/M17/M17Demodulator.cpp index 5d1d4e15..2890f307 100644 --- a/openrtx/src/protocols/M17/M17Demodulator.cpp +++ b/openrtx/src/protocols/M17/M17Demodulator.cpp @@ -309,6 +309,25 @@ uint8_t M17Demodulator::hammingDistance(uint8_t x, uint8_t y) return __builtin_popcount(x ^ y); } +int32_t M17Demodulator::syncwordSweep(int32_t offset) +{ + int32_t max_conv = 0, max_index = 0; + // Start from 5 samples behind, end 5 samples after + for(int i = -5; i <= 5; i++) + { + // TODO: Extend for LSF and BER syncwords + int32_t conv = convolution(offset + i, + stream_syncword, + M17_SYNCWORD_SYMBOLS); + if (conv > max_conv) + { + max_conv = conv; + max_index = i; + } + } + return max_index; +} + bool M17Demodulator::update() { M17::sync_t syncword = { 0, false }; @@ -392,11 +411,16 @@ bool M17Demodulator::update() std::swap(activeFrame, idleFrame); frame_index = 0; newFrame = true; + + // Locate syncword to correct clock skew between Tx and Rx + int32_t expected_sync = + offset + phase + M17_SAMPLES_PER_SYMBOL * decoded_syms; + int32_t new_sync = syncwordSweep(expected_sync); + phase += new_sync; } if (frame_index == M17_SYNCWORD_SYMBOLS) { - // If syncword is not valid, lock is lost, accept 2 bit errors uint8_t hammingSync = hammingDistance((*activeFrame)[0], stream_syncword_bytes[0]) @@ -409,7 +433,7 @@ bool M17Demodulator::update() lsf_syncword_bytes[1]); // Too many errors in the syncword, lock is lost - if ((hammingSync > 4) && (hammingLsf > 4)) + if ((hammingSync > 2) && (hammingLsf > 2)) { syncDetected = false; locked = false; diff --git a/scripts/plot_m17_demod_csv.py b/scripts/plot_m17_demod_csv.py index 22fdda4a..70549f61 100755 --- a/scripts/plot_m17_demod_csv.py +++ b/scripts/plot_m17_demod_csv.py @@ -14,6 +14,6 @@ plt.plot(df.index, df.Threshold * -1 / 10) plt.plot(df.index, df.Index) plt.plot(df.index, df.Max) plt.plot(df.index, df.Min) -plt.plot(df.index, df.Symbol * 4000) +plt.plot(df.index, df.Symbol * 10) plt.plot(df.index, df.I) plt.show()