Fix RDS data in XDR-GTK protocol messages

src/RdsPiBuffer.cpp

@@ -0,0 +1,78 @@
+#include "RdsPiBuffer.hpp"
+
+RdsPiBuffer::RdsPiBuffer()
+{
+    this->clear();
+}
+
+RdsPiBuffer::State
+RdsPiBuffer::add(uint16_t value,
+                 bool     error)
+{
+    this->pos = (this->pos + 1) % BUFF_SIZE;
+    this->buff[this->pos] = value;
+
+    const uint8_t errorPos = this->pos / 8;
+    const uint8_t errorBitPos = this->pos % 8;
+
+    if (error)
+    {
+        this->errorBuff[errorPos] |= (1 << errorBitPos);
+    }
+    else
+    {
+        this->errorBuff[errorPos] &= ~(1 << errorBitPos);
+    }
+
+    if (this->fill < BUFF_SIZE)
+    {
+        this->fill++;
+    }
+
+    return this->getState(value);
+}
+
+void
+RdsPiBuffer::clear()
+{
+    this->fill = 0;
+    this->pos = (uint8_t)-1;
+}
+
+RdsPiBuffer::State
+RdsPiBuffer::getState(uint16_t value)
+{
+    uint8_t count = 0;
+    uint8_t correctCount = 0;
+
+    for (uint8_t i = 0; i < this->fill; i++)
+    {
+        if (this->buff[i] == value)
+        {
+            count++;
+            if ((this->errorBuff[i / 8] & (1 << (i % 8))) == 0)
+            {
+                correctCount++;
+            }
+        }
+    }
+
+    if (correctCount >= 2)
+    {
+        return STATE_CORRECT;
+    }
+    else if (count >= 2 && correctCount)
+    {
+        return STATE_VERY_LIKELY;
+    }
+    else if (count >= 3)
+    {
+        return STATE_LIKELY;
+    }
+    else if (count == 2 || correctCount)
+    {
+        return STATE_UNLIKELY;
+    }
+
+    return STATE_INVALID;
+}

src/RdsPiBuffer.hpp

@@ -0,0 +1,33 @@
+#ifndef RDS_PI_BUFFER_H
+#define RDS_PI_BUFFER_H
+
+#include <stdint.h>
+
+class RdsPiBuffer
+{
+public:
+    enum State : uint8_t
+    {
+        STATE_CORRECT     = 0,
+        STATE_VERY_LIKELY = 1,
+        STATE_LIKELY      = 2,
+        STATE_UNLIKELY    = 3,
+        STATE_INVALID     = 4
+    };
+
+    RdsPiBuffer();
+    State add(uint16_t value, bool error);
+    void clear();
+
+private:
+    /* BUFF_SIZE must be a multiple of 8 */
+    static constexpr uint8_t BUFF_SIZE = 64;
+    uint16_t buff[BUFF_SIZE];
+    uint8_t errorBuff[BUFF_SIZE/8];
+    uint8_t fill;
+    uint8_t pos;
+
+    State getState(uint16_t value);
+};
+
+#endif

src/TEF6686.cpp

@@ -1493,6 +1493,7 @@ void TEF6686::readRDS(byte showrdserrors)
 
 void TEF6686::clearRDS (bool fullsearchrds) {
   devTEF_Radio_Set_RDS(fullsearchrds);
+  rds.piBuffer.clear();
   rds.stationName = "";
   rds.stationText = "";
   rds.stationText32 = "";

src/TEF6686.h

@@ -4,6 +4,7 @@
 #include "Arduino.h"
 #include "Tuner_Drv_Lithio.h"
 #include "Tuner_Interface.h"
+#include "RdsPiBuffer.hpp"
 
 extern const unsigned char tuner_init_tab[] PROGMEM;
 extern const unsigned char tuner_init_tab9216[] PROGMEM;
@@ -614,6 +615,7 @@ typedef struct _rds_ {
   bool fastps;
   bool rtbuffer = true;
   bool afreg;
+  RdsPiBuffer piBuffer;
 } rds_;
 
 typedef struct _af_ {

src/rds.cpp

@@ -359,12 +359,22 @@ void readRds() {
     }
 
     if ((RDSstatus && XDRGTKUSB) || (RDSstatus && XDRGTKTCP)) {
-      DataPrint ("P");
-      DataPrint (String(((radio.rds.rdsA >> 8) >> 4) & 0xF, HEX) + String((radio.rds.rdsA >> 8) & 0xF, HEX));
-      DataPrint (String(((radio.rds.rdsA) >> 4) & 0xF, HEX) + String((radio.rds.rdsA) & 0xF, HEX));
-      if (((radio.rds.rdsErr >> 14) & 0x02) > 2) DataPrint("?");
-      if (((radio.rds.rdsErr >> 14) & 0x01) > 1) DataPrint("?");
-      DataPrint ("\n");
+
+      uint8_t piError = radio.rds.rdsErr >> 14;
+      if (piError < 3) {
+        uint8_t piState = radio.rds.piBuffer.add(radio.rds.rdsA, piError);
+
+        if (piState != RdsPiBuffer::STATE_INVALID) {
+          DataPrint ("P");
+          DataPrint (String(((radio.rds.rdsA >> 8) >> 4) & 0xF, HEX) + String((radio.rds.rdsA >> 8) & 0xF, HEX));
+          DataPrint (String(((radio.rds.rdsA) >> 4) & 0xF, HEX) + String((radio.rds.rdsA) & 0xF, HEX));
+          while (piState != 0) {
+            DataPrint("?");
+            piState--;
+          }
+          DataPrint ("\n");
+        }
+      }
 
       XDRGTKRDS = "R";
       XDRGTKRDS += String(((radio.rds.rdsB >> 8) >> 4) & 0xF, HEX) + String((radio.rds.rdsB >> 8) & 0xF, HEX);
@@ -375,17 +385,12 @@ void readRds() {
       XDRGTKRDS += String(((radio.rds.rdsD) >> 4) & 0xF, HEX) + String((radio.rds.rdsD) & 0xF, HEX);
 
       uint8_t erroutput = 0;
-      erroutput |= (highByte(radio.rds.rdsErr) & 0x04) >> 2;
-      erroutput |= (highByte(radio.rds.rdsErr) & 0x02) << 2;
-      erroutput |= (highByte(radio.rds.rdsErr) & 0x01) << 6;
-      erroutput |= (highByte(radio.rds.rdsErr) & 0x08) >> 3;
-      erroutput |= (highByte(radio.rds.rdsErr) & 0x10) >> 1;
-      erroutput |= (highByte(radio.rds.rdsErr) & 0x40) << 1;
-      erroutput |= (highByte(radio.rds.rdsErr) & 0x80) >> 7;
-      erroutput |= (highByte(radio.rds.rdsErr) & 0x20) << 5;
+      erroutput |= (radio.rds.rdsErr >> 8) & B00110000 >> 4;
+      erroutput |= (radio.rds.rdsErr >> 8) & B00001100;
+      erroutput |= (radio.rds.rdsErr >> 8) & B00000011 << 4;
 
-      if (highByte(radio.rds.rdsErr) < 0x10) XDRGTKRDS += "0";
-      XDRGTKRDS += String(erroutput, HEX);
+      XDRGTKRDS += String((erroutput >> 4) & 0xF, HEX);
+      XDRGTKRDS += String(erroutput & 0xF, HEX);
       XDRGTKRDS += "\n";
 
       if (XDRGTKRDS != XDRGTKRDSold) {