[SYS] Implement a central queue (#1767)

to avoid concurrency issues when modules or/and gateways want to publish data

.gitignore

@@ -4,4 +4,5 @@
 **/.DS_Store
 node_modules
 *.ps1
-main/certs/private*
+main/certs/private*
+lib

main/User_config.h

@@ -354,9 +354,6 @@ int lowpowermode = DEFAULT_LOW_POWER_MODE;
 //#define ZgatewayRS232   "RS232"  //ESP8266, Arduino, ESP32
 
 /*-------------DEFINE YOUR MQTT ADVANCED PARAMETERS BELOW----------------*/
-#ifndef version_Topic
-#  define version_Topic "/version"
-#endif
 #ifndef will_Topic
 #  define will_Topic "/LWT"
 #endif
@@ -774,6 +771,9 @@ bool isAduplicateSignal(SIGNAL_SIZE_UL_ULL);
 void storeSignalValue(SIGNAL_SIZE_UL_ULL);
 #endif
 
+// Origin topics
+#define subjectBTtoMQTT "/BTtoMQTT"
+
 #define convertTemp_CtoF(c) ((c * 1.8) + 32)
 #define convertTemp_FtoC(f) ((f - 32) * 5 / 9)
 

main/ZactuatorONOFF.ino

@@ -144,11 +144,10 @@ void MQTTtoONOFF(char* topicOri, JsonObject& ONOFFdata) {
       if (ONOFFConfig.useLastStateOnStart) {
         ONOFFdata["save"] = true;
         ONOFFConfig_fromJson(ONOFFdata);
-        ONOFFdata.remove("save");
       }
 #    endif
       // we acknowledge the sending by publishing the value to an acknowledgement topic
-      pub(subjectGTWONOFFtoMQTT, ONOFFdata);
+      stateONOFFMeasures();
     } else {
       if (ONOFFdata["cmd"] == "high_pulse") {
         Log.notice(F("MQTTtoONOFF high_pulse ok" CR));
@@ -290,10 +289,9 @@ void ActuatorTrigger() {
   if (ONOFFConfig.useLastStateOnStart) {
     ONOFFdata["save"] = true;
     ONOFFConfig_fromJson(ONOFFdata);
-    ONOFFdata.remove("save");
   }
 #  endif
-  pub(subjectGTWONOFFtoMQTT, ONOFFdata);
+  stateONOFFMeasures();
 }
 
 void stateONOFFMeasures() {
@@ -301,6 +299,7 @@ void stateONOFFMeasures() {
   StaticJsonDocument<64> jsonBuffer;
   JsonObject ONOFFdata = jsonBuffer.to<JsonObject>();
   ONOFFdata["cmd"] = (int)digitalRead(ACTUATOR_ONOFF_GPIO);
-  pub(subjectGTWONOFFtoMQTT, ONOFFdata);
+  ONOFFdata["origin"] = subjectGTWONOFFtoMQTT;
+  enqueueJsonObject(ONOFFdata);
 }
 #endif

main/ZdisplaySSD1306.ino

@@ -188,7 +188,6 @@ void MQTTtoSSD1306(char* topicOri, JsonObject& SSD1306data) { // json object dec
     if (success) {
       stateSSD1306Display();
     } else {
-      // pub(subjectSSD1306toMQTT, "{\"Status\": \"Error\"}"); // Fail feedback
       Log.error(F("[ SSD1306 ] MQTTtoSSD1306 Fail json" CR), SSD1306data);
     }
   }
@@ -459,15 +458,16 @@ void OledSerial::drawLogo(int xshift, int yshift) {
 
 String stateSSD1306Display() {
   //Publish display state
-  StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+  StaticJsonDocument<JSON_MSG_BUFFER> DISPLAYdataBuffer;
-  JsonObject DISPLAYdata = jsonBuffer.to<JsonObject>();
+  JsonObject DISPLAYdata = DISPLAYdataBuffer.to<JsonObject>();
   DISPLAYdata["onstate"] = (bool)displayState;
   DISPLAYdata["brightness"] = (int)displayBrightness;
   DISPLAYdata["display-flip"] = (bool)displayFlip;
   DISPLAYdata["idlelogo"] = (bool)idlelogo;
   DISPLAYdata["log-oled"] = (bool)logToOLEDDisplay;
   DISPLAYdata["json-oled"] = (bool)jsonDisplay;
-  pub(subjectSSD1306toMQTT, DISPLAYdata);
+  DISPLAYdata["origin"] = subjectSSD1306toMQTT;
+  enqueueJsonObject(DISPLAYdata);
   // apply
   Oled.display->setBrightness(round(displayBrightness * 2.55));
 

main/Zgateway2G.ino

@@ -80,8 +80,8 @@ bool _2GtoMQTT() {
   // Get the memory locations of unread SMS messages.
   unreadSMSNum = A6l.getUnreadSMSLocs(unreadSMSLocs, 512);
   Log.trace(F("Creating SMS  buffer" CR));
-  StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+  StaticJsonDocument<JSON_MSG_BUFFER> SMSdataBuffer;
-  JsonObject SMSdata = jsonBuffer.to<JsonObject>();
+  JsonObject SMSdata = SMSdataBuffer.to<JsonObject>();
   for (int i = 0; i < unreadSMSNum; i++) {
     Log.notice(F("New  message at index: %d" CR), unreadSMSNum);
     sms = A6l.readSMS(unreadSMSLocs[i]);
@@ -90,7 +90,8 @@ bool _2GtoMQTT() {
     SMSdata["phone"] = (const char*)sms.number.c_str();
     A6l.deleteSMS(unreadSMSLocs[i]); // we delete the SMS received
     Log.trace(F("Adv data 2GtoMQTT" CR));
-    pub(subject2GtoMQTT, SMSdata);
+    SMSdata["origin"] = subject2GtoMQTT;
+    enqueueJsonObject(SMSdata);
     return true;
   }
   return false;

main/ZgatewayBLEConnect.h

@@ -6,8 +6,6 @@
 #  include "NimBLEDevice.h"
 #  include "config_BT.h"
 
-extern void pubBT(JsonObject& data);
-
 class zBLEConnect {
 public:
   NimBLEClient* m_pClient;

main/ZgatewayBLEConnect.ino

@@ -79,14 +79,15 @@ bool zBLEConnect::processActions(std::vector<BLEAction>& actions) {
   if (actions.size() > 0) {
     for (auto& it : actions) {
       if (NimBLEAddress(it.addr) == m_pClient->getPeerAddress()) {
-        JsonObject BLEresult = getBTJsonObject();
+        DynamicJsonDocument BLEdataBuffer(JSON_MSG_BUFFER);
-        BLEresult["id"] = it.addr;
+        JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
-        BLEresult["service"] = it.service.toString();
+        BLEdata["id"] = it.addr;
-        BLEresult["characteristic"] = it.characteristic.toString();
+        BLEdata["service"] = it.service.toString();
+        BLEdata["characteristic"] = it.characteristic.toString();
 
         if (it.write) {
           Log.trace(F("processing BLE write" CR));
-          BLEresult["write"] = it.value;
+          BLEdata["write"] = it.value;
           result = writeData(&it);
         } else {
           Log.trace(F("processing BLE read" CR));
@@ -95,31 +96,32 @@ bool zBLEConnect::processActions(std::vector<BLEAction>& actions) {
             switch (it.value_type) {
               case BLE_VAL_HEX: {
                 char* pHex = NimBLEUtils::buildHexData(nullptr, (uint8_t*)it.value.c_str(), it.value.length());
-                BLEresult["read"] = pHex;
+                BLEdata["read"] = pHex;
                 free(pHex);
                 break;
               }
               case BLE_VAL_INT: {
                 int ival = *(int*)it.value.data();
-                BLEresult["read"] = ival;
+                BLEdata["read"] = ival;
                 break;
               }
               case BLE_VAL_FLOAT: {
                 float fval = *(double*)it.value.data();
-                BLEresult["read"] = fval;
+                BLEdata["read"] = fval;
                 break;
               }
               default:
-                BLEresult["read"] = it.value.c_str();
+                BLEdata["read"] = it.value.c_str();
                 break;
             }
           }
         }
 
         it.complete = result;
-        BLEresult["success"] = result;
+        BLEdata["success"] = result;
         if (result || it.ttl <= 1) {
-          pubBT(BLEresult);
+          buildTopicFromId(BLEdata, subjectBTtoMQTT);
+          enqueueJsonObject(BLEdata);
         }
       }
     }
@@ -138,7 +140,8 @@ void LYWSD03MMC_connect::notifyCB(NimBLERemoteCharacteristic* pChar, uint8_t* pD
 
     if (length == 5) {
       Log.trace(F("Device identified creating BLE buffer" CR));
-      JsonObject BLEdata = getBTJsonObject();
+      DynamicJsonDocument BLEdataBuffer(JSON_MSG_BUFFER);
+      JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
       String mac_address = m_pClient->getPeerAddress().toString().c_str();
       mac_address.toUpperCase();
       for (std::vector<BLEdevice*>::iterator it = devices.begin(); it != devices.end(); ++it) {
@@ -157,8 +160,8 @@ void LYWSD03MMC_connect::notifyCB(NimBLERemoteCharacteristic* pChar, uint8_t* pD
       BLEdata["hum"] = (float)(pData[2]);
       BLEdata["volt"] = (float)(((pData[4] * 256) + pData[3]) / 1000.0);
       BLEdata["batt"] = (float)(((((pData[4] * 256) + pData[3]) / 1000.0) - 2.1) * 100);
-
+      buildTopicFromId(BLEdata, subjectBTtoMQTT);
-      pubBT(BLEdata);
+      enqueueJsonObject(BLEdata);
     } else {
       Log.notice(F("Invalid notification data" CR));
       return;
@@ -208,7 +211,8 @@ void DT24_connect::notifyCB(NimBLERemoteCharacteristic* pChar, uint8_t* pData, s
       // https://github.com/NiceLabs/atorch-console/blob/master/docs/protocol-design.md#dc-meter-report
       // Data comes as two packets ( 20 and 16 ), and am only processing first
       Log.trace(F("Device identified creating BLE buffer" CR));
-      JsonObject& BLEdata = getBTJsonObject();
+      DynamicJsonDocument BLEdataBuffer(JSON_MSG_BUFFER);
+      JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
       String mac_address = m_pClient->getPeerAddress().toString().c_str();
       mac_address.toUpperCase();
       BLEdata["model"] = "DT24";
@@ -221,8 +225,8 @@ void DT24_connect::notifyCB(NimBLERemoteCharacteristic* pChar, uint8_t* pData, s
       BLEdata["price"] = (float)(((m_data[17] * 256 * 256) + (m_data[18] * 256) + m_data[19]) / 100.0);
       BLEdata["tempc"] = (float)(m_data[24] * 256) + m_data[25];
       BLEdata["tempf"] = (float)(convertTemp_CtoF((m_data[24] * 256) + m_data[25]));
-
+      buildTopicFromId(BLEdata, subjectBTtoMQTT);
-      pubBT(BLEdata);
+      enqueueJsonObject(BLEdata);
     } else {
       Log.notice(F("Invalid notification data" CR));
       return;
@@ -264,7 +268,8 @@ void BM2_connect::notifyCB(NimBLERemoteCharacteristic* pChar, uint8_t* pData, si
     Log.trace(F("Callback from %s characteristic" CR), pChar->getUUID().toString().c_str());
     if (length == 16) {
       Log.trace(F("Device identified creating BLE buffer" CR));
-      JsonObject& BLEdata = getBTJsonObject();
+      DynamicJsonDocument BLEdataBuffer(JSON_MSG_BUFFER);
+      JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
       String mac_address = m_pClient->getPeerAddress().toString().c_str();
       mac_address.toUpperCase();
       BLEdata["model"] = "BM2 Battery Monitor";
@@ -297,7 +302,8 @@ void BM2_connect::notifyCB(NimBLERemoteCharacteristic* pChar, uint8_t* pData, si
       float volt = ((output[2] | (output[1] << 8)) >> 4) / 100.0f;
       BLEdata["volt"] = volt;
       Log.trace(F("volt: %F" CR), volt);
-      pubBT(BLEdata);
+      buildTopicFromId(BLEdata, subjectBTtoMQTT);
+      enqueueJsonObject(BLEdata);
     } else {
       Log.notice(F("Invalid notification data" CR));
       return;
@@ -347,13 +353,15 @@ void HHCCJCY01HHCC_connect::publishData() {
       value = pChar2->readValue();
       const char* val2 = value.c_str();
       batteryValue = val2[0];
-      JsonObject& BLEdata = getBTJsonObject();
+      DynamicJsonDocument BLEdataBuffer(JSON_MSG_BUFFER);
+      JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
       String mac_address = m_pClient->getPeerAddress().toString().c_str();
       mac_address.toUpperCase();
       BLEdata["model"] = "HHCCJCY01HHCC";
       BLEdata["id"] = (char*)mac_address.c_str();
       BLEdata["batt"] = (int)batteryValue;
-      pubBT(BLEdata);
+      buildTopicFromId(BLEdata, subjectBTtoMQTT);
+      enqueueJsonObject(BLEdata);
     } else {
       Log.notice(F("Failed getting characteristic" CR));
     }
@@ -372,7 +380,8 @@ void XMWSDJ04MMC_connect::notifyCB(NimBLERemoteCharacteristic* pChar, uint8_t* p
 
     if (length == 6) {
       Log.trace(F("Device identified creating BLE buffer" CR));
-      JsonObject BLEdata = getBTJsonObject();
+      DynamicJsonDocument BLEdataBuffer(JSON_MSG_BUFFER);
+      JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
       String mac_address = m_pClient->getPeerAddress().toString().c_str();
       mac_address.toUpperCase();
       BLEdata["model"] = "XMWSDJ04MMC";
@@ -383,8 +392,8 @@ void XMWSDJ04MMC_connect::notifyCB(NimBLERemoteCharacteristic* pChar, uint8_t* p
       BLEdata["hum"] = (float)((pData[2] | (pData[3] << 8)) * 0.1);
       BLEdata["volt"] = (float)((pData[4] | (pData[5] << 8)) / 1000.0);
       BLEdata["batt"] = (float)((((pData[4] | (pData[5] << 8)) / 1000.0) - 2.1) * 100);
-
+      buildTopicFromId(BLEdata, subjectBTtoMQTT);
-      pubBT(BLEdata);
+      enqueueJsonObject(BLEdata);
     } else {
       Log.notice(F("Invalid notification data" CR));
       return;
@@ -476,10 +485,12 @@ bool SBS1_connect::processActions(std::vector<BLEAction>& actions) {
 
         it.complete = result;
         if (result || it.ttl <= 1) {
-          JsonObject BLEresult = getBTJsonObject();
+          StaticJsonDocument<JSON_MSG_BUFFER> BLEdataBuffer;
-          BLEresult["id"] = it.addr;
+          JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
-          BLEresult["state"] = result ? it.value : it.value == "on" ? "off" : "on";
+          BLEdata["id"] = it.addr;
-          pubBT(BLEresult);
+          BLEdata["state"] = result ? it.value : it.value == "on" ? "off" : "on";
+          buildTopicFromId(BLEdata, subjectBTtoMQTT);
+          enqueueJsonObject(BLEdata);
         }
       }
     }

main/ZgatewayBT.ino

@@ -113,8 +113,9 @@ void btScanWDG() {
   if (!ProcessLock &&
       previousBtScanCount == scanCount &&
       scanCount != 0 &&
-      (now - lastBtScan > BTConfig.BLEinterval)) {
+      (now - lastBtScan > ((BTConfig.BLEinterval + BTConfig.scanDuration) < GeneralTimeOut ? GeneralTimeOut + 1000 : (BTConfig.BLEinterval + BTConfig.scanDuration)))) {
     Log.error(F("BLE Scan watchdog triggered at : %ds" CR), lastBtScan / 1000);
+    stopProcessing();
     ESPRestart(4);
   } else {
     previousBtScanCount = scanCount;
@@ -148,10 +149,6 @@ String stateBTMeasures(bool start) {
   jo["ignoreWBlist"] = BTConfig.ignoreWBlist;
   jo["presenceawaytimer"] = BTConfig.presenceAwayTimer;
   jo["movingtimer"] = BTConfig.movingTimer;
-  jo["btqblck"] = btQueueBlocked;
-  jo["btqsum"] = btQueueLengthSum;
-  jo["btqsnd"] = btQueueLengthCount;
-  jo["btqavg"] = (btQueueLengthCount > 0 ? btQueueLengthSum / (float)btQueueLengthCount : 0);
   jo["bletaskstack"] = uxTaskGetStackHighWaterMark(xProcBLETaskHandle);
   jo["blecoretaskstack"] = uxTaskGetStackHighWaterMark(xCoreTaskHandle);
 
@@ -163,7 +160,8 @@ String stateBTMeasures(bool start) {
   }
   String output;
   serializeJson(jo, output);
-  pub(subjectBTtoMQTT, jo);
+  jo["origin"] = subjectBTtoMQTT;
+  enqueueJsonObject(jo);
   return (output);
 }
 
@@ -315,103 +313,10 @@ void BTConfig_load() {
   }
 }
 
-void pubBTMainCore(JsonObject& data, bool haPresenceEnabled = true) {
-  if (abs((int)data["rssi"] | 0) < abs(BTConfig.minRssi) && data.containsKey("id")) {
-    String topic = data["id"].as<const char*>();
-    topic.replace(":", ""); // Initially publish topic ends with MAC address
-    if (BTConfig.pubBeaconUuidForTopic && !BTConfig.extDecoderEnable && data.containsKey("model_id") && data["model_id"].as<String>() == "IBEACON")
-      topic = data["uuid"].as<const char*>(); // If model_id is IBEACON, use uuid as topic
-    if (BTConfig.extDecoderEnable && !data.containsKey("model"))
-      topic = BTConfig.extDecoderTopic; // If external decoder, use this topic to send data
-    topic = subjectBTtoMQTT + String("/") + topic;
-    pub((char*)topic.c_str(), data);
-  }
-  if (haPresenceEnabled && data.containsKey("distance")) {
-    if (data.containsKey("servicedatauuid"))
-      data.remove("servicedatauuid");
-    if (data.containsKey("servicedata"))
-      data.remove("servicedata");
-    if (BTConfig.presenceUseBeaconUuid && data.containsKey("model_id") && data["model_id"].as<String>() == "IBEACON") {
-      data["mac"] = data["id"];
-      data["id"] = data["uuid"];
-    }
-    String topic = String(mqtt_topic) + BTConfig.presenceTopic + String(gateway_name);
-    Log.trace(F("Pub HA Presence %s" CR), topic.c_str());
-    pub_custom_topic((char*)topic.c_str(), data, false);
-  }
-}
-
-class JsonBundle {
-public:
-  StaticJsonDocument<JSON_MSG_BUFFER> buffer;
-  JsonObject object;
-  bool haPresence;
-
-  JsonObject& createObject(const char* json = NULL, bool haPresenceEnabled = true) {
-    buffer.clear();
-    haPresence = haPresenceEnabled;
-    object = buffer.to<JsonObject>();
-
-    if (json != nullptr) {
-      auto error = deserializeJson(buffer, json);
-      if (error) {
-        Log.error(F("deserialize object failed: %s" CR), error.c_str());
-      }
-    }
-    return object;
-  }
-};
-
 void PublishDeviceData(JsonObject& BLEdata);
 
 atomic_int forceBTScan;
 
-JsonBundle jsonBTBufferQueue[BTQueueSize];
-atomic_int jsonBTBufferQueueNext, jsonBTBufferQueueLast;
-int btQueueBlocked = 0;
-int btQueueLengthSum = 0;
-int btQueueLengthCount = 0;
-
-JsonObject& getBTJsonObject(const char* json, bool haPresenceEnabled) {
-  int next, last;
-  for (bool blocked = false;;) {
-    next = atomic_load_explicit(&jsonBTBufferQueueNext, ::memory_order_seq_cst); // use namespace std -> ambiguous error...
-    last = atomic_load_explicit(&jsonBTBufferQueueLast, ::memory_order_seq_cst); // use namespace std -> ambiguous error...
-    if ((2 * BTQueueSize + last - next) % (2 * BTQueueSize) != BTQueueSize) break;
-    if (!blocked) {
-      blocked = true;
-      btQueueBlocked++;
-    }
-    delay(1);
-  }
-  return jsonBTBufferQueue[last % BTQueueSize].createObject(json, haPresenceEnabled);
-}
-
-// should run from the BT core
-void pubBT(JsonObject& data) {
-  int last = atomic_load_explicit(&jsonBTBufferQueueLast, ::memory_order_seq_cst);
-  atomic_store_explicit(&jsonBTBufferQueueLast, (last + 1) % (2 * BTQueueSize), ::memory_order_seq_cst); // use namespace std -> ambiguous error...
-}
-
-// should run from the main core
-void emptyBTQueue() {
-  for (bool first = true;;) {
-    int next = atomic_load_explicit(&jsonBTBufferQueueNext, ::memory_order_seq_cst); // use namespace std -> ambiguous error...
-    int last = atomic_load_explicit(&jsonBTBufferQueueLast, ::memory_order_seq_cst); // use namespace std -> ambiguous error...
-    if (last == next) break;
-    if (first) {
-      int diff = (2 * BTQueueSize + last - next) % (2 * BTQueueSize);
-      btQueueLengthSum += diff;
-      btQueueLengthCount++;
-      first = false;
-    }
-    JsonBundle& bundle = jsonBTBufferQueue[next % BTQueueSize];
-    pubBTMainCore(bundle.object, bundle.haPresence);
-    atomic_store_explicit(&jsonBTBufferQueueNext, (next + 1) % (2 * BTQueueSize), ::memory_order_seq_cst); // use namespace std -> ambiguous error...
-    vTaskDelay(1);
-  }
-}
-
 void createOrUpdateDevice(const char* mac, uint8_t flags, int model, int mac_type = 0);
 
 BLEdevice* getDeviceByMac(const char* mac); // Declared here to avoid pre-compilation issue (misplaced auto declaration by pio)
@@ -509,11 +414,13 @@ void updateDevicesStatus() {
                                     p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::RUUVITAG_RAWV2)) { // We apply the offline status only for tracking device, can be extended further to all the devices
       if ((p->lastUpdate != 0) && (p->lastUpdate < (now - BTConfig.presenceAwayTimer) && (now > BTConfig.presenceAwayTimer)) &&
           (BTConfig.ignoreWBlist || ((!oneWhite || isWhite(p)) && !isBlack(p)))) { // Only if WBlist is disabled OR ((no white MAC OR this MAC is white) AND not a black listed MAC)) {
-        JsonObject BLEdata = getBTJsonObject();
+        StaticJsonDocument<JSON_MSG_BUFFER> BLEdataBuffer;
+        JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
         BLEdata["id"] = p->macAdr;
         BLEdata["state"] = "offline";
-        pubBT(BLEdata);
+        buildTopicFromId(BLEdata, subjectBTtoMQTT);
-        // We set the lastUpdate to 0 to avoid republishing the offline state
+        enqueueJsonObject(BLEdata);
+        // We set the lastUpdate to 0 to avoid replublishing the offline state
         p->lastUpdate = 0;
       }
     }
@@ -521,11 +428,13 @@ void updateDevicesStatus() {
     if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::BC08) {
       if ((p->lastUpdate != 0) && (p->lastUpdate < (now - BTConfig.movingTimer) && (now > BTConfig.movingTimer)) &&
           (BTConfig.ignoreWBlist || ((!oneWhite || isWhite(p)) && !isBlack(p)))) { // Only if WBlist is disabled OR ((no white MAC OR this MAC is white) AND not a black listed MAC)) {
-        JsonObject BLEdata = getBTJsonObject();
+        StaticJsonDocument<JSON_MSG_BUFFER> BLEdataBuffer;
+        JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
         BLEdata["id"] = p->macAdr;
         BLEdata["state"] = "offline";
-        pubBT(BLEdata);
+        buildTopicFromId(BLEdata, subjectBTtoMQTT);
-        // We set the lastUpdate to 0 to avoid republishing the offline state
+        enqueueJsonObject(BLEdata);
+        // We set the lastUpdate to 0 to avoid replublishing the offline state
         p->lastUpdate = 0;
       }
     }
@@ -682,16 +591,22 @@ void procBLETask(void* pvParameters) {
 
   for (;;) {
     xQueueReceive(BLEQueue, &advertisedDevice, portMAX_DELAY);
-
+    // Feed the watchdog
+    //esp_task_wdt_reset();
     if (!ProcessLock) {
       Log.trace(F("Creating BLE buffer" CR));
-      JsonObject& BLEdata = getBTJsonObject();
+      StaticJsonDocument<JSON_MSG_BUFFER> BLEdataBuffer;
-      String mac_adress = advertisedDevice->getAddress().toString().c_str();
+      JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
-      mac_adress.toUpperCase();
+      std::string mac_address = advertisedDevice->getAddress().toString();
-      BLEdata["id"] = (char*)mac_adress.c_str();
+
+      for (char& c : mac_address) {
+        c = std::toupper(c);
+      }
+
+      BLEdata["id"] = mac_address;
       BLEdata["mac_type"] = advertisedDevice->getAddress().getType();
       BLEdata["adv_type"] = advertisedDevice->getAdvType();
-      Log.notice(F("Device detected: %s" CR), (char*)mac_adress.c_str());
+      Log.notice(F("Device detected: %s" CR), BLEdata["id"].as<const char*>());
       BLEdevice* device = getDeviceByMac(BLEdata["id"].as<const char*>());
 
       if (BTConfig.filterConnectable && device->connect) {
@@ -718,31 +633,21 @@ void procBLETask(void* pvParameters) {
         if (advertisedDevice->haveServiceData()) {
           int serviceDataCount = advertisedDevice->getServiceDataCount();
           Log.trace(F("Get services data number: %d" CR), serviceDataCount);
-          char initialBLEDataCharArray[JSON_MSG_BUFFER];
-          serializeJson(BLEdata, initialBLEDataCharArray);
           for (int j = 0; j < serviceDataCount; j++) {
+            StaticJsonDocument<JSON_MSG_BUFFER> BLEdataBufferTemp;
+            JsonObject BLEdataTemp = BLEdataBufferTemp.to<JsonObject>();
+            BLEdataBufferTemp = BLEdataBuffer;
             std::string service_data = convertServiceData(advertisedDevice->getServiceData(j));
             Log.trace(F("Service data: %s" CR), service_data.c_str());
             std::string serviceDatauuid = advertisedDevice->getServiceDataUUID(j).toString();
             Log.trace(F("Service data UUID: %s" CR), (char*)serviceDatauuid.c_str());
-            if (j == 0) {
+            BLEdataTemp["servicedata"] = (char*)service_data.c_str();
-              BLEdata["servicedata"] = (char*)service_data.c_str();
+            BLEdataTemp["servicedatauuid"] = (char*)serviceDatauuid.c_str();
-              BLEdata["servicedatauuid"] = (char*)serviceDatauuid.c_str();
+            PublishDeviceData(BLEdataTemp);
-              PublishDeviceData(BLEdata);
-            } else {
-              StaticJsonDocument<JSON_MSG_BUFFER> doc;
-              deserializeJson(doc, initialBLEDataCharArray);
-              JsonObject& BLEdataTemp = getBTJsonObject();
-              BLEdataTemp = doc.as<JsonObject>();
-              BLEdataTemp["servicedata"] = (char*)service_data.c_str();
-              BLEdataTemp["servicedatauuid"] = (char*)serviceDatauuid.c_str();
-              PublishDeviceData(BLEdataTemp);
-            }
           }
         } else {
           PublishDeviceData(BLEdata);
         }
-
       } else {
         Log.trace(F("Filtered MAC device" CR));
       }
@@ -757,7 +662,7 @@ void procBLETask(void* pvParameters) {
  */
 void BLEscan() {
   // Don't start the next scan until processing of previous results is complete.
-  while (uxQueueMessagesWaiting(BLEQueue)) {
+  while (uxQueueMessagesWaiting(BLEQueue) || jsonQueue.size() != 0) {
     yield();
   }
   Log.notice(F("Scan begin" CR));
@@ -967,7 +872,7 @@ void setupBTTasksAndBLE() {
   xTaskCreatePinnedToCore(
       procBLETask, /* Function to implement the task */
       "procBLETask", /* Name of the task */
-      5120, /* Stack size in bytes */
+      8000, /* Stack size in bytes */
       NULL, /* Task input parameter */
       2, /* Priority of the task (set higher than core task) */
       &xProcBLETaskHandle, /* Task handle. */
@@ -1000,8 +905,6 @@ void setupBT() {
   Log.notice(F("Moving Timer: %d" CR), BTConfig.movingTimer);
 
   atomic_init(&forceBTScan, 0); // in theory, we don't need this
-  atomic_init(&jsonBTBufferQueueNext, 0); // in theory, we don't need this
-  atomic_init(&jsonBTBufferQueueLast, 0); // in theory, we don't need this
 
   semaphoreCreateOrUpdateDevice = xSemaphoreCreateBinary();
   xSemaphoreGive(semaphoreCreateOrUpdateDevice);
@@ -1009,7 +912,7 @@ void setupBT() {
   semaphoreBLEOperation = xSemaphoreCreateBinary();
   xSemaphoreGive(semaphoreBLEOperation);
 
-  BLEQueue = xQueueCreate(32, sizeof(NimBLEAdvertisedDevice*));
+  BLEQueue = xQueueCreate(QueueSize, sizeof(NimBLEAdvertisedDevice*));
 
   setupBTTasksAndBLE();
 
@@ -1191,6 +1094,7 @@ void PublishDeviceData(JsonObject& BLEdata) {
     process_bledata(BLEdata);
     // If the device is a random MAC and pubRandomMACs is false we don't publish this payload
     if (!BTConfig.pubRandomMACs && (BLEdata["type"].as<string>()).compare("RMAC") == 0) {
+      Log.trace(F("Random MAC, device filtered" CR));
       return;
     }
     // If pubAdvData is false we don't publish the adv data
@@ -1209,21 +1113,38 @@ void PublishDeviceData(JsonObject& BLEdata) {
     }
     // If the device is not a sensor and pubOnlySensors is true we don't publish this payload
     if (!BTConfig.pubOnlySensors || BLEdata.containsKey("model") || BLEdata.containsKey("distance")) { // Identified device
-      pubBT(BLEdata);
+      buildTopicFromId(BLEdata, subjectBTtoMQTT);
+      enqueueJsonObject(BLEdata);
+    } else {
+      Log.notice(F("Not a sensor device filtered" CR));
+      return;
     }
   } else if (BLEdata.containsKey("distance")) {
-    pubBT(BLEdata);
+    if (BLEdata.containsKey("servicedatauuid"))
+      BLEdata.remove("servicedatauuid");
+    if (BLEdata.containsKey("servicedata"))
+      BLEdata.remove("servicedata");
+    if (BLEdata.containsKey("manufacturerdata"))
+      BLEdata.remove("manufacturerdata");
+    if (BTConfig.presenceUseBeaconUuid && BLEdata.containsKey("model_id") && BLEdata["model_id"].as<String>() == "IBEACON") {
+      BLEdata["mac"] = BLEdata["id"];
+      BLEdata["id"] = BLEdata["uuid"];
+    }
+    enqueueJsonObject(BLEdata);
   } else {
-    Log.trace(F("Low rssi, device filtered" CR));
+    Log.notice(F("Low rssi, device filtered" CR));
+    return;
   }
 }
 
 void process_bledata(JsonObject& BLEdata) {
+  yield();
   if (!BLEdata.containsKey("id")) {
     Log.error(F("No mac address in the payload" CR));
     return;
   }
   const char* mac = BLEdata["id"].as<const char*>();
+  Log.trace(F("Processing BLE data %s" CR), BLEdata["id"].as<const char*>());
   int model_id = BTConfig.extDecoderEnable ? -1 : decoder.decodeBLEJson(BLEdata);
   int mac_type = BLEdata["mac_type"].as<int>();
   if ((BLEdata["type"].as<string>()).compare("RMAC") != 0 && model_id != TheengsDecoder::BLE_ID_NUM::IBEACON) { // Do not store in memory the random mac devices and iBeacons
@@ -1350,10 +1271,12 @@ void immediateBTAction(void* pvParameters) {
       xSemaphoreGive(semaphoreBLEOperation);
     } else {
       Log.error(F("BLE busy - command not sent" CR));
-      JsonObject result = getBTJsonObject();
+      StaticJsonDocument<JSON_MSG_BUFFER> BLEdataBuffer;
-      result["id"] = BLEactions.back().addr;
+      JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
-      result["success"] = false;
+      BLEdata["id"] = BLEactions.back().addr;
-      pubBT(result);
+      BLEdata["success"] = false;
+      buildTopicFromId(BLEdata, subjectBTtoMQTT);
+      enqueueJsonObject(BLEdata);
       BLEactions.pop_back();
       ProcessLock = false;
     }

main/ZgatewayGFSunInverter.ino

@@ -30,8 +30,8 @@
 GfSun2000 GF = GfSun2000();
 
 void GFSunInverterDataHandler(GfSun2000Data data) {
-  StaticJsonDocument<2 * JSON_MSG_BUFFER> jsonBuffer;
+  StaticJsonDocument<JSON_MSG_BUFFER> jdataBuffer;
-  JsonObject jdata = jsonBuffer.to<JsonObject>();
+  JsonObject jdata = jdataBuffer.to<JsonObject>();
 
   jdata["device_id"] = (char*)data.deviceID;
   Log.trace(F("Device ID     : %s\n" CR), data.deviceID);
@@ -47,8 +47,8 @@ void GFSunInverterDataHandler(GfSun2000Data data) {
   Log.trace(F("Total Energy  : %.1f\tkW/h\n" CR), data.totalEnergyCounter);
 
 #  ifdef GFSUNINVERTER_DEVEL
-  StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer2;
+  StaticJsonDocument<JSON_MSG_BUFFER> jregisterBuffer;
-  JsonObject jregister = jsonBuffer2.to<JsonObject>();
+  JsonObject jregister = jregisterBuffer.to<JsonObject>();
   char buffer[4];
   std::map<int16_t, int16_t>::iterator itr;
   for (itr = data.modbusRegistry.begin(); itr != data.modbusRegistry.end(); ++itr) {
@@ -58,19 +58,21 @@ void GFSunInverterDataHandler(GfSun2000Data data) {
   }
   jdata["register"] = jregister;
 #  endif
-  pub(subjectRFtoMQTT, jdata);
+  jdata["origin"] = subjectRFtoMQTT;
+  enqueueJsonObject(jdata);
 }
 
 void GFSunInverterErrorHandler(int errorId, char* errorMessage) {
   char buffer[50];
   sprintf(buffer, "Error response: %02X - %s\n", errorId, errorMessage);
   Log.error(buffer);
-  StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+  StaticJsonDocument<JSON_MSG_BUFFER> jdataBuffer;
-  JsonObject jdata = jsonBuffer.to<JsonObject>();
+  JsonObject jdata = jdataBuffer.to<JsonObject>();
   jdata["status"] = "error";
   jdata["msg"] = errorMessage;
   jdata["id"] = errorId;
-  pub(subjectRFtoMQTT, jdata);
+  jdata["origin"] = subjectRFtoMQTT;
+  enqueueJsonObject(jdata);
 }
 
 void setupGFSunInverter() {

main/ZgatewayIR.ino

@@ -115,8 +115,8 @@ void IRtoMQTT() {
 
   if (irrecv.decode(&results)) {
     Log.trace(F("Creating IR buffer" CR));
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> IRdataBuffer;
-    JsonObject IRdata = jsonBuffer.to<JsonObject>();
+    JsonObject IRdata = IRdataBuffer.to<JsonObject>();
 
     Log.trace(F("Rcv. IR" CR));
 #  ifdef ESP32
@@ -168,7 +168,8 @@ void IRtoMQTT() {
       Log.notice(F("--no pub unknwn prt--" CR));
     } else if (!isAduplicateSignal(MQTTvalue) && MQTTvalue != 0) { // conditions to avoid duplications of IR -->MQTT
       Log.trace(F("Adv data IRtoMQTT" CR));
-      pub(subjectIRtoMQTT, IRdata);
+      IRdata["origin"] = subjectIRtoMQTT;
+      enqueueJsonObject(IRdata);
       Log.trace(F("Store val: %D" CR), MQTTvalue);
       storeSignalValue(MQTTvalue);
       if (repeatIRwMQTT) {

main/ZgatewayLORA.ino

@@ -391,8 +391,8 @@ void setupLORA() {
 void LORAtoMQTT() {
   int packetSize = LoRa.parsePacket();
   if (packetSize) {
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> LORAdataBuffer;
-    JsonObject LORAdata = jsonBuffer.to<JsonObject>();
+    JsonObject LORAdata = LORAdataBuffer.to<JsonObject>();
     Log.trace(F("Rcv. LORA" CR));
 #  ifdef ESP32
     String taskMessage = "LORA Task running on core ";
@@ -427,7 +427,11 @@ void LORAtoMQTT() {
       LORAdata["hex"] = hex;
     } else {
       // ascii payload
-      deserializeJson(jsonBuffer, packet, packetSize);
+      std::string packetStrStd = (char*)packet;
+      auto error = deserializeJson(LORAdataBuffer, packetStrStd);
+      if (error) {
+        Log.error(F("LORA packet deserialization failed: %s, buffer capacity: %u" CR), error.c_str(), LORAdataBuffer.capacity());
+      }
     }
 
     LORAdata["rssi"] = (int)LoRa.packetRssi();
@@ -436,31 +440,25 @@ void LORAtoMQTT() {
     LORAdata["packetSize"] = (int)packetSize;
 
     if (LORAdata.containsKey("id")) {
-      // Replace ":" in topic
+      std::string id = LORAdata["id"];
-      std::string topic = LORAdata["id"].as<std::string>();
+      id.erase(std::remove(id.begin(), id.end(), ':'), id.end());
-      size_t pos = topic.find(":");
-      while (pos != std::string::npos) {
-        topic.erase(pos, 1);
-        pos = topic.find(":", pos);
-      }
-      std::string id = topic;
-      std::string subjectStr(subjectLORAtoMQTT);
-      topic = subjectStr + "/" + topic;
-
 #  ifdef ZmqttDiscovery
       if (SYSConfig.discovery) {
         if (!LORAdata.containsKey("model"))
-          LORAdata["model"] = "LORA_NODE";
+          LORAdataBuffer["model"] = "LORA_NODE";
         storeLORADiscovery(LORAdata, LORAdata["model"].as<char*>(), id.c_str());
       }
 #  endif
-      pub(topic.c_str(), LORAdata);
+      buildTopicFromId(LORAdata, subjectLORAtoMQTT);
     } else {
-      pub(subjectLORAtoMQTT, LORAdata);
+      LORAdataBuffer["origin"] = subjectLORAtoMQTT;
     }
+    enqueueJsonObject(LORAdata);
+
     if (repeatLORAwMQTT) {
       Log.trace(F("Pub LORA for rpt" CR));
-      pub(subjectMQTTtoLORA, LORAdata);
+      LORAdata["origin"] = subjectMQTTtoLORA;
+      enqueueJsonObject(LORAdata);
     }
   }
 }
@@ -489,7 +487,8 @@ void MQTTtoLORA(char* topicOri, JsonObject& LORAdata) { // json object decoding
 
       LoRa.endPacket();
       Log.trace(F("MQTTtoLORA OK" CR));
-      pub(subjectGTWLORAtoMQTT, LORAdata); // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+      // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+      pub(subjectGTWLORAtoMQTT, LORAdata);
     } else {
       Log.error(F("MQTTtoLORA Fail json" CR));
     }
@@ -517,13 +516,14 @@ void MQTTtoLORA(char* topicOri, JsonObject& LORAdata) { // json object decoding
 }
 #  endif
 #  if simpleReceiving
-void MQTTtoLORA(char* topicOri, char* LORAdata) { // json object decoding
+void MQTTtoLORA(char* topicOri, char* LORAarray) { // json object decoding
   if (cmpToMainTopic(topicOri, subjectMQTTtoLORA)) {
     LoRa.beginPacket();
-    LoRa.print(LORAdata);
+    LoRa.print(LORAarray);
     LoRa.endPacket();
     Log.notice(F("MQTTtoLORA OK" CR));
-    pub(subjectGTWLORAtoMQTT, LORAdata); // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+    // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+    pub(subjectGTWLORAtoMQTT, LORAarray);
   }
 }
 #  endif

main/ZgatewayPilight.ino

@@ -45,40 +45,47 @@ void pilightCallback(const String& protocol, const String& message, int status,
                      size_t repeats, const String& deviceID) {
   if (status == VALID) {
     Log.trace(F("Creating RF PiLight buffer" CR));
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> RFPiLightdataBuffer;
-    JsonObject RFPiLightdata = jsonBuffer.to<JsonObject>();
+    JsonObject RFPiLightdata = RFPiLightdataBuffer.to<JsonObject>();
     StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer2;
     JsonObject msg = jsonBuffer2.to<JsonObject>();
-    auto error = deserializeJson(jsonBuffer2, message);
+    if (message.length() > 0) {
-    if (error) {
+      auto error = deserializeJson(jsonBuffer2, message);
-      Log.error(F("deserializeJson() failed: %s" CR), error.c_str());
+      if (error) {
-      return;
+        Log.error(F("deserializeJson() failed: %s" CR), error.c_str());
+        return;
+      }
+      RFPiLightdata["message"] = msg;
     }
-    RFPiLightdata["message"] = msg;
+    if (protocol.length() > 0) {
-    RFPiLightdata["protocol"] = (const char*)protocol.c_str();
+      RFPiLightdata["protocol"] = protocol;
-    RFPiLightdata["length"] = (const char*)deviceID.c_str();
+    }
+    if (deviceID.length() > 0) {
+      RFPiLightdata["value"] = deviceID;
+      const char* device_id = deviceID.c_str();
+      if (!strlen(device_id) && !msg.isNull()) {
+        // deviceID returned from Pilight is only extracted from id field
+        // but some device may use another name as unique identifier
+        char* choices[] = {"key", "unit", "device_id", "systemcode", "unitcode", "programcode"};
 
-    const char* device_id = deviceID.c_str();
+        for (uint8_t i = 0; i < 6; i++) {
-    if (!strlen(device_id)) {
+          if (msg[choices[i]]) {
-      // deviceID returned from Pilight is only extracted from id field
+            device_id = (const char*)msg[choices[i]];
-      // but some device may use another name as unique identifier
+            break;
-      char* choices[] = {"key", "unit", "device_id", "systemcode", "unitcode", "programcode"};
+          }
-
-      for (uint8_t i = 0; i < 6; i++) {
-        if (msg[choices[i]]) {
-          device_id = (const char*)msg[choices[i]];
-          break;
         }
       }
+      RFPiLightdata["value"] = device_id;
     }
 
-    RFPiLightdata["value"] = device_id;
     RFPiLightdata["repeats"] = (int)repeats;
     RFPiLightdata["status"] = (int)status;
-    pub(subjectPilighttoMQTT, RFPiLightdata);
+    RFPiLightdata["origin"] = subjectPilighttoMQTT;
+    enqueueJsonObject(RFPiLightdata);
     if (repeatPilightwMQTT) {
       Log.trace(F("Pub Pilight for rpt" CR));
-      pub(subjectMQTTtoPilight, RFPiLightdata);
+      RFPiLightdata["origin"] = subjectMQTTtoPilight;
+      enqueueJsonObject(RFPiLightdata);
     }
   }
 }
@@ -92,16 +99,16 @@ void pilightRawCallback(const uint16_t* pulses, size_t length) {
     return;
   }
 
-  Log.trace(F("Creating RF PiLight buffer" CR));
+  StaticJsonDocument<JSON_MSG_BUFFER> RFPiLightdataBuffer;
-  StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+  JsonObject RFPiLightdata = RFPiLightdataBuffer.to<JsonObject>();
-  JsonObject RFPiLightdata = jsonBuffer.to<JsonObject>();
 
   RFPiLightdata["format"] = "RAW";
   RFPiLightdata["rawlen"] = length;
   RFPiLightdata["pulsesString"] = rf.pulseTrainToString(pulses, length); // c=pulse_array_key;p=pulse_types
 
-  // publish data
+  // Enqueue data
-  pub(subjectPilighttoMQTT, RFPiLightdata);
+  RFPiLightdata["origin"] = subjectPilighttoMQTT;
+  enqueueJsonObject(RFPiLightdata);
 }
 #  endif
 
@@ -196,7 +203,8 @@ void MQTTtoPilight(char* topicOri, JsonObject& Pilightdata) {
 #  endif
 
     if (success) {
-      pub(subjectGTWPilighttoMQTT, Pilightdata); // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+      // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+      pub(subjectGTWPilighttoMQTT, Pilightdata);
     } else {
       pub(subjectGTWPilighttoMQTT, "{\"Status\": \"Error\"}"); // Fail feedback
       Log.error(F("MQTTtoPilightProtocol Fail json" CR));
@@ -264,7 +272,8 @@ void MQTTtoPilight(char* topicOri, JsonObject& Pilightdata) {
       int msgLength = rf.send(protocol, message);
       if (msgLength > 0) {
         Log.trace(F("Adv data MQTTtoPilight push state via PilighttoMQTT" CR));
-        pub(subjectGTWPilighttoMQTT, Pilightdata);
+        Pilightdata["origin"] = subjectGTWPilighttoMQTT;
+        enqueueJsonObject(Pilightdata);
         success = true;
       } else {
         switch (msgLength) {
@@ -298,7 +307,8 @@ void MQTTtoPilight(char* topicOri, JsonObject& Pilightdata) {
     }
 #  endif
     if (success) {
-      pub(subjectGTWPilighttoMQTT, Pilightdata); // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+      // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+      pub(subjectGTWPilighttoMQTT, Pilightdata);
     } else {
       pub(subjectGTWPilighttoMQTT, "{\"Status\": \"Error\"}"); // Fail feedback
       Log.error(F("MQTTtoPilight Fail json" CR));

main/ZgatewayRF.ino

@@ -133,8 +133,8 @@ void setupRF() {
 
 void RFtoMQTT() {
   if (mySwitch.available()) {
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> RFdataBuffer;
-    JsonObject RFdata = jsonBuffer.to<JsonObject>();
+    JsonObject RFdata = RFdataBuffer.to<JsonObject>();
     Log.trace(F("Rcv. RF" CR));
 #  ifdef ESP32
     Log.trace(F("RF Task running on core :%d" CR), xPortGetCoreID());
@@ -150,13 +150,16 @@ void RFtoMQTT() {
     RFdata["tre_state"] = bin2tristate(binary);
     RFdata["binary"] = binary;
 
+#  if defined(ESP32) || defined(ESP8266)
     unsigned int* raw = mySwitch.getReceivedRawdata();
-    String rawDump = "";
+    std::string rawDump;
-    for (unsigned int i = 0; i <= length * 2; i++) {
+    for (unsigned int i = 0; i < length * 2; i++) {
-      rawDump = rawDump + String(raw[i]) + ",";
+      if (i != 0)
+        rawDump += ",";
+      rawDump += std::to_string(raw[i]);
     }
-    RFdata["raw"] = rawDump.c_str();
+    RFdata["raw"] = rawDump;
-
+#  endif
 #  ifdef ZradioCC1101 // set Receive off and Transmitt on
     RFdata["mhz"] = receiveMhz;
 #  endif
@@ -167,13 +170,15 @@ void RFtoMQTT() {
       if (SYSConfig.discovery)
         RFtoMQTTdiscovery(MQTTvalue);
 #  endif
-      pub(subjectRFtoMQTT, RFdata);
+      RFdata["origin"] = subjectRFtoMQTT;
+      enqueueJsonObject(RFdata);
       // Casting "receivedSignal[o].value" to (unsigned long) because ArduinoLog doesn't support uint64_t for ESP's
       Log.trace(F("Store val: %u" CR), (unsigned long)MQTTvalue);
       storeSignalValue(MQTTvalue);
       if (repeatRFwMQTT) {
         Log.trace(F("Pub RF for rpt" CR));
-        pub(subjectMQTTtoRF, RFdata);
+        RFdata["origin"] = subjectMQTTtoRF;
+        enqueueJsonObject(RFdata);
       }
     }
   }
@@ -274,7 +279,8 @@ void MQTTtoRF(char* topicOri, JsonObject& RFdata) { // json object decoding
       mySwitch.setProtocol(valuePRT, valuePLSL);
       mySwitch.send(data, valueBITS);
       Log.notice(F("MQTTtoRF OK" CR));
-      pub(subjectGTWRFtoMQTT, RFdata); // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+      // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+      pub(subjectGTWRFtoMQTT, RFdata);
       mySwitch.setRepeatTransmit(RF_EMITTER_REPEAT); // Restore the default value
     } else {
       bool success = false;
@@ -292,7 +298,8 @@ void MQTTtoRF(char* topicOri, JsonObject& RFdata) { // json object decoding
       }
 #    endif
       if (success) {
-        pub(subjectGTWRFtoMQTT, RFdata); // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+        // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+        pub(subjectGTWRFtoMQTT, RFdata);
       } else {
 #    ifndef ARDUINO_AVR_UNO // Space issues with the UNO
         pub(subjectGTWRFtoMQTT, "{\"Status\": \"Error\"}"); // Fail feedback

main/ZgatewayRF2.ino

@@ -118,10 +118,8 @@ void RF2toMQTTdiscovery(JsonObject& data) {
 
 void RF2toMQTT() {
   if (rf2rd.hasNewData) {
-    Log.trace(F("Creating RF2 buffer" CR));
+    StaticJsonDocument<JSON_MSG_BUFFER> RF2dataBuffer;
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    JsonObject RF2data = RF2dataBuffer.to<JsonObject>();
-    JsonObject RF2data = jsonBuffer.to<JsonObject>();
-
     rf2rd.hasNewData = false;
 
     Log.trace(F("Rcv. RF2" CR));
@@ -134,8 +132,8 @@ void RF2toMQTT() {
     if (SYSConfig.discovery)
       RF2toMQTTdiscovery(RF2data);
 #  endif
-
+    RF2data["origin"] = subjectRF2toMQTT;
-    pub(subjectRF2toMQTT, RF2data);
+    enqueueJsonObject(RF2data);
   }
 }
 
@@ -327,7 +325,8 @@ void MQTTtoRF2(char* topicOri, JsonObject& RF2data) { // json object decoding
     }
 #    endif
     if (success) {
-      pub(subjectGTWRF2toMQTT, RF2data); // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+      // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+      pub(subjectRF2toMQTT, RF2data);
     } else {
 #    ifndef ARDUINO_AVR_UNO // Space issues with the UNO
       pub(subjectGTWRF2toMQTT, "{\"Status\": \"Error\"}"); // Fail feedback

main/ZgatewayRFM69.ino

@@ -141,9 +141,8 @@ void setupRFM69(void) {
 bool RFM69toMQTT(void) {
   //check if something was received (could be an interrupt from the radio)
   if (radio.receiveDone()) {
-    Log.trace(F("Creating RFM69 buffer" CR));
+    StaticJsonDocument<JSON_MSG_BUFFER> RFM69dataBuffer;
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    JsonObject RFM69data = RFM69dataBuffer.to<JsonObject>();
-    JsonObject RFM69data = jsonBuffer.to<JsonObject>();
     uint8_t data[RF69_MAX_DATA_LEN + 1]; // For the null character
     uint8_t SENDERID = radio.SENDERID;
     uint8_t DATALEN = radio.DATALEN;
@@ -216,7 +215,8 @@ void MQTTtoRFM69(char* topicOri, JsonObject& RFM69data) {
       if (radio.sendWithRetry(valueRCV, data, strlen(data), 10)) {
         Log.notice(F(" OK " CR));
         // Acknowledgement to the GTWRF topic
-        pub(subjectGTWRFM69toMQTT, RFM69data); // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
+        RFM69data["origin"] = subjectGTWRFM69toMQTT;
+        enqueueJsonObject(RFM69data);
       } else {
         Log.error(F("MQTTtoRFM69 sending failed" CR));
       }

main/ZgatewayRTL_433.ino

@@ -257,7 +257,8 @@ void rtl_433_Callback(char* message) {
     if (SYSConfig.discovery)
       storeRTL_433Discovery(RFrtl_433_ESPdata, (char*)model.c_str(), (char*)uniqueid.c_str());
 #  endif
-    pub((char*)topic.c_str(), RFrtl_433_ESPdata);
+    RFrtl_433_ESPdata["origin"] = (char*)topic.c_str();
+    enqueueJsonObject(RFrtl_433_ESPdata);
     storeSignalValue(MQTTvalue);
     pubOled((char*)topic.c_str(), RFrtl_433_ESPdata);
   }

main/ZgatewaySRFB.ino

@@ -102,8 +102,8 @@ void _rfbDecode() {
     _rawToHex(&_uartbuf[1], buffer, RF_MESSAGE_SIZE);
 
     Log.trace(F("Creating SRFB buffer" CR));
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> SRFBdataBuffer;
-    JsonObject SRFBdata = jsonBuffer.to<JsonObject>();
+    JsonObject SRFBdata = SRFBdataBuffer.to<JsonObject>();
     SRFBdata["raw"] = String(buffer).substring(0, 18);
 
     int val_Tsyn = (int)(int)value_from_hex_data(buffer, 0, 4, false, false);
@@ -120,12 +120,14 @@ void _rfbDecode() {
 
     if (!isAduplicateSignal(MQTTvalue) && MQTTvalue != 0) { // conditions to avoid duplications of RF -->MQTT
       Log.trace(F("Adv data SRFBtoMQTT" CR));
-      pub(subjectSRFBtoMQTT, SRFBdata);
+      SRFBdata["origin"] = subjectSRFBtoMQTT;
+      enqueueJsonObject(SRFBdata);
       Log.trace(F("Store val: %lu" CR), MQTTvalue);
       storeSignalValue(MQTTvalue);
       if (repeatSRFBwMQTT) {
         Log.trace(F("Publish SRFB for rpt" CR));
-        pub(subjectMQTTtoSRFB, SRFBdata);
+        SRFBdata["origin"] = subjectMQTTtoSRFB;
+        enqueueJsonObject(SRFBdata);
       }
     }
     _rfbAck();
@@ -298,7 +300,6 @@ void MQTTtoSRFB(char* topicOri, JsonObject& SRFBdata) {
 
         Log.notice(F("MQTTtoSRFB OK" CR));
         _rfbSend(message_b, valueRPT);
-        // Acknowledgement to the GTWRF topic
         pub(subjectGTWSRFBtoMQTT, SRFBdata); // we acknowledge the sending by publishing the value to an acknowledgement topic, for the moment even if it is a signal repetition we acknowledge also
       } else {
         Log.error(F("MQTTtoSRFB error decoding value" CR));

main/ZgatewayWeatherStation.ino

@@ -36,11 +36,12 @@ void PairedDeviceAdded(byte newID) {
   Serial.print("ZgatewayWeatherStation: New device paired ");
   Serial.println(newID, DEC);
 #  endif
-  StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+  StaticJsonDocument<JSON_MSG_BUFFER> RFdataBuffer;
-  JsonObject RFdata = jsonBuffer.to<JsonObject>();
+  JsonObject RFdata = RFdataBuffer.to<JsonObject>();
   RFdata["sensor"] = newID;
   RFdata["action"] = "paired";
-  pub(subjectRFtoMQTT, RFdata);
+  RFdata["origin"] = subjectRFtoMQTT;
+  enqueueJsonObject(RFdata);
   wsdr.pair(NULL, PairedDeviceAdded);
 }
 
@@ -54,12 +55,13 @@ void setupWeatherStation() {
 void sendWindSpeedData(byte id, float wind_speed, byte battery_status) {
   unsigned long MQTTvalue = 10000 + round(wind_speed);
   if (!isAduplicateSignal(MQTTvalue)) { // conditions to avoid duplications of RF -->MQTT
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> RFdataBuffer;
-    JsonObject RFdata = jsonBuffer.to<JsonObject>();
+    JsonObject RFdata = RFdataBuffer.to<JsonObject>();
     RFdata["sensor"] = id;
     RFdata["wind_speed"] = wind_speed;
     RFdata["battery"] = bitRead(battery_status, 0) == 0 ? "OK" : "Low";
-    pub(subjectRFtoMQTT, RFdata);
+    RFdata["origin"] = subjectRFtoMQTT;
+    enqueueJsonObject(RFdata);
     Log.trace(F("Store wind speed val: %lu" CR), MQTTvalue);
     storeSignalValue(MQTTvalue);
   }
@@ -68,12 +70,13 @@ void sendWindSpeedData(byte id, float wind_speed, byte battery_status) {
 void sendRainData(byte id, float rain_volume, byte battery_status) {
   unsigned long MQTTvalue = 11000 + round(rain_volume * 10.0);
   if (!isAduplicateSignal(MQTTvalue)) { // conditions to avoid duplications of RF -->MQTT
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> RFdataBuffer;
-    JsonObject RFdata = jsonBuffer.to<JsonObject>();
+    JsonObject RFdata = RFdataBuffer.to<JsonObject>();
     RFdata["sensor"] = id;
     RFdata["rain_volume"] = rain_volume;
     RFdata["battery"] = bitRead(battery_status, 1) == 0 ? "OK" : "Low";
-    pub(subjectRFtoMQTT, RFdata);
+    RFdata["origin"] = subjectRFtoMQTT;
+    enqueueJsonObject(RFdata);
     Log.trace(F("Store rain_volume: %lu" CR), MQTTvalue);
     storeSignalValue(MQTTvalue);
   }
@@ -82,13 +85,14 @@ void sendRainData(byte id, float rain_volume, byte battery_status) {
 void sendWindData(byte id, int wind_direction, float wind_gust, byte battery_status) {
   unsigned long MQTTvalue = 20000 + round(wind_gust * 10.0) + wind_direction;
   if (!isAduplicateSignal(MQTTvalue)) { // conditions to avoid duplications of RF -->MQTT
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> RFdataBuffer;
-    JsonObject RFdata = jsonBuffer.to<JsonObject>();
+    JsonObject RFdata = RFdataBuffer.to<JsonObject>();
     RFdata["sensor"] = id;
     RFdata["wind_direction"] = wind_direction;
     RFdata["wind_gust"] = wind_gust;
     RFdata["battery"] = bitRead(battery_status, 0) == 0 ? "OK" : "Low";
-    pub(subjectRFtoMQTT, RFdata);
+    RFdata["origin"] = subjectRFtoMQTT;
+    enqueueJsonObject(RFdata);
     Log.trace(F("Store wind data val: %lu" CR), MQTTvalue);
     storeSignalValue(MQTTvalue);
   }
@@ -97,14 +101,15 @@ void sendWindData(byte id, int wind_direction, float wind_gust, byte battery_sta
 void sendTemperatureData(byte id, float temperature, int humidity, byte battery_status) {
   unsigned long MQTTvalue = 40000 + abs(round(temperature * 100.0)) + humidity;
   if (!isAduplicateSignal(MQTTvalue)) { // conditions to avoid duplications of RF -->MQTT
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> RFdataBuffer;
-    JsonObject RFdata = jsonBuffer.to<JsonObject>();
+    JsonObject RFdata = RFdataBuffer.to<JsonObject>();
     RFdata["sensor"] = id;
     RFdata["tempc"] = temperature;
     RFdata["tempf"] = wsdr.convertCtoF(temperature);
     RFdata["humidity"] = humidity;
     RFdata["battery"] = bitRead(battery_status, 0) == 0 ? "OK" : "Low";
-    pub(subjectRFtoMQTT, RFdata);
+    RFdata["origin"] = subjectRFtoMQTT;
+    enqueueJsonObject(RFdata);
     Log.trace(F("Store temp val: %lu" CR), MQTTvalue);
     storeSignalValue(MQTTvalue);
   }

main/ZsensorADC.ino

@@ -64,8 +64,8 @@ void MeasureADC() {
       if (val >= persistedadc + ThresholdReadingADC || val <= persistedadc - ThresholdReadingADC || (MinTimeInSecBetweenPublishingADC > 0 && millis() - timeadcpub > (MinTimeInSecBetweenPublishingADC * 1000UL))) {
         timeadcpub = millis();
         Log.trace(F("Creating ADC buffer" CR));
-        StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+        StaticJsonDocument<JSON_MSG_BUFFER> ADCdataBuffer;
-        JsonObject ADCdata = jsonBuffer.to<JsonObject>();
+        JsonObject ADCdata = ADCdataBuffer.to<JsonObject>();
         ADCdata["adc"] = (int)val;
         if (NumberOfReadingsADC > 1) {
           ADCdata["adc_reads"] = NumberOfReadingsADC;
@@ -93,7 +93,8 @@ void MeasureADC() {
         ADCdata["adc_sum"] = (int)sum_val;
         ADCdata["mvolt_scaled"] = (int)voltage; // real scaled/calibrated value in mV
 #  endif
-        pub(ADCTOPIC, ADCdata);
+        ADCdata["origin"] = ADCTOPIC;
+        enqueueJsonObject(ADCdata);
         persistedadc = val;
       }
     }

main/ZsensorAHTx0.ino

@@ -86,8 +86,8 @@ void MeasureAHTTempHum() {
       Log.error(F("Failed to read from sensor AHTx0!" CR));
     } else {
       Log.notice(F("Creating AHTx0 buffer" CR));
-      StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+      StaticJsonDocument<JSON_MSG_BUFFER> AHTx0dataBuffer;
-      JsonObject AHTx0data = jsonBuffer.to<JsonObject>();
+      JsonObject AHTx0data = AHTx0dataBuffer.to<JsonObject>();
       // Generate Temperature in degrees C
       if (ahtTempC.temperature != persisted_aht_tempc || AHTx0_always) {
         float ahtTempF = convertTemp_CtoF(ahtTempC.temperature);
@@ -103,10 +103,8 @@ void MeasureAHTTempHum() {
       } else {
         Log.notice(F("Same Humidity. Don't send it" CR));
       }
-
+      AHTx0data["origin"] = AHTTOPIC;
-      if (AHTx0data.size() > 0) {
+      enqueueJsonObject(AHTx0data);
-        pub(AHTTOPIC, AHTx0data);
-      }
     }
     persisted_aht_tempc = ahtTempC.temperature;
     persisted_aht_hum = ahtHum.relative_humidity;

main/ZsensorBH1750.ino

@@ -54,8 +54,8 @@ void setupZsensorBH1750() {
 void MeasureLightIntensity() {
   if (millis() > (timebh1750 + TimeBetweenReadingBH1750)) { //retrieving value of Lux, FtCd and Wattsm2 from BH1750
     Log.trace(F("Creating BH1750 buffer" CR));
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> BH1750dataBuffer;
-    JsonObject BH1750data = jsonBuffer.to<JsonObject>();
+    JsonObject BH1750data = BH1750dataBuffer.to<JsonObject>();
 
     timebh1750 = millis();
     unsigned int i = 0;
@@ -100,8 +100,8 @@ void MeasureLightIntensity() {
       } else {
         Log.trace(F("Same wattsm2 don't send it" CR));
       }
-      if (BH1750data.size() > 0)
+      BH1750data["origin"] = subjectBH1750toMQTT;
-        pub(subjectBH1750toMQTT, BH1750data);
+      enqueueJsonObject(BH1750data);
     }
     persistedll = Lux;
     persistedlf = ftcd;

main/ZsensorBME280.ino

@@ -147,8 +147,8 @@ void MeasureTempHumAndPressure() {
       Log.error(F("Failed to read from BME280/BMP280!" CR));
     } else {
       Log.trace(F("Creating BME280/BMP280 buffer" CR));
-      StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+      StaticJsonDocument<JSON_MSG_BUFFER> BME280dataBuffer;
-      JsonObject BME280data = jsonBuffer.to<JsonObject>();
+      JsonObject BME280data = BME280dataBuffer.to<JsonObject>();
       // Generate Temperature in degrees C
       if (BmeTempC != persisted_bme_tempc || bme280_always) {
         BME280data["tempc"] = (float)BmeTempC;
@@ -191,9 +191,8 @@ void MeasureTempHumAndPressure() {
       } else {
         Log.trace(F("Same Altitude Feet don't send it" CR));
       }
-      if (BME280data.size() > 0) {
+      BME280data["origin"] = BMETOPIC;
-        pub(BMETOPIC, BME280data);
+      enqueueJsonObject(BME280data);
-      }
     }
 
     persisted_bme_tempc = BmeTempC;

main/ZsensorC37_YL83_HMRD.ino

@@ -53,8 +53,8 @@ void MeasureC37_YL83_HMRDWater() {
     int sensorAnalogValue = analogRead(C37_YL83_HMRD_Analog_GPIO);
 
     Log.trace(F("Creating C37_YL83_HMRD buffer" CR));
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> C37_YL83_HMRDdataBuffer;
-    JsonObject C37_YL83_HMRDdata = jsonBuffer.to<JsonObject>();
+    JsonObject C37_YL83_HMRDdata = C37_YL83_HMRDdataBuffer.to<JsonObject>();
     if (sensorDigitalValue != persisteddigital || C37_YL83_HMRD_ALWAYS) {
       C37_YL83_HMRDdata["detected"] = (sensorDigitalValue == 1 ? "false" : "true");
     } else {
@@ -66,7 +66,8 @@ void MeasureC37_YL83_HMRDWater() {
       Log.trace(F("Same analog don't send it" CR));
     }
     if (C37_YL83_HMRDdata.size() > 0) {
-      pub(C37_YL83_HMRD_TOPIC, C37_YL83_HMRDdata);
+      C37_YL83_HMRDdata["origin"] = C37_YL83_HMRD_TOPIC;
+      enqueueJsonObject(C37_YL83_HMRDdata);
       delay(10);
 #  if defined(DEEP_SLEEP_IN_US) || defined(ESP32_EXT0_WAKE_PIN)
       if (sensorDigitalValue == 1) //No water detected, all good we can sleep

main/ZsensorDHT.ino

@@ -55,8 +55,8 @@ void MeasureTempAndHum() {
       Log.error(F("Failed to read from DHT sensor!" CR));
     } else {
       Log.trace(F("Creating DHT buffer" CR));
-      StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+      StaticJsonDocument<JSON_MSG_BUFFER> DHTdataBuffer;
-      JsonObject DHTdata = jsonBuffer.to<JsonObject>();
+      JsonObject DHTdata = DHTdataBuffer.to<JsonObject>();
       if (h != persistedh || dht_always) {
         DHTdata["hum"] = (float)h;
       } else {
@@ -68,8 +68,8 @@ void MeasureTempAndHum() {
       } else {
         Log.trace(F("Same temp don't send it" CR));
       }
-      if (DHTdata.size() > 0)
+      DHTdata["origin"] = DHTTOPIC;
-        pub(DHTTOPIC, DHTdata);
+      enqueueJsonObject(DHTdata);
     }
     persistedh = h;
     persistedt = t;

main/ZsensorDS1820.ino

@@ -140,8 +140,8 @@ void MeasureDS1820Temp() {
       Log.error(F("DS1820: Failed to identify any temperature sensors on 1-wire bus during setup!" CR));
     } else {
       Log.trace(F("DS1820: Reading temperature(s) from %d sensor(s)..." CR), ds1820_count);
-      StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+      StaticJsonDocument<JSON_MSG_BUFFER> DS1820dataBuffer;
-      JsonObject DS1820data = jsonBuffer.to<JsonObject>();
+      JsonObject DS1820data = DS1820dataBuffer.to<JsonObject>();
 
       for (uint8_t i = 0; i < ds1820_count; i++) {
         current_temp[i] = round(ds1820.getTempC(ds1820_devices[i]) * 10) / 10.0;
@@ -156,7 +156,9 @@ void MeasureDS1820Temp() {
             DS1820data["res"] = String(ds1820_resolution[i]) + String("bit");
             DS1820data["addr"] = ds1820_addr[i];
           }
-          pub((char*)(String(OW_TOPIC) + "/" + ds1820_addr[i]).c_str(), DS1820data);
+          String origin = String(OW_TOPIC) + "/" + ds1820_addr[i];
+          DS1820data["origin"] = origin;
+          enqueueJsonObject(DS1820data);
           delay(10);
 #  if defined(DEEP_SLEEP_IN_US) || defined(ESP32_EXT0_WAKE_PIN)
           ready_to_sleep = true;

main/ZsensorGPIOInput.ino

@@ -85,16 +85,16 @@ void MeasureGPIOInput() {
     // if the Input state has changed:
     if (reading != InputState) {
       Log.trace(F("Creating GPIOInput buffer" CR));
-      StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+      StaticJsonDocument<JSON_MSG_BUFFER> GPIOdataBuffer;
-      JsonObject GPIOdata = jsonBuffer.to<JsonObject>();
+      JsonObject GPIOdata = GPIOdataBuffer.to<JsonObject>();
       if (InputState == HIGH) {
         GPIOdata["gpio"] = "HIGH";
       }
       if (InputState == LOW) {
         GPIOdata["gpio"] = "LOW";
       }
-      if (GPIOdata.size() > 0)
+      GPIOdata["origin"] = subjectGPIOInputtoMQTT;
-        pub(subjectGPIOInputtoMQTT, GPIOdata);
+      enqueueJsonObject(GPIOdata);
 
 #  if defined(ZactuatorONOFF) && defined(ACTUATOR_TRIGGER)
       //Trigger the actuator if we are not at startup

main/ZsensorHCSR04.ino

@@ -44,8 +44,8 @@ void MeasureDistance() {
   if (millis() > (timeHCSR04 + TimeBetweenReadingHCSR04)) {
     timeHCSR04 = millis();
     Log.trace(F("Creating HCSR04 buffer" CR));
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> HCSR04dataBuffer;
-    JsonObject HCSR04data = jsonBuffer.to<JsonObject>();
+    JsonObject HCSR04data = HCSR04dataBuffer.to<JsonObject>();
     digitalWrite(HCSR04_TRI_GPIO, LOW);
     delayMicroseconds(2);
     digitalWrite(HCSR04_TRI_GPIO, HIGH);
@@ -69,8 +69,8 @@ void MeasureDistance() {
         Log.trace(F("HC SR04 Distance hasn't changed" CR));
       }
       distance = d;
-      if (HCSR04data.size() > 0)
+      HCSR04data["origin"] = subjectHCSR04;
-        pub(subjectHCSR04, HCSR04data);
+      enqueueJsonObject(HCSR04data);
     }
   }
 }

main/ZsensorHCSR501.ino

@@ -41,8 +41,8 @@ void setupHCSR501() {
 
 void MeasureHCSR501() {
   if (millis() > TimeBeforeStartHCSR501) { //let time to init the PIR sensor
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> HCSR501dataBuffer;
-    JsonObject HCSR501data = jsonBuffer.to<JsonObject>();
+    JsonObject HCSR501data = HCSR501dataBuffer.to<JsonObject>();
     static int pirState = LOW;
     int PresenceValue = digitalRead(HCSR501_GPIO);
 #  if defined(ESP8266) || defined(ESP32)
@@ -64,8 +64,8 @@ void MeasureHCSR501() {
 #  ifdef HCSR501_LED_NOTIFY_GPIO
     digitalWrite(HCSR501_LED_NOTIFY_GPIO, pirState == HCSR501_LED_ON);
 #  endif
-    if (HCSR501data.size() > 0)
+    HCSR501data["origin"] = subjectHCSR501toMQTT;
-      pub(subjectHCSR501toMQTT, HCSR501data);
+    enqueueJsonObject(HCSR501data);
   }
 }
 #endif

main/ZsensorHTU21.ino

@@ -83,8 +83,8 @@ void MeasureTempHum() {
       Log.error(F("Failed to read from sensor HTU21!" CR));
     } else {
       Log.notice(F("Creating HTU21 buffer" CR));
-      StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+      StaticJsonDocument<JSON_MSG_BUFFER> HTU21dataBuffer;
-      JsonObject HTU21data = jsonBuffer.to<JsonObject>();
+      JsonObject HTU21data = HTU21dataBuffer.to<JsonObject>();
       // Generate Temperature in degrees C
       if (HtuTempC != persisted_htu_tempc || htu21_always) {
         float HtuTempF = (HtuTempC * 1.8) + 32;
@@ -100,9 +100,8 @@ void MeasureTempHum() {
       } else {
         Log.notice(F("Same Humidity. Don't send it" CR));
       }
-
+      HTU21data["origin"] = HTUTOPIC;
-      if (HTU21data.size() > 0)
+      enqueueJsonObject(HTU21data);
-        pub(HTUTOPIC, HTU21data);
     }
     persisted_htu_tempc = HtuTempC;
     persisted_htu_hum = HtuHum;

main/ZsensorINA226.ino

@@ -53,8 +53,8 @@ void MeasureINA226() {
   if (millis() > (timeINA226 + TimeBetweenReadingINA226)) { //retrieving value of temperature and humidity of the box from DHT every xUL
     timeINA226 = millis();
     Log.trace(F("Creating INA226 buffer" CR));
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> INA226dataBuffer;
-    JsonObject INA226data = jsonBuffer.to<JsonObject>();
+    JsonObject INA226data = INA226dataBuffer.to<JsonObject>();
     // Topic on which we will send data
     Log.trace(F("Retrieving electrical data" CR));
     // Bus Spannung, read-only, 16Bit, 0...40.96V max., LSB 1.25mV
@@ -72,7 +72,8 @@ void MeasureINA226() {
     INA226data["volt"] = volt;
     INA226data["current"] = current;
     INA226data["power"] = power;
-    pub(subjectINA226toMQTT, INA226data);
+    INA226data["origin"] = subjectINA226toMQTT;
+    enqueueJsonObject(INA226data);
   }
 }
 

main/ZsensorLM75.ino

@@ -80,14 +80,15 @@ void MeasureTemp() {
       Log.error(F("Failed to read from sensor HLM75!" CR));
     } else {
       Log.notice(F("Creating LM75 buffer" CR));
-      StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+      StaticJsonDocument<JSON_MSG_BUFFER> LM75dataBuffer;
-      JsonObject LM75data = jsonBuffer.to<JsonObject>();
+      JsonObject LM75data = LM75dataBuffer.to<JsonObject>();
       // Generate Temperature in degrees C
       if (lm75TempC != persisted_lm75_tempc || lm75_always) {
         float lm75TempF = (lm75TempC * 1.8) + 32;
         LM75data["tempc"] = (float)lm75TempC;
         LM75data["tempf"] = (float)lm75TempF;
-        pub(LM75TOPIC, LM75data);
+        LM75data["origin"] = LM75TOPIC;
+        enqueueJsonObject(LM75data);
       } else {
         Log.notice(F("Same Temp. Don't send it" CR));
       }

main/ZsensorMQ2.ino

@@ -61,13 +61,13 @@ void MeasureGasMQ2() {
     timemq2 = millis();
 
     Log.trace(F("Creating MQ2 buffer" CR));
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> MQ2dataBuffer;
-    JsonObject MQ2data = jsonBuffer.to<JsonObject>();
+    JsonObject MQ2data = MQ2dataBuffer.to<JsonObject>();
 
     MQ2data["gas"] = analogRead(MQ2SENSORADCPIN);
     MQ2data["detected"] = digitalRead(MQ2SENSORDETECTPIN) == HIGH ? "false" : "true";
-
+    MQ2data["origin"] = subjectMQ2toMQTT;
-    pub(subjectMQ2toMQTT, MQ2data);
+    enqueueJsonObject(MQ2data);
   }
 }
 #endif

main/ZsensorRN8209.ino

@@ -40,7 +40,6 @@ float power = 0;
 TaskHandle_t rn8209TaskHandle = nullptr;
 
 unsigned long PublishingTimerRN8209 = 0;
-StaticJsonDocument<JSON_MSG_BUFFER> doc;
 
 void rn8209_loop(void* mode) {
   while (1) {
@@ -68,15 +67,16 @@ void rn8209_loop(void* mode) {
          !PublishingTimerRN8209 ||
          (abs(current - previousCurrent) > MinCurrentThreshold)) &&
         !ProcessLock) {
-      JsonObject data = doc.to<JsonObject>();
+      StaticJsonDocument<JSON_MSG_BUFFER> RN8209dataBuffer;
+      JsonObject RN8209data = RN8209dataBuffer.to<JsonObject>();
       if (retc == 0) {
-        data["current"] = round2(current);
+        RN8209data["current"] = round2(current);
       }
       uint32_t temp_power = 0;
       retp = rn8209c_read_power(phase_A, &temp_power);
       if (retv == 0) {
         voltage = (float)temp_voltage / 1000.0;
-        data["volt"] = round2(voltage);
+        RN8209data["volt"] = round2(voltage);
       }
       if (ret == 1) {
         power = temp_power;
@@ -85,15 +85,16 @@ void rn8209_loop(void* mode) {
       }
       if (retp == 0) {
         power = power / 10000.0;
-        data["power"] = round2(power);
+        RN8209data["power"] = round2(power);
       }
       PublishingTimerRN8209 = now;
       previousCurrent = current;
-      if (data) {
+      if (RN8209data) {
-        pub(subjectRN8209toMQTT, data);
+        RN8209data["origin"] = subjectRN8209toMQTT;
+        enqueueJsonObject(RN8209data);
       }
     }
-    esp_task_wdt_reset();
+    //esp_task_wdt_reset();
     delay(TimeBetweenReadingRN8209);
   }
 }
@@ -106,7 +107,7 @@ void setupRN8209() {
   set_user_param(cal);
   init_8209c_interface();
   xTaskCreate(rn8209_loop, "rn8209_loop", 5000, NULL, 10, &rn8209TaskHandle);
-  esp_task_wdt_add(rn8209TaskHandle);
+  //esp_task_wdt_add(rn8209TaskHandle);
   Log.trace(F("ZsensorRN8209 setup done " CR));
 }
 

main/ZsensorSHTC3.ino

@@ -47,8 +47,8 @@ void MeasureTempAndHum() {
         Log.error(F("Failed to read from SHTC3 sensor!" CR));
       } else {
         Log.trace(F("Creating SHTC3 buffer" CR));
-        StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+        StaticJsonDocument<JSON_MSG_BUFFER> SHTC3dataBuffer;
-        JsonObject SHTC3data = jsonBuffer.to<JsonObject>();
+        JsonObject SHTC3data = SHTC3dataBuffer.to<JsonObject>();
         if (h != persistedh || shtc3_always) {
           SHTC3data["hum"] = (float)h;
         } else {
@@ -60,8 +60,8 @@ void MeasureTempAndHum() {
         } else {
           Log.trace(F("Same temp don't send it" CR));
         }
-        if (SHTC3data.size() > 0)
+        SHTC3data["origin"] = SHTC3TOPIC;
-          pub(SHTC3TOPIC, SHTC3data);
+        enqueueJsonObject(SHTC3data);
       }
       persistedh = h;
       persistedt = t;

main/ZsensorTEMT6000.ino

@@ -53,8 +53,8 @@ void MeasureLightIntensityTEMT6000() {
     timetemt6000 = millis();
 
     Log.trace(F("Creating TEMT6000 buffer" CR));
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> TEMT6000dataBuffer;
-    JsonObject TEMT6000data = jsonBuffer.to<JsonObject>();
+    JsonObject TEMT6000data = TEMT6000dataBuffer.to<JsonObject>();
 
     float volts = analogRead(TEMT6000LIGHTSENSORPIN) * 5.0 / 1024.0;
     float amps = volts / 10000.0; // across 10,000 Ohms
@@ -67,8 +67,8 @@ void MeasureLightIntensityTEMT6000() {
       TEMT6000data["lux"] = (float)lux;
       TEMT6000data["ftcd"] = (float)(lux) / 10.764;
       TEMT6000data["wattsm2"] = (float)(lux) / 683.0;
-
+      TEMT6000data["origin"] = subjectTEMT6000toMQTT;
-      pub(subjectTEMT6000toMQTT, TEMT6000data);
+      enqueueJsonObject(TEMT6000data);
     } else {
       Log.trace(F("Same lux value, do not send" CR));
     }

main/ZsensorTSL2561.ino

@@ -90,8 +90,8 @@ void MeasureLightIntensityTSL2561() {
     timetsl2561 = millis();
 
     Log.trace(F("Creating TSL2561 buffer" CR));
-    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+    StaticJsonDocument<JSON_MSG_BUFFER> TSL2561dataBuffer;
-    JsonObject TSL2561data = jsonBuffer.to<JsonObject>();
+    JsonObject TSL2561data = TSL2561dataBuffer.to<JsonObject>();
 
     sensors_event_t event;
     tsl.getEvent(&event);
@@ -104,8 +104,8 @@ void MeasureLightIntensityTSL2561() {
         TSL2561data["lux"] = (float)event.light;
         TSL2561data["ftcd"] = (float)(event.light) / 10.764;
         TSL2561data["wattsm2"] = (float)(event.light) / 683.0;
-
+        TSL2561data["origin"] = subjectTSL12561toMQTT;
-        pub(subjectTSL12561toMQTT, TSL2561data);
+        enqueueJsonObject(TSL2561data);
       } else {
         Log.trace(F("Same lux value, do not send" CR));
       }

main/ZwebUI.ino

@@ -624,8 +624,8 @@ void handleWI() {
       return;
 
     } else if (server.hasArg("save")) {
-      StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+      StaticJsonDocument<JSON_MSG_BUFFER> WEBtoSYSBuffer;
-      JsonObject WEBtoSYS = jsonBuffer.to<JsonObject>();
+      JsonObject WEBtoSYS = WEBtoSYSBuffer.to<JsonObject>();
       bool update = false;
       if (server.hasArg("s1")) {
         WEBtoSYS["wifi_ssid"] = server.arg("s1");
@@ -705,8 +705,8 @@ void handleMQ() {
       WEBUI_TRACE_LOG(F("handleMQ Arg: %d, %s=%s" CR), i, server.argName(i).c_str(), server.arg(i).c_str());
     }
     if (server.hasArg("save")) {
-      StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+      StaticJsonDocument<JSON_MSG_BUFFER> WEBtoSYSBuffer;
-      JsonObject WEBtoSYS = jsonBuffer.to<JsonObject>();
+      JsonObject WEBtoSYS = WEBtoSYSBuffer.to<JsonObject>();
       bool update = false;
 
       if (server.hasArg("mh")) {
@@ -1567,7 +1567,6 @@ void MQTTtoWebUI(char* topicOri, JsonObject& WebUIdata) { // json object decodin
     if (success) {
       stateWebUIStatus();
     } else {
-      // pub(subjectWebUItoMQTT, "{\"Status\": \"Error\"}"); // Fail feedback
       Log.error(F("[ WebUI ] MQTTtoWebUI Fail json" CR), WebUIdata);
     }
   }
@@ -1575,8 +1574,8 @@ void MQTTtoWebUI(char* topicOri, JsonObject& WebUIdata) { // json object decodin
 
 String stateWebUIStatus() {
   //Publish display state
-  StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+  StaticJsonDocument<JSON_MSG_BUFFER> WebUIdataBuffer;
-  JsonObject WebUIdata = jsonBuffer.to<JsonObject>();
+  JsonObject WebUIdata = WebUIdataBuffer.to<JsonObject>();
   WebUIdata["displayMetric"] = (bool)displayMetric;
   WebUIdata["webUISecure"] = (bool)webUISecure;
   WebUIdata["displayQueue"] = uxQueueMessagesWaiting(webUIQueue);
@@ -1585,7 +1584,8 @@ String stateWebUIStatus() {
   serializeJson(WebUIdata, output);
 
   // WebUIdata["currentMessage"] = currentWebUIMessage;
-  pub(subjectWebUItoMQTT, WebUIdata);
+  WebUIdata["origin"] = subjectWebUItoMQTT;
+  enqueueJsonObject(WebUIdata);
   return output;
 }
 

main/config_BT.h

@@ -29,16 +29,13 @@
 extern void setupBT();
 extern bool BTtoMQTT();
 extern void MQTTtoBT(char* topicOri, JsonObject& RFdata);
-extern void emptyBTQueue();
+extern void pubMainCore(JsonObject& data);
 extern void launchBTDiscovery(bool overrideDiscovery);
 extern void stopProcessing();
 extern void lowPowerESP32();
 extern String stateBTMeasures(bool);
 
 #ifdef ESP32
-extern int btQueueBlocked;
-extern int btQueueLengthSum;
-extern int btQueueLengthCount;
 #  include "NimBLEDevice.h"
 #endif
 
@@ -119,10 +116,6 @@ extern int btQueueLengthCount;
 #  define HassPresence false //false if we publish into Home Assistant presence topic
 #endif
 
-#ifndef BTQueueSize
-#  define BTQueueSize 4 // lockless queue size for multi core cases (ESP32 currently)
-#endif
-
 #define HMSerialSpeed 9600 // Communication speed with the HM module, softwareserial doesn't support 115200
 //#define HM_BLUE_LED_STOP true //uncomment to stop the blue led light of HM1X
 
@@ -236,6 +229,4 @@ public:
   };
 };
 
-JsonObject& getBTJsonObject(const char* json = NULL, bool haPresenceEnabled = true);
-
 #endif

main/main.ino

@@ -60,6 +60,12 @@ unsigned long timer_sys_checks = 0;
 
 #  define ARDUINOJSON_USE_LONG_LONG     1
 #  define ARDUINOJSON_ENABLE_STD_STRING 1
+
+#  include <queue>
+#  ifndef QueueSize
+#    define QueueSize 32
+#  endif
+
 #endif
 
 /**
@@ -76,6 +82,15 @@ bool ready_to_sleep = false;
 
 #include <string>
 
+#if defined(ESP8266) || defined(ESP32) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega1280__)
+struct JsonBundle {
+  StaticJsonDocument<JSON_MSG_BUFFER> doc;
+};
+
+std::queue<JsonBundle> jsonQueue;
+int queueLengthSum = 0;
+#endif
+
 StaticJsonDocument<JSON_MSG_BUFFER> modulesBuffer;
 JsonArray modules = modulesBuffer.to<JsonArray>();
 bool ethConnected = false;
@@ -389,6 +404,105 @@ bool to_bool(String const& s) { // thanks Chris Jester-Young from stackoverflow
   return s != "0";
 }
 
+/*
+ * Publish a message depending on its origin
+ *
+*/
+void pubMainCore(JsonObject& data) {
+  if (data.containsKey("origin")) {
+    pub((char*)data["origin"].as<const char*>(), data);
+#ifdef ZgatewayBT
+    if (data.containsKey("distance")) {
+      String topic = String(mqtt_topic) + BTConfig.presenceTopic + String(gateway_name);
+      Log.trace(F("Pub HA Presence %s" CR), topic.c_str());
+      pub_custom_topic((char*)topic.c_str(), data, false);
+    }
+#endif
+  } else {
+    Log.error(F("No origin in JSON filtered" CR));
+  }
+}
+
+// Add a document to the queue
+void enqueueJsonObject(const StaticJsonDocument<JSON_MSG_BUFFER>& jsonDoc) {
+  if (jsonDoc.size() == 0) {
+    Log.error(F("Empty JSON, not enqueued" CR));
+    return;
+  }
+  Log.trace(F("Enqueue JSON" CR));
+#if defined(ESP8266) || defined(ESP32) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega1280__)
+  JsonBundle bundle;
+  bundle.doc = jsonDoc;
+  jsonQueue.push(bundle);
+  Log.trace(F("Queue length: %d" CR), jsonQueue.size());
+#else
+  // Pub to main core
+  JsonObject jsonObj = jsonDoc.to<JsonObject>();
+  pubMainCore(jsonObj); // Arduino UNO or other boards with small memory, we don't store and directly publish
+#endif
+}
+
+#if defined(ESP8266) || defined(ESP32) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega1280__)
+
+/*
+ * Add the jsonObject id as a topic to the jsonObject origin
+ *
+*/
+void buildTopicFromId(JsonObject& Jsondata, const char* origin) {
+  if (!Jsondata.containsKey("id")) {
+    Log.error(F("No id in Jsondata" CR));
+    return;
+  }
+
+  std::string topic = Jsondata["id"].as<std::string>();
+
+  // Replace ":" in topic
+  size_t pos = topic.find(":");
+  while (pos != std::string::npos) {
+    topic.erase(pos, 1);
+    pos = topic.find(":", pos);
+  }
+#  ifdef ZgatewayBT
+  if (BTConfig.pubBeaconUuidForTopic && !BTConfig.extDecoderEnable && Jsondata.containsKey("model_id") && Jsondata["model_id"].as<std::string>() == "IBEACON") {
+    if (Jsondata.containsKey("uuid")) {
+      topic = Jsondata["uuid"].as<std::string>();
+    } else {
+      Log.error(F("No uuid in Jsondata" CR));
+    }
+  }
+
+  if (BTConfig.extDecoderEnable && !Jsondata.containsKey("model"))
+    topic = BTConfig.extDecoderTopic.c_str();
+#  endif
+  std::string subjectStr(origin);
+  topic = subjectStr + "/" + topic;
+
+  Jsondata["origin"] = topic;
+
+  Log.trace(F("Origin: %s" CR), Jsondata["origin"].as<const char*>());
+}
+
+// Empty the documents queue
+void emptyQueue() {
+  while (true) {
+    JsonBundle bundle;
+
+    if (jsonQueue.empty()) {
+      break;
+    }
+    Log.trace(F("Dequeue JSON" CR));
+    bundle = jsonQueue.front();
+    jsonQueue.pop();
+
+    JsonObject obj = bundle.doc.as<JsonObject>();
+    pubMainCore(obj);
+    queueLengthSum++;
+  }
+}
+#else
+void emptyQueue() {}
+#endif
+
 /**
  * @brief Publish the payload on default MQTT topic.
  *
@@ -418,7 +532,13 @@ void pub(const char* topicori, JsonObject& data) {
   data["unixtime"] = unixtimestamp();
 #  endif
 #endif
-
+  if (data.containsKey("origin")) { // temporary, in the future use this instead of topicori
+    data.remove("origin");
+  }
+  if (data.size() == 0) {
+    Log.error(F("Empty JSON, not published" CR));
+    return;
+  }
   serializeJson(data, dataAsString);
   Log.notice(F("Send on %s msg %s" CR), topicori, dataAsString.c_str());
   String topic = String(mqtt_topic) + String(gateway_name) + String(topicori);
@@ -626,7 +746,7 @@ void delayWithOTA(long waitMillis) {
     WebUILoop();
 #  endif
 #  ifdef ESP32
-    esp_task_wdt_reset();
+    //esp_task_wdt_reset();
 #  endif
     delay(waitStep);
   }
@@ -686,8 +806,8 @@ void connectMQTT() {
 
   Log.warning(F("MQTT connection..." CR));
 #ifdef ESP32
-  esp_task_wdt_add(NULL);
+  //esp_task_wdt_add(NULL);
-  esp_task_wdt_reset();
+  //esp_task_wdt_reset();
 #endif
   char topic[mqtt_topic_max_size];
   strcpy(topic, mqtt_topic);
@@ -933,7 +1053,7 @@ void setup() {
   }
 #  endif
 #  ifdef ESP32
-  esp_task_wdt_init(GeneralTimeOut, true); //enable panic so ESP32 restarts
+  //esp_task_wdt_init(GeneralTimeOut, true); //enable panic so ESP32 restarts
 #  endif
 /*
  The 2 modules below are not connection dependent so start them before the connectivity functions
@@ -1245,7 +1365,7 @@ void setOTA() {
   ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) {
     Log.trace(F("Progress: %u%%\r" CR), (progress / (total / 100)));
 #  ifdef ESP32
-    esp_task_wdt_reset();
+    //esp_task_wdt_reset();
 #  endif
     last_ota_activity_millis = millis();
   });
@@ -1433,7 +1553,7 @@ void blockingWaitForReset() {
         }
 #    endif
 #    ifdef ESP32
-        esp_task_wdt_delete(NULL);
+        //esp_task_wdt_delete(NULL);
 #    endif
         InfoIndicatorOFF();
         SendReceiveIndicatorOFF();
@@ -1841,7 +1961,7 @@ void loop() {
 #endif
 
 #ifdef ESP32
-  esp_task_wdt_reset();
+  //esp_task_wdt_reset();
 #endif
 
   unsigned long now = millis();
@@ -1917,6 +2037,7 @@ void loop() {
         timer_sys_checks = millis();
       }
 #endif
+      emptyQueue();
 #ifdef ZsensorBME280
       MeasureTempHumAndPressure(); //Addon to measure Temperature, Humidity, Pressure and Altitude with a Bosch BME280/BMP280
 #endif
@@ -2001,7 +2122,6 @@ void loop() {
       if (SYSConfig.discovery)
         launchBTDiscovery(publishDiscovery);
 #  endif
-      emptyBTQueue();
 #endif
 #ifdef ZgatewaySRFB
       SRFBtoMQTT();
@@ -2041,14 +2161,14 @@ void loop() {
     }
   } else { // disconnected from network
 #ifdef ESP32
-    esp_task_wdt_reset();
+    //esp_task_wdt_reset();
 #endif
     connected = false;
     Log.warning(F("Network disconnected" CR));
     ErrorIndicatorON();
     delay(2000); // add a delay to avoid ESP32 crash and reset
 #ifdef ESP32
-    esp_task_wdt_reset();
+    //esp_task_wdt_reset();
 #endif
     ErrorIndicatorOFF();
     delay(2000);
@@ -2163,7 +2283,7 @@ void eraseAndRestart() {
   delay(5000);
   ESP.reset();
 #  else
-  esp_task_wdt_delete(NULL);
+  //esp_task_wdt_delete(NULL);
   nvs_flash_erase();
   ESP.restart();
 #  endif
@@ -2173,8 +2293,8 @@ void eraseAndRestart() {
 
 #if defined(ESP8266) || defined(ESP32) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega1280__)
 String stateMeasures() {
-  StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+  StaticJsonDocument<JSON_MSG_BUFFER> SYSdata;
-  JsonObject SYSdata = jsonBuffer.to<JsonObject>();
+
   SYSdata["uptime"] = uptime();
 
   SYSdata["version"] = OMG_VERSION;
@@ -2190,6 +2310,8 @@ String stateMeasures() {
   SYSdata["freemem"] = freeMem;
   SYSdata["mqttport"] = mqtt_port;
   SYSdata["mqttsecure"] = mqtt_secure;
+  SYSdata["msgproc"] = queueLengthSum;
+  SYSdata["msgspeed"] = round2((float)queueLengthSum / uptime());
 #    ifdef ESP32
   minFreeMem = ESP.getMinFreeHeap();
   SYSdata["minfreemem"] = minFreeMem;
@@ -2262,7 +2384,8 @@ String stateMeasures() {
 #  endif
   SYSdata["modules"] = modules;
 
-  pub(subjectSYStoMQTT, SYSdata);
+  SYSdata["origin"] = subjectSYStoMQTT;
+  enqueueJsonObject(SYSdata);
   pubOled(subjectSYStoMQTT, SYSdata);
 
   char jsonChar[100];
@@ -2415,7 +2538,7 @@ void receivingMQTT(char* topicOri, char* datacallback) {
 #  ifdef MQTT_HTTPS_FW_UPDATE
     MQTTHttpsFWUpdate(topicOri, jsondata);
 #  endif
-#  ifdef ZwebUI
+#  if defined(ZwebUI) && defined(ESP32)
     MQTTtoWebUI(topicOri, jsondata);
 #  endif
 #endif
@@ -2470,6 +2593,9 @@ String latestVersion;
  */
 bool checkForUpdates() {
   Log.notice(F("Update check, free heap: %d"), ESP.getFreeHeap());
+#      ifdef ZGatewayBT
+  ProcessLock = true;
+#      endif
   HTTPClient http;
   http.setTimeout((GeneralTimeOut - 1) * 1000); // -1 to avoid WDT
   http.setFollowRedirects(HTTPC_STRICT_FOLLOW_REDIRECTS);
@@ -2496,13 +2622,17 @@ bool checkForUpdates() {
     if (!jsondata.containsKey("release_summary"))
       jsondata["release_summary"] = "";
     latestVersion = jsondata["latest_version"].as<String>();
-    pub(subjectRLStoMQTT, jsondata);
+    jsondata["origin"] = subjectRLStoMQTT;
+    enqueueJsonObject(jsondata);
     Log.trace(F("Update file found on server" CR));
     return true;
   } else {
     Log.trace(F("No update file found on server" CR));
     return false;
   }
+#      ifdef ZGatewayBT
+  ProcessLock = false;
+#      endif
   Log.notice(F("Update check done, free heap: %d"), ESP.getFreeHeap());
 }
 
@@ -2558,16 +2688,16 @@ void MQTTHttpsFWUpdate(char* topicOri, JsonObject& HttpsFwUpdateData) {
       stopProcessing();
 #  endif
 #  ifdef ESP32
-      esp_task_wdt_delete(NULL); // Stop task watchdog during update
+      //esp_task_wdt_delete(NULL); // Stop task watchdog during update
 #  endif
       Log.warning(F("Starting firmware update" CR));
 
       SendReceiveIndicatorON();
       ErrorIndicatorON();
-      StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
+      StaticJsonDocument<JSON_MSG_BUFFER> jsondata;
-      JsonObject jsondata = jsonBuffer.to<JsonObject>();
       jsondata["release_summary"] = "Update in progress ...";
-      pub(subjectRLStoMQTT, jsondata);
+      jsondata["origin"] = subjectRLStoMQTT;
+      enqueueJsonObject(jsondata);
 
       const char* ota_cert = HttpsFwUpdateData["server_cert"];
       if (!ota_cert && !strstr(url, "http:")) {
@@ -2634,7 +2764,8 @@ void MQTTHttpsFWUpdate(char* topicOri, JsonObject& HttpsFwUpdateData) {
           Log.notice(F("HTTP_UPDATE_OK" CR));
           jsondata["release_summary"] = "Update success !";
           jsondata["installed_version"] = latestVersion;
-          pub(subjectRLStoMQTT, jsondata);
+          jsondata["origin"] = subjectRLStoMQTT;
+          enqueueJsonObject(jsondata);
           ota_server_cert = ota_cert;
 #  ifndef ESPWifiManualSetup
           saveConfig();