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

@@ -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.

openrtx/include/protocols/M17/M17Modulator.h

@@ -28,6 +28,7 @@
 #include <interfaces/audio_stream.h>
 #include <cstdint>
 #include <array>
+#include <dsp.h>
 
 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 */

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;

openrtx/src/core/dsp.cpp

@@ -20,15 +20,22 @@
 
 #include <dsp.h>
 
-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)

openrtx/src/protocols/M17/M17Modulator.cpp

@@ -19,7 +19,6 @@
  ***************************************************************************/
 
 #include <new>
-#include <dsp.h>
 #include <cstddef>
 #include <cstring>
 #include <experimental/array>
@@ -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
     {