M17: Demodulator: improve sampling point estimation

ClockRecovery module provides the best sampling point estimate based on
the previous baseband history, tracking clock drifts more promptly than
methods based on syncword correlation.

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

openrtx/include/protocols/M17/M17Demodulator.hpp

@@ -41,6 +41,7 @@
 #include "protocols/M17/Correlator.hpp"
 #include "protocols/M17/Synchronizer.hpp"
 #include "protocols/M17/DevEstimator.hpp"
+#include "protocols/M17/ClockRecovery.hpp"
 
 namespace M17
 {
@@ -137,10 +138,8 @@ private:
 
     /**
      * State handler function for DemodState::SYNC_UPDATE
-     *
-     * @param sample: current baseband sample
      */
-    void syncUpdateState(int16_t sample);
+    void syncUpdateState();
 
     /**
      * M17 baseband signal sampled at 24kHz, half of an M17 frame is processed
@@ -161,7 +160,7 @@ private:
         UNLOCKED,   ///< Not locked
         SYNCED,     ///< Synchronized, validate syncword
         LOCKED,     ///< Locked
-        SYNC_UPDATE ///< Updating the sampling point
+        SYNC_UPDATE ///< Updating the synchronization state
     };
 
     /**
@@ -177,13 +176,14 @@ private:
     std::unique_ptr<frame_t >      demodFrame;      ///< Frame being demodulated.
     std::unique_ptr<frame_t >      readyFrame;      ///< Fully demodulated frame to be returned.
     bool                           newFrame;        ///< A new frame has been fully decoded.
+    bool                           resetClockRec;   ///< Clock recovery reset request.
+    bool                           updateSampPoint; ///< Sampling point update pending.
     uint16_t                       frameIndex;      ///< Index for filling the raw frame.
     uint32_t                       sampleIndex;     ///< Sample index, from 0 to (SAMPLES_PER_SYMBOL - 1)
     uint32_t                       samplingPoint;   ///< Symbol sampling point
     uint32_t                       sampleCount;     ///< Free-running sample counter
     uint8_t                        missedSyncs;     ///< Counter of missed synchronizations
     uint32_t                       initCount;       ///< Downcounter for initialization
-    uint32_t                       syncCount;       ///< Downcounter for resynchronization
     float                          corrThreshold;   ///< Correlation threshold
     struct dcBlock                 dcBlock;         ///< State of the DC removal filter
 
@@ -191,6 +191,7 @@ private:
     Synchronizer < M17_SYNCWORD_SYMBOLS, SAMPLES_PER_SYMBOL > streamSync{{ -3, -3, -3, -3, +3, +3, -3, +3 }};
     Iir          < 3 >                                        sampleFilter{sfNum, sfDen};
     DevEstimator                                              devEstimator;
+    ClockRecovery< SAMPLES_PER_SYMBOL >                       clockRec;
 };
 
 } /* M17 */

openrtx/src/protocols/M17/M17Demodulator.cpp

@@ -249,7 +249,23 @@ bool M17Demodulator::update(const bool invertPhase)
         if(invertPhase) elem   = 0.0f - elem;
         sample = static_cast< int16_t >(M17::rrc_24k(elem));
 
-        // Update correlator and sample filter for correlation thresholds
+        // Clock recovery reset MUST come before sampling
+        if((sampleIndex == 0) && resetClockRec) {
+            clockRec.reset();
+            resetClockRec = false;
+            updateSampPoint = false;
+        }
+
+        // Update sample point only when "far enough" from the last sampling,
+        // to avoid sampling issues when SP rolls over.
+        int diff = samplingPoint - sampleIndex;
+        if(updateSampPoint && (std::abs(diff) == SAMPLES_PER_SYMBOL/2)) {
+            clockRec.update();
+            samplingPoint = clockRec.samplingPoint();
+            updateSampPoint = false;
+        }
+
+        clockRec.sample(sample);
         correlator.sample(sample);
         corrThreshold = sampleFilter(std::abs(sample));
 
@@ -277,7 +293,7 @@ bool M17Demodulator::update(const bool invertPhase)
                 break;
 
             case DemodState::SYNC_UPDATE:
-                syncUpdateState(sample);
+                syncUpdateState();
                 break;
         }
 
@@ -312,14 +328,6 @@ void M17Demodulator::quantize(stream_sample_t sample)
 
     setSymbol(*demodFrame, frameIndex, symbol);
     frameIndex += 1;
-
-    if(frameIndex >= M17_FRAME_SYMBOLS)
-    {
-        devEstimator.update();
-        std::swap(readyFrame, demodFrame);
-        frameIndex = 0;
-        newFrame   = true;
-    }
 }
 
 void M17Demodulator::reset()
@@ -376,58 +384,35 @@ void M17Demodulator::lockedState(int16_t sample)
     if(sampleIndex != samplingPoint)
         return;
 
-    // Quantize and update frame at each sampling point
     quantize(sample);
     devEstimator.sample(sample);
 
-    // When we have reached almost the end of a frame, switch
+    if(frameIndex == M17_FRAME_SYMBOLS) {
-    // to syncpoint update.
+        devEstimator.update();
-    if(frameIndex == (M17_FRAME_SYMBOLS - M17_SYNCWORD_SYMBOLS/2)) {
+        std::swap(readyFrame, demodFrame);
+
+        frameIndex = 0;
+        newFrame = true;
+        updateSampPoint = true;
         demodState = DemodState::SYNC_UPDATE;
-        syncCount  = SYNCWORD_SAMPLES * 2;
     }
 }
 
-void M17Demodulator::syncUpdateState(int16_t sample)
+void M17Demodulator::syncUpdateState()
 {
-    // Keep filling the ongoing frame!
+    uint8_t streamHd = hammingDistance((*demodFrame)[0], STREAM_SYNC_WORD[0])
-    if(sampleIndex == samplingPoint) {
+                     + hammingDistance((*demodFrame)[1], STREAM_SYNC_WORD[1]);
-        quantize(sample);
-        devEstimator.sample(sample);
-    }
-
-    // Find the new correlation peak
-    int32_t syncThresh = static_cast< int32_t >(corrThreshold * 33.0f);
-    int8_t  syncStatus = streamSync.update(correlator, syncThresh, -syncThresh);
-
-    // Correlation has to coincide with a syncword!
-    if((syncStatus != 0) && (frameIndex == M17_SYNCWORD_SYMBOLS)) {
-        uint8_t hd = hammingDistance((*demodFrame)[0], STREAM_SYNC_WORD[0])
-                   + hammingDistance((*demodFrame)[1], STREAM_SYNC_WORD[1]);
-
-        // Valid sync found: update deviation and sample
-        // point, then go back to locked state
-        if(hd <= 1) {
-            samplingPoint = streamSync.samplingIndex();
-            missedSyncs = 0;
-            demodState = DemodState::LOCKED;
-            return;
-        }
-    }
-
-    // No syncword found within the window, increase the count
-    // of missed syncs and choose where to go. The lock is lost
-    // after four consecutive sync misses.
-    if(syncCount == 0) {
-        if(missedSyncs >= 4)
-            demodState = DemodState::UNLOCKED;
-        else
-            demodState = DemodState::LOCKED;
 
+    if(streamHd <= 1)
+        missedSyncs = 0;
+    else
         missedSyncs += 1;
-    }
 
-    syncCount -= 1;
+    // The lock is lost after four consecutive sync misses.
+    if(missedSyncs > 4)
+        demodState = DemodState::UNLOCKED;
+    else
+        demodState = DemodState::LOCKED;
 }
 
 constexpr std::array < float, 3 > M17Demodulator::sfNum;