core: dsp: refactor DC block filter implementation

New DC block filter implementation using fixed-point math and guaranteeing
zero DC component on the output signal.

Signed-off-by: Silvano Seva <silseva@fastwebnet.it>

openrtx/include/core/dsp.h

@@ -29,41 +29,49 @@
 extern "C" {
 #endif
 
-typedef int16_t audio_sample_t;
-
 /*
  * This header contains various DSP utilities which can be used to condition
  * input or output signals when implementing digital modes on OpenRTX.
  */
 
 /**
- * Data structure holding the internal state of a filter.
+ * Reset the state variables of a DSP object.
- */
-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.
+ * @param state: state variable.
  */
-void dsp_resetFilterState(filter_state_t *state);
+#define dsp_resetState(state) memset(&state, 0x00, sizeof(state))
 
 /**
- * Remove the DC offset from a collection of audio samples, processing data
+ * Data structure holding the internal state of a DC blocking filter.
- * in-place.
+ */
+struct dcBlock {
+    int32_t accum;
+    int32_t prevIn;
+    int32_t prevOut;
+};
+
+/**
+ * Run a single step of the DC blocking filter.
+ *
+ * @param dcb: pointer to DC filter state.
+ * @param sample: input sample.
+ * @return filtered sample
+ */
+int16_t dsp_dcBlockFilter(struct dcBlock *dcb, int16_t sample);
+
+/**
+ * Remove the DC offset from a collection of 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(filter_state_t *state, audio_sample_t *buffer, size_t length);
+static inline void dsp_removeDcOffset(struct dcBlock *dcb, int16_t *buffer,
+                                      const size_t length)
+{
+    for (size_t i = 0; i < length; i++)
+        buffer[i] = dsp_dcBlockFilter(dcb, buffer[i]);
+}
 
 /*
  * Inverts the phase of the audio buffer passed as paramenter.

openrtx/include/protocols/M17/M17Demodulator.hpp

@@ -162,7 +162,7 @@ private:
     uint32_t                       syncCount;       ///< Downcounter for resynchronization
     std::pair < int32_t, int32_t > outerDeviation;  ///< Deviation of outer symbols
     float                          corrThreshold;   ///< Correlation threshold
-    filter_state_t                 dcrState;        ///< State of the DC removal filter
+    struct dcBlock                 dcBlock;         ///< State of the DC removal filter
 
     Correlator   < M17_SYNCWORD_SYMBOLS, SAMPLES_PER_SYMBOL > correlator;
     Synchronizer < M17_SYNCWORD_SYMBOLS, SAMPLES_PER_SYMBOL > streamSync{{ -3, -3, -3, -3, +3, +3, -3, +3 }};

openrtx/src/core/audio_codec.c

@@ -54,12 +54,10 @@ static uint8_t          numElements;
 static uint64_t         dataBuffer[BUF_SIZE];
 
 #ifdef PLATFORM_MOD17
-static const uint8_t micGainPre  = 4;
+static const uint8_t micGain = 12;
-static const uint8_t micGainPost = 3;
 #else
 #ifndef PLATFORM_LINUX
-static const uint8_t micGainPre  = 8;
+static const uint8_t micGain = 32;
-static const uint8_t micGainPost = 4;
 #endif
 #endif
 
@@ -194,7 +192,7 @@ static void *encodeFunc(void *arg)
     pathId          iPath = *((pathId*) arg);
     stream_sample_t audioBuf[320];
     struct CODEC2   *codec2;
-    filter_state_t  dcrState;
+    struct dcBlock  dcBlock;
 
     iStream = audioStream_start(iPath, audioBuf, 320, 8000,
                                 STREAM_INPUT | BUF_CIRC_DOUBLE);
@@ -205,7 +203,7 @@ static void *encodeFunc(void *arg)
         return NULL;
     }
 
-    dsp_resetFilterState(&dcrState);
+    dsp_resetState(dcBlock);
     codec2 = codec2_create(CODEC2_MODE_3200);
 
     while(reqStop == false)
@@ -219,14 +217,12 @@ static void *encodeFunc(void *arg)
             break;
 
         #ifndef PLATFORM_LINUX
-        // Pre-amplification stage
+        for(size_t i = 0; i < audio.len; i++) {
-        for(size_t i = 0; i < audio.len; i++) audio.data[i] *= micGainPre;
+            int16_t sample;
 
-        // DC removal
+            sample = dsp_dcBlockFilter(&dcBlock, audio.data[i]);
-        dsp_dcRemoval(&dcrState, audio.data, audio.len);
+            audio.data[i] = sample * micGain;
-
+        }
-        // Post-amplification stage
-        for(size_t i = 0; i < audio.len; i++) audio.data[i] *= micGainPost;
         #endif
 
         // CODEC2 encodes 160ms of speech into 8 bytes: here we write the

openrtx/src/core/dsp.cpp

@@ -20,51 +20,22 @@
 
 #include <dsp.h>
 
-void dsp_resetFilterState(filter_state_t *state)
+int16_t dsp_dcBlockFilter(struct dcBlock *dcb, int16_t sample)
-{
-    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_dcRemoval(filter_state_t *state, audio_sample_t *buffer, size_t length)
 {
     /*
-     * Removal of DC component performed using an high-pass filter with
+     * Implementation of a fixed-point DC block filter with noise shaping,
-     * transfer function G(z) = (z - 1)/(z - 0.999).
+     * ensuring zero DC component at the output.
-     * Recursive implementation of the filter is:
+     * Filter pole set at 0.995
-     * y(k) = u(k) - u(k-1) + 0.999*y(k-1)
+     *
+     * Code adapted from https://dspguru.com/dsp/tricks/fixed-point-dc-blocking-filter-with-noise-shaping/
      */
+    dcb->accum -= dcb->prevIn;
+    dcb->prevIn = static_cast<int32_t>(sample) << 15;
+    dcb->accum += dcb->prevIn;
+    dcb->accum -= 164 * dcb->prevOut; // 32768.0 * (1.0 - pole)
+    dcb->prevOut = dcb->accum >> 15;
 
-    if(length < 2) return;
+    return static_cast<int16_t>(dcb->prevOut);
-
-    static constexpr float alpha = 0.999f;
-    size_t pos = 0;
-
-    if(state->initialised == false)
-    {
-        state->u[1] = static_cast< float >(buffer[0]);
-        state->initialised = true;
-        pos = 1;
-    }
-
-    for(; pos < length; pos++)
-    {
-        state->u[0] = static_cast< float >(buffer[pos]);
-        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[pos] = static_cast< audio_sample_t >(state->y[0] + 0.5f);
-    }
 }
 
 void dsp_invertPhase(audio_sample_t *buffer, uint16_t length)

