Merge branch 'master' of https://github.com/jomjol/AI-on-the-edge-device

2025-12-09 04:56:53 +03:00 · 2023-03-11 21:21:12 +01:00
parent 98d85b2c6c 6642f6f995
commit c81f901a19
29 changed files with 898 additions and 188 deletions
--- a/FeatureRequest.md
+++ b/FeatureRequest.md
@@ -10,6 +10,9 @@
   

 ____
+#### #36 Run demo without camera
+Demo mode requires a working camera (if not, one receives a 'Cam bad' error). Would be nice to demo or play around on other ESP32 boards (or on ESP32-CAM boards when you broke the camera cable...).
+
 #### #35 Use the same model, but provide the image from a Smartphone Camera
 as reading the Electricity or Water meter every few minutues only delivers apparent accuracy (DE: "Scheingenauigkeit") you could just as well take a picture with your Smartphone evey so often (e.g. once a week when you are in the Basement anyway), then with some "semi clever" tricks pass this image to the model developed here, and the values then on to who ever needs them e.g. via MQTT.
 IMO: It is not needed to have that many readings (datapoints) as our behaviour (Use of electricity or water) doesn't vary that much, say, over a weeks time. The interpolation between weekly readings will give sufficient information on the power and/or water usage. 
--- a/code/components/jomjol_controlGPIO/server_GPIO.cpp
+++ b/code/components/jomjol_controlGPIO/server_GPIO.cpp
@@ -82,10 +82,10 @@ static void gpioHandlerTask(void *arg) {

 void GpioPin::gpioInterrupt(int value) {
 #ifdef ENABLE_MQTT    
-    if (_mqttTopic != "") {
+    if (_mqttTopic.compare("") != 0) {
        ESP_LOGD(TAG, "gpioInterrupt %s %d", _mqttTopic.c_str(), value);

-        MQTTPublish(_mqttTopic, value ? "true" : "false");        
+        MQTTPublish(_mqttTopic, value ? "true" : "false", 1);        
    }
 #endif //ENABLE_MQTT
    currentState = value;
@@ -115,7 +115,7 @@ void GpioPin::init()
    }

 #ifdef ENABLE_MQTT
-    if ((_mqttTopic != "") && ((_mode == GPIO_PIN_MODE_OUTPUT) || (_mode == GPIO_PIN_MODE_OUTPUT_PWM) || (_mode == GPIO_PIN_MODE_BUILT_IN_FLASH_LED))) {
+    if ((_mqttTopic.compare("") != 0) && ((_mode == GPIO_PIN_MODE_OUTPUT) || (_mode == GPIO_PIN_MODE_OUTPUT_PWM) || (_mode == GPIO_PIN_MODE_BUILT_IN_FLASH_LED))) {
        std::function<bool(std::string, char*, int)> f = std::bind(&GpioPin::handleMQTT, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
        MQTTregisterSubscribeFunction(_mqttTopic, f);
    }
@@ -141,8 +141,8 @@ void GpioPin::setValue(bool value, gpio_set_source setSource, std::string* error
        gpio_set_level(_gpio, value);

 #ifdef ENABLE_MQTT
-        if ((_mqttTopic != "") && (setSource != GPIO_SET_SOURCE_MQTT)) {
-            MQTTPublish(_mqttTopic, value ? "true" : "false");
+        if ((_mqttTopic.compare("") != 0) && (setSource != GPIO_SET_SOURCE_MQTT)) {
+            MQTTPublish(_mqttTopic, value ? "true" : "false", 1);
        }
 #endif //ENABLE_MQTT
    }
@@ -153,7 +153,8 @@ void GpioPin::publishState() {
    if (newState != currentState) {
        ESP_LOGD(TAG,"publish state of GPIO %d new state %d", _gpio, newState);
 #ifdef ENABLE_MQTT
-        MQTTPublish(_mqttTopic, newState ? "true" : "false");
+    if (_mqttTopic.compare("") != 0)
+        MQTTPublish(_mqttTopic, newState ? "true" : "false", 1);
 #endif //ENABLE_MQTT
        currentState = newState;
    }
@@ -359,9 +360,9 @@ bool GpioHandler::readConfig()
            gpio_int_type_t intType = resolveIntType(toLower(splitted[2]));
            uint16_t dutyResolution = (uint8_t)atoi(splitted[3].c_str());
 #ifdef ENABLE_MQTT 
-            bool mqttEnabled = toLower(splitted[4]) == "true";
+            bool mqttEnabled = (toLower(splitted[4]) == "true");
 #endif // ENABLE_MQTT
-            bool httpEnabled = toLower(splitted[5]) == "true";
+            bool httpEnabled = (toLower(splitted[5]) == "true");
            char gpioName[100];
            if (splitted.size() >= 7) {
                strcpy(gpioName, trim(splitted[6]).c_str());
--- a/code/components/jomjol_flowcontroll/ClassFlowControll.cpp
+++ b/code/components/jomjol_flowcontroll/ClassFlowControll.cpp
@@ -277,7 +277,7 @@ void ClassFlowControll::InitFlow(std::string config)
    aktstatusWithTime = aktstatus;

    //#ifdef ENABLE_MQTT
-        //MQTTPublish(mqttServer_getMainTopic() + "/" + "status", "Initialization", false); // Right now, not possible -> MQTT Service is going to be started later
+        //MQTTPublish(mqttServer_getMainTopic() + "/" + "status", "Initialization", 1, false); // Right now, not possible -> MQTT Service is going to be started later
    //#endif //ENABLE_MQTT
    
    string line;
@@ -352,7 +352,7 @@ void ClassFlowControll::doFlowTakeImageOnly(string time)
            aktstatus = TranslateAktstatus(FlowControll[i]->name());
            aktstatusWithTime = aktstatus + " (" + zw_time + ")";
            #ifdef ENABLE_MQTT
-                MQTTPublish(mqttServer_getMainTopic() + "/" + "status", aktstatus, false);
+                MQTTPublish(mqttServer_getMainTopic() + "/" + "status", aktstatus, 1, false);
            #endif //ENABLE_MQTT

            FlowControll[i]->doFlow(time);
@@ -366,6 +366,7 @@ bool ClassFlowControll::doFlow(string time)
    bool result = true;
    std::string zw_time;
    int repeat = 0;
+    int qos = 1;

    #ifdef DEBUG_DETAIL_ON 
        LogFile.WriteHeapInfo("ClassFlowControll::doFlow - Start");
@@ -386,7 +387,7 @@ bool ClassFlowControll::doFlow(string time)
        aktstatusWithTime = aktstatus + " (" + zw_time + ")";
        //LogFile.WriteToFile(ESP_LOG_INFO, TAG, aktstatusWithTime);
        #ifdef ENABLE_MQTT
-            MQTTPublish(mqttServer_getMainTopic() + "/" + "status", aktstatus, false);
+            MQTTPublish(mqttServer_getMainTopic() + "/" + "status", aktstatus, qos, false);
        #endif //ENABLE_MQTT

        #ifdef DEBUG_DETAIL_ON
@@ -420,7 +421,7 @@ bool ClassFlowControll::doFlow(string time)
    aktstatusWithTime = aktstatus + " (" + zw_time + ")";
    //LogFile.WriteToFile(ESP_LOG_INFO, TAG, aktstatusWithTime);
    #ifdef ENABLE_MQTT
-        MQTTPublish(mqttServer_getMainTopic() + "/" + "status", aktstatus, false);
+        MQTTPublish(mqttServer_getMainTopic() + "/" + "status", aktstatus, qos, false);
    #endif //ENABLE_MQTT

    return result;
@@ -611,8 +612,8 @@ bool ClassFlowControll::ReadParameter(FILE* pfile, string& aktparamgraph)
        }

        /* TimeServer and TimeZone got already read from the config, see setupTime () */
-
-        #ifdef WLAN_USE_MESH_ROAMING
+        
+        #if (defined WLAN_USE_ROAMING_BY_SCANNING || (defined WLAN_USE_MESH_ROAMING && defined WLAN_USE_MESH_ROAMING_ACTIVATE_CLIENT_TRIGGERED_QUERIES))
        if ((toUpper(splitted[0]) == "RSSITHRESHOLD") && (splitted.size() > 1))
        {
            int RSSIThresholdTMP = atoi(splitted[1].c_str());
--- a/code/components/jomjol_flowcontroll/ClassFlowMQTT.cpp
+++ b/code/components/jomjol_flowcontroll/ClassFlowMQTT.cpp
@@ -156,6 +156,9 @@ bool ClassFlowMQTT::ReadParameter(FILE* pfile, string& aktparamgraph)
            else if (toUpper(splitted[1]) == "ENERGY_MWH") {
                mqttServer_setMeterType("energy", "MWh", "h", "MW");
            }
+            else if (toUpper(splitted[1]) == "ENERGY_GJ") {
+                mqttServer_setMeterType("energy", "GJ", "h", "GJ/h");
+            }
        }

        if ((toUpper(splitted[0]) == "CLIENTID") && (splitted.size() > 1))
@@ -222,8 +225,12 @@ bool ClassFlowMQTT::doFlow(string zwtime)
    std::string resultchangabs = "";
    string zw = "";
    string namenumber = "";
+    int qos = 1;

-    success = publishSystemData();
+    /* Send the the Homeassistant Discovery and the Static Topics in case they where scheduled */
+    sendDiscovery_and_static_Topics();
+
+    success = publishSystemData(qos);

    if (flowpostprocessing && getMQTTisConnected())
    {
@@ -249,13 +256,13 @@ bool ClassFlowMQTT::doFlow(string zwtime)


            if (result.length() > 0)   
-                success |= MQTTPublish(namenumber + "value", result, SetRetainFlag);
+                success |= MQTTPublish(namenumber + "value", result, qos, SetRetainFlag);

            if (resulterror.length() > 0)  
-                success |= MQTTPublish(namenumber + "error", resulterror, SetRetainFlag);
+                success |= MQTTPublish(namenumber + "error", resulterror, qos, SetRetainFlag);

            if (resultrate.length() > 0) {
-                success |= MQTTPublish(namenumber + "rate", resultrate, SetRetainFlag);
+                success |= MQTTPublish(namenumber + "rate", resultrate, qos, SetRetainFlag);
                
                std::string resultRatePerTimeUnit;
                if (getTimeUnit() == "h") { // Need conversion to be per hour
@@ -264,22 +271,22 @@ bool ClassFlowMQTT::doFlow(string zwtime)
                else { // Keep per minute
                    resultRatePerTimeUnit = resultrate;
                }
-                success |= MQTTPublish(namenumber + "rate_per_time_unit", resultRatePerTimeUnit, SetRetainFlag);
+                success |= MQTTPublish(namenumber + "rate_per_time_unit", resultRatePerTimeUnit, qos, SetRetainFlag);
            }

            if (resultchangabs.length() > 0) {
-                success |= MQTTPublish(namenumber + "changeabsolut", resultchangabs, SetRetainFlag); // Legacy API
-                success |= MQTTPublish(namenumber + "rate_per_digitalization_round", resultchangabs, SetRetainFlag);
+                success |= MQTTPublish(namenumber + "changeabsolut", resultchangabs, qos, SetRetainFlag); // Legacy API
+                success |= MQTTPublish(namenumber + "rate_per_digitalization_round", resultchangabs, qos, SetRetainFlag);
            }

            if (resultraw.length() > 0)   
-                success |= MQTTPublish(namenumber + "raw", resultraw, SetRetainFlag);
+                success |= MQTTPublish(namenumber + "raw", resultraw, qos, SetRetainFlag);

            if (resulttimestamp.length() > 0)
-                success |= MQTTPublish(namenumber + "timestamp", resulttimestamp, SetRetainFlag);
+                success |= MQTTPublish(namenumber + "timestamp", resulttimestamp, qos, SetRetainFlag);

            std::string json = flowpostprocessing->getJsonFromNumber(i, "\n");
-            success |= MQTTPublish(namenumber + "json", json, SetRetainFlag);
+            success |= MQTTPublish(namenumber + "json", json, qos, SetRetainFlag);
        }
    }
    
@@ -297,7 +304,7 @@ bool ClassFlowMQTT::doFlow(string zwtime)
    //                 result = result + "\t" + zw;
    //         }
    //     }
-    //     success |= MQTTPublish(topic, result, SetRetainFlag);
+    //     success |= MQTTPublish(topic, result, qos, SetRetainFlag);
    // }
    
    OldValue = result;
--- a/code/components/jomjol_logfile/ClassLogFile.cpp
+++ b/code/components/jomjol_logfile/ClassLogFile.cpp
@@ -15,6 +15,7 @@ extern "C" {
 #endif

 #include "Helper.h"
+#include "time_sntp.h"
 #include "../../include/defines.h"

 static const char *TAG = "LOGFILE";
@@ -321,15 +322,23 @@ void ClassLogFile::RemoveOldLogFile()
            //ESP_LOGD(TAG, "compare log file: %s to %s", entry->d_name, cmpfilename);
            if ((strlen(entry->d_name) == strlen(cmpfilename)) && (strcmp(entry->d_name, cmpfilename) < 0)) {
                //ESP_LOGD(TAG, "delete log file: %s", entry->d_name);
-                std::string filepath = logroot + "/" + entry->d_name; 
-                if (unlink(filepath.c_str()) == 0) {
-                    deleted ++;
-                } else {
-                    ESP_LOGE(TAG, "can't delete file: %s", entry->d_name);
-                    notDeleted ++;
+                std::string filepath = logroot + "/" + entry->d_name;
+                if ((strcmp(entry->d_name, "log_1970-01-01.txt") == 0) && getTimeWasNotSetAtBoot()) { // keep logfile log_1970-01-01.txt if time was not set at boot (some boot logs are in there)
+                    //ESP_LOGD(TAG, "Skip deleting this file: %s", entry->d_name); 
+                    notDeleted++;
                }
-            } else {
-                notDeleted ++;
+                else {          
+                    if (unlink(filepath.c_str()) == 0) {
+                        deleted++;
+                    } 
+                    else {
+                        ESP_LOGE(TAG, "can't delete file: %s", entry->d_name);
+                        notDeleted++;
+                    }
+                }
+            } 
+            else {
+                notDeleted++;
            }
        }
    }
--- a/code/components/jomjol_mqtt/interface_mqtt.cpp
+++ b/code/components/jomjol_mqtt/interface_mqtt.cpp
@@ -31,7 +31,7 @@ bool SetRetainFlag;
 void (*callbackOnConnected)(std::string, bool) = NULL;


-bool MQTTPublish(std::string _key, std::string _content, bool retained_flag) 
+bool MQTTPublish(std::string _key, std::string _content, int qos, bool retained_flag) 
 {
    if (!mqtt_enabled) {                            // MQTT sevice not started / configured (MQTT_Init not called before)      
        return false;
@@ -51,7 +51,7 @@ bool MQTTPublish(std::string _key, std::string _content, bool retained_flag)
        #ifdef DEBUG_DETAIL_ON 
            long long int starttime = esp_timer_get_time();
        #endif
-        int msg_id = esp_mqtt_client_publish(client, _key.c_str(), _content.c_str(), 0, 1, retained_flag);
+        int msg_id = esp_mqtt_client_publish(client, _key.c_str(), _content.c_str(), 0, qos, retained_flag);
        #ifdef DEBUG_DETAIL_ON 
            ESP_LOGD(TAG, "Publish msg_id %d in %lld ms", msg_id, (esp_timer_get_time() - starttime)/1000);
        #endif
@@ -60,7 +60,7 @@ bool MQTTPublish(std::string _key, std::string _content, bool retained_flag)
            #ifdef DEBUG_DETAIL_ON 
                starttime = esp_timer_get_time();
            #endif
-            msg_id = esp_mqtt_client_publish(client, _key.c_str(), _content.c_str(), 0, 1, retained_flag);
+            msg_id = esp_mqtt_client_publish(client, _key.c_str(), _content.c_str(), 0, qos, retained_flag);
            #ifdef DEBUG_DETAIL_ON 
                ESP_LOGD(TAG, "Publish msg_id %d in %lld ms", msg_id, (esp_timer_get_time() - starttime)/1000);
            #endif
@@ -234,7 +234,7 @@ int MQTT_Init() {
        .buffer_size = 1536,                    // size of MQTT send/receive buffer (Default: 1024)
        .reconnect_timeout_ms = 15000,          // Try to reconnect to broker (Default: 10000ms)
        .network_timeout_ms = 20000,            // Network Timeout (Default: 10000ms)
-        .message_retransmit_timeout = 3000      // Tiem after message resent when broker not acknowledged (QoS1, QoS2)
+        .message_retransmit_timeout = 3000      // Time after message resent when broker not acknowledged (QoS1, QoS2)

    };

@@ -345,7 +345,7 @@ void MQTTconnected(){
            }
        }

-        vTaskDelay(10000 / portTICK_PERIOD_MS);                 // Delay execution of callback routine after connection got established   
+        /* Send Static Topics and Homeassistant Discovery */
        if (callbackOnConnected) {                              // Call onConnected callback routine --> mqtt_server
            callbackOnConnected(maintopic, SetRetainFlag);
        }
--- a/code/components/jomjol_mqtt/interface_mqtt.h
+++ b/code/components/jomjol_mqtt/interface_mqtt.h
@@ -15,7 +15,7 @@ bool MQTT_Configure(std::string _mqttURI, std::string _clientid, std::string _us
 int MQTT_Init();
 void MQTTdestroy_client(bool _disable);

-bool MQTTPublish(std::string _key, std::string _content, bool retained_flag = 1);            // retained Flag as Standart
+bool MQTTPublish(std::string _key, std::string _content, int qos, bool retained_flag = 1);            // retained Flag as Standart

 bool getMQTTisEnabled();
 bool getMQTTisConnected();
--- a/code/components/jomjol_mqtt/mqtt_outbox.c
+++ b/code/components/jomjol_mqtt/mqtt_outbox.c
@@ -0,0 +1,301 @@
+/* This is a modification of https://github.com/espressif/esp-mqtt/blob/master/lib/mqtt_outbox.c
+ * to use the PSRAM instead of the internal heap.
+*/
+#include "mqtt_outbox.h"
+#include <stdlib.h>
+#include <string.h>
+#include "sys/queue.h"
+#include "esp_log.h"
+#include "esp_heap_caps.h"
+
+#define USE_PSRAM
+
+#ifdef CONFIG_MQTT_CUSTOM_OUTBOX
+static const char *TAG = "outbox";
+
+typedef struct outbox_item {
+    char *buffer;
+    int len;
+    int msg_id;
+    int msg_type;
+    int msg_qos;
+    outbox_tick_t tick;
+    pending_state_t pending;
+    STAILQ_ENTRY(outbox_item) next;
+} outbox_item_t;
+
+STAILQ_HEAD(outbox_list_t, outbox_item);
+
+
+outbox_handle_t outbox_init(void)
+{
+#ifdef USE_PSRAM
+    outbox_handle_t outbox = heap_caps_calloc(1, sizeof(struct outbox_list_t), MALLOC_CAP_8BIT | MALLOC_CAP_SPIRAM);
+#else
+    outbox_handle_t outbox = calloc(1, sizeof(struct outbox_list_t));
+#endif
+    //ESP_MEM_CHECK(TAG, outbox, return NULL);
+    STAILQ_INIT(outbox);
+    return outbox;
+}
+
+outbox_item_handle_t outbox_enqueue(outbox_handle_t outbox, outbox_message_handle_t message, outbox_tick_t tick)
+{
+#ifdef USE_PSRAM
+    outbox_item_handle_t item = heap_caps_calloc(1, sizeof(outbox_item_t), MALLOC_CAP_8BIT | MALLOC_CAP_SPIRAM);
+#else
+    outbox_item_handle_t item = calloc(1, sizeof(outbox_item_t));
+#endif
+    //ESP_MEM_CHECK(TAG, item, return NULL);
+    item->msg_id = message->msg_id;
+    item->msg_type = message->msg_type;
+    item->msg_qos = message->msg_qos;
+    item->tick = tick;
+    item->len =  message->len + message->remaining_len;
+    item->pending = QUEUED;
+#ifdef USE_PSRAM
+    item->buffer = heap_caps_malloc(message->len + message->remaining_len, MALLOC_CAP_8BIT | MALLOC_CAP_SPIRAM);
+#else
+    item->buffer = malloc(message->len + message->remaining_len);
+#endif
+    /*ESP_MEM_CHECK(TAG, item->buffer, {
+        free(item);
+        return NULL;
+    });*/
+    memcpy(item->buffer, message->data, message->len);
+    if (message->remaining_data) {
+        memcpy(item->buffer + message->len, message->remaining_data, message->remaining_len);
+    }
+    STAILQ_INSERT_TAIL(outbox, item, next);
+    ESP_LOGD(TAG, "ENQUEUE msgid=%d, msg_type=%d, len=%d, size=%d", message->msg_id, message->msg_type, message->len + message->remaining_len, outbox_get_size(outbox));
+    return item;
+}
+
+outbox_item_handle_t outbox_get(outbox_handle_t outbox, int msg_id)
+{
+    outbox_item_handle_t item;
+    STAILQ_FOREACH(item, outbox, next) {
+        if (item->msg_id == msg_id) {
+            return item;
+        }
+    }
+    return NULL;
+}
+
+outbox_item_handle_t outbox_dequeue(outbox_handle_t outbox, pending_state_t pending, outbox_tick_t *tick)
+{
+    outbox_item_handle_t item;
+    STAILQ_FOREACH(item, outbox, next) {
+        if (item->pending == pending) {
+            if (tick) {
+                *tick = item->tick;
+            }
+            return item;
+        }
+    }
+    return NULL;
+}
+
+esp_err_t outbox_delete_item(outbox_handle_t outbox, outbox_item_handle_t item_to_delete)
+{
+    outbox_item_handle_t item;
+    STAILQ_FOREACH(item, outbox, next) {
+        if (item == item_to_delete) {
+            STAILQ_REMOVE(outbox, item, outbox_item, next);
+#ifdef USE_PSRAM
+            heap_caps_free(item->buffer);
+            heap_caps_free(item);
+#else
+            free(item->buffer);
+            free(item);
+#endif
+            return ESP_OK;
+        }
+    }
+    return ESP_FAIL;
+}
+
+uint8_t *outbox_item_get_data(outbox_item_handle_t item,  size_t *len, uint16_t *msg_id, int *msg_type, int *qos)
+{
+    if (item) {
+        *len = item->len;
+        *msg_id = item->msg_id;
+        *msg_type = item->msg_type;
+        *qos = item->msg_qos;
+        return (uint8_t *)item->buffer;
+    }
+    return NULL;
+}
+
+esp_err_t outbox_delete(outbox_handle_t outbox, int msg_id, int msg_type)
+{
+    outbox_item_handle_t item, tmp;
+    STAILQ_FOREACH_SAFE(item, outbox, next, tmp) {
+        if (item->msg_id == msg_id && (0xFF & (item->msg_type)) == msg_type) {
+            STAILQ_REMOVE(outbox, item, outbox_item, next);
+#ifdef USE_PSRAM
+            heap_caps_free(item->buffer);
+            heap_caps_free(item);
+#else
+            free(item->buffer);
+            free(item);
+#endif
+            ESP_LOGD(TAG, "DELETED msgid=%d, msg_type=%d, remain size=%d", msg_id, msg_type, outbox_get_size(outbox));
+            return ESP_OK;
+        }
+
+    }
+    return ESP_FAIL;
+}
+esp_err_t outbox_delete_msgid(outbox_handle_t outbox, int msg_id)
+{
+    outbox_item_handle_t item, tmp;
+    STAILQ_FOREACH_SAFE(item, outbox, next, tmp) {
+        if (item->msg_id == msg_id) {
+            STAILQ_REMOVE(outbox, item, outbox_item, next);
+#ifdef USE_PSRAM
+            heap_caps_free(item->buffer);
+            heap_caps_free(item);
+#else
+            free(item->buffer);
+            free(item);
+#endif
+        }
+
+    }
+    return ESP_OK;
+}
+esp_err_t outbox_set_pending(outbox_handle_t outbox, int msg_id, pending_state_t pending)
+{
+    outbox_item_handle_t item = outbox_get(outbox, msg_id);
+    if (item) {
+        item->pending = pending;
+        return ESP_OK;
+    }
+    return ESP_FAIL;
+}
+
+pending_state_t outbox_item_get_pending(outbox_item_handle_t item)
+{
+    if (item) {
+        return item->pending;
+    }
+    return QUEUED;
+}
+
+esp_err_t outbox_set_tick(outbox_handle_t outbox, int msg_id, outbox_tick_t tick)
+{
+    outbox_item_handle_t item = outbox_get(outbox, msg_id);
+    if (item) {
+        item->tick = tick;
+        return ESP_OK;
+    }
+    return ESP_FAIL;
+}
+
+esp_err_t outbox_delete_msgtype(outbox_handle_t outbox, int msg_type)
+{
+    outbox_item_handle_t item, tmp;
+    STAILQ_FOREACH_SAFE(item, outbox, next, tmp) {
+        if (item->msg_type == msg_type) {
+            STAILQ_REMOVE(outbox, item, outbox_item, next);
+#ifdef USE_PSRAM
+            heap_caps_free(item->buffer);
+            heap_caps_free(item);
+#else
+            free(item->buffer);
+            free(item);
+#endif
+        }
+
+    }
+    return ESP_OK;
+}
+int outbox_delete_single_expired(outbox_handle_t outbox, outbox_tick_t current_tick, outbox_tick_t timeout)
+{
+    int msg_id = -1;
+    outbox_item_handle_t item;
+    STAILQ_FOREACH(item, outbox, next) {
+        if (current_tick - item->tick > timeout) {
+            STAILQ_REMOVE(outbox, item, outbox_item, next);
+
+#ifdef USE_PSRAM
+            heap_caps_free(item->buffer);
+#else
+            free(item->buffer);
+#endif
+
+            msg_id = item->msg_id;
+
+#ifdef USE_PSRAM
+            heap_caps_free(item);
+#else
+            free(item);
+#endif
+
+            return msg_id;
+        }
+
+    }
+    return msg_id;
+}
+
+int outbox_delete_expired(outbox_handle_t outbox, outbox_tick_t current_tick, outbox_tick_t timeout)
+{
+    int deleted_items = 0;
+    outbox_item_handle_t item, tmp;
+    STAILQ_FOREACH_SAFE(item, outbox, next, tmp) {
+        if (current_tick - item->tick > timeout) {
+            STAILQ_REMOVE(outbox, item, outbox_item, next);
+#ifdef USE_PSRAM
+            heap_caps_free(item->buffer);
+            heap_caps_free(item);
+#else
+            free(item->buffer);
+            free(item);
+#endif
+            deleted_items ++;
+        }
+
+    }
+    return deleted_items;
+}
+
+int outbox_get_size(outbox_handle_t outbox)
+{
+    int siz = 0;
+    outbox_item_handle_t item;
+    STAILQ_FOREACH(item, outbox, next) {
+        // Suppressing "use after free" warning as this could happen only if queue is in inconsistent state
+        // which never happens if STAILQ interface used
+        siz += item->len; // NOLINT(clang-analyzer-unix.Malloc)
+    }
+    return siz;
+}
+
+void outbox_delete_all_items(outbox_handle_t outbox)
+{
+    outbox_item_handle_t item, tmp;
+    STAILQ_FOREACH_SAFE(item, outbox, next, tmp) {
+        STAILQ_REMOVE(outbox, item, outbox_item, next);
+#ifdef USE_PSRAM
+            heap_caps_free(item->buffer);
+            heap_caps_free(item);
+#else
+            free(item->buffer);
+            free(item);
+#endif
+    }
+}
+void outbox_destroy(outbox_handle_t outbox)
+{
+    outbox_delete_all_items(outbox);
+
+#ifdef USE_PSRAM
+    heap_caps_free(outbox);
+#else
+    free(outbox);
+#endif
+}
+
+#endif /* CONFIG_MQTT_CUSTOM_OUTBOX */
--- a/code/components/jomjol_mqtt/mqtt_outbox.h
+++ b/code/components/jomjol_mqtt/mqtt_outbox.h
@@ -0,0 +1,66 @@
+/* This is an adaption of https://github.com/espressif/esp-mqtt/blob/master/lib/include/mqtt_outbox.h
+ * This file is subject to the terms and conditions defined in
+ * file 'LICENSE', which is part of this source code package.
+ * Tuan PM <tuanpm at live dot com>
+ */
+#ifndef _MQTT_OUTOBX_H_
+#define _MQTT_OUTOBX_H_
+//#include "platform.h"
+#include "esp_err.h"
+
+#ifdef  __cplusplus
+extern "C" {
+#endif
+
+struct outbox_item;
+
+typedef struct outbox_list_t *outbox_handle_t;
+typedef struct outbox_item *outbox_item_handle_t;
+typedef struct outbox_message *outbox_message_handle_t;
+typedef long long outbox_tick_t;
+
+typedef struct outbox_message {
+    uint8_t *data;
+    int len;
+    int msg_id;
+    int msg_qos;
+    int msg_type;
+    uint8_t *remaining_data;
+    int remaining_len;
+} outbox_message_t;
+
+typedef enum pending_state {
+    QUEUED,
+    TRANSMITTED,
+    ACKNOWLEDGED,
+    CONFIRMED
+} pending_state_t;
+
+outbox_handle_t outbox_init(void);
+outbox_item_handle_t outbox_enqueue(outbox_handle_t outbox, outbox_message_handle_t message, outbox_tick_t tick);
+outbox_item_handle_t outbox_dequeue(outbox_handle_t outbox, pending_state_t pending, outbox_tick_t *tick);
+outbox_item_handle_t outbox_get(outbox_handle_t outbox, int msg_id);
+uint8_t *outbox_item_get_data(outbox_item_handle_t item,  size_t *len, uint16_t *msg_id, int *msg_type, int *qos);
+esp_err_t outbox_delete(outbox_handle_t outbox, int msg_id, int msg_type);
+esp_err_t outbox_delete_msgid(outbox_handle_t outbox, int msg_id);
+esp_err_t outbox_delete_msgtype(outbox_handle_t outbox, int msg_type);
+esp_err_t outbox_delete_item(outbox_handle_t outbox, outbox_item_handle_t item);
+int outbox_delete_expired(outbox_handle_t outbox, outbox_tick_t current_tick, outbox_tick_t timeout);
+/**
+ * @brief Deletes single expired message returning it's message id
+ *
+ * @return msg id of the deleted message, -1 if no expired message in the outbox
+ */
+int outbox_delete_single_expired(outbox_handle_t outbox, outbox_tick_t current_tick, outbox_tick_t timeout);
+
+esp_err_t outbox_set_pending(outbox_handle_t outbox, int msg_id, pending_state_t pending);
+pending_state_t outbox_item_get_pending(outbox_item_handle_t item);
+esp_err_t outbox_set_tick(outbox_handle_t outbox, int msg_id, outbox_tick_t tick);
+int outbox_get_size(outbox_handle_t outbox);
+void outbox_destroy(outbox_handle_t outbox);
+void outbox_delete_all_items(outbox_handle_t outbox);
+
+#ifdef  __cplusplus
+}
+#endif
+#endif
--- a/code/components/jomjol_mqtt/server_mqtt.cpp
+++ b/code/components/jomjol_mqtt/server_mqtt.cpp
@@ -32,6 +32,7 @@ float roundInterval; // Minutes
 int keepAlive = 0; // Seconds
 bool retainFlag;
 static std::string maintopic;
+bool sendingOf_DiscoveryAndStaticTopics_scheduled = true; // Set it to true to make sure it gets sent at least once after startup


 void mqttServer_setParameter(std::vector<NumberPost*>* _NUMBERS, int _keepAlive, float _roundInterval) {
@@ -48,7 +49,8 @@ void mqttServer_setMeterType(std::string _meterType, std::string _valueUnit, std
 }

 bool sendHomeAssistantDiscoveryTopic(std::string group, std::string field,
-    std::string name, std::string icon, std::string unit, std::string deviceClass, std::string stateClass, std::string entityCategory) {
+    std::string name, std::string icon, std::string unit, std::string deviceClass, std::string stateClass, std::string entityCategory,
+    int qos) {
    std::string version = std::string(libfive_git_version());

    if (version == "") {
@@ -131,10 +133,10 @@ bool sendHomeAssistantDiscoveryTopic(std::string group, std::string field,
    "}"  +
    "}";

-    return MQTTPublish(topicFull, payload, true);
+    return MQTTPublish(topicFull, payload, qos, true);
 }

-bool MQTThomeassistantDiscovery() {  
+bool MQTThomeassistantDiscovery(int qos) {  
    bool allSendsSuccessed = false;

    if (!getMQTTisConnected()) {
@@ -142,18 +144,20 @@ bool MQTThomeassistantDiscovery() {
        return false;
    }

-    LogFile.WriteToFile(ESP_LOG_INFO, TAG, "MQTT - Sending Homeassistant Discovery Topics (Meter Type: " + meterType + ", Value Unit: " + valueUnit + " , Rate Unit: " + rateUnit + ")...");
+    LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Publishing Homeassistant Discovery topics (Meter Type: '" + meterType + "', Value Unit: '" + valueUnit + "' , Rate Unit: '" + rateUnit + "') ...");
+
+	int aFreeInternalHeapSizeBefore = heap_caps_get_free_size(MALLOC_CAP_8BIT | MALLOC_CAP_INTERNAL);

    //                                                   Group | Field            | User Friendly Name | Icon                      | Unit | Device Class     | State Class  | Entity Category
-    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "uptime",          "Uptime",            "clock-time-eight-outline", "s",   "",                "",            "diagnostic");
-    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "MAC",             "MAC Address",       "network-outline",          "",    "",                "",            "diagnostic");
-    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "hostname",        "Hostname",          "network-outline",          "",    "",                "",            "diagnostic");
-    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "freeMem",         "Free Memory",       "memory",                   "B",   "",                "measurement", "diagnostic");
-    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "wifiRSSI",        "Wi-Fi RSSI",        "wifi",                     "dBm", "signal_strength", "",            "diagnostic");
-    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "CPUtemp",         "CPU Temperature",   "thermometer",              "°C",  "temperature",     "measurement", "diagnostic");
-    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "interval",        "Interval",          "clock-time-eight-outline", "min",  ""           ,    "measurement", "diagnostic");
-    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "IP",              "IP",                "network-outline",           "",    "",               "",            "diagnostic");
-    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "status",          "Status",            "list-status",               "",    "",               "",            "diagnostic");
+    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "uptime",          "Uptime",            "clock-time-eight-outline", "s",   "",                "",            "diagnostic", qos);
+    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "MAC",             "MAC Address",       "network-outline",          "",    "",                "",            "diagnostic", qos);
+    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "hostname",        "Hostname",          "network-outline",          "",    "",                "",            "diagnostic", qos);
+    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "freeMem",         "Free Memory",       "memory",                   "B",   "",                "measurement", "diagnostic", qos);
+    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "wifiRSSI",        "Wi-Fi RSSI",        "wifi",                     "dBm", "signal_strength", "",            "diagnostic", qos);
+    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "CPUtemp",         "CPU Temperature",   "thermometer",              "°C",  "temperature",     "measurement", "diagnostic", qos);
+    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "interval",        "Interval",          "clock-time-eight-outline", "min",  ""           ,    "measurement", "diagnostic", qos);
+    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "IP",              "IP",                "network-outline",           "",    "",               "",            "diagnostic", qos);
+    allSendsSuccessed |= sendHomeAssistantDiscoveryTopic("",     "status",          "Status",            "list-status",               "",    "",               "",            "diagnostic", qos);


    for (int i = 0; i < (*NUMBERS).size(); ++i) {
@@ -162,24 +166,32 @@ bool MQTThomeassistantDiscovery() {
            group = "";
        }

-    //                                                       Group | Field                 | User Friendly Name                | Icon                   | Unit     | Device Class | State Class       | Entity Category
-        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "value",              "Value",                            "gauge",                 valueUnit, meterType,     "total_increasing", "");
-        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "raw",                "Raw Value",                        "raw",                   "",        "",            "",                 "diagnostic");
-        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "error",              "Error",                            "alert-circle-outline",  "",        "",            "",                 "diagnostic");
+    //                                                       Group | Field                          | User Friendly Name                | Icon                   | Unit                 | Device Class | State Class       | Entity Category
+        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "value",                      "Value",                            "gauge",                 valueUnit,             meterType,      "total_increasing", "", qos);
+        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "raw",                        "Raw Value",                        "raw",                   "",                    "",             "",                 "diagnostic", qos);
+        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "error",                      "Error",                            "alert-circle-outline",  "",                    "",             "",                 "diagnostic", qos);
        /* Not announcing "rate" as it is better to use rate_per_time_unit resp. rate_per_digitalization_round */
        // allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "rate",               "Rate (Unit/Minute)",               "swap-vertical",         "",        "",            "",                 ""); // Legacy, always Unit per Minute
