Improve frequency offset handling.

CarrierDetect.h

@@ -18,7 +18,9 @@ struct CarrierDetect
     FloatType lock_;
     FloatType unlock_;
     std::array<FloatType, N> samples_;
+    std::array<FloatType, N> variances_;
     FloatType sum_ = 0.0;
+    FloatType var_ = 0.0;
     size_t index_ = 0;
     bool locked_ = false;
 
@@ -30,22 +32,25 @@ struct CarrierDetect
     
     result_t operator()(FloatType evm)
     {
-        auto tmp = evm * evm;
-        sum_ = sum_ - samples_[index_] + tmp;
-        samples_[index_++] = tmp;
+        sum_ = sum_ - samples_[index_] + evm;
+        auto var = evm - (sum_ / N);
+        var_ = var_ - variances_[index_] + (var * var);
+        variances_[index_] = (var * var);
+        samples_[index_++] = evm;
         if (index_ == N) index_ = 0;
         
-        auto rms = std::sqrt(sum_ / N);
+        auto variance = var_ / N;
+        auto stdev = sqrt(variance);
         if (!locked_)
         {
-            if (rms < lock_) locked_ = true;
+            if (stdev < lock_) locked_ = true;
         }
         else
         {
-            if (rms > unlock_) locked_ = false;
+            if (stdev > unlock_) locked_ = false;
         }
 
-        return std::make_tuple(locked_, rms);
+        return std::make_tuple(locked_, stdev);
     }
 };
 

FrequencyError.h

@@ -0,0 +1,66 @@
+// Copyright 2020 Mobilinkd LLC.
+
+#pragma once
+
+#include "IirFilter.h"
+
+#include <array>
+#include <algorithm>
+#include <numeric>
+
+namespace mobilinkd
+{
+
+template <typename FloatType, size_t N = 32>
+struct FrequencyError
+{
+    using float_type = FloatType;
+    using array_t = std::array<FloatType, N>;
+    using filter_type = BaseIirFilter<FloatType, 3>;
+
+    static constexpr std::array<FloatType, 3> evm_b{0.02008337, 0.04016673, 0.02008337};
+    static constexpr std::array<FloatType, 3> evm_a{1.0, -1.56101808, 0.64135154};
+
+    array_t samples_{0};
+    size_t index_ = 0;
+    float_type accum_ = 0.0;
+    filter_type filter_{makeIirFilter(evm_b, evm_a)};
+
+
+    const float_type ZERO = 0.0;
+
+    FrequencyError()
+    {
+        samples_.fill(0.0);
+    }
+    
+    auto operator()(float_type sample)
+    {
+        FloatType evm = 0;
+        bool use = true;
+
+        if (sample > 2)
+        {
+            evm = sample - 3;
+        }
+        else if (sample >= -2)
+        {
+            use = false;
+        }
+        else
+        {
+            evm = sample + 3;
+        }
+
+        if (use)
+        {
+            accum_ = accum_ - samples_[index_] + evm;
+            samples_[index_++] = evm;
+            if (index_ == N) index_ = 0;
+        }
+
+        return filter_(accum_ / N);
+    }
+};
+
+} // mobilinkd

Fsk4Demod.h

@@ -5,6 +5,7 @@
 #include "FirFilter.h"
 #include "PhaseEstimator.h"
 #include "DeviationError.h"
+#include "FrequencyError.h"
 #include "SymbolEvm.h"
 
 #include <array>
