Update revert parts

Nyovelt · Oct 14, 2021 · bffda72 · bffda72
1 parent 3fe223f
commit bffda72
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 8 deletions.
diff --git a/Native-Modem/Protocol.cs b/Native-Modem/Protocol.cs
@@ -1,10 +1,13 @@
 using NAudio.Wave;
+using System;
 
 namespace Native_Modem
 {
     public class Protocol
     {
         public float[] Header { get; }
+        public float HeaderPower { get; }
+        public float HeaderMagnitude { get; }
         public float[] One { get; }
         public float[] Zero { get; }
         public WaveFormat WaveFormat { get; }
@@ -15,6 +18,12 @@ public class Protocol
         public Protocol(float[] header, SinusoidalSignal one, SinusoidalSignal zero, int sampleRate, int samplesPerBit, int frameSize, float threshold)
         {
             Header = header.Clone() as float[];
+            HeaderPower = 0f;
+            foreach (float sample in header)
+            {
+                HeaderPower += sample * sample;
+            }
+            HeaderMagnitude = MathF.Sqrt(HeaderPower);
             WaveFormat = WaveFormat.CreateIeeeFloatWaveFormat(sampleRate, 1);
             SamplesPerBit = samplesPerBit;
             FrameSize = frameSize;

diff --git a/Native-Modem/SynchronousModem.cs b/Native-Modem/SynchronousModem.cs
@@ -214,8 +214,6 @@ async Task Modulate()
         }
 
         const int DECODE_WAIT_SAMPLES = 200;
-        static readonly float K1 = 63f / 64f;
-        static readonly float K2 = 1f / 64f;
 
         enum DemodulateState
         {
@@ -225,8 +223,13 @@ enum DemodulateState
 
         async Task Demodulate(Action<BitArray> onFrameReceived, float syncPowerThreshold)
         {
-            float power = 0f;
             bool decode = false;
+            RingBuffer<float> powerBuffer = new RingBuffer<float>(protocol.Header.Length);
+            for (int i = 0; i < protocol.Header.Length; i++)
+            {
+                powerBuffer.Add(0f);
+            }
+            float powerSum = 0f;
             RingBuffer<float> syncBuffer = new RingBuffer<float>(protocol.Header.Length);
             for (int i = 0; i < protocol.Header.Length; i++)
             {
@@ -246,21 +249,27 @@ async Task Demodulate(Action<BitArray> onFrameReceived, float syncPowerThreshold
                 }
 
                 float sample = RxFIFO.Pop();
-                power = power * K1 + sample * sample * K2;
+                powerSum -= powerBuffer.ReadAndRemoveNext();
+                float temp = sample * sample;
+                powerSum += temp;
+                powerBuffer.Add(temp);
+
+                syncBuffer.ReadAndRemoveNext();
+                syncBuffer.Add(sample);
 
                 switch (state)
                 {
                     case DemodulateState.Sync:
-                        syncBuffer.ReadAndRemoveNext();
-                        syncBuffer.Add(sample);
                         float syncPower = 0f;
+                        float gain = protocol.HeaderMagnitude / MathF.Sqrt(powerSum);
                         for (int j = 0; j < protocol.Header.Length; j++)
                         {
                             syncPower += syncBuffer[j] * protocol.Header[j];
                         }
-                        //Console.WriteLine($"syncPower: {syncPower}, localMax: {syncPowerLocalMax}, threshold: {protocol.HeaderPower * syncPowerThreshold}");
+                        syncPower *= gain;
                         if (syncPower > protocol.HeaderPower * syncPowerThreshold && syncPower > syncPowerLocalMax)
                         {
+                            Console.WriteLine($"syncPower: {syncPower}, localMax: {syncPowerLocalMax}, threshold: {protocol.HeaderPower * syncPowerThreshold}");
                             syncPowerLocalMax = syncPower;
                             decode = true;
                             decodeFrame.Clear();

diff --git a/Native-Modem/TaskUtilities.cs b/Native-Modem/TaskUtilities.cs
@@ -8,7 +8,7 @@ namespace Native_Modem
 {
     public static class TaskUtilities
     {
-        const int SMALL_DELAY = 25;
+        const int SMALL_DELAY = 10;
 
         public static async Task WaitUntil(Func<bool> untilTrue)
         {