-        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "rate_per_time_unit", "Rate (" + rateUnit + ")",          "swap-vertical",         rateUnit,  "",            "",                 "");        
-        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "rate_per_digitalization_round",  "Change since last digitalization round", "arrow-expand-vertical", valueUnit, "",            "measurement",      ""); // correctly the Unit is Uint/Interval!
-        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "timestamp",          "Timestamp",                  "clock-time-eight-outline", "",        "timestamp",   "",                "diagnostic");
-        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "json",               "JSON",                       "code-json",                "",        "",            "",                 "diagnostic");
-        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "problem",            "Problem",                    "alert-outline",            "",        "problem",            "",                 ""); // Special binary sensor which is based on error topic
+        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "rate_per_time_unit",         "Rate (" + rateUnit + ")",          "swap-vertical",         rateUnit,              "",             "measurement",      "", qos);
+        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "rate_per_digitalization_round",  "Change since last digitalization round",  "arrow-expand-vertical", valueUnit,  "",             "measurement",      "", qos); // correctly the Unit is Unit/Interval!
+        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "timestamp",                  "Timestamp",                     "clock-time-eight-outline", "",                    "timestamp",    "",                 "diagnostic", qos);
+        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "json",                       "JSON",                             "code-json",             "",                    "",             "",                 "diagnostic", qos);
+        allSendsSuccessed |= sendHomeAssistantDiscoveryTopic(group,   "problem",                    "Problem",                          "alert-outline",         "",                    "problem",      "",                 "", qos); // Special binary sensor which is based on error topic
    }

    LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Successfully published all Homeassistant Discovery MQTT topics");