@@ -47,11 +48,12 @@ inline const auto evm_a = std::experimental::make_array<double>(1.0, -1.56101808
 struct Fsk4Demod
 {
     using demod_result_t = std::tuple<double, double, int, double>;
-    using result_t = std::optional<std::tuple<double, double, int, double, double, double>>;
+    using result_t = std::optional<std::tuple<double, double, int, double, double, double, double>>;
 
     BaseFirFilter<double, std::tuple_size<decltype(detail::rrc_taps)>::value> rrc = makeFirFilter(detail::rrc_taps);
     PhaseEstimator<double> phase = PhaseEstimator<double>(48000, 4800);
     DeviationError<double> deviation;
+    FrequencyError<double, 32> frequency;
     SymbolEvm<double,  std::tuple_size<decltype(detail::evm_b)>::value> symbol_evm = makeSymbolEvm(makeIirFilter(detail::evm_b, detail::evm_a));
 
     double sample_rate = 48000;
@@ -64,6 +66,7 @@ struct Fsk4Demod
     bool sample_now = false;
     double estimated_deviation = 1.0;
     double estimated_frequency_offset = 0.0;
+    double evm_average = 0.0;
 
     Fsk4Demod(double sample_rate, double symbol_rate, double gain = 0.04)
     : sample_rate(sample_rate)
@@ -78,8 +81,10 @@ struct Fsk4Demod
     demod_result_t demod()
     {
         estimated_deviation = deviation(samples[1]);
+        for (auto& sample : samples) sample *= estimated_deviation;
     
-        for (auto& sample : samples) sample = (sample * estimated_deviation) - estimated_frequency_offset;
+        estimated_frequency_offset = frequency(samples[1]);
+        for (auto& sample : samples) sample -= estimated_frequency_offset;
     
         auto phase_estimate = phase(samples);
         if (samples[1] < 0) phase_estimate *= -1;
@@ -88,7 +93,7 @@ struct Fsk4Demod
         t += dt;
         
         auto [symbol, evm] = symbol_evm(samples[1]);
-        estimated_frequency_offset = symbol_evm.evm();
+        evm_average = symbol_evm.evm();
         samples[0] = samples[2];
 
         return std::make_tuple(samples[1], phase_estimate, symbol, evm);
@@ -109,7 +114,7 @@ struct Fsk4Demod
             samples[2] = filtered_sample;
             sample_now = false;
             auto [prev_sample, phase_estimate, symbol, evm] = demod();
-            return std::make_tuple(prev_sample, phase_estimate, symbol, evm, estimated_deviation, estimated_frequency_offset);
+            return std::make_tuple(prev_sample, phase_estimate, symbol, evm, estimated_deviation, estimated_frequency_offset, evm_average);
         }
             
         t += dt;

M17FrameDecoder.h

@@ -117,7 +117,7 @@ struct M17FrameDecoder
     {
         derandomize_(buffer);
         interleaver_.deinterleave(buffer);
-        size_t ber = -1;
+        size_t ber = 1000;
 
         switch(state_)
         {

m17-demod.cpp

@@ -46,8 +46,8 @@ int main(int argc, char* argv[])
 {
     using namespace mobilinkd;
 
-    auto demod = Fsk4Demod(48000.0, 4800.0, 0.03);
-    auto dcd = CarrierDetect<double, 10>(0.1, 0.4);
+    auto demod = Fsk4Demod(48000.0, 4800.0, 0.02);
+    auto dcd = CarrierDetect<double, 32>(0.2, 1.0);
     auto synch = M17Synchronizer();
     auto framer = M17Framer();
     auto decoder = M17FrameDecoder();
@@ -63,12 +63,12 @@ int main(int argc, char* argv[])
     {
         int16_t sample;
         std::cin.read(reinterpret_cast<char*>(&sample), 2);
-        auto result = demod(sample / 5000.0);
+        auto result = demod(sample / 5600.0);
         if (result)
         {
             count += 1;
-            auto [prev_sample, phase_estimate, symbol, evm, estimated_deviation, estimated_frequency_offset] = *result;
-            auto [locked, rms] = dcd(estimated_frequency_offset);
+            auto [prev_sample, phase_estimate, symbol, evm, estimated_deviation, estimated_frequency_offset, evma] = *result;
+            auto [locked, rms] = dcd(evm);
 
             switch (state)
             {
@@ -114,11 +114,12 @@ int main(int argc, char* argv[])
             {
                 std::cerr << "\r count: " << std::setw(8) << count
                     << ", phase: " << std::setprecision(2) << std::setw(8) << phase_estimate
-                    << ", evm: " << std::setprecision(2) << std::setw(8) << rms
+                    << ", evm: " << std::setprecision(2) << std::setw(8) << evma
                     << ", deviation: " << std::setprecision(2) << std::setw(8) << estimated_deviation
                     << ", freq offset: " << std::setprecision(2) << std::setw(8) << estimated_frequency_offset
                     << ", locked: " << std::boolalpha << std::setw(6) << locked
-                    << ", ber: " << ber << std::ends;
+                    << ", jitter: " << std::setprecision(2) << std::setw(8) << rms
+                    << ", ber: " << ber << "         "  << std::ends;
             }
         }
     }