From d05679630b13cf0eea27a8d57448ed09b6c410af Mon Sep 17 00:00:00 2001 From: Rob Riggs Date: Wed, 30 Jun 2021 20:16:15 -0500 Subject: [PATCH] Simplify FIR filter code. --- TNC/Afsk1200Demodulator.hpp | 6 ++-- TNC/AfskDemodulator.cpp | 6 +++- TNC/AfskDemodulator.hpp | 1 + TNC/FirFilter.cpp | 4 +-- TNC/FirFilter.hpp | 64 ++++++++++++------------------------- TNC/M17Demodulator.cpp | 21 +++++++++--- TNC/M17Demodulator.h | 1 + 7 files changed, 49 insertions(+), 54 deletions(-) diff --git a/TNC/Afsk1200Demodulator.hpp b/TNC/Afsk1200Demodulator.hpp index 30c601f..1b64121 100644 --- a/TNC/Afsk1200Demodulator.hpp +++ b/TNC/Afsk1200Demodulator.hpp @@ -68,9 +68,9 @@ struct Afsk1200Demodulator : IDemodulator // rx_twist is 6dB for discriminator input and 0db for de-emphasized input. auto twist = kiss::settings().rx_twist; - filter_1.init(*filter::fir::AfskFilters[twist + 3]); - filter_2.init(*filter::fir::AfskFilters[twist + 6]); - filter_3.init(*filter::fir::AfskFilters[twist + 9]); + filter_1.init(filter::fir::AfskFilters[twist + 3]->taps); + filter_2.init(filter::fir::AfskFilters[twist + 6]->taps); + filter_3.init(filter::fir::AfskFilters[twist + 9]->taps); last_fcs = 0; last_counter = 0; diff --git a/TNC/AfskDemodulator.cpp b/TNC/AfskDemodulator.cpp index d4dba76..289e393 100644 --- a/TNC/AfskDemodulator.cpp +++ b/TNC/AfskDemodulator.cpp @@ -9,7 +9,11 @@ hdlc::IoFrame* Demodulator::operator()(q15_t* samples, size_t len) { hdlc::IoFrame* result = 0; - float* fa = audio_filter_(samples); + for (size_t i = 0; i != len; i++) { + audio_filter_input_buffer[i] = float(samples[i]); + } + + float* fa = audio_filter_(audio_filter_input_buffer); for (size_t i = 0; i != len; i++) { buffer_[i] = int16_t(fa[i]); diff --git a/TNC/AfskDemodulator.hpp b/TNC/AfskDemodulator.hpp index 73e442a..26b4625 100644 --- a/TNC/AfskDemodulator.hpp +++ b/TNC/AfskDemodulator.hpp @@ -46,6 +46,7 @@ struct Demodulator { size_t sample_rate_; emphasis_filter_type& audio_filter_; + float audio_filter_input_buffer[ADC_BUFFER_SIZE]; libafsk::FixedDelayLine<40> delay_line_; DPLL pll_; Q15FirFilter lpf_filter_; diff --git a/TNC/FirFilter.cpp b/TNC/FirFilter.cpp index 8c39404..b7c1f81 100644 --- a/TNC/FirFilter.cpp +++ b/TNC/FirFilter.cpp @@ -5,7 +5,7 @@ namespace mobilinkd { namespace tnc { namespace fir_detail { -float filter_input[MAX_BLOCK_SIZE]; -float filter_output[MAX_BLOCK_SIZE]; +//float filter_input[MAX_BLOCK_SIZE]; +//float filter_output[MAX_BLOCK_SIZE]; }}} // mobilinkd::tnc::fir_detail diff --git a/TNC/FirFilter.hpp b/TNC/FirFilter.hpp index 506eadb..eead3da 100644 --- a/TNC/FirFilter.hpp +++ b/TNC/FirFilter.hpp @@ -7,7 +7,7 @@ #include "arm_math.h" -#include +#include #include namespace mobilinkd { namespace tnc { @@ -27,73 +27,49 @@ struct FirCoefficients { {} }; -constexpr size_t MAX_BLOCK_SIZE = 192; - -namespace fir_detail { -extern float filter_input[MAX_BLOCK_SIZE]; -extern float filter_output[MAX_BLOCK_SIZE]; -} // fir_detail - template struct FirFilter { - static_assert(BLOCK_SIZE <= MAX_BLOCK_SIZE, "You must increase MAX_BLOCK_SIZE" ); - const float* filter_taps; float filter_state[BLOCK_SIZE + FILTER_SIZE - 1]; - float vgnd_; - size_t filter_size; + float filter_output[BLOCK_SIZE]; arm_fir_instance_f32 instance; FirFilter() - : filter_taps(0), filter_state() - , vgnd_(0.0f) - , filter_size(FILTER_SIZE), instance() {} - FirFilter(const float* taps, size_t len = FILTER_SIZE) - : filter_taps(taps), filter_state() - , filter_size(len), instance() + template + FirFilter(std::array const& taps) { - init(taps, len); + init(taps); } - void init(const float* taps, size_t len = FILTER_SIZE) + FirFilter(const float* taps) { - vgnd_ = float(audio::virtual_ground); - filter_size = len; - filter_taps = taps; - arm_fir_init_f32(&instance, filter_size, (float32_t*)filter_taps, + init(taps); + } + + template + void init(std::array const& taps) + { + arm_fir_init_f32(&instance, FILTER_SIZE, (float*)taps.data(), filter_state, BLOCK_SIZE); } - void init(const FirCoefficients& filt) + void init(const float* taps) { - vgnd_ = float(audio::virtual_ground); - filter_size = filt.size; - filter_taps = filt.taps; - arm_fir_init_f32(&instance, filter_size, (float32_t*)filter_taps, + arm_fir_init_f32(&instance, FILTER_SIZE, (float*)taps, filter_state, BLOCK_SIZE); } - // ADC input - float* operator()(const int16_t* input, float scale = 1.f) // __attribute__((section(".bss2"))) + float* operator()(float* input) { - for (size_t i = 0; i != BLOCK_SIZE; i++) { - fir_detail::filter_input[i] = float(input[i]) * scale; - } - arm_fir_f32(&instance, fir_detail::filter_input, fir_detail::filter_output, BLOCK_SIZE); - return fir_detail::filter_output; - } - - float* operator()(float* input) // __attribute__((section(".bss2"))) - { - arm_fir_f32(&instance, input, fir_detail::filter_output, BLOCK_SIZE); - return fir_detail::filter_output; + arm_fir_f32(&instance, input, filter_output, BLOCK_SIZE); + return filter_output; } float operator()(float input) // __attribute__((section(".bss2"))) { - arm_fir_f32(&instance, &input, fir_detail::filter_output, 1); - return *fir_detail::filter_output; + arm_fir_f32(&instance, &input, filter_output, 1); + return filter_output[0]; } }; diff --git a/TNC/M17Demodulator.cpp b/TNC/M17Demodulator.cpp index f017779..2a3b21c 100644 --- a/TNC/M17Demodulator.cpp +++ b/TNC/M17Demodulator.cpp @@ -24,7 +24,7 @@ void M17Demodulator::start() SysClock72(); HAL_RCCEx_DisableLSCO(); - demod_filter.init(m17::rrc_taps_f15.data()); + demod_filter.init(m17::rrc_taps_f15); passall(kiss::settings().options & KISS_OPTION_PASSALL); polarity = kiss::settings().rx_rev_polarity() ? -1 : 1; audio::virtual_ground = (VREF + 1) / 2; @@ -85,7 +85,14 @@ void M17Demodulator::dcd_off() void M17Demodulator::initialize(const q15_t* input) { - auto filtered = demod_filter(input, 1.f / 2560.f); + + static constexpr float scale = 1.f / 2560.f; + + for (size_t i = 0; i != ADC_BLOCK_SIZE; i++) { + demod_buffer[i] = float(input[i]) * scale; + } + + auto filtered = demod_filter(demod_buffer.data()); for (size_t i = 0; i != ADC_BLOCK_SIZE; ++i) { auto filtered_sample = filtered[i]; @@ -306,7 +313,7 @@ void M17Demodulator::do_frame(float filtered_sample, hdlc::IoFrame*& frame_resul float sample = filtered_sample - dev.offset(); sample *= idev; - auto n = llr(sample); + auto n = mobilinkd::llr(sample); int8_t* tmp; auto len = framer(n, &tmp); if (len != 0) @@ -388,7 +395,13 @@ hdlc::IoFrame* M17Demodulator::operator()(const q15_t* input) // Do adc_micro_adjustment() here? // adc_micro_adjustment(); - auto filtered = demod_filter(input, 1.f / 2560.f); + static constexpr float scale = 1.f / 2560.f; + + for (size_t i = 0; i != ADC_BLOCK_SIZE; i++) { + demod_buffer[i] = float(input[i]) * scale; + } + + auto filtered = demod_filter(demod_buffer.data()); // getModulator().loopback(filtered); for (size_t i = 0; i != ADC_BLOCK_SIZE; ++i) diff --git a/TNC/M17Demodulator.h b/TNC/M17Demodulator.h index 85b43dd..70a6800 100644 --- a/TNC/M17Demodulator.h +++ b/TNC/M17Demodulator.h @@ -52,6 +52,7 @@ struct M17Demodulator : IDemodulator enum class DemodState { UNLOCKED, LSF_SYNC, STREAM_SYNC, PACKET_SYNC, FRAME }; audio_filter_t demod_filter; + std::array demod_buffer; m17::DataCarrierDetect dcd{2500, 4000, 1.0, 10.0}; m17::ClockRecovery clock_recovery;