+
+    int aFreeInternalHeapSizeAfter = heap_caps_get_free_size(MALLOC_CAP_8BIT | MALLOC_CAP_INTERNAL);
+    int aMinFreeInternalHeapSize =  heap_caps_get_minimum_free_size(MALLOC_CAP_8BIT | MALLOC_CAP_INTERNAL);
+
+    LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Int. Heap Usage before Publishing Homeassistand Discovery Topics: " + 
+            to_string(aFreeInternalHeapSizeBefore) + ", after: " + to_string(aFreeInternalHeapSizeAfter) + ", delta: " + 
+            to_string(aFreeInternalHeapSizeBefore - aFreeInternalHeapSizeAfter) + ", lowest free: " + to_string(aMinFreeInternalHeapSize));
+
    return allSendsSuccessed;
 }

-bool publishSystemData() {
+bool publishSystemData(int qos) {
    bool allSendsSuccessed = false;

    if (!getMQTTisConnected()) {
@@ -189,28 +201,38 @@ bool publishSystemData() {

    char tmp_char[50];

-    LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Publishing system MQTT topics...");
+    LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Publishing System MQTT topics...");

-    allSendsSuccessed |= MQTTPublish(maintopic + "/" + std::string(LWT_TOPIC), LWT_CONNECTED, retainFlag); // Publish "connected" to maintopic/connection
+	int aFreeInternalHeapSizeBefore = heap_caps_get_free_size(MALLOC_CAP_8BIT | MALLOC_CAP_INTERNAL);
+
+    allSendsSuccessed |= MQTTPublish(maintopic + "/" + std::string(LWT_TOPIC), LWT_CONNECTED, qos, retainFlag); // Publish "connected" to maintopic/connection

    sprintf(tmp_char, "%ld", (long)getUpTime());
-    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "uptime", std::string(tmp_char), retainFlag);
+    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "uptime", std::string(tmp_char), qos, retainFlag);
    
    sprintf(tmp_char, "%lu", (long) getESPHeapSize());
-    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "freeMem", std::string(tmp_char), retainFlag);
+    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "freeMem", std::string(tmp_char), qos, retainFlag);

    sprintf(tmp_char, "%d", get_WIFI_RSSI());
