diff --git a/App/app/fm.c b/App/app/fm.c
index 93831a8b..d3f53e3a 100644
--- a/App/app/fm.c
+++ b/App/app/fm.c
@@ -182,65 +182,45 @@ 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 Test2     = BK1080_ReadRegister(BK1080_REG_07);
     const uint16_t Deviation = BK1080_REG_07_GET_FREQD(Test2);
 
-    if (BK1080_REG_07_GET_SNR(Test2) <= 2) {
-        BK1080_FrequencyDeviation = Deviation;
-        BK1080_BaseFrequency      = Frequency;
+    // Helper macro to update globals and return
+    #define RETURN(val) \
+        do { \
+            BK1080_FrequencyDeviation = Deviation; \
+            BK1080_BaseFrequency      = Frequency; \
+            return (val); \
+        } while (0)
 
-        return ret;
-    }
+    if (BK1080_REG_07_GET_SNR(Test2) <= 2)
+        RETURN(-1);
 
     const uint16_t Status = BK1080_ReadRegister(BK1080_REG_10);
+    if ((Status & BK1080_REG_10_MASK_AFCRL) != BK1080_REG_10_AFCRL_NOT_RAILED ||
+        BK1080_REG_10_GET_RSSI(Status) < 10)
+        RETURN(-1);
 
-    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 (Deviation >= 280 && Deviation <= 3815)
+        RETURN(-1);
 
-        return ret;
-    }
-
-    //if (Deviation > -281 && Deviation < 280)
-    if (Deviation >= 280 && Deviation <= 3815) {
-        BK1080_FrequencyDeviation = Deviation;
-        BK1080_BaseFrequency      = Frequency;
-
-        return ret;
-    }
-
-    // not BLE(less than or equal)
+    // Scanning upward: previous deviation was negative (bit 11 set) or near zero
     if (Frequency > LowerLimit && (Frequency - BK1080_BaseFrequency) == 1) {
-        if (BK1080_FrequencyDeviation & 0x800 || (BK1080_FrequencyDeviation < 20))
-        {
-            BK1080_FrequencyDeviation = Deviation;
-            BK1080_BaseFrequency      = Frequency;
-
-            return ret;
-        }
+        if (BK1080_FrequencyDeviation & 0x800 || BK1080_FrequencyDeviation < 20)
+            RETURN(-1);
     }
 
-    // not BLT(less than)
-
+    // Scanning downward: previous deviation was positive or saturated high
     if (Frequency >= LowerLimit && (BK1080_BaseFrequency - Frequency) == 1) {
-        if ((BK1080_FrequencyDeviation & 0x800) == 0 || (BK1080_FrequencyDeviation > 4075))
-        {
-            BK1080_FrequencyDeviation = Deviation;
-            BK1080_BaseFrequency      = Frequency;
-
-            return ret;
-        }
+        if ((BK1080_FrequencyDeviation & 0x800) == 0 || BK1080_FrequencyDeviation > 4075)
+            RETURN(-1);
     }
 
-    ret = 0;
+    #undef RETURN
+
     BK1080_FrequencyDeviation = Deviation;
     BK1080_BaseFrequency      = Frequency;
-
-    return ret;
+    return 0;
 }
 
 static void Key_DIGITS(KEY_Code_t Key, uint8_t state)