From d1651a702cc8cb3148c5ddb529cb75e6f020b5a6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Izzo?= <izzo.niccolo@gmail.com>
Date: Tue, 12 Apr 2022 19:27:23 +0200
Subject: [PATCH] Add proper DC-bias removal, tuned EMAs

---
 .../include/protocols/M17/M17Demodulator.h    | 12 ++++++----
 openrtx/src/protocols/M17/M17Demodulator.cpp  | 22 +++++++++----------
 2 files changed, 19 insertions(+), 15 deletions(-)

diff --git a/openrtx/include/protocols/M17/M17Demodulator.h b/openrtx/include/protocols/M17/M17Demodulator.h
index f2f16c41..19d86078 100644
--- a/openrtx/include/protocols/M17/M17Demodulator.h
+++ b/openrtx/include/protocols/M17/M17Demodulator.h
@@ -32,6 +32,7 @@
 #include <cstddef>
 #include <interfaces/audio_stream.h>
 #include <hwconfig.h>
+#include <dsp.h>
 #include <deque>
 #include "M17Datatypes.h"
 
@@ -122,9 +123,8 @@ private:
     static constexpr size_t M17_SAMPLE_BUF_SIZE    = M17_FRAME_SAMPLES / 2;
     static constexpr size_t M17_BRIDGE_SIZE        = M17_SAMPLES_PER_SYMBOL * M17_SYNCWORD_SYMBOLS;
 
-    static constexpr size_t M17_CONV_THRESHOLD     = 50000;
-    static constexpr float  CONV_STATS_ALPHA       = 0.001f;
-    static constexpr float  QNT_STATS_ALPHA        = 0.9f;
+    static constexpr float  CONV_STATS_ALPHA       = 0.01f;
+    static constexpr float  QNT_STATS_ALPHA        = 0.995f;
     static constexpr float  CONV_THRESHOLD_FACTOR  = 3.40;
 
     /**
@@ -159,7 +159,6 @@ private:
     /*
      * Convolution statistics computation
      */
-    float conv_ema   = 0.0f;    /// Exponential moving average from all the correlation values.
     float conv_emvar = 0.0f;
 
     /*
@@ -169,6 +168,11 @@ private:
     float qnt_max = 0.0f;   ///< Max hold of the sliced samples
     float qnt_min = 0.0f;   ///< Min hold of the sliced samples.
 
+    /*
+     * DSP filter state
+     */
+    filter_state_t dsp_state;
+
     /**
      * Resets the exponential mean and variance/stddev computation.
      */
diff --git a/openrtx/src/protocols/M17/M17Demodulator.cpp b/openrtx/src/protocols/M17/M17Demodulator.cpp
index 9c33baef..95eb48c0 100644
--- a/openrtx/src/protocols/M17/M17Demodulator.cpp
+++ b/openrtx/src/protocols/M17/M17Demodulator.cpp
@@ -93,6 +93,8 @@ void M17Demodulator::startBasebandSampling()
                                    M17_RX_SAMPLE_RATE);
     // Clean start of the demodulation statistics
     resetCorrelationStats();
+    // DC removal filter reset
+    dsp_resetFilterState(&dsp_state);
 }
 
 void M17Demodulator::stopBasebandSampling()
@@ -102,8 +104,7 @@ void M17Demodulator::stopBasebandSampling()
 
 void M17Demodulator::resetCorrelationStats()
 {
-    conv_ema   = 0.0f;
-    conv_emvar = 800000000.0f;
+    conv_emvar = 80000000.0f;
 }
 
 /**
@@ -112,10 +113,8 @@ void M17Demodulator::resetCorrelationStats()
  */
 void M17Demodulator::updateCorrelationStats(int32_t value)
 {
-    float delta = (float) value - conv_ema;
-    float incr  = CONV_STATS_ALPHA * delta;
-    conv_ema   += incr;
-    conv_emvar  = (1.0f - CONV_STATS_ALPHA) * (conv_emvar + delta * incr);
+    float incr  = CONV_STATS_ALPHA * static_cast<float>(value);
+    conv_emvar  = (1.0f - CONV_STATS_ALPHA) * (conv_emvar + static_cast<float>(value) * incr);
 }
 
 float M17Demodulator::getCorrelationStddev()
@@ -201,21 +200,19 @@ sync_t M17Demodulator::nextFrameSync(int32_t offset)
 
         fprintf(csv_log, "%" PRId16 ",%d,%f,%d\n",
                 sample,
-                conv - static_cast< int32_t >(conv_ema),
+                conv,
                 CONV_THRESHOLD_FACTOR * getCorrelationStddev(),
                 i);
         #endif
 
         // Positive correlation peak -> frame syncword
-        if ((conv - static_cast< int32_t >(conv_ema)) >
-            (getCorrelationStddev() * CONV_THRESHOLD_FACTOR))
+        if (conv > (getCorrelationStddev() * CONV_THRESHOLD_FACTOR))
         {
             syncword.lsf = false;
             syncword.index = i;
         }
         // Negative correlation peak -> LSF syncword
-        else if ((conv - static_cast< int32_t >(conv_ema)) <
-                 -(getCorrelationStddev() * CONV_THRESHOLD_FACTOR))
+        else if (conv < -(getCorrelationStddev() * CONV_THRESHOLD_FACTOR))
         {
             syncword.lsf = true;
             syncword.index = i;
@@ -278,6 +275,9 @@ bool M17Demodulator::update()
     // Read samples from the ADC
     baseband = inputStream_getData(basebandId);
 
+    // Apply DC removal filter
+    dsp_dcRemoval(&dsp_state, baseband.data, baseband.len);
+
     #ifdef PLATFORM_LINUX
     FILE *csv_log = fopen("demod_log_2.csv", "a");
     #endif