-    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "wifiRSSI", std::string(tmp_char), retainFlag);
+    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "wifiRSSI", std::string(tmp_char), qos, retainFlag);

    sprintf(tmp_char, "%d", (int)temperatureRead());
-    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "CPUtemp", std::string(tmp_char), retainFlag);
+    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "CPUtemp", std::string(tmp_char), qos, retainFlag);

    LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Successfully published all System MQTT topics");
+
+	int aFreeInternalHeapSizeAfter = heap_caps_get_free_size(MALLOC_CAP_8BIT | MALLOC_CAP_INTERNAL);
+	int aMinFreeInternalHeapSize =  heap_caps_get_minimum_free_size(MALLOC_CAP_8BIT | MALLOC_CAP_INTERNAL);
+
+    LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Int. Heap Usage before publishing System Topics: " + 
+            to_string(aFreeInternalHeapSizeBefore) + ", after: " + to_string(aFreeInternalHeapSizeAfter) + ", delta: " + 
+            to_string(aFreeInternalHeapSizeBefore - aFreeInternalHeapSizeAfter) + ", lowest free: " + to_string(aMinFreeInternalHeapSize));
+
    return allSendsSuccessed;
 }


-bool publishStaticData() {
+bool publishStaticData(int qos) {
    bool allSendsSuccessed = false;

    if (!getMQTTisConnected()) {
@@ -219,69 +241,66 @@ bool publishStaticData() {
    }

    LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Publishing static MQTT topics...");
-    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "MAC", getMac(), retainFlag);
-    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "IP", *getIPAddress(), retainFlag);
-    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "hostname", wlan_config.hostname, retainFlag);
+
+	int aFreeInternalHeapSizeBefore = heap_caps_get_free_size(MALLOC_CAP_8BIT | MALLOC_CAP_INTERNAL);
+
+    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "MAC", getMac(), qos, retainFlag);
+    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "IP", *getIPAddress(), qos, retainFlag);
+    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "hostname", wlan_config.hostname, qos, retainFlag);

    std::stringstream stream;
    stream << std::fixed << std::setprecision(1) << roundInterval; // minutes
-    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "interval", stream.str(), retainFlag);
+    allSendsSuccessed |= MQTTPublish(maintopic + "/" + "interval", stream.str(), qos, retainFlag);

    LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Successfully published all Static MQTT topics");
+
+	int aFreeInternalHeapSizeAfter = heap_caps_get_free_size(MALLOC_CAP_8BIT | MALLOC_CAP_INTERNAL);
+	int aMinFreeInternalHeapSize =  heap_caps_get_minimum_free_size(MALLOC_CAP_8BIT | MALLOC_CAP_INTERNAL);
+
+    LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Int. Heap Usage before Publishing Static Topics: " + 
+            to_string(aFreeInternalHeapSizeBefore) + ", after: " + to_string(aFreeInternalHeapSizeAfter) + ", delta: " + 
+            to_string(aFreeInternalHeapSizeBefore - aFreeInternalHeapSizeAfter) + ", lowest free: " + to_string(aMinFreeInternalHeapSize));
+
    return allSendsSuccessed;
 }

