feat(mqtt): filter temperature values using kalman (#38)

AquaMQTT/include/config/Configuration.h

@@ -59,6 +59,34 @@ constexpr uint32_t MQTT_FULL_UPDATE_MS = 1000 * 60 * 30;
  */
 constexpr uint16_t MQTT_STATS_UPDATE_MS = 5000;
 
+/**
+ * Use kalman filters for removing noise from the temperature values. This reduces updates sent to MQTT.
+ * Note: Filters are only applied within the MQTT channel, the communication between HMI and Controller
+ * are still using raw temperature values and is not altered.
+ */
+constexpr bool MQTT_FILTER_TEMPERATURE_NOISE = true;
+
+/**
+ * Parametrize kalman filter for reading temperature values
+ * Measurement Uncertainty - How much do we expect to our measurement vary
+ */
+
+constexpr float KALMAN_MEA_E = 0.1;
+
+/**
+ * Parametrize kalman filter for reading temperature values
+ * Estimation Uncertainty - Can be initialized with the same value as e_mea since the kalman filter will adjust its
+ * value.
+ */
+
+constexpr float KALMAN_EST_E = 0.1;
+
+/**
+ * Parametrize kalman filter for reading temperature values
+ * Process Variance - usually a small number between 0.001 and 1 - how fast your measurement moves.
+ */
+constexpr float KALMAN_Q = 0.01;
+
 /**
  * Self explanatory internal settings: most probably you don't want to change them.
  */

AquaMQTT/include/message/MainStatusMessage.h

@@ -19,12 +19,20 @@ class MainStatusMessage
 
     float hotWaterTemp();
 
+    void setHotWaterTemp(float temp);
+
     float airTemp();
 
+    void setAirTemp(float temp);
+
     float evaporatorLowerAirTemp();
 
+    void setEvaporatorLowerAirTemp(float temp);
+
     float evaporatorUpperAirTemp();
 
+    void setEvaporatorUpperAirTemp(float temp);
+
     float fanSpeedPwm();
 
     bool stateHeatingElement();

AquaMQTT/include/task/MQTTTask.h

@@ -4,6 +4,7 @@
 #include <MQTT.h>
 #include <WiFiClient.h>
 
+#include "SimpleKalmanFilter.h"
 #include "config/Configuration.h"
 #include "message/HMIMessage.h"
 #include "message/MainEnergyMessage.h"
@@ -52,6 +53,11 @@ class MQTTTask
     uint8_t* mLastProcessedEnergyMessage;
     uint8_t* mLastProcessedMainMessage;
 
+    SimpleKalmanFilter mEvaporatorLowerAirTempFilter;
+    SimpleKalmanFilter mEvaporatorUpperAirTempFilter;
+    SimpleKalmanFilter mAirTempFilter;
+    SimpleKalmanFilter mHotWaterTempFilter;
+
     // helper to avoid code duplication
     void publishFloat(const char* subtopic, const char* topic, float value, bool retained = false);
     void publishString(const char* subtopic, const char* topic, const char* value, bool retained = false);
@@ -63,6 +69,7 @@ class MQTTTask
             const char*   topic,
             unsigned long value,
             bool          retained = false);
+    void applyTemperatureFilter(message::MainStatusMessage* pMessage);
 };
 }  // namespace aquamqtt
 

AquaMQTT/platformio.ini

@@ -34,4 +34,5 @@ lib_deps =
     adafruit/Adafruit BusIO
     adafruit/RTClib
     Wire
-    SPI
+    SPI
+    https://github.com/denyssene/SimpleKalmanFilter.git

AquaMQTT/src/message/MainStatusMessage.cpp

@@ -9,19 +9,50 @@ float message::MainStatusMessage::hotWaterTemp()
 {
     return (float) (((short int) (mData[2] << 8) | mData[1]) / 10.0);
 }
+
+void MainStatusMessage::setHotWaterTemp(float temp)
+{
+    short int rawValue = temp * 100 / 10;
+    mData[1]           = rawValue & 0xFF;
+    mData[2]           = (rawValue >> 8) & 0xFF;
+}
+
 float MainStatusMessage::airTemp()
 {
     return (float) (((short int) (mData[4] << 8) | mData[3]) / 10.0);
 }
+
+void MainStatusMessage::setAirTemp(float temp)
+{
+    short int rawValue = temp * 100 / 10;
+    mData[3]           = rawValue & 0xFF;
+    mData[4]           = (rawValue >> 8) & 0xFF;
+}
+
 float MainStatusMessage::evaporatorLowerAirTemp()
 {
     return (float) (((short int) (mData[6] << 8) | mData[5]) / 10.0);
 }
+
+void MainStatusMessage::setEvaporatorLowerAirTemp(float temp)
+{
+    short int rawValue = temp * 100 / 10;
+    mData[5]           = rawValue & 0xFF;
+    mData[6]           = (rawValue >> 8) & 0xFF;
+}
+
 float MainStatusMessage::evaporatorUpperAirTemp()
 {
     return (float) (((short int) (mData[8] << 8) | mData[7]) / 10.0);
 }
 
