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;
+}