openrtx/src/protocols/M17/M17Demodulator.cpp

@@ -237,16 +237,16 @@ bool M17Demodulator::update(const bool invertPhase)
     dataBlock_t baseband = inputStream_getData(basebandId);
     if(baseband.data != NULL)
     {
-        // Apply DC removal filter
-        dsp_dcRemoval(&dcrState, baseband.data, baseband.len);
-
         // Process samples
         for(size_t i = 0; i < baseband.len; i++)
         {
+            // Apply DC removal filter
+            int16_t sample = dsp_dcBlockFilter(&dcBlock, baseband.data[i]);
+
             // Apply RRC on the baseband sample
-            float           elem   = static_cast< float >(baseband.data[i]);
+            float           elem   = static_cast< float >(sample);
             if(invertPhase) elem   = 0.0f - elem;
-            int16_t         sample = static_cast< int16_t >(M17::rrc_24k(elem));
+            sample = static_cast< int16_t >(M17::rrc_24k(elem));
 
             // Update correlator and sample filter for correlation thresholds
             correlator.sample(sample);
@@ -429,7 +429,7 @@ void M17Demodulator::reset()
     demodState  = DemodState::INIT;
     initCount   = RX_SAMPLE_RATE / 50;  // 50ms of init time
 
-    dsp_resetFilterState(&dcrState);
+    dsp_resetState(dcBlock);
 }
 
 

scripts/clang_format.sh

@@ -53,6 +53,7 @@ openrtx/include/core/audio_codec.h
 openrtx/include/core/battery.h
 openrtx/include/core/beeps.h
 openrtx/include/core/crc.h
+openrtx/include/core/dsp.h
 openrtx/include/core/data_conversion.h
 openrtx/include/core/datatypes.h
 openrtx/include/core/memory_profiling.h
@@ -68,6 +69,7 @@ openrtx/include/interfaces/radio.h
 openrtx/include/peripherals/gps.h
 openrtx/include/peripherals/rng.h
 openrtx/include/peripherals/rtc.h
+openrtx/src/core/dsp.cpp
 openrtx/src/core/memory_profiling.cpp
 platform/drivers/ADC/ADC0_GDx.h
 platform/drivers/audio/MAX9814.h

tests/platform/mic_test.c

@@ -20,6 +20,7 @@
 
 #include <stdio.h>
 #include <stdlib.h>
+#include <string.h>
 #include <interfaces/delays.h>
 #include <interfaces/audio.h>
 #include <audio_path.h>
@@ -31,8 +32,8 @@ int main()
 {
     platform_init();
 
-    filter_state_t dcrState;
+    struct dcBlock dcb;
-    dsp_resetFilterState(&dcrState);
+    dsp_resetState(dcb);
 
     static const size_t numSamples = 45 * 1024; // 90kB
     void *sampleBuf = malloc(numSamples * sizeof(stream_sample_t));
@@ -51,16 +52,10 @@ int main()
     platform_ledOn(RED);
     sleepFor(10u, 0u);
 
-    // Pre-processing gain
+    for (size_t i = 0; i < audio.len; i++) {
-    for (size_t i = 0; i < audio.len; i++)
+        int16_t sample = dsp_dcBlockFilter(&dcb, audio.data[i]);
-        audio.data[i] <<= 3;
+        audio.data[i] = sample * 32;
-
+    }
-    // DC removal
-    dsp_dcRemoval(&dcrState, audio.data, audio.len);
-
-    // Post-processing gain
-    for (size_t i = 0; i < audio.len; i++)
-        audio.data[i] *= 10;
 
     uint16_t *ptr = ((uint16_t *)audio.data);
     for (size_t i = 0; i < audio.len; i++)