Fix F+4 verify VHF harmonic before accepting frequency

App/app/scanner.c

@@ -41,6 +41,101 @@ bool              gScanUseCssResult;
 STEP_Setting_t    stepSetting;
 uint8_t           scanHitCount;
 
+typedef enum
+{
+    SCAN_FREQ_VERIFY_OFF,
+    SCAN_FREQ_VERIFY_FUNDAMENTAL,
+    SCAN_FREQ_VERIFY_HARMONIC
+} SCAN_FrequencyVerifyState_t;
+
+static SCAN_FrequencyVerifyState_t scanFreqVerifyState;
+static uint32_t                    scanVerifyFundamental;
+static uint32_t                    scanVerifyHarmonic;
+static uint16_t                    scanVerifyFundamentalRssi;
+
+static bool SCANNER_ShouldVerifyVhfSecondHarmonic(const uint32_t frequency)
+{
+    const uint32_t fundamental = frequency / 2;
+
+    return frequency >= (frequencyBandTable[BAND3_137MHz].lower * 2) &&
+           frequency <  (frequencyBandTable[BAND4_174MHz].lower * 2) &&
+           fundamental >= frequencyBandTable[BAND3_137MHz].lower &&
+           fundamental <  frequencyBandTable[BAND4_174MHz].lower;
+}
+
+static void SCANNER_StartCssScanAtFrequency(const uint32_t frequency)
+{
+    gScanFrequency         = frequency;
+    gScanCssResultCode     = 0xFF;
+    gScanCssResultType     = 0xFF;
+    scanHitCount           = 0;
+    gScanUseCssResult      = false;
+    gScanProgressIndicator = 0;
+    gScanCssState          = SCAN_CSS_STATE_SCANNING;
+    gScanDelay_10ms        = scan_delay_10ms;
+
+    BK4819_SetScanFrequency(gScanFrequency);
+
+    if (!gCssBackgroundScan)
+        GUI_SelectNextDisplay(DISPLAY_SCANNER);
+
+    gUpdateStatus = true;
+}
+
+static void SCANNER_TuneForFrequencyVerification(const uint32_t frequency)
+{
+    BK4819_SetFrequency(frequency);
+    BK4819_PickRXFilterPathBasedOnFrequency(frequency);
+    BK4819_RX_TurnOn();
+}
+
+static uint16_t SCANNER_ReadVerificationRssi(void)
+{
+    (void)BK4819_GetRSSI();
+    return BK4819_GetRSSI();
+}
+
+static void SCANNER_StartFrequencyVerification(const uint32_t harmonic)
+{
+    scanVerifyFundamental = harmonic / 2;
+    scanVerifyHarmonic    = harmonic;
+    scanFreqVerifyState   = SCAN_FREQ_VERIFY_FUNDAMENTAL;
+
+    SCANNER_TuneForFrequencyVerification(scanVerifyFundamental);
+    gScanDelay_10ms = 2;
+}
+
+static bool SCANNER_HandleFrequencyVerification(void)
+{
+    switch (scanFreqVerifyState) {
+        case SCAN_FREQ_VERIFY_FUNDAMENTAL:
+            scanVerifyFundamentalRssi = SCANNER_ReadVerificationRssi();
+            scanFreqVerifyState       = SCAN_FREQ_VERIFY_HARMONIC;
+            SCANNER_TuneForFrequencyVerification(scanVerifyHarmonic);
+            gScanDelay_10ms = 2;
+            return true;
+
+        case SCAN_FREQ_VERIFY_HARMONIC: {
+            const uint16_t harmonicRssi = SCANNER_ReadVerificationRssi();
+            const uint16_t margin       = 4;
+            uint32_t verifiedFrequency  = scanVerifyHarmonic;
+
+            if (scanVerifyFundamentalRssi > harmonicRssi &&
+                scanVerifyFundamentalRssi - harmonicRssi >= margin)
+            {
+                verifiedFrequency = scanVerifyFundamental;
+            }
+
+            scanFreqVerifyState = SCAN_FREQ_VERIFY_OFF;
+            SCANNER_StartCssScanAtFrequency(verifiedFrequency);
+            return true;
+        }
+
+        default:
+            return false;
+    }
+}
+
 static void SCANNER_Key_DIGITS(KEY_Code_t Key, bool bKeyPressed, bool bKeyHeld)
 {
     if (!bKeyHeld && bKeyPressed)
@@ -367,6 +462,7 @@ void SCANNER_Start(bool singleFreq)
     g_SquelchLost          = false;
     gScannerSaveState      = SCAN_SAVE_NO_PROMPT;
     gScanProgressIndicator = 0;
+    scanFreqVerifyState    = SCAN_FREQ_VERIFY_OFF;
 }
 
 void SCANNER_Stop(void)
@@ -378,6 +474,7 @@ void SCANNER_Stop(void)
         gUpdateStatus            = true;
         gCssBackgroundScan       = false;
         gScanUseCssResult        = false;
+        scanFreqVerifyState      = SCAN_FREQ_VERIFY_OFF;
 #ifdef ENABLE_VOICE
         gAnotherVoiceID          = VOICE_ID_CANCEL;
 #endif
@@ -399,6 +496,10 @@ void SCANNER_TimeSlice10ms(void)
         return;
     }
 
+    if (SCANNER_HandleFrequencyVerification()) {
+        return;
+    }
+
     switch (gScanCssState) {
         case SCAN_CSS_STATE_OFF: {
             // must be RF frequency scanning if we're here ?
@@ -421,22 +522,16 @@ void SCANNER_TimeSlice10ms(void)
             if (scanHitCount < 3) {
                 BK4819_SetFrequencyScan(true);
             }
+            else if (SCANNER_ShouldVerifyVhfSecondHarmonic(gScanFrequency)) {
+                SCANNER_StartFrequencyVerification(gScanFrequency);
+            }
             else {
-                BK4819_SetScanFrequency(gScanFrequency);
-                gScanCssResultCode     = 0xFF;
-                gScanCssResultType     = 0xFF;
-                scanHitCount           = 0;
-                gScanUseCssResult      = false;
-                gScanProgressIndicator = 0;
-                gScanCssState          = SCAN_CSS_STATE_SCANNING;
-
-                if(!gCssBackgroundScan)
-                    GUI_SelectNextDisplay(DISPLAY_SCANNER);
-
-                gUpdateStatus          = true;
+                SCANNER_StartCssScanAtFrequency(gScanFrequency);
             }
 
-            gScanDelay_10ms = scan_delay_10ms;
+            if (scanFreqVerifyState == SCAN_FREQ_VERIFY_OFF) {
+                gScanDelay_10ms = scan_delay_10ms;
+            }
             //gScanDelay_10ms = 1;   // 10ms
             break;
         }