From 2f90b401cbbb3d4a51e77dd2632cca6c0a228373 Mon Sep 17 00:00:00 2001
From: kusy <110433144+mrkusypl@users.noreply.github.com>
Date: Wed, 4 Mar 2026 18:06:41 +0000
Subject: [PATCH] Restore FM_CheckFrequencyLock()

---
 App/app/fm.c | 55 +++++++++++++++++++++++++++++++++++++++-------------
 1 file changed, 41 insertions(+), 14 deletions(-)

diff --git a/App/app/fm.c b/App/app/fm.c
index 567c24b0..93831a8b 100644
--- a/App/app/fm.c
+++ b/App/app/fm.c
@@ -182,38 +182,65 @@ void FM_PlayAndUpdate(void)
 
 int FM_CheckFrequencyLock(uint16_t Frequency, uint16_t LowerLimit)
 {
+    int ret = -1;
+
     const uint16_t Test2 = BK1080_ReadRegister(BK1080_REG_07);
 
     // This is supposed to be a signed value, but above function is unsigned
     const uint16_t Deviation = BK1080_REG_07_GET_FREQD(Test2);
+
+    if (BK1080_REG_07_GET_SNR(Test2) <= 2) {
+        BK1080_FrequencyDeviation = Deviation;
+        BK1080_BaseFrequency      = Frequency;
+
+        return ret;
+    }
+
     const uint16_t Status = BK1080_ReadRegister(BK1080_REG_10);
 
-    BK1080_FrequencyDeviation = Deviation;
-    BK1080_BaseFrequency = Frequency;
+    if ((Status & BK1080_REG_10_MASK_AFCRL) != BK1080_REG_10_AFCRL_NOT_RAILED || BK1080_REG_10_GET_RSSI(Status) < 10) {
+        BK1080_FrequencyDeviation = Deviation;
+        BK1080_BaseFrequency      = Frequency;
 
-    if (BK1080_REG_07_GET_SNR(Test2) <= 2)
-        return -1;
-
-    if ((Status & BK1080_REG_10_MASK_AFCRL) != BK1080_REG_10_AFCRL_NOT_RAILED || BK1080_REG_10_GET_RSSI(Status) < 10)
-        return -1;
+        return ret;
+    }
 
     //if (Deviation > -281 && Deviation < 280)
-    if (Deviation >= 280 && Deviation <= 3815)
-        return -1;
-    
+    if (Deviation >= 280 && Deviation <= 3815) {
+        BK1080_FrequencyDeviation = Deviation;
+        BK1080_BaseFrequency      = Frequency;
+
+        return ret;
+    }
+
     // not BLE(less than or equal)
     if (Frequency > LowerLimit && (Frequency - BK1080_BaseFrequency) == 1) {
-        if ((BK1080_FrequencyDeviation & 0x800) || (BK1080_FrequencyDeviation < 20))
-            return -1;
+        if (BK1080_FrequencyDeviation & 0x800 || (BK1080_FrequencyDeviation < 20))
+        {
+            BK1080_FrequencyDeviation = Deviation;
+            BK1080_BaseFrequency      = Frequency;
+
+            return ret;
+        }
     }
 
     // not BLT(less than)
+
     if (Frequency >= LowerLimit && (BK1080_BaseFrequency - Frequency) == 1) {
         if ((BK1080_FrequencyDeviation & 0x800) == 0 || (BK1080_FrequencyDeviation > 4075))
-            return -1;
+        {
+            BK1080_FrequencyDeviation = Deviation;
+            BK1080_BaseFrequency      = Frequency;
+
+            return ret;
+        }
     }
 
-    return 0;
+    ret = 0;
+    BK1080_FrequencyDeviation = Deviation;
+    BK1080_BaseFrequency      = Frequency;
+
+    return ret;
 }
 
 static void Key_DIGITS(KEY_Code_t Key, uint8_t state)