Update to version 2.4.4. Clock recovery improvements. Update the samp…

…le index more frequently to reduce EVM. Re-wrote the symbol deviation, offset, EVM code. More code comments.

TNC/ClockRecovery.h

@@ -1,4 +1,4 @@
-// Copyright 2021 Mobilinkd LLC.
+// Copyright 2021-2022 Mobilinkd LLC.
 
 #pragma once
 
@@ -19,8 +19,6 @@ namespace mobilinkd { namespace m17 {
 template <typename FloatType = float, size_t SamplesPerSymbol = 10>
 struct ClockRecovery
 {
-    static constexpr FloatType MAX_CLOCK_OFFSET = 0.0005; // 500ppm
-
     KalmanFilter<FloatType, SamplesPerSymbol> kf_;
     size_t count_ = 0;
     int8_t sample_index_ = 0;
@@ -41,17 +39,10 @@ struct ClockRecovery
         ++count_;
     }
 
-    static FloatType clock_limiter(FloatType offset)
-    {
-        return std::min(MAX_CLOCK_OFFSET, std::max(-MAX_CLOCK_OFFSET, offset));
-    }
-
     bool update(uint8_t sw)
     {
-        INFO("CR Update %d", int(sw));
-        if (count_ < 480)
+        if (count_ < 8)
         {
-            sample_index_ = sw;
             return false;
         }
 
@@ -77,8 +68,7 @@ struct ClockRecovery
      */
     bool update()
     {
-        INFO("CR Update");
-        auto csw = std::fmod((sample_estimate_ + (1.0 + clock_estimate_) * count_), SamplesPerSymbol);
+        auto csw = std::fmod((sample_estimate_ + clock_estimate_ * count_), SamplesPerSymbol);
         if (csw < 0.) csw += SamplesPerSymbol;
         else if (csw >= SamplesPerSymbol) csw -= SamplesPerSymbol;
 

TNC/FreqDevEstimator.h

@@ -1,129 +1,142 @@
-// Copyright 2021 Rob Riggs <[email protected]>
+// Copyright 2021-2022 Rob Riggs <[email protected]>
 // All rights reserved.
 
 #pragma once
 
-#include "IirFilter.hpp"
+#include "KalmanFilter.h"
+#include "StandardDeviation.hpp"
 
-#include <algorithm>
-#include <array>
 #include <cmath>
 #include <cstddef>
 
 namespace mobilinkd { namespace m17 {
-
-/**
- * Deviation and zero-offset estimator.
- *
- * Accepts samples which are periodically used to update estimates of the
- * input signal deviation and zero offset.
- *
- * Samples must be provided at the ideal sample point (the point with the
- * peak bit energy).
- *
- * Estimates are expected to be updated at each sync word.  But they can
- * be updated more frequently, such as during the preamble.
- */
-template <typename FloatType>
+template <typename FloatType, size_t SYNC_WORD_LEN = 8>
 class FreqDevEstimator
 {
-    using sample_filter_t = tnc::IirFilter<3>;
-
-    // IIR with Nyquist of 1/4.
-    static constexpr std::array<float, 3> dc_b = { 0.09763107,  0.19526215,  0.09763107 };
-    static constexpr std::array<float, 3> dc_a = { 1.        , -0.94280904,  0.33333333 };
-
-    static constexpr FloatType MAX_DC_ERROR = 0.2;
-
-    FloatType min_est_ = 0.0;
-	FloatType max_est_ = 0.0;
-	FloatType min_cutoff_ = 0.0;
-	FloatType max_cutoff_ = 0.0;
-	FloatType min_var_ = 0.0;
-	FloatType max_var_ = 0.0;
-	size_t min_count_ = 1;
-	size_t max_count_ = 1;
-	FloatType deviation_ = 0.0;
-	FloatType offset_ = 0.0;
-	FloatType error_ = 0.0;
-	FloatType idev_ = 1.0;
-    sample_filter_t dc_filter_{dc_b, dc_a};
+    static constexpr FloatType DEVIATION = 1.;
+
+    SymbolKalmanFilter<FloatType> minFilter_;
+    SymbolKalmanFilter<FloatType> maxFilter_;
+    RunningStandardDeviation<FloatType, 184> stddev_;
+    FloatType idev_ = 1.;
+    FloatType offset_ = 0.;
+    uint8_t count_ = 0;
+    FloatType min_ = 0.;
+    FloatType max_ = 0.;
+    uint8_t minCount_ = 0;
+    uint8_t maxCount_ = 0;
+    size_t updateCount_ = 0;
+    bool reset_ = true;
 
 public:
 
-	void reset()
-	{
-		min_est_ = 0.0;
-		max_est_ = 0.0;
-		min_var_ = 0.0;
-		max_var_ = 0.0;
-		min_count_ = 1;
-		max_count_ = 1;
-		min_cutoff_ = 0.0;
-		max_cutoff_ = 0.0;
-	}
-
-	void sample(FloatType sample)
-	{
-		if (sample < 1.5 * min_est_)
-		{
-			min_count_ = 1;
-			min_est_ = sample;
-			min_var_ = 0.0;
-			min_cutoff_ = min_est_ * 0.666666;
-		}
-		else if (sample < min_cutoff_)
-		{
-			min_count_ += 1;
-			min_est_ += sample;
-			FloatType var = (min_est_ / min_count_) - sample;
-			min_var_ += var * var;
-		}
-		else if (sample > 1.5 * max_est_)
-		{
-			max_count_ = 1;
-			max_est_ = sample;
-			max_var_ = 0.0;
-			max_cutoff_ = max_est_ * 0.666666;
-		}
-		else if (sample > max_cutoff_)
-		{
-			max_count_ += 1;
-			max_est_ += sample;
-			FloatType var = (max_est_ / max_count_) - sample;
-			max_var_ += var * var;
-		}
-	}
-
-	/**
-	 * Update the estimates for deviation, offset, and EVM (error).  Note
-	 * that the estimates for error are using a sloppy implementation for
-	 * calculating variance to reduce the memory requirements.  This is
-	 * because this is designed for embedded use.
-	 */
-	void update()
-	{
-		if (max_count_ < 2 || min_count_ < 2) return;
-		FloatType max_ = max_est_ / max_count_;
-		FloatType min_ = min_est_ / min_count_;
-		deviation_ = (max_ - min_) / 6.0;
-		if (deviation_ > 0) idev_ = 1.0 / deviation_;
-		offset_ =  dc_filter_(std::max(std::min(max_ + min_, deviation_ * MAX_DC_ERROR), deviation_ * -MAX_DC_ERROR));
-		error_ = (std::sqrt(max_var_ / (max_count_ - 1)) + std::sqrt(min_var_ / (min_count_ - 1))) * 0.5 * idev_;
-		min_cutoff_ = offset_ - deviation_ * 2;
-		max_cutoff_ = offset_ + deviation_ * 2;
-		max_est_ = max_;
-		min_est_ = min_;
-		max_count_ = 1;
-		min_count_ = 1;
-		max_var_ = 0.0;
-		min_var_ = 0.0;
-	}
-
-	FloatType deviation() const { return deviation_; }
-	FloatType offset() const { return offset_; }
-	FloatType error() const { return error_; }
-	FloatType idev() const { return idev_; }
+    void reset()
+    {
+        idev_ = 1.;
+        offset_ = 0.;
+        count_ = 0;
+        min_ = 0.;
+        max_ = 0.;
+        minCount_ = 0;
+        maxCount_ = 0;
+        updateCount_ = 0;
+        stddev_.reset();
+        reset_ = true;
+    }
+
+    /**
+     * This function takes the index samples from the correlator to build
+     * the outer symbol samples for the frequency offset (signal DC offset)
+     * and the deviation (signal magnitude). It expects bursts of 8 samples,
+     * one for each symbol in a sync word.
+     *
+     * @param sample
+     */
+    void sample(FloatType sample)
+    {
+        count_ += 1;
+
+        if (sample < 0)
+        {
+            minCount_ += 1;
+            min_ += sample;
+        }
+        else
+        {
+            maxCount_ += 1;
+            max_ += sample;
+        }
+
+        if (count_ == SYNC_WORD_LEN)
+        {
+            auto minAvg = min_ / minCount_;
+            auto maxAvg = max_ / maxCount_;
+            if (reset_)
+            {
+                minFilter_.reset(minAvg);
+                maxFilter_.reset(maxAvg);
+                idev_ = 6.0 / (maxAvg - minAvg);
+                offset_ = maxAvg + minAvg;
+                reset_ = false;
+            }
+            else
+            {
+                auto minFiltered = minFilter_.update(minAvg, count_ + updateCount_);
+                auto maxFiltered = maxFilter_.update(maxAvg, count_ + updateCount_);
+                idev_ = 6.0 / (maxFiltered[0] - minFiltered[0]);
+                offset_ = maxFiltered[0] + minFiltered[0];
+            }
+
+            count_ = 0;
+            updateCount_ = 0;
+            min_ = 0.;
+            max_ = 0.;
+            minCount_ = 0;
+            maxCount_ = 0;
+        }
+    }
+
+    FloatType normalize(FloatType sample) const
+    {
+        return (sample - offset_) * idev_;
+    }
+
+    FloatType evm() const { return stddev_.stdev(); }
+
+    void update() const {}
+
+    /**
+     * Capture EVM of a symbol.
+     *
+     * @param sample is a normalized sample captured at the best sample point.
+     */
+    void update(FloatType sample)
+    {
+        if (sample > 2)
+        {
+            stddev_.capture(sample - 3);
+        }
+        else if (sample > 0)
+        {
+            stddev_.capture(sample - 1);
+        }
+        else if (sample > -2)
+        {
+            stddev_.capture(sample + 1);
+        }
+        else
+        {
+            stddev_.capture(sample + 3);
+        }
+
+        updateCount_ += 1;
+    }
+
+    FloatType idev() const { return idev_; }
+    FloatType offset() const { return offset_; }
+    FloatType deviation() const { return DEVIATION / idev_; }
+    FloatType error() const { return evm(); }
 };
 
+
 }} // mobilinkd::m17

TNC/KalmanFilter.h

@@ -63,10 +63,10 @@ struct KalmanFilter
         auto K = P * blaze::trans(H) * (1.0 / S(0, 0));
 
         // Normalize incoming index
-        if (z - x[0] < (SamplesPerSymbol / -2.0))
+        if (z - x[0] < (SamplesPerSymbol / -2.0)) // wrapped forwards 9 -> 0
             z += SamplesPerSymbol;
-        else if (z - x[0] > (SamplesPerSymbol / 2.0))
-            z-= SamplesPerSymbol;
+        else if (z - x[0] > (SamplesPerSymbol / 2.0)) // wrapped 0 -> 9
+            z -= SamplesPerSymbol;
 
         auto y = z - H * x;
 
@@ -80,4 +80,46 @@ struct KalmanFilter
     }
 };
 
+template <typename FloatType>
+struct SymbolKalmanFilter
+{
+    blaze::StaticVector<FloatType, 2> x;
+    blaze::StaticMatrix<FloatType, 2, 2> P;
+    blaze::StaticMatrix<FloatType, 2, 2> F;
+    blaze::StaticMatrix<FloatType, 1, 2> H = {{1., 0.}};
+    blaze::StaticMatrix<FloatType, 1, 1> R = {{0.5}};
+    blaze::StaticMatrix<FloatType, 2, 2> Q = {{6.25e-4, 1.25e-3},{1.25e-3, 2.50e-3}};
+
+    SymbolKalmanFilter()
+    {
+        reset(0.);
+    }
+
+    void reset(FloatType z)
+    {
+        x = {z, 0.};
+        P = {{4., 0.}, {0., 0.00000025}};
+        F = {{1., 1.}, {0., 1.}};
+    }
+
+    [[gnu::noinline]]
+    auto update(FloatType z, size_t dt)
+    {
+        F(0,1) = FloatType(dt);
+
+        x = F * x;
+        P = F * P * blaze::trans(F) + Q;
+        auto S = H * P * blaze::trans(H) + R;
+        auto K = P * blaze::trans(H) * (1.0 / S(0, 0));
+
+        auto y = z - H * x;
+
+        x += K * y;
+
+        // Normalize the filtered sample point
+        P = P - K * H * P;
+        return x;
+    }
+};
+
 }} // mobilinkd::m17

TNC/KissHardware.cpp

@@ -1,4 +1,4 @@
-// Copyright 2018-2021 Rob Riggs <[email protected]>
+// Copyright 2018-2022 Rob Riggs <[email protected]>
 // All rights reserved.
 
 #include "KissHardware.hpp"
@@ -43,13 +43,13 @@ int powerOffViaUSB(void)
 namespace mobilinkd { namespace tnc { namespace kiss {
 
 #if defined(NUCLEOTNC)
-const char FIRMWARE_VERSION[] = "2.4.3";
+const char FIRMWARE_VERSION[] = "2.4.4";
 const char HARDWARE_VERSION[] = "Mobilinkd NucleoTNC";
 #elif defined(STM32L433xx)
-const char FIRMWARE_VERSION[] = "2.4.3";
+const char FIRMWARE_VERSION[] = "2.4.4";
 const char HARDWARE_VERSION[] = "Mobilinkd TNC3 2.1.1";
 #elif defined(STM32L4P5xx)
-const char FIRMWARE_VERSION[] = "2.4.3";
+const char FIRMWARE_VERSION[] = "2.4.4";
 const char HARDWARE_VERSION[] = "Mobilinkd TNC3+ Rev A";
 #endif
 Hardware& settings()