From bb747bdbbd8f04591caf608d2a07745fbe4b0f00 Mon Sep 17 00:00:00 2001
From: Sjef Verhoeven PE5PVB <pe5pvb@het-bar.net>
Date: Thu, 2 Nov 2023 20:53:46 +0100
Subject: [PATCH] Fix for AFREG and AF detector

---
 TEF6686_ESP32.ino | 5 ++---
 src/TEF6686.cpp   | 8 ++++----
 2 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/TEF6686_ESP32.ino b/TEF6686_ESP32.ino
index 01dd934..3d67e33 100644
--- a/TEF6686_ESP32.ino
+++ b/TEF6686_ESP32.ino
@@ -432,6 +432,7 @@ void setup() {
   AMLevelOffset = EEPROM.readInt(EE_INT16_AMLEVELOFFSET);
   unit = EEPROM.readByte(EE_BYTE_UNIT);
   af = EEPROM.readByte(EE_BYTE_AF);
+  if (af == 2) radio.rds.afreg = true; else radio.rds.afreg = false;
   StereoToggle = EEPROM.readByte(EE_BYTE_STEREO);
   batteryoptions = EEPROM.readByte(EE_BYTE_BATTERY_OPTIONS);
   amcodect = EEPROM.readByte(EE_BYTE_AM_CO_DECT);
@@ -745,7 +746,6 @@ void loop() {
       if (freqold != frequency) {
         ShowFreq(0);
         if (radio.afmethodB) {
-          if (af == 2) radio.rds.afreg = true; else radio.rds.afreg = false;
           afmethodBold = true;
           radio.clearRDS(fullsearchrds);
         }
@@ -762,7 +762,6 @@ void loop() {
         if (freqold != frequency) {
           ShowFreq(0);
           if (radio.afmethodB) {
-            if (af == 2) radio.rds.afreg = true; else radio.rds.afreg = false;
             afmethodBold = true;
             radio.clearRDS(fullsearchrds);
           }
@@ -778,7 +777,6 @@ void loop() {
           if (freqold != frequency) {
             ShowFreq(0);
             if (radio.afmethodB) {
-              if (af == 2) radio.rds.afreg = true; else radio.rds.afreg = false;
               afmethodBold = true;
               radio.clearRDS(fullsearchrds);
             }
@@ -1884,6 +1882,7 @@ void ModeButtonPress() {
         EEPROM.writeByte(EE_BYTE_MWREGION, mwstepsize);
         EEPROM.writeByte(EE_BYTE_SPISPEED, spispeed);
         EEPROM.commit();
+        if (af == 2) radio.rds.afreg = true; else radio.rds.afreg = false;
         Serial.end();
         if (wifi) remoteip = IPAddress (WiFi.localIP()[0], WiFi.localIP()[1], WiFi.localIP()[2], subnetclient);
         if (USBmode) Serial.begin(19200); else Serial.begin(115200);
diff --git a/src/TEF6686.cpp b/src/TEF6686.cpp
index 7e4146b..9709b5a 100644
--- a/src/TEF6686.cpp
+++ b/src/TEF6686.cpp
@@ -32,8 +32,9 @@ void TEF6686::TestAFEON() {
         devTEF_Set_Cmd(TEF_FM, Cmd_Tune_To, 7, 4, af[x].frequency);
         delay(200);
         devTEF_Radio_Get_RDS_Status(&rds.rdsStat, &rds.rdsA, &rds.rdsB, &rds.rdsC, &rds.rdsD, &rds.rdsErr);
+
         if (rds.rdsStat & (1 << 9)) {
-          if (((afmethodB && rds.afreg && ((rds.rdsA >> 8) & 0x0F) == (rds.correctPI & 0x0F)) || (!afmethodB && rds.rdsA == rds.correctPI)) && (((rds.rdsErr >> 14) & 0x03) == 0)) {
+          if ((afmethodB && rds.afreg ? (((rds.rdsA >> 8) & 0xF) > 2 && ((rds.correctPI >> 8) & 0xF) > 2 && ((rds.rdsA >> 12) & 0xF) == ((rds.correctPI >> 12) & 0xF) && (rds.rdsA & 0xFF) == (rds.correctPI & 0xFF)) || rds.rdsA == rds.correctPI : rds.rdsA == rds.correctPI)) {
             af[x].checked = true;
             af[x].afvalid = true;
           } else {
@@ -98,7 +99,7 @@ uint16_t TEF6686::TestAF() {
       delay(200);
       devTEF_Radio_Get_RDS_Status(&rds.rdsStat, &rds.rdsA, &rds.rdsB, &rds.rdsC, &rds.rdsD, &rds.rdsErr);
       if (rds.rdsStat & (1 << 9)) {
-        if (((afmethodB && rds.afreg && ((rds.rdsA >> 8) & 0x0F) == (rds.correctPI & 0x0F)) || (!afmethodB && rds.rdsA == rds.correctPI)) && (((rds.rdsErr >> 14) & 0x03) == 0)) {
+        if ((afmethodB && rds.afreg ? (((rds.rdsA >> 8) & 0xF) > 2 && ((rds.correctPI >> 8) & 0xF) > 2 && ((rds.rdsA >> 12) & 0xF) == ((rds.correctPI >> 12) & 0xF) && (rds.rdsA & 0xFF) == (rds.correctPI & 0xFF)) || rds.rdsA == rds.correctPI : rds.rdsA == rds.correctPI)) {
           currentfreq = af[highestIndex].frequency;
           for (byte y = 0; y < 50; y++) {
             af[y].frequency = 0;
@@ -1420,8 +1421,7 @@ void TEF6686::readRDS(byte showrdserrors)
   }
 }
 
-void TEF6686::clearRDS (bool fullsearchrds)
-{
+void TEF6686::clearRDS (bool fullsearchrds) {
   devTEF_Radio_Set_RDS(fullsearchrds);
   rds.stationName = "";
   rds.stationText = "";