diff --git a/firmware/baseband/clock_recovery.cpp b/firmware/baseband/clock_recovery.cpp index 7fb69dea..17cb5c70 100644 --- a/firmware/baseband/clock_recovery.cpp +++ b/firmware/baseband/clock_recovery.cpp @@ -20,12 +20,3 @@ */ #include "clock_recovery.hpp" - -void ClockRecovery::configure( - const uint32_t symbol_rate, - const uint32_t sampling_rate -) { - phase_increment = phase_increment_u32( - fractional_symbol_rate(symbol_rate, sampling_rate) - ); -} diff --git a/firmware/baseband/clock_recovery.hpp b/firmware/baseband/clock_recovery.hpp index 4224032d..69723375 100644 --- a/firmware/baseband/clock_recovery.hpp +++ b/firmware/baseband/clock_recovery.hpp @@ -22,72 +22,125 @@ #ifndef __CLOCK_RECOVERY_H__ #define __CLOCK_RECOVERY_H__ -#include +#include +#include +#include -class ClockRecovery { +#include "linear_resampler.hpp" + +namespace clock_recovery { + +class GardnerTimingErrorDetector { public: - void configure( - const uint32_t symbol_rate, - const uint32_t sampling_rate - ); + static constexpr size_t samples_per_symbol { 2 }; + /* + Expects retimed samples at a rate of twice the expected symbol rate. + Calculates timing error, sends symbol and error to handler. + */ template - void execute( + void operator()( const float in, SymbolHandler symbol_handler ) { - const bool phase_0 = (phase_last >> 31) & (!(phase >> 31)); - const bool phase_180 = (!(phase_last >> 31)) & (phase >> 31); + /* NOTE: Algorithm is sensitive to input magnitude. Timing error value + * will scale proportionally. Best practice is to use error sign only. + */ + t[2] = t[1]; + t[1] = t[0]; + t[0] = in; - if( phase_0 || phase_180 ) { - t2 = t1; - t1 = t0; - - const uint32_t phase_boundary = phase_180 ? (1U << 31) : 0; - const float alpha = (phase_boundary - phase_last) / float(phase_increment + phase_adjustment); - const float t = last_sample + alpha * (in - last_sample); - t0 = t; + if( symbol_phase == 0 ) { + const auto symbol = t[0]; + const float lateness = (t[0] - t[2]) * t[1]; + symbol_handler(symbol, lateness); } - if( phase_0 ) { - symbol_handler(t0); - - const float error = (t0 - t2) * t1; - // + error == late == decrease/slow phase - // - error == early == increase/fast phase - - error_filtered = 0.75f * error_filtered + 0.25f * error; - - // Correct phase (don't change frequency!) - phase_adjustment = -phase_increment * error_filtered / 200.0f; - } - - phase_last = phase; - phase += phase_increment + phase_adjustment; - last_sample = in; + symbol_phase = (symbol_phase + 1) % samples_per_symbol; } private: - uint32_t phase { 0 }; - uint32_t phase_last { 0 }; - uint32_t phase_adjustment { 0 }; - uint32_t phase_increment { 0 }; - float last_sample { 0 }; - float t0 { 0 }; - float t1 { 0 }; - float t2 { 0 }; - float error_filtered { 0 }; + std::array t { { 0.0f, 0.0f, 0.0f } }; + size_t symbol_phase { 0 }; +}; - static constexpr float fractional_symbol_rate( - const uint32_t symbol_rate, - const uint32_t sampling_rate +class LinearErrorFilter { +public: + float operator()( + const float error ) { - return float(symbol_rate) / float(sampling_rate); + error_filtered = filter_alpha * error_filtered + (1.0f - filter_alpha) * error; + return error_filtered * error_weight; } - static constexpr uint32_t phase_increment_u32(const float fractional_symbol_rate) { - return 4294967296.0f * fractional_symbol_rate; +private: + float filter_alpha { 0.95f }; + float error_filtered { 0.0f }; + float error_weight { 0.5f }; +}; + +class FixedErrorFilter { +public: + float operator()( + const float lateness + ) { + return (lateness < 0.0f) ? weight : -weight; + } + +private: + float weight { 1.0f / 16.0f }; +}; + +class ClockRecovery { +public: + ClockRecovery( + const float sampling_rate, + const float symbol_rate, + std::function symbol_handler + ) : resampler(sampling_rate, symbol_rate * timing_error_detector.samples_per_symbol), + symbol_handler { symbol_handler } + { + } + + void configure( + const float sampling_rate, + const float symbol_rate + ) { + resampler.configure(sampling_rate, symbol_rate * timing_error_detector.samples_per_symbol); + } + + void operator()( + const float baseband_sample + ) { + resampler(baseband_sample, + [this](const float interpolated_sample) { + this->resampler_callback(interpolated_sample); + } + ); + } + +private: + dsp::interpolation::LinearResampler resampler; + GardnerTimingErrorDetector timing_error_detector; + FixedErrorFilter error_filter; + std::function symbol_handler; + + void resampler_callback(const float interpolated_sample) { + timing_error_detector(interpolated_sample, + [this](const float symbol, const float lateness) { + this->symbol_callback(symbol, lateness); + } + ); + } + + void symbol_callback(const float symbol, const float lateness) { + symbol_handler(symbol); + + const float adjustment = error_filter(lateness); + resampler.advance(adjustment); } }; +} /* namespace clock_recovery */ + #endif/*__CLOCK_RECOVERY_H__*/ diff --git a/firmware/baseband/proc_fsk.cpp b/firmware/baseband/proc_fsk.cpp index a4ccdbdc..f2b1ef06 100644 --- a/firmware/baseband/proc_fsk.cpp +++ b/firmware/baseband/proc_fsk.cpp @@ -43,8 +43,8 @@ FSKProcessor::~FSKProcessor() { } void FSKProcessor::configure(const FSKConfiguration new_configuration) { - demod.configure(76800, 2 * new_configuration.symbol_rate); - clock_recovery.configure(new_configuration.symbol_rate, 76800); + demod.configure(sampling_rate, 2 * new_configuration.symbol_rate); + clock_recovery.configure(sampling_rate / 4, new_configuration.symbol_rate); access_code_correlator.configure( new_configuration.access_code, new_configuration.access_code_length, @@ -78,12 +78,6 @@ void FSKProcessor::execute(buffer_c8_t buffer) { decimator_out.sampling_rate * channel_filter_taps.stop_frequency_normalized ); - const auto symbol_handler_fn = [this](const float value) { - const uint_fast8_t symbol = (value >= 0.0f) ? 1 : 0; - const bool access_code_found = this->access_code_correlator.execute(symbol); - this->consume_symbol(symbol, access_code_found); - }; - // 76.8k const buffer_s16_t work_demod_buffer { @@ -93,16 +87,16 @@ void FSKProcessor::execute(buffer_c8_t buffer) { auto demodulated = demod.execute(channel, work_demod_buffer); - i2s::i2s0::tx_mute(); - - for(size_t i=0; i