From e5b5daba85faa741ebe2b89669d467eb2e2551fa Mon Sep 17 00:00:00 2001 From: Silvano Seva Date: Sat, 2 Apr 2022 14:57:15 +0200 Subject: [PATCH] Moved state variables of PWM compensator and DC removal filter outside the function bodies to allow keeping their consistency among function calls --- openrtx/include/core/dsp.h | 26 ++++++- openrtx/include/protocols/M17/M17Modulator.h | 5 ++ openrtx/src/core/audio_codec.c | 5 +- openrtx/src/core/dsp.cpp | 73 +++++++++++++------- openrtx/src/protocols/M17/M17Modulator.cpp | 9 ++- 5 files changed, 88 insertions(+), 30 deletions(-) diff --git a/openrtx/include/core/dsp.h b/openrtx/include/core/dsp.h index 2098ace7..ef872923 100644 --- a/openrtx/include/core/dsp.h +++ b/openrtx/include/core/dsp.h @@ -39,23 +39,45 @@ typedef int16_t audio_sample_t; extern "C" { #endif +/** + * Data structure holding the internal state of a filter. + */ +typedef struct +{ + float u[3]; // input values u(k), u(k-1), u(k-2) + float y[3]; // output values y(k), y(k-1), y(k-2) + bool initialised; // state variables initialised +} +filter_state_t; + + +/** + * Reset the filter state variables. + * + * @param state: pointer to the data structure containing the filter state. + */ +void dsp_resetFilterState(filter_state_t *state); + /** * Compensate for the filtering applied by the PWM output over the modulated * signal. The buffer is be processed in place to save memory. * + * @param state: pointer to the data structure containing the filter state. * @param buffer: the buffer to be used as both source and destination. * @param length: the length of the input buffer. */ -void dsp_pwmCompensate(audio_sample_t *buffer, size_t length); +void dsp_pwmCompensate(filter_state_t *state, audio_sample_t *buffer, + size_t length); /** * Remove the DC offset from a collection of audio samples, processing data * in-place. * + * @param state: pointer to the data structure containing the filter state. * @param buffer: buffer containing the audio samples. * @param length: number of samples contained in the buffer. */ -void dsp_dcRemoval(audio_sample_t *buffer, size_t length); +void dsp_dcRemoval(filter_state_t *state, audio_sample_t *buffer, size_t length); /* * Inverts the phase of the audio buffer passed as paramenter. diff --git a/openrtx/include/protocols/M17/M17Modulator.h b/openrtx/include/protocols/M17/M17Modulator.h index 00e96f8c..8c932018 100644 --- a/openrtx/include/protocols/M17/M17Modulator.h +++ b/openrtx/include/protocols/M17/M17Modulator.h @@ -28,6 +28,7 @@ #include #include #include +#include namespace M17 { @@ -125,6 +126,10 @@ private: streamId outStream; ///< Baseband output stream ID. bool txRunning; ///< Transmission running. bool stopTx; ///< Stop transmission request. + + #if defined(PLATFORM_MD3x0) || defined(PLATFORM_MDUV3x0) + filter_state_t pwmFilterState; + #endif }; } /* M17 */ diff --git a/openrtx/src/core/audio_codec.c b/openrtx/src/core/audio_codec.c index 6423c3da..532e550b 100644 --- a/openrtx/src/core/audio_codec.c +++ b/openrtx/src/core/audio_codec.c @@ -174,6 +174,9 @@ static void *encodeFunc(void *arg) { (void) arg; + filter_state_t dcrState; + dsp_resetFilterState(&dcrState); + codec2 = codec2_create(CODEC2_MODE_3200); while(running) @@ -187,7 +190,7 @@ static void *encodeFunc(void *arg) for(size_t i = 0; i < audio.len; i++) audio.data[i] *= micGainPre; // DC removal - dsp_dcRemoval(audio.data, audio.len); + dsp_dcRemoval(&dcrState, audio.data, audio.len); // Post-amplification stage for(size_t i = 0; i < audio.len; i++) audio.data[i] *= micGainPost; diff --git a/openrtx/src/core/dsp.cpp b/openrtx/src/core/dsp.cpp index 8249bcda..d261bbf2 100644 --- a/openrtx/src/core/dsp.cpp +++ b/openrtx/src/core/dsp.cpp @@ -20,15 +20,22 @@ #include -void dsp_pwmCompensate(audio_sample_t *buffer, size_t length) +void dsp_resetFilterState(filter_state_t *state) { - float u = 0.0f; // Current input value - float y = 0.0f; // Current output value - float uo = 0.0f; // u(k-1) - float uoo = 0.0f; // u(k-2) - float yo = 0.0f; // y(k-1) - float yoo = 0.0f; // y(k-2) + state->u[0] = 0.0f; + state->u[1] = 0.0f; + state->u[2] = 0.0f; + state->y[0] = 0.0f; + state->y[1] = 0.0f; + state->y[2] = 0.0f; + + state->initialised = false; +} + +void dsp_pwmCompensate(filter_state_t *state, audio_sample_t *buffer, + size_t length) +{ static constexpr float a = 4982680082321166792352.0f; static constexpr float b = -6330013275146484168000.0f; static constexpr float c = 1871109008789062500000.0f; @@ -38,22 +45,32 @@ void dsp_pwmCompensate(audio_sample_t *buffer, size_t length) // Initialise filter with first two values, for smooth transient. if(length <= 2) return; - uoo = static_cast< float >(buffer[0]); - uo = static_cast< float >(buffer[1]); + + if(state->initialised == false) + { + state->u[2] = static_cast< float >(buffer[0]); + state->u[1] = static_cast< float >(buffer[1]); + state->initialised = true; + } for(size_t i = 2; i < length; i++) { - u = static_cast< float >(buffer[i]); - y = (a/d)*u + (b/d)*uo + (c/d)*uoo - (e/d)*yo - (f/d)*yoo; - uoo = uo; - uo = u; - yoo = yo; - yo = y; - buffer[i] = static_cast< audio_sample_t >(y * 0.5f); + state->u[0] = static_cast< float >(buffer[i]); + state->y[0] = (a/d)*(state->u[0]) + + (b/d)*(state->u[1]) + + (c/d)*(state->u[2]) + - (e/d)*(state->y[1]) + - (f/d)*(state->y[2]); + + state->u[2] = state->u[1]; + state->u[1] = state->u[0]; + state->y[2] = state->y[1]; + state->y[1] = state->y[0]; + buffer[i] = static_cast< audio_sample_t >((state->y[0] * 0.5f) + 0.5f); } } -void dsp_dcRemoval(audio_sample_t *buffer, size_t length) +void dsp_dcRemoval(filter_state_t *state, audio_sample_t *buffer, size_t length) { /* * Removal of DC component performed using an high-pass filter with @@ -64,19 +81,25 @@ void dsp_dcRemoval(audio_sample_t *buffer, size_t length) if(length < 2) return; - audio_sample_t uo = buffer[0]; - audio_sample_t yo = 0; + if(state->initialised == false) + { + state->u[1] = static_cast< float >(buffer[0]); + state->initialised = true; + } + static constexpr float alpha = 0.99f; for(size_t i = 1; i < length; i++) { - float yold = static_cast< float >(yo) * alpha; - audio_sample_t u = buffer[i]; - buffer[i] = u - uo + static_cast< audio_sample_t >(yold); - uo = u; - yo = buffer[i]; + state->u[0] = static_cast< float >(buffer[i]); + 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[0] = buffer[1]; } void dsp_invertPhase(audio_sample_t *buffer, uint16_t length) diff --git a/openrtx/src/protocols/M17/M17Modulator.cpp b/openrtx/src/protocols/M17/M17Modulator.cpp index f2fde84f..27009097 100644 --- a/openrtx/src/protocols/M17/M17Modulator.cpp +++ b/openrtx/src/protocols/M17/M17Modulator.cpp @@ -19,7 +19,6 @@ ***************************************************************************/ #include -#include #include #include #include @@ -54,6 +53,9 @@ void M17Modulator::init() baseband_buffer = new int16_t[2 * M17_FRAME_SAMPLES_48K]; idleBuffer = baseband_buffer; txRunning = false; + #if defined(PLATFORM_MD3x0) || defined(PLATFORM_MDUV3x0) + dsp_resetFilterState(&pwmFilterState); + #endif } void M17Modulator::terminate() @@ -106,7 +108,7 @@ void M17Modulator::generateBaseband() void M17Modulator::emitBaseband() { #if defined(PLATFORM_MD3x0) || defined(PLATFORM_MDUV3x0) - dsp_pwmCompensate(idleBuffer, M17_FRAME_SAMPLES_48K); + dsp_pwmCompensate(&pwmFilterState, idleBuffer, M17_FRAME_SAMPLES_48K); dsp_invertPhase(idleBuffer, M17_FRAME_SAMPLES_48K); #endif @@ -135,6 +137,9 @@ void M17Modulator::emitBaseband() stopTx = false; txRunning = false; idleBuffer = baseband_buffer; + #if defined(PLATFORM_MD3x0) || defined(PLATFORM_MDUV3x0) + dsp_resetFilterState(&pwmFilterState); + #endif } else {