diff --git a/main/ZgatewayBT.ino b/main/ZgatewayBT.ino index 73a6239..f80d712 100644 --- a/main/ZgatewayBT.ino +++ b/main/ZgatewayBT.ino @@ -44,6 +44,7 @@ FreeRTOS::Semaphore semaphoreCreateOrUpdateDevice = FreeRTOS::Semaphore("createO # include # include # include +# include # include "soc/timer_group_reg.h" # include "soc/timer_group_struct.h" @@ -90,9 +91,101 @@ int minRssi = abs(MinimumRSSI); //minimum rssi value unsigned int scanCount = 0; +void pubBTMainCore(JsonObject& data, bool haPresenceEnabled = true) { + if (abs((int)data["rssi"] | 0) < minRssi) { + String mac_address = data["id"].as(); + mac_address.replace(":", ""); + String mactopic = subjectBTtoMQTT + String("/") + mac_address; + pub((char*)mactopic.c_str(), data); + } + if (haPresenceEnabled && data.containsKey("distance")) { + data.remove("servicedatauuid"); + data.remove("servicedata"); + String topic = String(Base_Topic) + "home_presence/" + String(gateway_name); + pub_custom_topic((char*)topic.c_str(), data, false); + } +} + +class JsonBundle { +public: + StaticJsonBuffer buffer; + JsonObject* object; + bool haPresence; + + JsonObject& createObject(const char* json = NULL, bool haPresenceEnabled = true) { + buffer.clear(); + haPresence = haPresenceEnabled; + object = &(json == NULL ? buffer.createObject() : buffer.parseObject(json)); + return *object; + } +}; + +void PublishDeviceData(JsonObject& BLEdata, bool processBLEData = true); + # ifdef ESP32 static TaskHandle_t xCoreTaskHandle; + +atomic_int forceBTScan; + +JsonBundle jsonBTBufferQueue[BTQueueSize]; +atomic_int jsonBTBufferQueueNext, jsonBTBufferQueueLast; +int btQueueBlocked = 0; +int btQueueLengthSum = 0; +int btQueueLengthCount = 0; + +JsonObject& getBTJsonObject(const char* json = NULL, bool haPresenceEnabled = true) { + 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... + } +} + +# else + +JsonBundle jsonBTBuffer; + +JsonObject& getBTJsonObject(const char* json = NULL, bool haPresenceEnabled = true) { + return jsonBTBuffer.createObject(); +} + +void pubBT(JsonObject& data) { + pubBTMainCore(data); +} + # endif + bool ProcessLock = false; // Process lock when we want to use a critical function like OTA for example BLEdevice* getDeviceByMac(const char* mac); @@ -409,10 +502,18 @@ void INodeEMDiscovery(char* mac, char* sensorModel) {} static int taskCore = 0; class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks { + std::string convertServiceData(std::string deviceServiceData) { + int serviceDataLength = (int)deviceServiceData.length(); + char spr[2 * serviceDataLength + 1]; + for (int i = 0; i < serviceDataLength; i++) sprintf(spr + 2 * i, "%.2x", (unsigned char)deviceServiceData[i]); + spr[2 * serviceDataLength] = 0; + Log.trace("Converted service data (%d) to %s" CR, serviceDataLength, spr); + return spr; + } + void onResult(BLEAdvertisedDevice* advertisedDevice) { Log.trace(F("Creating BLE buffer" CR)); - StaticJsonBuffer jsonBuffer; - JsonObject& BLEdata = jsonBuffer.createObject(); + JsonObject& BLEdata = getBTJsonObject(); String mac_adress = advertisedDevice->getAddress().toString().c_str(); mac_adress.toUpperCase(); BLEdata.set("id", (char*)mac_adress.c_str()); @@ -441,28 +542,47 @@ class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks { int serviceDataCount = advertisedDevice->getServiceDataCount(); Log.trace(F("Get services data number: %d" CR), serviceDataCount); for (int j = 0; j < serviceDataCount; j++) { - std::string serviceData = advertisedDevice->getServiceData(j); - int serviceDataLength = serviceData.length(); - String returnedString = ""; - for (int i = 0; i < serviceDataLength; i++) { - int a = serviceData[i]; - if (a < 16) { - returnedString += F("0"); - } - returnedString += String(a, HEX); - } - char service_data[returnedString.length() + 1]; - returnedString.toCharArray(service_data, returnedString.length() + 1); - service_data[returnedString.length()] = '\0'; - Log.trace(F("Service data: %s" CR), service_data); - BLEdata.set("servicedata", service_data); + std::string service_data = convertServiceData(advertisedDevice->getServiceData(j)); + Log.trace(F("Service data: %s" CR), service_data.c_str()); + BLEdata.set("servicedata", (char*)service_data.c_str()); std::string serviceDatauuid = advertisedDevice->getServiceDataUUID(j).toString(); Log.trace(F("Service data UUID: %s" CR), (char*)serviceDatauuid.c_str()); BLEdata.set("servicedatauuid", (char*)serviceDatauuid.c_str()); - PublishDeviceData(BLEdata); + process_bledata(BLEdata); // this will force to resolve all the service data + } + + if (serviceDataCount > 1) { + BLEdata.remove("servicedata"); + BLEdata.remove("servicedatauuid"); + + int msglen = BLEdata.measureLength() + 1; + char jsonmsg[msglen]; + char jsonmsgb[msglen]; + BLEdata.printTo(jsonmsgb, sizeof(jsonmsgb)); + for (int j = 0; j < serviceDataCount; j++) { + strcpy(jsonmsg, jsonmsgb); // the parse _destroys_ the message buffer + JsonObject& BLEdataLocal = getBTJsonObject(jsonmsg, j == 0); // note, that first time we will get here the BLEdata itself; haPresence for the first msg + if (!BLEdataLocal.containsKey("id")) { // would crash without id + Log.trace("Json parsing error for %s" CR, jsonmsgb); + break; + } + std::string service_data = convertServiceData(advertisedDevice->getServiceData(j)); + std::string serviceDatauuid = advertisedDevice->getServiceDataUUID(j).toString(); + + int last = atomic_load_explicit(&jsonBTBufferQueueLast, ::memory_order_seq_cst) % BTQueueSize; + int size1 = jsonBTBufferQueue[last].buffer.size(); + BLEdataLocal.set("servicedata", (char*)service_data.c_str()); + int size2 = jsonBTBufferQueue[last].buffer.size(); + BLEdataLocal.set("servicedatauuid", (char*)serviceDatauuid.c_str()); + int size3 = jsonBTBufferQueue[last].buffer.size(); + Log.trace("Buffersize for %d : %d -> %d -> %d" CR, j, size1, size2, size3); + PublishDeviceData(BLEdataLocal); + } + } else { + PublishDeviceData(BLEdata, false); // easy case } } else { - PublishDeviceData(BLEdata); // publish device even if there is no service data + PublishDeviceData(BLEdata); // PublishDeviceData has its own logic whether it needs to publish the json or not } } else { Log.trace(F("Filtered mac device" CR)); @@ -502,8 +622,7 @@ void notifyCB( if (length == 5) { Log.trace(F("Device identified creating BLE buffer" CR)); - StaticJsonBuffer jsonBuffer; - JsonObject& BLEdata = jsonBuffer.createObject(); + JsonObject& BLEdata = getBTJsonObject(); String mac_adress = pBLERemoteCharacteristic->getRemoteService()->getClient()->getPeerAddress().toString().c_str(); mac_adress.toUpperCase(); for (vector::iterator p = devices.begin(); p != devices.end(); ++p) { @@ -522,9 +641,7 @@ void notifyCB( BLEdata.set("volt", (float)(((pData[4] * 256) + pData[3]) / 1000.0)); BLEdata.set("batt", (float)(((((pData[4] * 256) + pData[3]) / 1000.0) - 2.1) * 100)); - mac_adress.replace(":", ""); - String mactopic = subjectBTtoMQTT + String("/") + mac_adress; - pub((char*)mactopic.c_str(), BLEdata); + pubBT(BLEdata); } else { Log.notice(F("Device not identified" CR)); } @@ -587,6 +704,7 @@ void BLEconnect() { void stopProcessing() { Log.notice(F("Stop BLE processing" CR)); ProcessLock = true; + delay(Scan_duration < 2000 ? Scan_duration : 2000); } void startProcessing() { @@ -600,14 +718,14 @@ void coreTask(void* pvParameters) { Log.trace(F("BT Task running on core: %d" CR), xPortGetCoreID()); if (!ProcessLock) { int n = 0; - while (client.state() != 0 && n <= InitialMQTTConnectionTimeout) { + while (client.state() != 0 && n <= InitialMQTTConnectionTimeout && !ProcessLock) { n++; Log.trace(F("Wait for MQTT on core: %d attempt: %d" CR), xPortGetCoreID(), n); delay(1000); } if (client.state() != 0) { Log.warning(F("MQTT client disconnected no BLE scan" CR)); - } else { + } else if (!ProcessLock) { BLEscan(); // Launching a connect every BLEscanBeforeConnect if (!(scanCount % BLEscanBeforeConnect) || scanCount == 1) @@ -618,8 +736,14 @@ void coreTask(void* pvParameters) { } if (lowpowermode) { lowPowerESP32(); + int scan = atomic_exchange_explicit(&forceBTScan, 0, ::memory_order_seq_cst); // is this enough, it will wait the full deepsleep... + if (scan == 1) BTforceScan(); } else { - delay(BLEinterval); + for (int interval = BLEinterval, waitms; interval > 0; interval -= waitms) { + int scan = atomic_exchange_explicit(&forceBTScan, 0, ::memory_order_seq_cst); + if (scan == 1) BTforceScan(); // should we break after this? + delay(waitms = interval > 100 ? 100 : interval); // 100ms + } } } else { Log.trace(F("BLE core task canceled by processLock" CR)); @@ -680,6 +804,10 @@ void setupBT() { Log.notice(F("minrssi: %d" CR), minRssi); Log.notice(F("Low Power Mode: %d" CR), lowpowermode); + 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 + // we setup a task with priority one to avoid conflict with other gateways xTaskCreatePinnedToCore( coreTask, /* Function to implement the task */ @@ -789,8 +917,7 @@ bool BTtoMQTT() { restData = token.substring(d[5].start, (d[5].start + restDataLength)); Log.trace(F("Creating BLE buffer" CR)); - StaticJsonBuffer jsonBuffer; - JsonObject& BLEdata = jsonBuffer.createObject(); + JsonObject& BLEdata = getBTJsonObject(); Log.trace(F("Id %s" CR), (char*)mac.c_str()); BLEdata.set("id", (char*)mac.c_str()); @@ -861,21 +988,20 @@ void launchDiscovery() { } } -void PublishDeviceData(JsonObject& BLEdata) { +void PublishDeviceData(JsonObject& BLEdata, bool processBLEData) { if (abs((int)BLEdata["rssi"] | 0) < minRssi) { // process only the devices close enough - JsonObject& BLEdataOut = process_bledata(BLEdata); - if (!publishOnlySensors || BLEdataOut.containsKey("model")) { + if (processBLEData) process_bledata(BLEdata); + if (!publishOnlySensors || BLEdata.containsKey("model") || BLEdata.containsKey("distance")) { # if !pubBLEServiceUUID - RemoveJsonPropertyIf(BLEdataOut, "servicedatauuid", BLEdataOut.containsKey("servicedatauuid")); + RemoveJsonPropertyIf(BLEdata, "servicedatauuid", BLEdata.containsKey("servicedatauuid")); # endif # if !pubKnownBLEServiceData - RemoveJsonPropertyIf(BLEdataOut, "servicedata", BLEdataOut.containsKey("model") && BLEdataOut.containsKey("servicedata")); + RemoveJsonPropertyIf(BLEdata, "servicedata", BLEdata.containsKey("model") && BLEdata.containsKey("servicedata")); # endif - String mactopic = BLEdataOut["id"].as(); - mactopic.replace(":", ""); - mactopic = subjectBTtoMQTT + String("/") + mactopic; - pub((char*)mactopic.c_str(), BLEdataOut); + pubBT(BLEdata); } + } else if (BLEdata.containsKey("distance")) { + pubBT(BLEdata); } else { Log.trace(F("Low rssi, device filtered" CR)); } @@ -1254,11 +1380,21 @@ void haRoomPresence(JsonObject& HomePresence) { } HomePresence["distance"] = distance; Log.trace(F("Ble distance %D" CR), distance); - String topic = String(Base_Topic) + "home_presence/" + String(gateway_name); - pub_custom_topic((char*)topic.c_str(), HomePresence, false); } # endif +void BTforceScan() { + if (!ProcessLock) { + BTtoMQTT(); + Log.trace(F("Scan done" CR)); +# ifdef ESP32 + BLEconnect(); +# endif + } else { + Log.trace(F("Cannot launch scan due to other process running" CR)); + } +} + void MQTTtoBT(char* topicOri, JsonObject& BTdata) { // json object decoding if (cmpToMainTopic(topicOri, subjectMQTTtoBTset)) { Log.trace(F("MQTTtoBT json set" CR)); @@ -1274,22 +1410,17 @@ void MQTTtoBT(char* topicOri, JsonObject& BTdata) { // json object decoding // Scan interval set if (BTdata.containsKey("interval")) { Log.trace(F("BLE interval setup" CR)); - // storing BLE interval for further use if needed - unsigned int prevBLEinterval = BLEinterval; - Log.trace(F("Previous interval: %d ms" CR), BLEinterval); - BLEinterval = (unsigned int)BTdata["interval"]; - Log.notice(F("New interval: %d ms" CR), BLEinterval); - if (BLEinterval == 0) { - if (!ProcessLock) { - BTtoMQTT(); - Log.trace(F("Scan done" CR)); + unsigned int interval = BTdata["interval"]; + if (interval == 0) { # ifdef ESP32 - BLEconnect(); + atomic_store_explicit(&forceBTScan, 1, ::memory_order_seq_cst); // ask the other core to do the scan for us +# else + BTforceScan(); # endif - BLEinterval = prevBLEinterval; // as 0 was just used as a command we recover previous scan duration - } else { - Log.trace(F("Cannot launch scan due to other process running" CR)); - } + } else { + Log.trace(F("Previous interval: %d ms" CR), BLEinterval); + BLEinterval = interval; + Log.notice(F("New interval: %d ms" CR), BLEinterval); } } // Number of scan before a connect set diff --git a/main/config_BT.h b/main/config_BT.h index 08691c2..474f030 100644 --- a/main/config_BT.h +++ b/main/config_BT.h @@ -30,6 +30,12 @@ extern void setupBT(); extern bool BTtoMQTT(); extern void MQTTtoBT(char* topicOri, JsonObject& RFdata); +#ifdef ESP32 +extern int btQueueBlocked; +extern int btQueueLengthSum; +extern int btQueueLengthCount; +#endif + /*----------------------BT topics & parameters-------------------------*/ #define subjectBTtoMQTT "/BTtoMQTT" #define subjectMQTTtoBTset "/commands/MQTTtoBT/config" @@ -48,6 +54,10 @@ extern void MQTTtoBT(char* topicOri, JsonObject& RFdata); # define PublishOnlySensors false //false if we publish all BLE devices discovered or true only the identified sensors (like temperature sensors) #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 diff --git a/main/main.ino b/main/main.ino index 7c0d30d..aaa2fdd 100644 --- a/main/main.ino +++ b/main/main.ino @@ -1250,13 +1250,7 @@ void loop() { #ifdef ZmqttDiscovery if (disc) { if (!connectedOnce) { -# if defined(ZgatewayBT) && defined(ESP32) - stopProcessing(); // Avoid publication concurrency issues on ESP32 BLE -# endif pubMqttDiscovery(); // at first connection we publish the discovery payloads -# if defined(ZgatewayBT) && defined(ESP32) - startProcessing(); -# endif } } #endif @@ -1328,6 +1322,8 @@ void loop() { # ifndef ESP32 if (BTtoMQTT()) Log.trace(F("BTtoMQTT OK" CR)); +# else + emptyBTQueue(); # endif #endif #ifdef ZgatewaySRFB @@ -1407,6 +1403,10 @@ void stateMeasures() { # ifdef ZgatewayBT # ifdef ESP32 SYSdata["lowpowermode"] = (int)lowpowermode; + SYSdata["btqblck"] = btQueueBlocked; + SYSdata["btqsum"] = btQueueLengthSum; + SYSdata["btqsnd"] = btQueueLengthCount; + SYSdata["btqavg"] = (btQueueLengthCount > 0 ? btQueueLengthSum / (float)btQueueLengthCount : 0); # endif SYSdata["interval"] = BLEinterval; SYSdata["scanbcnct"] = BLEscanBeforeConnect;