From 58285e492bc75568284473f47ed7969f8ad43634 Mon Sep 17 00:00:00 2001 From: Thomas Popp Date: Sun, 13 Oct 2024 14:28:39 +0200 Subject: [PATCH] feat(filter): filter temperature values published to mqtt using kalman filter --- AquaMQTT/include/config/Configuration.h | 28 +++++++++++++++++ AquaMQTT/include/message/MainStatusMessage.h | 8 +++++ AquaMQTT/include/task/MQTTTask.h | 7 +++++ AquaMQTT/platformio.ini | 3 +- AquaMQTT/src/message/MainStatusMessage.cpp | 31 ++++++++++++++++++ AquaMQTT/src/task/MQTTTask.cpp | 33 ++++++++++++++++---- 6 files changed, 103 insertions(+), 7 deletions(-) diff --git a/AquaMQTT/include/config/Configuration.h b/AquaMQTT/include/config/Configuration.h index 0c872e6..91bb2dc 100644 --- a/AquaMQTT/include/config/Configuration.h +++ b/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. */ diff --git a/AquaMQTT/include/message/MainStatusMessage.h b/AquaMQTT/include/message/MainStatusMessage.h index 6bc651a..6f29aed 100644 --- a/AquaMQTT/include/message/MainStatusMessage.h +++ b/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(); diff --git a/AquaMQTT/include/task/MQTTTask.h b/AquaMQTT/include/task/MQTTTask.h index 1efe2b3..d979846 100644 --- a/AquaMQTT/include/task/MQTTTask.h +++ b/AquaMQTT/include/task/MQTTTask.h @@ -4,6 +4,7 @@ #include #include +#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 diff --git a/AquaMQTT/platformio.ini b/AquaMQTT/platformio.ini index b43723a..9b42edf 100644 --- a/AquaMQTT/platformio.ini +++ b/AquaMQTT/platformio.ini @@ -34,4 +34,5 @@ lib_deps = adafruit/Adafruit BusIO adafruit/RTClib Wire - SPI \ No newline at end of file + SPI + https://github.com/denyssene/SimpleKalmanFilter.git \ No newline at end of file diff --git a/AquaMQTT/src/message/MainStatusMessage.cpp b/AquaMQTT/src/message/MainStatusMessage.cpp index 287618c..b3ac40b 100644 --- a/AquaMQTT/src/message/MainStatusMessage.cpp +++ b/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); diff --git a/AquaMQTT/src/task/MQTTTask.cpp b/AquaMQTT/src/task/MQTTTask.cpp index eea76e1..6312a36 100644 --- a/AquaMQTT/src/task/MQTTTask.cpp +++ b/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(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(mTopicBuffer), reinterpret_cast(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