+void MainStatusMessage::setEvaporatorUpperAirTemp(float temp)
+{
+    short int rawValue = temp * 100 / 10;
+    mData[7]           = rawValue & 0xFF;
+    mData[8]           = (rawValue >> 8) & 0xFF;
+}
+
 float MainStatusMessage::fanSpeedPwm()
 {
     return (float) (((short int) (mData[19] << 8) | mData[18]) / 10.0);

AquaMQTT/src/task/MQTTTask.cpp

@@ -26,6 +26,10 @@ MQTTTask::MQTTTask()
     , mLastProcessedHMIMessage(nullptr)
     , mLastProcessedEnergyMessage(nullptr)
     , mLastProcessedMainMessage(nullptr)
+    , mHotWaterTempFilter(config::KALMAN_MEA_E, config::KALMAN_EST_E, config::KALMAN_Q)
+    , mAirTempFilter(config::KALMAN_MEA_E, config::KALMAN_EST_E, config::KALMAN_Q)
+    , mEvaporatorLowerAirTempFilter(config::KALMAN_MEA_E, config::KALMAN_EST_E, config::KALMAN_Q)
+    , mEvaporatorUpperAirTempFilter(config::KALMAN_MEA_E, config::KALMAN_EST_E, config::KALMAN_Q)
 {
 }
 
@@ -234,7 +238,7 @@ void MQTTTask::spawn()
 
 #pragma clang diagnostic push
 #pragma clang diagnostic ignored "-Wformat"
-void                     MQTTTask::setup()
+void MQTTTask::setup()
 {
     mMQTTClient.begin(aquamqtt::config::brokerAddr, aquamqtt::config::brokerPort, mWiFiClient);
     sprintf(reinterpret_cast<char*>(mTopicBuffer),
@@ -380,7 +384,7 @@ void MQTTTask::loop()
 
 #pragma clang diagnostic push
 #pragma clang diagnostic ignored "-Wformat"
-void                     MQTTTask::updateStats()
+void MQTTTask::updateStats()
 {
     publishString(
             STATS_SUBTOPIC,
@@ -480,9 +484,12 @@ void                     MQTTTask::updateStats()
 
 #pragma clang diagnostic push
 #pragma clang diagnostic ignored "-Wformat"
-void                     MQTTTask::updateMainStatus(bool fullUpdate)
+void MQTTTask::updateMainStatus(bool fullUpdate)
 {
     message::MainStatusMessage message(mTransferBuffer);
+
+    applyTemperatureFilter(&message);
+
     message.compareWith(fullUpdate ? nullptr : mLastProcessedMainMessage);
 
     if (message.hotWaterTempChanged())
@@ -582,7 +589,7 @@ void                     MQTTTask::updateMainStatus(bool fullUpdate)
 
 #pragma clang diagnostic push
 #pragma clang diagnostic ignored "-Wformat"
-void                     MQTTTask::updateHMIStatus(bool fullUpdate)
+void MQTTTask::updateHMIStatus(bool fullUpdate)
 {
     message::HMIMessage message(mTransferBuffer);
     message.compareWith(fullUpdate ? nullptr : mLastProcessedHMIMessage);
@@ -713,7 +720,7 @@ void                     MQTTTask::updateHMIStatus(bool fullUpdate)
 
 #pragma clang diagnostic push
 #pragma clang diagnostic ignored "-Wformat"
-void                     MQTTTask::updateEnergyStats(bool fullUpdate)
+void MQTTTask::updateEnergyStats(bool fullUpdate)
 {
     message::MainEnergyMessage message(mTransferBuffer);
     message.compareWith(fullUpdate ? nullptr : mLastProcessedEnergyMessage);
@@ -760,7 +767,8 @@ void                     MQTTTask::updateEnergyStats(bool fullUpdate)
         publishul(ENERGY_SUBTOPIC, ENERGY_POWER_TOTAL, message.powerOverall());
     }
 
-    if(message.totalWaterProductionChanged()) {
+    if (message.totalWaterProductionChanged())
+    {
         publishul(ENERGY_SUBTOPIC, ENERGY_TOTAL_WATER_PRODUCTION, message.totalWaterProduction());
     }
 
@@ -953,4 +961,17 @@ void MQTTTask::publishul(
     mMQTTClient.publish(reinterpret_cast<char*>(mTopicBuffer), reinterpret_cast<char*>(mPayloadBuffer), retained, 0);
 }
 
+void MQTTTask::applyTemperatureFilter(message::MainStatusMessage* message)
+{
+    if (config::MQTT_FILTER_TEMPERATURE_NOISE)
+    {
+        message->setEvaporatorLowerAirTemp(
+                mEvaporatorLowerAirTempFilter.updateEstimate(message->evaporatorLowerAirTemp()));
+        message->setEvaporatorUpperAirTemp(
+                mEvaporatorUpperAirTempFilter.updateEstimate(message->evaporatorUpperAirTemp()));
+        message->setAirTemp(mAirTempFilter.updateEstimate(message->airTemp()));
+        message->setHotWaterTemp(mHotWaterTempFilter.updateEstimate(message->hotWaterTemp()));
+    }
+}
+
 }  // namespace aquamqtt