-esp_err_t sendDiscovery_and_static_Topics(httpd_req_t *req) {
+
+esp_err_t scheduleSendingDiscovery_and_static_Topics(httpd_req_t *req) {
+    sendingOf_DiscoveryAndStaticTopics_scheduled = true;
+    char msg[] = "MQTT Homeassistant Discovery and Static Topics scheduled";
+    httpd_resp_send(req, msg, strlen(msg));  
+    return ESP_OK;
+}
+
+
+esp_err_t sendDiscovery_and_static_Topics(void) {
    bool success = false;

-    if (HomeassistantDiscovery) {
-        success = MQTThomeassistantDiscovery();
+    if (!sendingOf_DiscoveryAndStaticTopics_scheduled) {
+        // Flag not set, nothing to do
+        return ESP_OK;
    }

-    success |= publishStaticData();
+    if (HomeassistantDiscovery) {
+        success = MQTThomeassistantDiscovery(1);
+    }

-    if (success) {
-        char msg[] = "MQTT Homeassistant Discovery and Static Topics sent!";
-        httpd_resp_send(req, msg, strlen(msg));  
+    success |= publishStaticData(1);
+
+    if (success) { // Success, clear the flag
+        sendingOf_DiscoveryAndStaticTopics_scheduled = false;
        return ESP_OK;
    }
    else {
-        LogFile.WriteToFile(ESP_LOG_WARN, TAG, "One or more MQTT topics failed to be published!");
-        char msg[] = "Failed to send MQTT topics!";
-        httpd_resp_send(req, msg, strlen(msg)); 
+        LogFile.WriteToFile(ESP_LOG_WARN, TAG, "One or more MQTT topics failed to be published, will try sending them in the next round!");
+        /* Keep sendingOf_DiscoveryAndStaticTopics_scheduled set so we can retry after the next round */
        return ESP_FAIL;
    }
 }

+
 void GotConnected(std::string maintopic, bool retainFlag) {
-    static bool initialStaticOrHomeassistantDiscoveryTopicsGotSent = false;
-    bool success = false;
-
-    /* Only send Homeassistant Discovery and Static topics on the first time connecting */
-    if (!initialStaticOrHomeassistantDiscoveryTopicsGotSent) {
-        if (HomeassistantDiscovery) {
-            success = MQTThomeassistantDiscovery();
-        }
-
-        success |= publishStaticData();
-
-        if (success) {
-            /* Sending of all Homeassistant Discovery and Static Topics was successfull.
-             * Will no no longer send it on a re-connect!
-             * (But it is still possible to trigger sending through the REST API). */
-            initialStaticOrHomeassistantDiscoveryTopicsGotSent = true;
-        }
-        else {
-        LogFile.WriteToFile(ESP_LOG_WARN, TAG, "One or more static or Homeassistant Discovery MQTT topics failed to be published! Will try again on the next round.");
-        }
-    }
-
-    /* The System Data changes at runtime, therefore we always send it after a re-connect */
-    success |= publishSystemData();
-
-    if (!success) {
-        LogFile.WriteToFile(ESP_LOG_WARN, TAG, "One or more MQTT topics failed to be published!");
-    }
+    // Nothing to do
 }

 void register_server_mqtt_uri(httpd_handle_t server) {
@@ -289,7 +308,7 @@ void register_server_mqtt_uri(httpd_handle_t server) {
    uri.method    = HTTP_GET;

    uri.uri       = "/mqtt_publish_discovery";
-    uri.handler   = sendDiscovery_and_static_Topics;
+    uri.handler   = scheduleSendingDiscovery_and_static_Topics;
    uri.user_ctx  = (void*) "";    
    httpd_register_uri_handler(server, &uri); 
 }
--- a/code/components/jomjol_mqtt/server_mqtt.h
+++ b/code/components/jomjol_mqtt/server_mqtt.h
@@ -16,10 +16,11 @@ std::string mqttServer_getMainTopic();

 void register_server_mqtt_uri(httpd_handle_t server);

-bool publishSystemData();
+bool publishSystemData(int qos);

 std::string getTimeUnit(void);
 void GotConnected(std::string maintopic, bool SetRetainFlag);
+esp_err_t sendDiscovery_and_static_Topics(void);


 #endif //SERVERMQTT_H
--- a/code/components/jomjol_tfliteclass/server_tflite.cpp
+++ b/code/components/jomjol_tfliteclass/server_tflite.cpp
@@ -909,11 +909,11 @@ void task_autodoFlow(void *pvParameter)
            LogFile.RemoveOldDataLog();
        }

-        //Round finished -> Logfile
+        // Round finished -> Logfile
        LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Round #" + std::to_string(countRounds) + 
                " completed (" + std::to_string(getUpTime() - roundStartTime) + " seconds)");
        
-        //CPU Temp -> Logfile
+        // CPU Temp -> Logfile
        LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "CPU Temperature: " + std::to_string((int)temperatureRead()) + "°C");
        
        // WIFI Signal Strength (RSSI) -> Logfile
@@ -921,10 +921,20 @@ void task_autodoFlow(void *pvParameter)

        // Check if time is synchronized (if NTP is configured)
        if (getUseNtp() && !getTimeIsSet()) {
-            LogFile.WriteToFile(ESP_LOG_WARN, TAG, "Time server is configured, but time is not yet set. Check configuration");
+            LogFile.WriteToFile(ESP_LOG_WARN, TAG, "Time server is configured, but time is not yet set!");
            StatusLED(TIME_CHECK, 1, false);
        }

+        #if (defined WLAN_USE_MESH_ROAMING && defined WLAN_USE_MESH_ROAMING_ACTIVATE_CLIENT_TRIGGERED_QUERIES)
+            wifiRoamingQuery();
+        #endif
+        
+        // Scan channels and check if an AP with better RSSI is available, then disconnect and try to reconnect to AP with better RSSI
+        // NOTE: Keep this direct before the following task delay, because scan is done in blocking mode and this takes ca. 1,5 - 2s.
+        #ifdef WLAN_USE_ROAMING_BY_SCANNING
+            wifiRoamByScanning();
+        #endif
+        
        fr_delta_ms = (esp_timer_get_time() - fr_start) / 1000;
        if (auto_interval > fr_delta_ms)
        {
@@ -945,11 +955,12 @@ void TFliteDoAutoStart()

    ESP_LOGD(TAG, "getESPHeapInfo: %s", getESPHeapInfo().c_str());

-    xReturned = xTaskCreatePinnedToCore(&task_autodoFlow, "task_autodoFlow", 16 * 1024, NULL, tskIDLE_PRIORITY+2, &xHandletask_autodoFlow, 0);
-    //xReturned = xTaskCreate(&task_autodoFlow, "task_autodoFlow", 16 * 1024, NULL, tskIDLE_PRIORITY+2, &xHandletask_autodoFlow);
+    uint32_t stackSize = 16 * 1024;
+    xReturned = xTaskCreatePinnedToCore(&task_autodoFlow, "task_autodoFlow", stackSize, NULL, tskIDLE_PRIORITY+2, &xHandletask_autodoFlow, 0);
    if( xReturned != pdPASS )
    {
-       ESP_LOGD(TAG, "ERROR task_autodoFlow konnte nicht erzeugt werden!");
+        LogFile.WriteToFile(ESP_LOG_ERROR, TAG, "Creation task_autodoFlow failed. Requested stack size:" + std::to_string(stackSize));
+        LogFile.WriteHeapInfo("Creation task_autodoFlow failed");
    }
    ESP_LOGD(TAG, "getESPHeapInfo: %s", getESPHeapInfo().c_str());
 }
--- a/code/components/jomjol_time_sntp/time_sntp.cpp
+++ b/code/components/jomjol_time_sntp/time_sntp.cpp
@@ -25,6 +25,8 @@ static const char *TAG = "SNTP";
 static std::string timeZone = "";
 static std::string timeServer = "undefined";
 static bool useNtp = true;
+static bool timeWasNotSetAtBoot = false;
+static bool timeWasNotSetAtBoot_PrintStartBlock = false;

 std::string getNtpStatusText(sntp_sync_status_t status);
 static void setTimeZone(std::string _tzstring);
@@ -59,7 +61,13 @@ std::string getCurrentTimeString(const char * frm)

 void time_sync_notification_cb(struct timeval *tv)
 {
-    LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Time is now successfully synced with NTP Server " +
+    if (timeWasNotSetAtBoot_PrintStartBlock) {
+        LogFile.WriteToFile(ESP_LOG_INFO, TAG, "=================================================");
+        LogFile.WriteToFile(ESP_LOG_INFO, TAG, "==================== Start ======================");
+        LogFile.WriteToFile(ESP_LOG_INFO, TAG, "== Logs before time sync -> log_1970-01-01.txt ==");
+        timeWasNotSetAtBoot_PrintStartBlock = false;
+    }
+    LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Time is synced with NTP Server " +
            getServerName() + ": " + getCurrentTimeString("%Y-%m-%d %H:%M:%S"));
 }

@@ -111,6 +119,11 @@ bool getUseNtp(void) {
    return useNtp;
 }

