From 00b4452cb2a94473d13c10192c2fb795c7d4f58e Mon Sep 17 00:00:00 2001
From: Mikhail Yudin <fagci.nsk@gmail.com>
Date: Fri, 28 Jul 2023 00:04:05 +0700
Subject: [PATCH] feat: squelch modes by 0, sticky (solid line) and non-sticky
 (dashed line)

---
 libs/k5_uv_system/system.cpp    |  2 +-
 libs/k5_uv_system/system.hpp    |  2 +-
 libs/radio/radio.hpp            |  8 ++++++++
 src/spectrum_fagci/spectrum.hpp | 33 ++++++++++++++++++---------------
 4 files changed, 28 insertions(+), 17 deletions(-)

diff --git a/libs/k5_uv_system/system.cpp b/libs/k5_uv_system/system.cpp
index e1de4c8..d716dd0 100644
--- a/libs/k5_uv_system/system.cpp
+++ b/libs/k5_uv_system/system.cpp
@@ -22,4 +22,4 @@ void System::CopyDataSection()
 extern "C" unsigned int __wrap___udivsi3(unsigned int a, unsigned int b)
 {
    return OrgFunc_01_26.IntDivide(a, b);
-}
\ No newline at end of file
+}
diff --git a/libs/k5_uv_system/system.hpp b/libs/k5_uv_system/system.hpp
index 56f15d1..0b32933 100644
--- a/libs/k5_uv_system/system.hpp
+++ b/libs/k5_uv_system/system.hpp
@@ -122,4 +122,4 @@ namespace System
    };
 }
 
-extern "C" void __libc_init_array();
\ No newline at end of file
+extern "C" void __libc_init_array();
diff --git a/libs/radio/radio.hpp b/libs/radio/radio.hpp
index bc9721b..7688655 100644
--- a/libs/radio/radio.hpp
+++ b/libs/radio/radio.hpp
@@ -120,6 +120,14 @@ public:
     Fw.BK4819Write(0x30, Reg);
   }
 
+  static void ToggleRXDSP(bool enabled) {
+    auto Reg = Fw.BK4819Read(0x30);
+    Reg &= ~1;
+    if (enabled)
+      Reg |= 1;
+    Fw.BK4819Write(0x30, Reg);
+  }
+
   void SendSyncAirCopyMode72(unsigned char *p8Data) {
     Fw.BK4819ConfigureAndStartTxFsk();
     Fw.AirCopyFskSetup();
diff --git a/src/spectrum_fagci/spectrum.hpp b/src/spectrum_fagci/spectrum.hpp
index 5ce90b4..ef0ee1c 100644
--- a/src/spectrum_fagci/spectrum.hpp
+++ b/src/spectrum_fagci/spectrum.hpp
@@ -31,7 +31,7 @@ public:
   CSpectrum()
       : DisplayBuff(FwData.pDisplayBuffer), FontSmallNr(FwData.pSmallDigs),
         Display(DisplayBuff), scanDelay(800), sampleZoom(2), scanStep(25_KHz),
-        frequencyChangeStep(100_KHz), rssiTriggerLevel(65) {
+        frequencyChangeStep(100_KHz), rssiTriggerLevel(65), stickyPeakTrigger(false) {
     Display.SetFont(&FontSmallNr);
   };
 
@@ -47,17 +47,12 @@ public:
       RadioDriver.ToggleAFDAC(true);
     }
 
-    // measure peak for this moment
+    Listen(1000000);
 
-    highestPeakRssi = GetRssi(); // also sets freq for us
+    highestPeakRssi = GetRssi();
     rssiHistory[highestPeakX >> sampleZoom] = highestPeakRssi;
 
-    if (highestPeakRssi >= rssiTriggerLevel) {
-      Listen(1000000);
-      return true;
-    }
-
-    return false;
+    return true;
   }
 
   inline void Scan() {
@@ -67,6 +62,7 @@ public:
 
     rssiMin = 255;
     fMeasure = FStart;
+
     RadioDriver.ToggleAFDAC(false);
     Fw.BK4819Write(0x47, 0);
 
@@ -124,7 +120,9 @@ public:
   }
 
   inline void DrawRssiTriggerLevel() {
-    Display.DrawLine(0, 127, Rssi2Y(rssiTriggerLevel));
+    for (u8 x = 0; x < 128; x += stickyPeakTrigger ? 2 : 4) {
+      Display.DrawLine(x, x + 2, Rssi2Y(rssiTriggerLevel));
+    }
   }
 
   inline void DrawTicks() {
@@ -194,8 +192,9 @@ public:
       break;
     case 5:
       ToggleBacklight();
-    default:
-      isUserInput = false;
+    case 0:
+      stickyPeakTrigger = !stickyPeakTrigger;
+      OnUserInput();
     }
   }
 
@@ -261,7 +260,6 @@ public:
   }
 
   inline void OnUserInput() {
-    isUserInput = true;
     u32 halfOfScanRange = scanStep << (6 - sampleZoom);
     FStart = currentFreq - halfOfScanRange;
 
@@ -316,7 +314,12 @@ private:
   }
 
   u8 GetRssi() {
-    Fw.BK4819Read(0x67); //reset RSSI =)
+    if (!stickyPeakTrigger) {
+      // reset RSSI register
+      RadioDriver.ToggleRXDSP(false);
+      RadioDriver.ToggleRXDSP(true);
+    }
+
     Fw.DelayUs(scanDelay);
     return Fw.BK4819Read(0x67);
   }
@@ -365,8 +368,8 @@ private:
   u32 scanStep;
   u32 frequencyChangeStep;
   u8 rssiTriggerLevel;
+  bool stickyPeakTrigger;
 
   bool working = false;
-  bool isUserInput = false;
   bool bDisplayCleared = true;
 };