diff --git a/TNC/ClockRecovery.h b/TNC/ClockRecovery.h index d2d1803..96060c2 100644 --- a/TNC/ClockRecovery.h +++ b/TNC/ClockRecovery.h @@ -2,204 +2,108 @@ #pragma once +#include "KalmanFilter.h" +#include "Log.h" + +#include #include +#include #include #include +#include #include namespace mobilinkd { namespace m17 { -/** - * Calculate the phase estimates for each sample position. - * - * This performs a running calculation of the phase of each bit position. - * It is very noisy for individual samples, but quite accurate when - * averaged over an entire M17 frame. - * - * It is designed to be used to calculate the best bit position for each - * frame of data. Samples are collected and averaged. When update() is - * called, the best sample index and clock are estimated, and the counters - * reset for the next frame. - * - * It starts counting bit 0 as the first bit received after a reset. - * - * This is very efficient as it only uses addition and subtraction for - * each bit sample. And uses one multiply and divide per update (per - * frame). - * - * This will permit a clock error of up to 500ppm. This allows up to - * 250ppm error for both transmitter and receiver clocks. This is - * less than one sample per frame when the sample rate is 48000 SPS. - * - * @inv current_index_ is in the interval [0, SAMPLES_PER_SYMBOL). - * @inv sample_index_ is in the interval [0, SAMPLES_PER_SYMBOL). - * @inv clock_ is in the interval [0.9995, 1.0005] - */ -template -class ClockRecovery + +template +struct ClockRecovery { - static constexpr size_t SAMPLES_PER_SYMBOL = SampleRate / SymbolRate; - static constexpr int8_t MAX_OFFSET = SAMPLES_PER_SYMBOL / 2; - static constexpr FloatType dx = 1.0 / SAMPLES_PER_SYMBOL; - static constexpr FloatType MAX_CLOCK = 1.0005; - static constexpr FloatType MIN_CLOCK = 0.9995; + static constexpr FloatType MAX_CLOCK_OFFSET = 0.0005; // 500ppm - std::array estimates_; - size_t sample_count_ = 0; - uint16_t frame_count_ = 0; - uint8_t sample_index_ = 0; - uint8_t prev_sample_index_ = 0; - uint8_t index_ = 0; - FloatType offset_ = 0.0; - FloatType clock_ = 1.0; - FloatType prev_sample_ = 0.0; + KalmanFilter kf_; + size_t count_ = 0; + int8_t sample_index_ = 0; + FloatType clock_estimate_ = 0.; + FloatType sample_estimate_ = 0.; + + void reset(FloatType z) + { + INFO("CR Reset"); + kf_.reset(z); + count_ = 0; + sample_index_ = z; + clock_estimate_ = 0.; + } + + void operator()(FloatType) + { + ++count_; + } + + static FloatType clock_limiter(FloatType offset) + { + return std::min(MAX_CLOCK_OFFSET, std::max(-MAX_CLOCK_OFFSET, offset)); + } + + bool update(uint8_t sw) + { + INFO("CR Update %d", int(sw)); + if (count_ < 480) + { + sample_index_ = sw; + return false; + } + + auto f = kf_.update(sw, count_); + + // Constrain sample index to [0..SamplesPerSymbol), wrapping if needed. + sample_estimate_ = f[0]; + sample_index_ = int8_t(round(sample_estimate_)); + sample_index_ = sample_index_ < 0 ? sample_index_ + SamplesPerSymbol : sample_index_; + sample_index_ = sample_index_ >= int8_t(SamplesPerSymbol) ? sample_index_ - SamplesPerSymbol : sample_index_; + clock_estimate_ = f[1]; + count_ = 0; + + return true; + } /** - * Find the sample index. + * This is used when no sync word is found. The sample index is updated + * based on the current clock estimate, the last known good sample + * estimate, and the number of samples processed. * - * There are @p SAMPLES_PER_INDEX bins. It is expected that half are - * positive values and half are negative. The positive and negative - * bins will be grouped together such that there is a single transition - * from positive values to negative values. - * - * The best bit position is always the position with the positive value - * at that transition point. It will be the bit index with the highest - * energy. - * - * @post sample_index_ contains the best sample point. + * The sample and clock estimates from the filter remain unchanged. */ - void update_sample_index_() + bool update() { - uint8_t index = 0; + INFO("CR Update"); + auto csw = std::fmod((sample_estimate_ + (1.0 + clock_estimate_) * count_), SamplesPerSymbol); + if (csw < 0.) csw += SamplesPerSymbol; + else if (csw >= SamplesPerSymbol) csw -= SamplesPerSymbol; - // Find falling edge. - bool is_positive = false; - for (size_t i = 0; i != SAMPLES_PER_SYMBOL; ++i) - { - FloatType phase = estimates_[i]; + // Constrain sample index to [0..SamplesPerSymbol), wrapping if needed. + sample_index_ = int8_t(round(csw)); + sample_index_ = sample_index_ < 0 ? sample_index_ + SamplesPerSymbol : sample_index_; + sample_index_ = sample_index_ >= int8_t(SamplesPerSymbol) ? sample_index_ - SamplesPerSymbol : sample_index_; - if (!is_positive && phase > 0) - { - is_positive = true; - } - else if (is_positive && phase < 0) - { - index = i; - break; - } - } - - sample_index_ = index == 0 ? SAMPLES_PER_SYMBOL - 1 : index - 1; - } - - /** - * Compute the drift in sample points from the last update. - * - * This should never be greater than one. - */ - FloatType calc_offset_() - { - int8_t offset = sample_index_ - prev_sample_index_; - - // When in spec, the clock should drift by less than 1 sample per frame. - if (__builtin_expect(offset >= MAX_OFFSET, 0)) - { - offset -= SAMPLES_PER_SYMBOL; - } - else if (__builtin_expect(offset <= -MAX_OFFSET, 0)) - { - offset += SAMPLES_PER_SYMBOL; - } - - return offset; - } - - void update_clock_() - { - // update_sample_index_() must be called first. - - if (__builtin_expect((frame_count_ == 0), 0)) - { - prev_sample_index_ = sample_index_; - offset_ = 0.0; - clock_ = 1.0; - return; - } - - offset_ += calc_offset_(); - prev_sample_index_ = sample_index_; - clock_ = 1.0 + (offset_ / (frame_count_ * sample_count_)); - clock_ = std::min(MAX_CLOCK, std::max(MIN_CLOCK, clock_)); - } - -public: - ClockRecovery() - { - estimates_.fill(0); - } - - /** - * Update clock recovery with the given sample. This will advance the - * current sample index by 1. - */ - void operator()(FloatType sample) - { - FloatType dy = (sample - prev_sample_); - - if (sample + prev_sample_ < 0) - { - // Invert the phase estimate when sample midpoint is less than 0. - dy = -dy; - } - - prev_sample_ = sample; - - estimates_[index_] += dy; - index_ += 1; - if (index_ == SAMPLES_PER_SYMBOL) - { - index_ = 0; - } - sample_count_ += 1; - } - - /** - * Reset the state of the clock recovery system. This should be called - * when a new transmission is detected. - */ - void reset() - { - sample_count_ = 0; - frame_count_ = 0; - index_ = 0; - sample_index_ = 0; - estimates_.fill(0); - clock_ = 1.0; - } - - /** - * Return the current sample index. This will always be in the range of - * [0..SAMPLES_PER_SYMBOL). - */ - uint8_t current_index() const - { - return index_; + return true; } /** * Return the estimated sample clock increment based on the last update. - * + * * The value is only valid after samples have been collected and update() * has been called. */ FloatType clock_estimate() const { - return clock_; + return clock_estimate_; } /** * Return the estimated "best sample index" based on the last update. - * + * * The value is only valid after samples have been collected and update() * has been called. */ @@ -207,35 +111,6 @@ public: { return sample_index_; } - - /** - * Update the sample index and clock estimates, and reset the state for - * the next frame of data. - * - * @pre index_ = 0 - * @pre sample_count_ > 0 - * - * After this is called, sample_index() and clock_estimate() will have - * valid, updated results. - * - * The more samples between calls to update, the more accurate the - * estimates will be. - * - * @return true if the preconditions are met and the update has been - * performed, otherwise false. - */ - bool update() - { - assert(sample_count_ != 0 && index_ == 0); - - update_sample_index_(); - update_clock_(); - - frame_count_ = std::min(0x1000, 1 + frame_count_); - sample_count_ = 0; - estimates_.fill(0); - return true; - } }; }} // mobilinkd::m17 diff --git a/TNC/M17Demodulator.cpp b/TNC/M17Demodulator.cpp index 02d82a9..a6670af 100644 --- a/TNC/M17Demodulator.cpp +++ b/TNC/M17Demodulator.cpp @@ -1,6 +1,9 @@ // Copyright 2020-2021 Rob Riggs // All rights reserved. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wvolatile" + #include "AudioLevel.hpp" #include "M17Demodulator.h" #include "Util.h" @@ -8,6 +11,7 @@ #include "main.h" #include "stm32l4xx_hal.h" +#pragma GCC diagnostic pop #include #include @@ -21,6 +25,22 @@ namespace mobilinkd { namespace tnc { static float scale = 1.f / 2560.f; +static float dc_block(float x) +{ +#if 1 + return x; +#else + static float xm1 = 0.0; + static float ym1 = 0.0; + + float y = x - xm1 + 0.9999 * ym1; + xm1 = x; + ym1 = y; + + return y; +#endif +} + void M17Demodulator::start() { SysClock72(); @@ -28,11 +48,11 @@ void M17Demodulator::start() HAL_RCCEx_DisableLSCO(); #endif - demod_filter.init(m17::rrc_taps_f15); + demod_filter.init(m17::rrc_taps_f); passall(kiss::settings().options & KISS_OPTION_PASSALL); polarity = kiss::settings().rx_rev_polarity() ? -1 : 1; scale = 1.f / 2560.f * polarity; - audio::virtual_ground = (VREF + 1) / 2; + // audio::virtual_ground = (VREF + 1) / 2; hadc1.Init.OversamplingMode = ENABLE; if (HAL_ADC_Init(&hadc1) != HAL_OK) @@ -67,25 +87,22 @@ void M17Demodulator::update_values(uint8_t index) void M17Demodulator::dcd_on() { // Data carrier newly detected. - INFO("dcd = %d", int(dcd.level() * 100)); + INFO("dcd = %d", int(dcd.level() * 1000)); dcd_ = true; sync_count = 0; dev.reset(); framer.reset(); decoder.reset(); missing_sync_count = 0; -// dcd_indicator.off(); } void M17Demodulator::dcd_off() { // Just lost data carrier. - INFO("dcd = %d", int(dcd.level() * 100)); + INFO("dcd = %d", int(dcd.level() * 1000)); dcd_ = false; demodState = DemodState::UNLOCKED; -// dcd_indicator.on(); adc_timing_adjust = 0; - prev_clock_estimate = 1.; } void M17Demodulator::initialize(const q15_t* input) @@ -97,14 +114,14 @@ void M17Demodulator::initialize(const q15_t* input) auto filtered = demod_filter(demod_buffer.data()); for (size_t i = 0; i != ADC_BLOCK_SIZE; ++i) { - auto filtered_sample = filtered[i]; + auto filtered_sample = dc_block(filtered[i]); correlator.sample(filtered_sample); } } void M17Demodulator::update_dcd(const q15_t* input) { - static constexpr float inv = 1.0 / 2048.0; + static constexpr float inv = 1.0 / 16384.0; if (!dcd_ && dcd.dcd()) { @@ -200,7 +217,7 @@ void M17Demodulator::do_lsf_sync() // INFO("PSync = %d", int(sync_triggered)); if (sync_triggered > 0.1) { - ITM_SendChar('.'); + sync_count += 1; return; } sync_triggered = lsf_sync.triggered(correlator); @@ -235,9 +252,17 @@ void M17Demodulator::do_lsf_sync() } else if (++missing_sync_count > 192) { - demodState = DemodState::UNLOCKED; - INFO("l unlock %d", int(missing_sync_count)); - missing_sync_count = 0; + if (sync_count >= 10) { + missing_sync_count = 0; + need_clock_update_ = true; + INFO("long preamble"); + } else { + sync_count = 0; + demodState = DemodState::UNLOCKED; + dcd.unlock(); + INFO("l unlock %d", int(missing_sync_count)); + missing_sync_count = 0; + } } else { @@ -255,36 +280,62 @@ void M17Demodulator::do_lsf_sync() [[gnu::noinline]] void M17Demodulator::do_stream_sync() { + static bool eot_flag = false; + + sync_count += 1; + if (sync_count < MIN_SYNC_INDEX) { + return; + } + + if (eot_sync.triggered(correlator) > 0.1) { + sync_word_type = M17FrameDecoder::SyncWordType::STREAM; + demodState = DemodState::FRAME; + eot_flag = true; + INFO("EOT"); + return; + } + uint8_t sync_index = lsf_sync(correlator); int8_t sync_updated = lsf_sync.updated(); - sync_count += 1; if (sync_updated < 0) { missing_sync_count = 0; - if (sync_count > 70) + update_values(sync_index); + sync_word_type = M17FrameDecoder::SyncWordType::STREAM; + demodState = DemodState::FRAME; + INFO("s sync %d", int(sync_count)); + eot_flag = false; + } + else if (sync_count > MAX_SYNC_INDEX) + { + // update_values(sync_index); + if (ber >= 0 && ber < 80) { - update_values(sync_index); + // Sync word missed but we are still decoding a stream reasonably well. sync_word_type = M17FrameDecoder::SyncWordType::STREAM; demodState = DemodState::FRAME; - INFO("s sync %d", int(sync_index)); + if (!missing_sync_count) missing_sync_count = 1; + INFO("s bunsync"); } - return; - } - else if (sync_count > 87) - { - update_values(sync_index); - missing_sync_count += 1; - if (missing_sync_count < MAX_MISSING_SYNC) + else if (eot_flag) { + demodState = DemodState::UNLOCKED; + dcd.unlock(); + INFO("s eot"); + } + else if (missing_sync_count < MAX_MISSING_SYNC) { + missing_sync_count += 1; sync_word_type = M17FrameDecoder::SyncWordType::STREAM; demodState = DemodState::FRAME; INFO("s unsync %d", int(missing_sync_count)); } else { - demodState = DemodState::LSF_SYNC; + demodState = DemodState::UNLOCKED; + dcd.unlock(); INFO("s unlock"); } + eot_flag = false; } } @@ -295,10 +346,15 @@ void M17Demodulator::do_stream_sync() [[gnu::noinline]] void M17Demodulator::do_packet_sync() { + sync_count += 1; + if (sync_count < MIN_SYNC_INDEX) { + return; + } + auto sync_index = packet_sync(correlator); auto sync_updated = packet_sync.updated(); - sync_count += 1; - if (sync_count > 70 && sync_updated) + + if (sync_updated) { missing_sync_count = 0; update_values(sync_index); @@ -307,7 +363,7 @@ void M17Demodulator::do_packet_sync() INFO("k sync"); return; } - else if (sync_count > 87) + else if (sync_count > MAX_SYNC_INDEX) { missing_sync_count += 1; if (missing_sync_count < MAX_MISSING_SYNC) @@ -316,9 +372,16 @@ void M17Demodulator::do_packet_sync() demodState = DemodState::FRAME; INFO("k unsync"); } + else if (ber >= 0 && ber < 80) + { + sync_word_type = M17FrameDecoder::SyncWordType::PACKET; + demodState = DemodState::FRAME; + INFO("k bunsync"); + } else { demodState = DemodState::UNLOCKED; + dcd.unlock(); INFO("k unlock"); } } @@ -330,10 +393,15 @@ void M17Demodulator::do_packet_sync() [[gnu::noinline]] void M17Demodulator::do_bert_sync() { + sync_count += 1; + if (sync_count < MIN_SYNC_INDEX) { + return; + } + auto sync_index = packet_sync(correlator); auto sync_updated = packet_sync.updated(); - sync_count += 1; - if (sync_count > 70 && sync_updated < 0) + + if (sync_updated < 0) { missing_sync_count = 0; update_values(sync_index); @@ -341,7 +409,7 @@ void M17Demodulator::do_bert_sync() INFO("b sync"); demodState = DemodState::FRAME; } - else if (sync_count > 87) + else if (sync_count > MAX_SYNC_INDEX) { missing_sync_count += 1; if (missing_sync_count < MAX_MISSING_SYNC) @@ -350,9 +418,16 @@ void M17Demodulator::do_bert_sync() demodState = DemodState::FRAME; INFO("b unsync"); } + else if (ber >= 0 && ber < 80) + { + sync_word_type = M17FrameDecoder::SyncWordType::BERT; + demodState = DemodState::FRAME; + INFO("b bunsync"); + } else { demodState = DemodState::UNLOCKED; + dcd.unlock(); INFO("b unlock"); } } @@ -375,11 +450,11 @@ void M17Demodulator::do_frame(float filtered_sample, hdlc::IoFrame*& frame_resul std::copy(tmp, tmp + len, buffer.begin()); auto valid = decoder(sync_word_type, buffer, frame_result, ber); - INFO("demod: %d, dt: %7d, evma: %5d, dev: %5d, freq: %5d, index: %d, %d, %d ber: %d", + INFO("demod: %d, dt: %7d, evma: %5d, dev: %5d, freq: %5d, dcd: %d, index: %d, %d ber: %d", int(decoder.state()), int(clock_recovery.clock_estimate() * 1000000), int(dev.error() * 1000), int(dev.deviation() * 1000), - int(dev.offset() * 1000), - int(sample_index), int(clock_recovery.sample_index()), int(sync_sample_index), + int(dev.offset() * 1000), int(dcd.level() * 1000), + int(clock_recovery.sample_index()), int(sync_sample_index), ber); switch (decoder.state()) @@ -412,7 +487,7 @@ void M17Demodulator::do_frame(float filtered_sample, hdlc::IoFrame*& frame_resul } break; case M17FrameDecoder::DecodeResult::EOS: - demodState = DemodState::LSF_SYNC; + // demodState = DemodState::LSF_SYNC; INFO("EOS"); break; case M17FrameDecoder::DecodeResult::OK: @@ -460,29 +535,28 @@ hdlc::IoFrame* M17Demodulator::operator()(const q15_t* input) for (size_t i = 0; i != ADC_BLOCK_SIZE; ++i) { - auto filtered_sample = filtered[i]; + auto filtered_sample = dc_block(filtered[i]); correlator.sample(filtered_sample); if (correlator.index() == 0) { if (need_clock_reset_) { - clock_recovery.reset(); + clock_recovery.reset(sync_sample_index); need_clock_reset_ = false; sample_index = sync_sample_index; adc_timing_adjust = 0; } else if (need_clock_update_) // must avoid update immediately after reset. { - clock_recovery.update(); - uint8_t clock_index = clock_recovery.sample_index(); - uint8_t clock_diff = std::abs(sample_index - clock_index); - uint8_t sync_diff = std::abs(sample_index - sync_sample_index); - bool clock_diff_ok = clock_diff <= 1 || clock_diff == 9; - bool sync_diff_ok = sync_diff <= 1 || sync_diff == 9; - if (clock_diff_ok) sample_index = clock_index; - else if (sync_diff_ok) sample_index = sync_sample_index; - // else unchanged. + if (!missing_sync_count) { + // Have a real sync word match. + clock_recovery.update(sync_sample_index); + } else if (decoder.state() != M17FrameDecoder::State::LSF) { + // Not waiting on clock recovery. + clock_recovery.update(); + } + sample_index = clock_recovery.sample_index(); need_clock_update_ = false; } } diff --git a/TNC/M17Demodulator.h b/TNC/M17Demodulator.h index 41d614c..a158795 100644 --- a/TNC/M17Demodulator.h +++ b/TNC/M17Demodulator.h @@ -44,22 +44,25 @@ struct M17Demodulator : IDemodulator static constexpr float sample_rate = SAMPLE_RATE; static constexpr float symbol_rate = SYMBOL_RATE; - static constexpr uint8_t MAX_MISSING_SYNC = 5; + static constexpr uint8_t MAX_MISSING_SYNC = 10; + static constexpr uint8_t MIN_SYNC_INDEX = 79; + static constexpr uint8_t MAX_SYNC_INDEX = 88; - using audio_filter_t = FirFilter; + using audio_filter_t = FirFilter; using sync_word_t = m17::SyncWord; enum class DemodState { UNLOCKED, LSF_SYNC, STREAM_SYNC, PACKET_SYNC, BERT_SYNC, FRAME }; audio_filter_t demod_filter; std::array demod_buffer; - m17::DataCarrierDetect dcd{2500, 4000, 1.0, 10.0}; - m17::ClockRecovery clock_recovery; + m17::DataCarrierDetect dcd{2400, 4800, 0.8f, 10.0f}; + m17::ClockRecovery clock_recovery; m17::Correlator correlator; sync_word_t preamble_sync{{+3,-3,+3,-3,+3,-3,+3,-3}, 29.f}; sync_word_t lsf_sync{{+3,+3,+3,+3,-3,-3,+3,-3}, 31.f, -31.f}; sync_word_t packet_sync{{3,-3,3,3,-3,-3,-3,-3}, 31.f, -31.f}; + sync_word_t eot_sync{{+3,+3,+3,+3,+3,+3,-3,+3}, 31.f}; m17::FreqDevEstimator dev; @@ -82,7 +85,6 @@ struct M17Demodulator : IDemodulator uint16_t missing_sync_count = 0; uint8_t sync_sample_index = 0; int16_t adc_timing_adjust = 0; - float prev_clock_estimate = 1.; virtual ~M17Demodulator() {} diff --git a/TNC/M17FrameDecoder.h b/TNC/M17FrameDecoder.h index 65151b3..913ff75 100644 --- a/TNC/M17FrameDecoder.h +++ b/TNC/M17FrameDecoder.h @@ -442,12 +442,14 @@ struct M17FrameDecoder if (ber < 128) stream->push_back(255 - ber * 2); else stream->push_back(0); +#if 0 // Using EOT sync word now if ((ber < 60) && (stream_segment[0] & 0x80)) { INFO("EOS"); state_ = State::LSF; result = DecodeResult::EOS; } +#endif // Bogus CRC bytes to be dropped. stream->push_back(0);