+bool getTimeWasNotSetAtBoot(void)
+{
+    return timeWasNotSetAtBoot;
+}
+

 std::string getServerName(void) {
    char buf[100];
@@ -140,7 +153,7 @@ bool setupTime() {
    ConfigFile configFile = ConfigFile(CONFIG_FILE); 

    if (!configFile.ConfigFileExists()){
-        LogFile.WriteToFile(ESP_LOG_INFO, TAG, "No ConfigFile defined - exit setupTime() ");
+        LogFile.WriteToFile(ESP_LOG_WARN, TAG, "No ConfigFile defined - exit setupTime()!");
        return false;
    }

@@ -225,6 +238,8 @@ bool setupTime() {
        LogFile.WriteToFile(ESP_LOG_INFO, TAG, "The local time is unknown, starting with " + std::string(strftime_buf));
        if (useNtp) {
            LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Once the NTP server provides a time, we will switch to that one");
+            timeWasNotSetAtBoot = true;
+            timeWasNotSetAtBoot_PrintStartBlock = true;
        }
    }

--- a/code/components/jomjol_time_sntp/time_sntp.h
+++ b/code/components/jomjol_time_sntp/time_sntp.h
@@ -22,6 +22,7 @@ std::string ConvertTimeToString(time_t _time, const char * frm);


 bool getTimeIsSet(void);
+bool getTimeWasNotSetAtBoot(void);

 bool getUseNtp(void);
 bool setupTime();
--- a/code/components/jomjol_wlan/connect_wlan.cpp
+++ b/code/components/jomjol_wlan/connect_wlan.cpp
@@ -39,26 +39,31 @@

 static const char *TAG = "WIFI";

+static bool APWithBetterRSSI = false;
+static bool WIFIConnected = false;
+static int WIFIReconnectCnt = 0;

-bool WIFIConnected = false;
-int WIFIReconnectCnt = 0;
+
+
+void strinttoip4(const char *ip, int &a, int &b, int &c, int &d) {
+    std::string zw = std::string(ip);
+    std::stringstream s(zw);
+    char ch; //to temporarily store the '.'
+    s >> a >> ch >> b >> ch >> c >> ch >> d;
+}
+
+
+std::string BssidToString(const char* c) {
+	char cBssid[25];
+	sprintf(cBssid, "%02x:%02x:%02x:%02x:%02x:%02x", c[0], c[1], c[2], c[3], c[4], c[5]);
+	return std::string(cBssid);
+}


 #ifdef WLAN_USE_MESH_ROAMING
-
-int RSSI_Threshold = WLAN_WIFI_RSSI_THRESHOLD;
-
 /* rrm ctx */
 int rrm_ctx = 0;

-/* FreeRTOS event group to signal when we are connected & ready to make a request */
-static EventGroupHandle_t wifi_event_group;
-
-/* esp netif object representing the WIFI station */
-static esp_netif_t *sta_netif = NULL;
-
-//static const char *TAG = "roaming_example";
-
 static inline uint32_t WPA_GET_LE32(const uint8_t *a)
 {
 	return ((uint32_t) a[3] << 24) | (a[2] << 16) | (a[1] << 8) | a[0];
@@ -81,6 +86,7 @@ static inline uint32_t WPA_GET_LE32(const uint8_t *a)
 #define ETH_ALEN 6
 #endif

+
 #define MAX_NEIGHBOR_LEN 512
 static char * get_btm_neighbor_list(uint8_t *report, size_t report_len)
 {
@@ -97,10 +103,10 @@ static char * get_btm_neighbor_list(uint8_t *report, size_t report_len)
 	 * PHY Type[1]
 	 * Optional Subelements[variable]
 	 */
-#define NR_IE_MIN_LEN (ETH_ALEN + 4 + 1 + 1 + 1)
+	#define NR_IE_MIN_LEN (ETH_ALEN + 4 + 1 + 1 + 1)

 	if (!report || report_len == 0) {
-		ESP_LOGI(TAG, "RRM neighbor report is not valid");
+		ESP_LOGD(TAG, "Roaming: RRM neighbor report is not valid");
 		return NULL;
 	}

@@ -116,14 +122,14 @@ static char * get_btm_neighbor_list(uint8_t *report, size_t report_len)

 		if (pos[0] != WLAN_EID_NEIGHBOR_REPORT ||
 		    nr_len < NR_IE_MIN_LEN) {
-			ESP_LOGI(TAG, "CTRL: Invalid Neighbor Report element: id=%u len=%u",
+			ESP_LOGD(TAG, "Roaming CTRL: Invalid Neighbor Report element: id=%u len=%u",
 					data[0], nr_len);
 			ret = -1;
 			goto cleanup;
 		}

 		if (2U + nr_len > report_len) {
-			ESP_LOGI(TAG, "CTRL: Invalid Neighbor Report element: id=%u len=%zu nr_len=%u",
+			ESP_LOGD(TAG, "Roaming CTRL: Invalid Neighbor Report element: id=%u len=%zu nr_len=%u",
 					data[0], report_len, nr_len);
 			ret = -1;
 			goto cleanup;
@@ -166,8 +172,8 @@ static char * get_btm_neighbor_list(uint8_t *report, size_t report_len)

 			pos += s_len;
 		}
-
-		ESP_LOGI(TAG, "RMM neigbor report bssid=" MACSTR
+				
+		ESP_LOGI(TAG, "Roaming: RMM neigbor report bssid=" MACSTR
 				" info=0x%x op_class=%u chan=%u phy_type=%u%s%s%s%s",
 				MAC2STR(nr), WPA_GET_LE32(nr + ETH_ALEN),
 				nr[ETH_ALEN + 4], nr[ETH_ALEN + 5],
@@ -175,6 +181,10 @@ static char * get_btm_neighbor_list(uint8_t *report, size_t report_len)
 				lci[0] ? " lci=" : "", lci,
 				civic[0] ? " civic=" : "", civic);

+		
+		LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Roaming: RMM neigbor report BSSID: " + BssidToString((char*)nr) + 
+		                                        ", Channel: " + std::to_string(nr[ETH_ALEN + 5]));
+
 		/* neighbor start */
 		len += snprintf(buf + len, MAX_NEIGHBOR_LEN - len, " neighbor=");
 		/* bssid */
@@ -212,18 +222,19 @@ void neighbor_report_recv_cb(void *ctx, const uint8_t *report, size_t report_len
 	int *val = (int*) ctx;
 	uint8_t *pos = (uint8_t *)report;
 	int cand_list = 0;
+	int ret;

 	if (!report) {
-		ESP_LOGE(TAG, "report is null");
+		ESP_LOGD(TAG, "Roaming: Neighbor report is null");
 		return;
 	}
 	if (*val != rrm_ctx) {
-		ESP_LOGE(TAG, "rrm_ctx value didn't match, not initiated by us");
+		ESP_LOGE(TAG, "Roaming: rrm_ctx value didn't match, not initiated by us");
 		return;
 	}
 	/* dump report info */
-	ESP_LOGI(TAG, "rrm: neighbor report len=%d", report_len);
-	ESP_LOG_BUFFER_HEXDUMP(TAG, pos, report_len, ESP_LOG_INFO);
+	ESP_LOGD(TAG, "Roaming: RRM neighbor report len=%d", report_len);
+	ESP_LOG_BUFFER_HEXDUMP(TAG, pos, report_len, ESP_LOG_DEBUG);

 	/* create neighbor list */
 	char *neighbor_list = get_btm_neighbor_list(pos + 1, report_len - 1);
@@ -242,8 +253,10 @@ void neighbor_report_recv_cb(void *ctx, const uint8_t *report, size_t report_len
 		esp_wifi_scan_get_ap_records(&number, &ap_records);
 		cand_list = 1;
 	}
-	/* send AP btm query, this will cause STA to roam as well */
-	esp_wnm_send_bss_transition_mgmt_query(REASON_FRAME_LOSS, neighbor_list, cand_list);
+	/* send AP btm query requesting to roam depending on candidate list of AP */
+	// btm_query_reasons: https://github.com/espressif/esp-idf/blob/release/v4.4/components/wpa_supplicant/esp_supplicant/include/esp_wnm.h
+	ret = esp_wnm_send_bss_transition_mgmt_query(REASON_FRAME_LOSS, neighbor_list, cand_list);	// query reason 16 -> LOW RSSI --> (btm_query_reason)16
+	ESP_LOGD(TAG, "neighbor_report_recv_cb retval - bss_transisition_query: %d", ret);

 cleanup:
 	if (neighbor_list)
@@ -251,28 +264,151 @@ cleanup:
 }


-static void esp_bss_rssi_low_handler(void* arg, esp_event_base_t event_base,
-		int32_t event_id, void* event_data)
+static void esp_bss_rssi_low_handler(void* arg, esp_event_base_t event_base, int32_t event_id, void* event_data)
 {
+	int retval = -1;
 	wifi_event_bss_rssi_low_t *event = (wifi_event_bss_rssi_low_t*) event_data;

-	ESP_LOGI(TAG, "%s:bss rssi is=%d", __func__, event->rssi);
-	/* Lets check channel conditions */
-	rrm_ctx++;
-	if (esp_rrm_send_neighbor_rep_request(neighbor_report_recv_cb, &rrm_ctx) < 0) {
-		/* failed to send neighbor report request */
-		ESP_LOGI(TAG, "failed to send neighbor report request");
-		if (esp_wnm_send_bss_transition_mgmt_query(REASON_FRAME_LOSS, NULL, 0) < 0) {
-			ESP_LOGI(TAG, "failed to send btm query");
-		}
+	LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Roaming Event: RSSI " + std::to_string(event->rssi) + 
+								" < RSSI_Threshold " + std::to_string(wlan_config.rssi_threshold));
+
+	/* If RRM is supported, call RRM and then send BTM query to AP */
+	if (esp_rrm_is_rrm_supported_connection() && esp_wnm_is_btm_supported_connection()) 
+	{
+		/* Lets check channel conditions */
+		rrm_ctx++;
+
+		retval = esp_rrm_send_neighbor_rep_request(neighbor_report_recv_cb, &rrm_ctx);
+		ESP_LOGD(TAG, "esp_rrm_send_neighbor_rep_request retval: %d", retval);
+		if (retval == 0)
+			LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Roaming: RRM + BTM query sent");
+		else
+			ESP_LOGD(TAG, "esp_rrm_send_neighbor_rep_request retval: %d", retval);
+	}
+
+	/* If RRM is not supported or RRM request failed, send directly BTM query to AP */
+	if (retval < 0 && esp_wnm_is_btm_supported_connection()) 
+	{
+		// btm_query_reasons: https://github.com/espressif/esp-idf/blob/release/v4.4/components/wpa_supplicant/esp_supplicant/include/esp_wnm.h
+		retval = esp_wnm_send_bss_transition_mgmt_query(REASON_FRAME_LOSS, NULL, 0);	// query reason 16 -> LOW RSSI --> (btm_query_reason)16
+		ESP_LOGD(TAG, "esp_wnm_send_bss_transition_mgmt_query retval: %d", retval);
+		if (retval == 0)
+			LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Roaming: BTM query sent");
+		else
+			ESP_LOGD(TAG, "esp_wnm_send_bss_transition_mgmt_query retval: %d", retval);
 	}
 }


-#endif
+void printRoamingFeatureSupport(void) 
+{
+	if (esp_rrm_is_rrm_supported_connection())
+		LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Roaming: RRM (802.11k) supported by AP");
+	else
+		LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Roaming: RRM (802.11k) NOT supported by AP");

-//////////////////////////////////
-//////////////////////////////////
+	if (esp_wnm_is_btm_supported_connection())
+		LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Roaming: BTM (802.11v) supported by AP");
+	else
+		LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Roaming: BTM (802.11v) NOT supported by AP");
+}
+
+
+#ifdef WLAN_USE_MESH_ROAMING_ACTIVATE_CLIENT_TRIGGERED_QUERIES
+void wifiRoamingQuery(void) 
+{
+	/* Query only if WIFI is connected and feature is supported by AP */
+	if (WIFIConnected && (esp_rrm_is_rrm_supported_connection() || esp_wnm_is_btm_supported_connection())) {
+		/* Client is allowed to send query to AP for roaming request if RSSI is lower than threshold */
+		/* Note 1: Set RSSI threshold funtion needs to be called to trigger WIFI_EVENT_STA_BSS_RSSI_LOW */
+		/* Note 2: Additional querys will be sent after flow round is finshed --> server_tflite.cpp - function "task_autodoFlow" */
+		/* Note 3: RSSI_Threshold = 0 --> Disable client query by application (WebUI parameter) */
+		
+		if (wlan_config.rssi_threshold != 0 && get_WIFI_RSSI() != -127 && (get_WIFI_RSSI() < wlan_config.rssi_threshold))
+			esp_wifi_set_rssi_threshold(wlan_config.rssi_threshold);
+	}
+}
+#endif // WLAN_USE_MESH_ROAMING_ACTIVATE_CLIENT_TRIGGERED_QUERIES
+#endif // WLAN_USE_MESH_ROAMING
+
+
+#ifdef WLAN_USE_ROAMING_BY_SCANNING
+std::string getAuthModeName(const wifi_auth_mode_t auth_mode)
+{
+	std::string AuthModeNames[] = {"OPEN", "WEP", "WPA PSK", "WPA2 PSK", "WPA WPA2 PSK", "WPA2 ENTERPRISE",
+                                   "WPA3 PSK", "WPA2 WPA3 PSK", "WAPI_PSK", "MAX"};
+    return AuthModeNames[auth_mode];
+}
+
+
+void wifi_scan(void)
+{
+    wifi_scan_config_t wifi_scan_config;
+    memset(&wifi_scan_config, 0, sizeof(wifi_scan_config));
+
+    wifi_scan_config.ssid = (uint8_t*)wlan_config.ssid.c_str(); // only scan for configured SSID
+    wifi_scan_config.show_hidden = true;            // scan also hidden SSIDs
+	wifi_scan_config.channel = 0;                   // scan all channels
+
+    esp_wifi_scan_start(&wifi_scan_config, true);   // not using event handler SCAN_DONE by purpose to keep SYS_EVENT heap smaller 
+                                                    // and the calling task task_autodoFlow is after scan is finish in wait state anyway
+                                                    // Scan duration: ca. (120ms + 30ms) * Number of channels -> ca. 1,5 - 2s
+
+    uint16_t max_number_of_ap_found = 10;           // max. number of APs, value will be updated by function "esp_wifi_scan_get_ap_num"
+	esp_wifi_scan_get_ap_num(&max_number_of_ap_found); // get actual found APs
+    wifi_ap_record_t* wifi_ap_records = new wifi_ap_record_t[max_number_of_ap_found]; // Allocate necessary record datasets
+	if (wifi_ap_records == NULL) {
+		esp_wifi_scan_get_ap_records(0, NULL); // free internal heap
+		LogFile.WriteToFile(ESP_LOG_ERROR, TAG, "wifi_scan: Failed to allocate heap for wifi_ap_records");
+		return;
+	}
+	else {
+    	if (esp_wifi_scan_get_ap_records(&max_number_of_ap_found, wifi_ap_records) != ESP_OK) { // Retrieve results (and free internal heap)
+			LogFile.WriteToFile(ESP_LOG_ERROR, TAG, "wifi_scan: esp_wifi_scan_get_ap_records: Error retrieving datasets");
+			free(wifi_ap_records);
+			return;
+		}
+	}
+
+	wifi_ap_record_t currentAP;
+	esp_wifi_sta_get_ap_info(&currentAP);
+
+	LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Roaming: Current AP BSSID=" + BssidToString((char*)currentAP.bssid));
+    LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Roaming: Scan completed, APs found with configured SSID: " + std::to_string(max_number_of_ap_found));
+    for (int i = 0; i < max_number_of_ap_found; i++) {
+        LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Roaming: " + std::to_string(i+1) +
+                                                ": SSID=" + std::string((char*)wifi_ap_records[i].ssid) +
+                                                ", BSSID=" + BssidToString((char*)wifi_ap_records[i].bssid) + 
+                                                ", RSSI=" + std::to_string(wifi_ap_records[i].rssi) + 
+                                                ", CH=" + std::to_string(wifi_ap_records[i].primary) + 
+                                                ", AUTH=" + getAuthModeName(wifi_ap_records[i].authmode));
+		if (wifi_ap_records[i].rssi > (currentAP.rssi + 5) && // RSSI is better than actual RSSI + 5 --> Avoid switching to AP with roughly same RSSI
+           (strcmp(BssidToString((char*)wifi_ap_records[i].bssid).c_str(), BssidToString((char*)currentAP.bssid).c_str()) != 0))
+        {
+			APWithBetterRSSI = true;
+        }
+	}
+    free(wifi_ap_records);
+}
+
+
+void wifiRoamByScanning(void)
+{
+	if (wlan_config.rssi_threshold != 0 && get_WIFI_RSSI() != -127 && (get_WIFI_RSSI() < wlan_config.rssi_threshold)) {
+		LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Roaming: Start scan of all channels for SSID " + wlan_config.ssid);
+		wifi_scan();
+
+		if (APWithBetterRSSI) {
+			APWithBetterRSSI = false;
+			LogFile.WriteToFile(ESP_LOG_WARN, TAG, "Roaming: AP with better RSSI in range, disconnecting to switch AP...");
+			esp_wifi_disconnect();
+		} 
+		else {
+			LogFile.WriteToFile(ESP_LOG_DEBUG, TAG, "Roaming: Scan completed, stay on current AP");
+		}
+	}
+}
+#endif // WLAN_USE_ROAMING_BY_SCANNING


 std::string* getIPAddress()
@@ -294,13 +430,12 @@ static void event_handler(void* arg, esp_event_base_t event_base, int32_t event_
        WIFIConnected = false;
        esp_wifi_connect();
    }
-	
 	else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) 
 	{
 		/* Disconnect reason: https://github.com/espressif/esp-idf/blob/d825753387c1a64463779bbd2369e177e5d59a79/components/esp_wifi/include/esp_wifi_types.h */
 		wifi_event_sta_disconnected_t *disconn = (wifi_event_sta_disconnected_t *)event_data;
 		if (disconn->reason == WIFI_REASON_ROAMING) {
-			LogFile.WriteToFile(ESP_LOG_WARN, TAG, "Disconnected (" + std::to_string(disconn->reason) + ", Roaming)");
+			LogFile.WriteToFile(ESP_LOG_WARN, TAG, "Disconnected (" + std::to_string(disconn->reason) + ", Roaming 802.11kv)");
 			// --> no reconnect neccessary, it should automatically reconnect to new AP
 		}
 		else {
@@ -334,14 +469,21 @@ static void event_handler(void* arg, esp_event_base_t event_base, int32_t event_
 			LogFile.WriteToFile(ESP_LOG_ERROR, TAG, "Disconnected, multiple reconnect attempts failed (" + 
 													 std::to_string(disconn->reason) + "), still retrying...");
 		}
-	}
-	
+	}	
 	else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_CONNECTED) 
 	{
        LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Connected to: " + wlan_config.ssid + ", RSSI: " + 
 												std::to_string(get_WIFI_RSSI()));
-	}
-	
+
+		#ifdef WLAN_USE_MESH_ROAMING	
+			printRoamingFeatureSupport();
+
+			#ifdef WLAN_USE_MESH_ROAMING_ACTIVATE_CLIENT_TRIGGERED_QUERIES
+			// wifiRoamingQuery();	// Avoid client triggered query during processing flow (reduce risk of heap shortage). Request will be triggered at the end of every round anyway
+			#endif //WLAN_USE_MESH_ROAMING_ACTIVATE_CLIENT_TRIGGERED_QUERIES
+			
+		#endif //WLAN_USE_MESH_ROAMING
+	}	
 	else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) 
 	{
        WIFIConnected = true;
@@ -349,7 +491,7 @@ static void event_handler(void* arg, esp_event_base_t event_base, int32_t event_

 		ip_event_got_ip_t* event = (ip_event_got_ip_t*) event_data;
        wlan_config.ipaddress = std::string(ip4addr_ntoa((const ip4_addr*) &event->ip_info.ip));
-		LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Got IP: " + wlan_config.ipaddress);
+		LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Assigned IP: " + wlan_config.ipaddress);

 		#ifdef ENABLE_MQTT
            if (getMQTTisEnabled()) {
@@ -361,14 +503,6 @@ static void event_handler(void* arg, esp_event_base_t event_base, int32_t event_
 }


-void strinttoip4(const char *ip, int &a, int &b, int &c, int &d) {
-    std::string zw = std::string(ip);
-    std::stringstream s(zw);
-    char ch; //to temporarily store the '.'
-    s >> a >> ch >> b >> ch >> c >> ch >> d;
-}
-
-
 esp_err_t wifi_init_sta(void)
 {
 	esp_err_t retval = esp_netif_init();
@@ -462,6 +596,18 @@ esp_err_t wifi_init_sta(void)

    wifi_config_t wifi_config = { };

+	wifi_config.sta.scan_method = WIFI_ALL_CHANNEL_SCAN;		// Scan all channels instead of stopping after first match
+	wifi_config.sta.sort_method = WIFI_CONNECT_AP_BY_SIGNAL;	// Sort by signal strength and keep up to 4 best APs
+	//wifi_config.sta.failure_retry_cnt = 3;					// IDF version 5.0 will support this
+
+	#ifdef WLAN_USE_MESH_ROAMING
+	wifi_config.sta.rm_enabled = 1;		 // 802.11k (Radio Resource Management)
+	wifi_config.sta.btm_enabled = 1;	 // 802.11v (BSS Transition Management)
+	//wifi_config.sta.mbo_enabled = 1;	 // Multiband Operation (better use of Wi-Fi network resources in roaming decisions) -> not activated to save heap
+	wifi_config.sta.pmf_cfg.capable = 1; // 802.11w (Protected Management Frame, activated by default if other device also advertizes PMF capability)
+	//wifi_config.sta.ft_enabled = 1;	 // 802.11r (BSS Fast Transition) -> Upcoming IDF version 5.0 will support 11r
+	#endif
+
    strcpy((char*)wifi_config.sta.ssid, (const char*)wlan_config.ssid.c_str());
    strcpy((char*)wifi_config.sta.password, (const char*)wlan_config.password.c_str());

--- a/code/components/jomjol_wlan/connect_wlan.h
+++ b/code/components/jomjol_wlan/connect_wlan.h
@@ -12,4 +12,12 @@ int get_WIFI_RSSI();
 bool getWIFIisConnected();
 void WIFIDestroy();

+#if (defined WLAN_USE_MESH_ROAMING && defined WLAN_USE_MESH_ROAMING_ACTIVATE_CLIENT_TRIGGERED_QUERIES)
+void wifiRoamingQuery(void);
+#endif
+
+#ifdef WLAN_USE_ROAMING_BY_SCANNING
+void wifiRoamByScanning(void);
+#endif
+
 #endif //CONNECT_WLAN_H
--- a/code/components/jomjol_wlan/read_wlanini.cpp
+++ b/code/components/jomjol_wlan/read_wlanini.cpp
@@ -145,8 +145,8 @@ int LoadWlanFromFile(std::string fn)
                wlan_config.dns = tmp;
                LogFile.WriteToFile(ESP_LOG_INFO, TAG, "DNS: " + wlan_config.dns);
            }
-            #ifdef WLAN_USE_MESH_ROAMING
-            else if ((splitted.size() > 1) && (toUpper(splitted[0]) == "RSSITHRESHOLD")) {
+            #if (defined WLAN_USE_ROAMING_BY_SCANNING || (defined WLAN_USE_MESH_ROAMING && defined WLAN_USE_MESH_ROAMING_ACTIVATE_CLIENT_TRIGGERED_QUERIES))
+            else if ((splitted.size() > 1) && (toUpper(splitted[0]) == "RSSITHRESHOLD")){
                tmp = trim(splitted[1]);
                if ((tmp[0] == '"') && (tmp[tmp.length()-1] == '"')){
                    tmp = tmp.substr(1, tmp.length()-2);
@@ -266,7 +266,7 @@ bool ChangeHostName(std::string fn, std::string _newhostname)
    return true;
 }

-#ifdef WLAN_USE_MESH_ROAMING
+#if (defined WLAN_USE_ROAMING_BY_SCANNING || (defined WLAN_USE_MESH_ROAMING && defined WLAN_USE_MESH_ROAMING_ACTIVATE_CLIENT_TRIGGERED_QUERIES))
 bool ChangeRSSIThreshold(std::string fn, int _newrssithreshold)
 {
    if (wlan_config.rssi_threshold == _newrssithreshold)
--- a/code/include/defines.h
+++ b/code/include/defines.h
@@ -161,9 +161,22 @@
        }
    #define SUPRESS_TFLITE_ERRORS // use, to avoid error messages from TFLITE

-    //connect_wlan
-    #define WLAN_USE_MESH_ROAMING
-    #define WLAN_WIFI_RSSI_THRESHOLD -50
+
+    // connect_wlan.cpp
+    //******************************
+    /* WIFI roaming functionalities 802.11k+v (uses ca. 6kB - 8kB internal RAM; if SCAN CACHE activated: + 1kB / beacon)
+    PLEASE BE AWARE: The following CONFIG parameters have to to be set in 
+    sdkconfig.defaults before use of this function is possible!!
+    CONFIG_WPA_11KV_SUPPORT=y
+    CONFIG_WPA_SCAN_CACHE=n
+    CONFIG_WPA_MBO_SUPPORT=n
+    CONFIG_WPA_11R_SUPPORT=n
+    */
+    //#define WLAN_USE_MESH_ROAMING   // 802.11v (BSS Transition Management) + 802.11k (Radio Resource Management) (ca. 6kB - 8kB internal RAM neccessary)
+    //#define WLAN_USE_MESH_ROAMING_ACTIVATE_CLIENT_TRIGGERED_QUERIES  // Client can send query to AP requesting to roam (if RSSI lower than RSSI threshold)
+
+    /* WIFI roaming only client triggered by scanning the channels after each round (only if RSSI < RSSIThreshold) and trigger a disconnect to switch AP */
+    #define WLAN_USE_ROAMING_BY_SCANNING


    //ClassFlowCNNGeneral
--- a/code/main/main.cpp
+++ b/code/main/main.cpp
@@ -12,6 +12,7 @@
 //#include "esp_psram.h" // Comming in IDF 5.0, see https://docs.espressif.com/projects/esp-idf/en/v5.0-beta1/esp32/migration-guides/release-5.x/system.html?highlight=esp_psram_get_size
 //#include "spiram.h"
 #include "esp32/spiram.h"
+#include "esp_pm.h"


 // SD-Card ////////////////////
@@ -88,6 +89,7 @@ extern std::string getHTMLcommit(void);

 std::vector<std::string> splitString(const std::string& str);
 void migrateConfiguration(void);
+bool setCpuFrequency(void);

 static const char *TAG = "MAIN";

@@ -238,6 +240,12 @@ extern "C" void app_main(void)
    // ********************************************
    setupTime();    // NTP time service: Status of time synchronization will be checked after every round (server_tflite.cpp)

+
+    // Set CPU Frequency
+    // ********************************************
+    setCpuFrequency();
+
+
    // SD card: Create further mandatory directories (if not already existing)
    // Correct creation of these folders will be checked with function "SDCardCheckFolderFilePresence"
    // ********************************************
@@ -435,8 +443,8 @@ extern "C" void app_main(void)
    // ********************************************
    esp_chip_info_t chipInfo;
    esp_chip_info(&chipInfo);
-    LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Device info: CPU frequency: " + std::to_string(CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ) + 
-                                           "Mhz, CPU cores: " + std::to_string(chipInfo.cores) + 
+    
+    LogFile.WriteToFile(ESP_LOG_INFO, TAG, "Device info: CPU cores: " + std::to_string(chipInfo.cores) + 
                                           ", Chip revision: " + std::to_string(chipInfo.revision));
    
    // Print SD-Card info
@@ -696,3 +704,70 @@ std::vector<std::string> splitString(const std::string& str) {

    return found;
 }*/
+
+
+bool setCpuFrequency(void) {
+    ConfigFile configFile = ConfigFile(CONFIG_FILE); 
+    string cpuFrequency = "160";
+    esp_pm_config_esp32_t  pm_config; 
+
+    if (!configFile.ConfigFileExists()){
+        LogFile.WriteToFile(ESP_LOG_WARN, TAG, "No ConfigFile defined - exit setCpuFrequency()!");
+        return false;
+    }
+
+    std::vector<std::string> splitted;
+    std::string line = "";
+    bool disabledLine = false;
+    bool eof = false;
+
+
+    /* Load config from config file */
+    while ((!configFile.GetNextParagraph(line, disabledLine, eof) || 
+            (line.compare("[System]") != 0)) && !eof) {}
+    if (eof) {
+        return false;
+    }
+
+    if (disabledLine) {
+        return false;
+    }
+
+    while (configFile.getNextLine(&line, disabledLine, eof) && 
+            !configFile.isNewParagraph(line)) {
+        splitted = ZerlegeZeile(line);
+
+        if (toUpper(splitted[0]) == "CPUFREQUENCY") {
+            cpuFrequency = splitted[1];
+            break;
+        }
+    }
+
+    if (esp_pm_get_configuration(&pm_config) != ESP_OK) {
+        LogFile.WriteToFile(ESP_LOG_ERROR, TAG, "Failed to read CPU Frequency!");
+        return false;
+    }
+
+    if (cpuFrequency == "160") { // 160 is the default
+        // No change needed
+    }
+    else if (cpuFrequency == "240") {
+        pm_config.max_freq_mhz = 240;
+        pm_config.min_freq_mhz = pm_config.max_freq_mhz;
+        if (esp_pm_configure(&pm_config) != ESP_OK) {
+            LogFile.WriteToFile(ESP_LOG_ERROR, TAG, "Failed to set new CPU frequency!");
+            return false;
+        }
+    }
+    else {
+        LogFile.WriteToFile(ESP_LOG_ERROR, TAG, "Unknown CPU frequency: " + cpuFrequency + "! "
+                "It must be 160 or 240!");
+        return false;
+    }
+
+    if (esp_pm_get_configuration(&pm_config) == ESP_OK) {
+        LogFile.WriteToFile(ESP_LOG_INFO, TAG, string("CPU frequency: ") + to_string(pm_config.max_freq_mhz) + " MHz");
+    }
+
+    return true;
+}
--- a/code/sdkconfig.defaults
+++ b/code/sdkconfig.defaults
@@ -99,6 +99,8 @@ CONFIG_SPIRAM_MALLOC_ALWAYSINTERNAL=16384
 CONFIG_SPIRAM_MALLOC_RESERVE_INTERNAL=40960
 CONFIG_SPIRAM_CACHE_WORKAROUND=y
 CONFIG_SPIRAM_IGNORE_NOTFOUND=y
+CONFIG_SPIRAM_TRY_ALLOCATE_WIFI_LWIP=y
+CONFIG_SPIRAM_ALLOW_BSS_SEG_EXTERNAL_MEMORY=y

 CONFIG_ESP_INT_WDT_TIMEOUT_MS=300

@@ -118,6 +120,9 @@ CONFIG_MQTT_MSG_ID_INCREMENTAL=y
 CONFIG_MQTT_SKIP_PUBLISH_IF_DISCONNECTED=y
 CONFIG_MQTT_TASK_CORE_SELECTION_ENABLED=y
 CONFIG_MQTT_USE_CORE_0=y
+CONFIG_MQTT_USE_CUSTOM_CONFIG=y
+#CONFIG_MQTT_OUTBOX_EXPIRED_TIMEOUT_MS=5000
+CONFIG_MQTT_CUSTOM_OUTBOX=y

 CONFIG_FREERTOS_TASK_FUNCTION_WRAPPER=n

@@ -135,7 +140,14 @@ CONFIG_BF3005_SUPPORT=n

 CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE=4864

-#only necessary for task analysis (include/defines -> TASK_ANALYSIS_ON)
+#only necessary for WIFI mesh roaming (include/defines.h -> WLAN_USE_MESH_ROAMING)
+#CONFIG_WPA_11KV_SUPPORT=y
+#CONFIG_WPA_SCAN_CACHE=n
+#CONFIG_WPA_MBO_SUPPORT=n
+#CONFIG_WPA_11R_SUPPORT=n // Will be supported with ESP-IDF v5.0
+#CONFIG_WPA_DEBUG_PRINT=n
+
+#only necessary for task analysis (include/defines.h -> TASK_ANALYSIS_ON)
 #set in [env:esp32cam-dev-task-analysis]
 #CONFIG_FREERTOS_USE_TRACE_FACILITY=1
 #CONFIG_FREERTOS_GENERATE_RUN_TIME_STATS=y
@@ -146,3 +158,5 @@ CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE=4864
 #I (2112) esp_himem: Initialized. Using last 8 32KB address blocks for bank switching on 4352 KB of physical memory.
 CONFIG_SPIRAM_BANKSWITCH_ENABLE=n
 #CONFIG_SPIRAM_BANKSWITCH_RESERVE is not set
+
+CONFIG_PM_ENABLE=y
--- a/sd-card/config/config.ini
+++ b/sd-card/config/config.ini
@@ -22,7 +22,7 @@ FlipImageSize = false
 /config/ref1.jpg 442 142

 [Digits]
-Model = /config/dig-cont_0600_s3.tflite
+Model = /config/dig-cont_0611_s3_q.tflite
 CNNGoodThreshold = 0.5
 ;ROIImagesLocation = /log/digit
 ;ROIImagesRetention = 3
@@ -31,7 +31,7 @@ main.dig2 343 126 30 54 false
 main.dig3 391 126 30 54 false

 [Analog]
-Model = /config/ana-cont_11.3.1_s2.tflite
+Model = /config/ana-cont_1105_s2_q.tflite
 CNNGoodThreshold = 0.5
 ;ROIImagesLocation = /log/analog
 ;ROIImagesRetention = 3
@@ -107,4 +107,5 @@ TimeZone = CET-1CEST,M3.5.0,M10.5.0/3
 ;TimeServer = pool.ntp.org
 ;Hostname = undefined
 ;RSSIThreshold = 0
+CPUFrequency = 160
 SetupMode = true
--- a/sd-card/config/dig-class100_0160_s2_q.tflite
+++ b/sd-card/config/dig-class100_0160_s2_q.tflite
--- a/sd-card/config/dig-cont_0600_s3.tflite
+++ b/sd-card/config/dig-cont_0600_s3.tflite
--- a/sd-card/config/dig-cont_0610_s3.tflite
+++ b/sd-card/config/dig-cont_0610_s3.tflite
--- a/sd-card/config/dig-cont_0610_s3_q.tflite
+++ b/sd-card/config/dig-cont_0610_s3_q.tflite
--- a/sd-card/config/dig-cont_0611_s3.tflite
+++ b/sd-card/config/dig-cont_0611_s3.tflite
--- a/sd-card/config/dig-cont_0611_s3_q.tflite
+++ b/sd-card/config/dig-cont_0611_s3_q.tflite
--- a/sd-card/html/edit_config_param.html
+++ b/sd-card/html/edit_config_param.html
@@ -187,7 +187,7 @@ textarea {
 				<class id="TakeImage_ImageQuality_text" style="color:black;">Image Quality</class>
 			</td>
 			<td>
-				<input type="number" id="TakeImage_ImageQuality_value1" size="13" min="0" max="63">
+				<input type="number" id="TakeImage_ImageQuality_value1" size="13" min="8" max="63">
 			</td>
 			<td>$TOOLTIP_TakeImage_ImageQuality</td>
 		</tr>
@@ -654,6 +654,7 @@ textarea {
 					<option value="energy_wh">Energymeter (Value: Wh, Rate: W)</option>
 					<option value="energy_kwh">Energymeter (Value: kWh, Rate: kW)</option>
 					<option value="energy_mwh">Energymeter (Value: MWh, Rate: MW)</option>
+					<option value="energy_gj">Energymeter (Value: GJ, Rate: GJ/h)</option>
 				</select>
 			</td>
 			<td>$TOOLTIP_MQTT_MeterType</td>
@@ -1344,6 +1345,20 @@ textarea {
 			<td>$TOOLTIP_System_RSSIThreshold</td>
 		</tr>

+		<tr class="expert"  id="System_CPUFrequency">
+			<td class="indent1">
+				<input type="checkbox" id="System_CPUFrequency_enabled" value="1"  onclick = 'InvertEnableItem("System", "CPUFrequency")' unchecked >
+				<label for=System_CPUFrequency_enabled><class id="System_CPUFrequency_text" style="color:black;">CPU Frequency</class></label>
+			</td>
+			<td>
+				<select id="System_CPUFrequency_value1">
+					<option value="160" selected>160 MHz</option>
+					<option value="240">240 MHz</option>
+				</select>
+			</td>
+			<td>$TOOLTIP_System_CPUFrequency</td>
+		</tr>
+
 	</table>

 	<p>
@@ -1823,6 +1838,7 @@ function UpdateInput() {
 	WriteParameter(param, category, "System", "Hostname", true);	
 	WriteParameter(param, category, "System", "TimeServer", true);
 	WriteParameter(param, category, "System", "RSSIThreshold", true);
+	WriteParameter(param, category, "System", "CPUFrequency", true);

 	WriteModelFiles();
 }
@@ -1960,6 +1976,7 @@ function ReadParameterAll()
 	ReadParameter(param, "System", "Hostname", true);	
 	ReadParameter(param, "System", "TimeServer", true); 
 	ReadParameter(param, "System", "RSSIThreshold", true);
+	ReadParameter(param, "System", "CPUFrequency", true);

 	var sel = document.getElementById("Numbers_value1");	
 	UpdateInputIndividual(sel);
--- a/sd-card/html/readconfigparam.js
+++ b/sd-card/html/readconfigparam.js
@@ -262,6 +262,7 @@ function ParseConfig() {
     ParamAddValue(param, catname, "TimeServer");         
     ParamAddValue(param, catname, "Hostname");   
     ParamAddValue(param, catname, "RSSIThreshold");   
+     ParamAddValue(param, catname, "CPUFrequency");
     ParamAddValue(param, catname, "SetupMode"); 
     
     
@@ -315,7 +316,7 @@ function ParseConfig() {
          param["DataLogging"]["DataFilesRetention"]["value1"] = "3";
     }

-     // Downward compatiblity: Create RSSIThreshold if not available
+     // Downward compatibility: Create RSSIThreshold if not available
     if (param["System"]["RSSIThreshold"]["found"] == false)
     {
          param["System"]["RSSIThreshold"]["found"] = true;