From 8f675f685980aae06983f723b22dbe5c251fc02f Mon Sep 17 00:00:00 2001
From: Zoe Pfister <zoe.pfister@uibk.ac.at>
Date: Tue, 7 Feb 2023 11:03:59 +0100
Subject: [PATCH] Getting Ack-Response time down to <15ms
---
client/client_mock/src/main.cpp | 15 +-
client/libs/espnow/src/ESPNow.cpp | 230 +++++++++---------
client/libs/espnow/src/Message.cpp | 2 +-
client/libs/mock_sensor/MockSensor.cpp | 32 +--
.../lib/Utilities/SDCardLogger.cpp | 6 +-
.../lib/Utilities/Utilities.cpp | 3 +-
.../lib/Utilities/Utilities.h | 2 +-
host/host_central_mast/platformio.ini | 4 +-
host/host_central_mast/src/main.cpp | 136 +++++++----
9 files changed, 238 insertions(+), 192 deletions(-)
diff --git a/client/client_mock/src/main.cpp b/client/client_mock/src/main.cpp
index 407e3c4..151f202 100644
--- a/client/client_mock/src/main.cpp
+++ b/client/client_mock/src/main.cpp
@@ -37,7 +37,7 @@ void send_msgs(const std::__cxx11::list<Message> msgs) {
// one loop takes ~2200 ms
void setup() {
// disable brownout
- WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
+// WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
unsigned long ts = millis();
Serial.begin(115200);
@@ -56,11 +56,13 @@ void setup() {
mock_channel2.setChannel(2);
mock_channel3.setChannel(3);
- ESP_LOGD(TAG, "Setup took %ld ms", millis() - ts);
+ // disable led
+ gpio_set_direction(GPIO_NUM_32, GPIO_MODE_OUTPUT);
- try {
- // FIXME: put me into seperate trys? No data will be sent when 1 exception occurs
+ ESP_LOGD(TAG, "Setup took %ld ms", millis() - ts);
+ try {
+ // FIXME: put me into seperate trys? No data will be sent when 1 exception occurs
ts = millis();
auto messages0 = mock_channel0.buildMessages();
auto messages1 = mock_channel1.buildMessages();
@@ -126,11 +128,6 @@ void setup() {
ts = millis();
Message::sendMessages(messages);
-// send_msgs(messages0);
-// send_msgs(messages1);
-// send_msgs(messages2);
-// send_msgs(messages3);
- // roughly takes 3s in ideal conditions
ESP_LOGD(TAG, "Sending messages took %ld ms", millis() - ts);
} catch (const NoDataAvailableException &e) {
diff --git a/client/libs/espnow/src/ESPNow.cpp b/client/libs/espnow/src/ESPNow.cpp
index e230b55..6ef9054 100644
--- a/client/libs/espnow/src/ESPNow.cpp
+++ b/client/libs/espnow/src/ESPNow.cpp
@@ -7,129 +7,141 @@ esp_now_peer_info_t hostInfo;
Preferences preferences;
bool msg_recv = false;
-bool was_msg_received(){
- if(msg_recv){
- msg_recv = false;
- return true;
- }
- return false;
+bool was_msg_received() {
+ if (msg_recv) {
+ msg_recv = false;
+ return true;
+ }
+ return false;
}
-void get_host_mac(uint8_t *destination)
-{
- preferences.begin("config", true);
- if (preferences.isKey("host")) {
- preferences.getBytes("host", destination, sizeof(uint8_t) * 6);
- } else {
- memcpy(destination, BROADCAST_MAC, sizeof(BROADCAST_MAC));
- ESP_LOGI(TAG, "Backup MAC address used");
- }
- preferences.end();
+void get_host_mac(uint8_t *destination) {
+ preferences.begin("config", true);
+ if (preferences.isKey("host")) {
+ preferences.getBytes("host", destination, sizeof(uint8_t) * 6);
+ } else {
+ memcpy(destination, BROADCAST_MAC, sizeof(BROADCAST_MAC));
+ ESP_LOGI(TAG, "Backup MAC address used");
+ }
+ preferences.end();
}
-esp_err_t add_host_to_peers(config received){
- esp_now_peer_info_t host;
- memset(&host, 0, sizeof(host));
- memcpy(host.peer_addr, received.host, sizeof(received.host));
- host.encrypt = false;
- host.channel = 0;
- return esp_now_add_peer(&host);
+esp_err_t add_host_to_peers(config received) {
+ esp_now_peer_info_t host;
+ memset(&host, 0, sizeof(host));
+ memcpy(host.peer_addr, received.host, sizeof(received.host));
+ host.encrypt = false;
+ host.channel = 0;
+ return esp_now_add_peer(&host);
}
-void on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status)
-{
- ESP_LOGE(TAG, "Message sent to %02X:%02X:%02X:%02X:%02X:%02X", mac_addr[0],
- mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
+void on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status) {
+ ESP_LOGE(TAG, "Message sent to %02X:%02X:%02X:%02X:%02X:%02X", mac_addr[0],
+ mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
}
-void on_data_recv(const uint8_t *mac, const uint8_t *incomingData, int len)
-{
- // is msg host -> yes -> set bool
- // assume host change not happening, rare event
- // => on host change, broadcast
- ESP_LOGD(TAG, "Message received");
- config received_msg;
- memcpy(&received_msg, incomingData, sizeof(received_msg)); // TODO: check for valid mac
- // all the esp32 macs so far use the same first 3(?) bytes so maybe use that
- // First three bytes of MACs are always vendor specific. But vendors have a range of them.
- // all Espressif registered MAC starting bytes: https://maclookup.app/vendors/espressif-inc
- // you can also set your own MAC https://randomnerdtutorials.com/get-change-esp32-esp8266-mac-address-arduino/
- switch (received_msg.type){
- case hostChange:{
- ESP_LOGI(TAG, "hostChange received");
- Time::getInstance().setTime(received_msg.epoch_seconds);
- // delete old host
- preferences.begin("config", false);
- if(preferences.isKey("host")){
- ESP_LOGI(TAG, "removing old host");
- uint8_t old[6];
- preferences.end();
- get_host_mac(old); // maybe problem here, re-opening preferences
- esp_now_del_peer(old);
- }
- // add new host
- preferences.begin("config", false);
- if(preferences.putBytes("host", received_msg.host, sizeof(received_msg.host)) > 0){
- ESP_LOGI(TAG, "Host MAC address saved to flash %02X:%02X:%02X:%02X:%02X:%02X", received_msg.host[0],
- received_msg.host[1],received_msg.host[2],received_msg.host[3],received_msg.host[4],received_msg.host[5]);
- }
- else{
- ESP_LOGI(TAG, "Couldn't save Host Mac to flash");
- }
- preferences.end();
- add_host_to_peers(received_msg);
- }
- case dataAck:{
- ESP_LOGI(TAG, "dataAck received.");
- Time::getInstance().setTime(
- received_msg.epoch_seconds); // see https://www.esp32.com/viewtopic.php?t=9965, maybe this needs an offset
-
- ESP_LOGI(TAG, "Timestamp received: %ld", Time::getInstance().getEpochSeconds());
- preferences.begin("config", false);
- if (!preferences.isKey("host")) {
- if(preferences.putBytes("host", received_msg.host, sizeof(received_msg.host)) > 0){
- ESP_LOGI(TAG, "host MAC address saved to flash %02X:%02X:%02X:%02X:%02X:%02X", received_msg.host[0],
- received_msg.host[1],received_msg.host[2],received_msg.host[3],received_msg.host[4],received_msg.host[5]);
- }
-
- // add host to peers
- add_host_to_peers(received_msg);
- }
- preferences.end();
- // delay(50);
- msg_recv = true;
- }
- default:{
- break;
- }
- }
+void on_data_recv(const uint8_t *mac, const uint8_t *incomingData, int len) {
+ // is msg host -> yes -> set bool
+ // assume host change not happening, rare event
+ // => on host change, broadcast
+// ESP_LOGD(TAG, "Message received");
+// time this
+ auto start = millis();
+ config received_msg;
+ memcpy(&received_msg,
+ incomingData,
+ sizeof(received_msg)); // TODO: check for valid mac
+ // all the esp32 macs so far use the same first 3(?) bytes so maybe use that
+ // First three bytes of MACs are always vendor specific. But vendors have a range of them.
+ // all Espressif registered MAC starting bytes: https://maclookup.app/vendors/espressif-inc
+ // you can also set your own MAC https://randomnerdtutorials.com/get-change-esp32-esp8266-mac-address-arduino/
+ switch (received_msg.type) {
+ case hostChange: {
+ ESP_LOGI(TAG, "hostChange received");
+ Time::getInstance().setTime(received_msg.epoch_seconds);
+ // delete old host
+ preferences.begin("config", false);
+ if (preferences.isKey("host")) {
+ ESP_LOGI(TAG, "removing old host");
+ uint8_t old[6];
+ preferences.end();
+ get_host_mac(old); // maybe problem here, re-opening preferences
+ esp_now_del_peer(old);
+ }
+ // add new host
+ preferences.begin("config", false);
+ if (preferences.putBytes("host",
+ received_msg.host,
+ sizeof(received_msg.host)) > 0) {
+ ESP_LOGI(TAG,
+ "Host MAC address saved to flash %02X:%02X:%02X:%02X:%02X:%02X",
+ received_msg.host[0],
+ received_msg.host[1],
+ received_msg.host[2],
+ received_msg.host[3],
+ received_msg.host[4],
+ received_msg.host[5]);
+ } else {
+ ESP_LOGI(TAG, "Couldn't save Host Mac to flash");
+ }
+ preferences.end();
+ add_host_to_peers(received_msg);
+ }
+ case dataAck: {
+// ESP_LOGI(TAG, "dataAck received.");
+ Time::getInstance().setTime(
+ received_msg.epoch_seconds); // see https://www.esp32.com/viewtopic.php?t=9965, maybe this needs an offset
+
+// ESP_LOGI(TAG, "Timestamp received: %ld", Time::getInstance().getEpochSeconds());
+ preferences.begin("config", false);
+ if (!preferences.isKey("host")) {
+ if (preferences.putBytes("host",
+ received_msg.host,
+ sizeof(received_msg.host)) > 0) {
+// ESP_LOGI(TAG, "host MAC address saved to flash %02X:%02X:%02X:%02X:%02X:%02X", received_msg.host[0],
+// received_msg.host[1],received_msg.host[2],received_msg.host[3],received_msg.host[4],received_msg.host[5]);
+// }
+
+ // add host to peers
+ add_host_to_peers(received_msg);
+ }
+ }
+ preferences.end();
+ // delay(50);
+ msg_recv = true;
+ }
+ default: {
+ break;
+ }
+ }
+ auto end = millis();
+
}
-esp_err_t espnow_setup()
-{
- esp_err_t result;
- WiFi.mode(WIFI_STA);
- result = esp_now_init();
- if (result != ESP_OK) {
- // initialization failed
- ESP_LOGE(TAG, "ESPNow setup failed");
- return result; // not sure about this
- }
+esp_err_t espnow_setup() {
+ esp_err_t result;
+ WiFi.mode(WIFI_STA);
+ result = esp_now_init();
+ if (result != ESP_OK) {
+ // initialization failed
+ ESP_LOGE(TAG, "ESPNow setup failed");
+ return result; // not sure about this
+ }
- get_host_mac(hostInfo.peer_addr); // check if there is a host saved in flash mem, broadcast otherwise
+ get_host_mac(hostInfo.peer_addr); // check if there is a host saved in flash mem, broadcast otherwise
- hostInfo.channel = 0;
+ hostInfo.channel = 0;
- // TODO: PMK is used to encrypt LMK with the AES-128 algorithm. Call esp_now_set_pmk() to set PMK. If PMK is not
- // set, a default PMK will be used.
- // https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/network/esp_now.html
- hostInfo.encrypt = false;
- esp_now_add_peer(&hostInfo);
+ // TODO: PMK is used to encrypt LMK with the AES-128 algorithm. Call esp_now_set_pmk() to set PMK. If PMK is not
+ // set, a default PMK will be used.
+ // https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/network/esp_now.html
+ hostInfo.encrypt = false;
+ esp_now_add_peer(&hostInfo);
- esp_now_register_recv_cb(on_data_recv);
- esp_now_register_send_cb(on_data_sent);
+ esp_now_register_recv_cb(on_data_recv);
+ esp_now_register_send_cb(on_data_sent);
- ESP_LOGI(TAG, "ESPNow started. MAC: %s", WiFi.macAddress().c_str());
- return ESP_OK;
+ ESP_LOGI(TAG, "ESPNow started. MAC: %s", WiFi.macAddress().c_str());
+ return ESP_OK;
}
diff --git a/client/libs/espnow/src/Message.cpp b/client/libs/espnow/src/Message.cpp
index fb3dd18..0df90ac 100644
--- a/client/libs/espnow/src/Message.cpp
+++ b/client/libs/espnow/src/Message.cpp
@@ -32,7 +32,7 @@ esp_err_t Message::sendMessages(const std::array<Message, 6> &messages) {
std::array<ClientDataPackage, 6> clientDataPackages{};
// max 6 messages
int i = 0;
- for (const auto &message : messages) {
+ for (const auto &message: messages) {
if (i >= 6) {
ESP_LOGE(TAG, "Too many messages to send");
break;
diff --git a/client/libs/mock_sensor/MockSensor.cpp b/client/libs/mock_sensor/MockSensor.cpp
index e70f207..bc1deaf 100644
--- a/client/libs/mock_sensor/MockSensor.cpp
+++ b/client/libs/mock_sensor/MockSensor.cpp
@@ -5,36 +5,36 @@ static const char *TAG = "MOCK";
void MockSensor::setup() {
- ESP_LOGD(TAG, "MOCK Sensor initialized");
- delay(100);
- channel = 0;
+ ESP_LOGD(TAG, "MOCK Sensor initialized");
+ delay(100);
+ channel = 0;
}
Measurement MockSensor::readData() {
- // generate a random float value between 0 and 100
- float randomValue = (float) rand() / (float) RAND_MAX * 100.0;
- randomValue += static_cast<float>(DeepSleep::bootCount);
- ESP_LOGD(TAG, "MOCK Sensor read value: %f", randomValue);
- return {randomValue, channel, NO_I2C_ADDRESS, MeasurementType::MOCK, ErrorType::DATA_OK};
+ // generate a random float value between 0 and 100
+ float randomValue = DeepSleep::bootCount * 10 + channel;
+ ESP_LOGD(TAG, "MOCK Sensor read value: %f", randomValue);
+ return {randomValue, channel, NO_I2C_ADDRESS, MeasurementType::MOCK,
+ ErrorType::DATA_OK};
}
void MockSensor::setChannel(int c) {
- channel = c;
+ channel = c;
}
std::list<Message> MockSensor::buildMessages() {
- std::list<Message> messages;
- auto measurement = readData();
+ std::list<Message> messages;
+ auto measurement = readData();
// Measurement
// MockData{data, channel, NO_I2C_ADDRESS, MeasurementType::MOCK, ErrorType::DATA_OK};
- messages.emplace_back(measurement,
- sensorInformation,
- Time::getInstance().getEpochSeconds());
- return messages;
+ messages.emplace_back(measurement,
+ sensorInformation,
+ Time::getInstance().getEpochSeconds());
+ return messages;
}
SensorInformation MockSensor::getSensorInformation() const {
- return sensorInformation;
+ return sensorInformation;
}
diff --git a/host/host_central_mast/lib/Utilities/SDCardLogger.cpp b/host/host_central_mast/lib/Utilities/SDCardLogger.cpp
index f3ee7a6..d0c5991 100644
--- a/host/host_central_mast/lib/Utilities/SDCardLogger.cpp
+++ b/host/host_central_mast/lib/Utilities/SDCardLogger.cpp
@@ -15,10 +15,14 @@ namespace SDCardLogger {
int vprintf_into_sd(const char *szFormat, va_list args) {
String logstring = "[" + rtc.getDateTime() + "] ";
logstring += szFormat;
-
+ // print current core
+ logstring += "Core: ";
+ logstring += xPortGetCoreID();
+ logstring += ". ";
if (!SDUtilities::isSDAvailable()) {
if (printToSerial) {
logstring += " (SD card not available)\n";
+
vprintf(logstring.c_str(), args);
}
return 1;
diff --git a/host/host_central_mast/lib/Utilities/Utilities.cpp b/host/host_central_mast/lib/Utilities/Utilities.cpp
index 98909c0..1f3c8d8 100644
--- a/host/host_central_mast/lib/Utilities/Utilities.cpp
+++ b/host/host_central_mast/lib/Utilities/Utilities.cpp
@@ -305,8 +305,7 @@ String documentToServerReadableString(const DynamicJsonDocument &doc) {
return serverString;
}
-String compressedDataPackageToServerReadableString(ClientDataPackage clientDataPackage,
- const String &clientMacAddress) {
+String dataPackageToServerReadableString(ClientDataPackage clientDataPackage, const String &clientMacAddress) {
StaticJsonDocument<300> serverDoc;
String hostMacAddressString = WiFi.macAddress();
hostMacAddressString.replace(":", "");
diff --git a/host/host_central_mast/lib/Utilities/Utilities.h b/host/host_central_mast/lib/Utilities/Utilities.h
index 7ac445f..e3a7abb 100644
--- a/host/host_central_mast/lib/Utilities/Utilities.h
+++ b/host/host_central_mast/lib/Utilities/Utilities.h
@@ -35,6 +35,6 @@ String getMacAddressAsString(const uint8_t *mac);
String documentToLineProtocolString(const DynamicJsonDocument &doc);
DynamicJsonDocument parseReceivedJsonData(char *data);
String documentToServerReadableString(const DynamicJsonDocument &doc);
-String compressedDataPackageToServerReadableString(ClientDataPackage clientDataPackage, const String &clientMacAddress);
+String dataPackageToServerReadableString(ClientDataPackage clientDataPackage, const String &clientMacAddress);
#endif // HOST_CENTRAL_MAST_UTILITIES_H
diff --git a/host/host_central_mast/platformio.ini b/host/host_central_mast/platformio.ini
index 3a473d3..5cd5785 100644
--- a/host/host_central_mast/platformio.ini
+++ b/host/host_central_mast/platformio.ini
@@ -14,8 +14,8 @@ board = esp-wrover-kit
framework = arduino
monitor_speed = 115200
lib_ldf_mode = deep
-monitor_port = /dev/ttyACM0
-upload_port = /dev/ttyACM0
+monitor_port = /dev/ttyACM1
+upload_port = /dev/ttyACM1
lib_extra_dirs =
../../shared-libs
build_flags =
diff --git a/host/host_central_mast/src/main.cpp b/host/host_central_mast/src/main.cpp
index f3351d7..6faa74c 100644
--- a/host/host_central_mast/src/main.cpp
+++ b/host/host_central_mast/src/main.cpp
@@ -20,6 +20,7 @@
#include <esp_log.h>
#include <esp_now.h>
#include <queue>
+#include <soc/rtc_cntl_reg.h>
#include <sys/unistd.h>
ESP32Time rtc;
@@ -50,20 +51,19 @@ SemaphoreHandle_t xMutex;
TaskHandle_t ESPNOWTask;
-static std::queue<String> queue;
+struct QueueStruct {
+ std::array<ClientDataPackage, 6> data;
+ uint8_t mac[6]{};
+};
-void on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status) {
- // go to sleep
+void sendResponse(const uint8_t *mac) {
+ response response = {};
+ response.type = dataAck;
+ esp_read_mac(response.mac, ESP_MAC_WIFI_STA);
+ response.time = rtc.getEpoch();
+ esp_err_t success = esp_now_send(mac, (uint8_t *)&response, sizeof(response));
}
-
-/**
- * @brief ESPNOW callback function that is called when data is received
- * @param mac
- * @param incomingData
- * @param len length of the incoming data in bytes
- */
-void on_data_recv(const uint8_t *mac, const uint8_t *incomingData, int len) {
-
+void checkPeerExistence(const uint8_t *mac) {
if (!esp_now_is_peer_exist(mac)) {
esp_now_peer_info_t client = {};
memcpy(client.peer_addr, mac, sizeof(uint8_t) * 6);
@@ -75,48 +75,26 @@ void on_data_recv(const uint8_t *mac, const uint8_t *incomingData, int len) {
esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW, "Failed to add new Peer: %d", status);
}
}
+}
+void handleDataReceive(const uint8_t *mac, const uint8_t *incomingData);
+static std::queue<QueueStruct> queue;
- response response = {};
- response.type = dataAck;
- esp_read_mac(response.mac, ESP_MAC_WIFI_STA);
- response.time = rtc.getEpoch();
- esp_err_t success = esp_now_send(mac, (uint8_t *)&response, sizeof(response));
-
- esp_log_write(ESP_LOG_INFO, TAG_ESPNOW, "Message recieved\n");
- esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW, (success == ESP_OK) ? "Response sent\n" : "Failed to respond\n");
-
- // copy received data to a char array
- std::array<ClientDataPackage, 6> compressedDataPackage{};
- memcpy(&compressedDataPackage, incomingData, sizeof(ClientDataPackage) * 6);
- // esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW, "Raw received Data: %s\n", data);
-
- String macAddress = getMacAddressAsString(mac);
-
- for (auto &data : compressedDataPackage) {
-
- // ignore padding messages
- if (data.getMeasurementData().getErrorType() == ErrorType::NULL_MESSAGE) {
- continue;
- }
-
- auto doc = compressedDataPackageToServerReadableString(data, macAddress);
- // log received data
- esp_log_write(ESP_LOG_INFO, TAG_ESPNOW, "Received Data: %s\n",
- data.getDataPackageAsMinifiedJsonString().c_str());
+void on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status) {
+ // go to sleep
+}
- // serialize json document again
- std::string dataString{};
+/**
+ * @brief ESPNOW callback function that is called when data is received
+ * @param mac
+ * @param incomingData
+ * @param len length of the incoming data in bytes
+ */
+void on_data_recv(const uint8_t *mac, const uint8_t *incomingData, int len) {
+ checkPeerExistence(mac);
- try {
- SDUtilities::saveStringToSDCard(dataString);
- } catch (const std::exception &e) {
- esp_log_write(ESP_LOG_ERROR, TAG_ESPNOW, "Failed to save data to SD card: %s", e.what());
- }
+ sendResponse(mac);
- xSemaphoreTake(xMutex, portMAX_DELAY);
- queue.emplace(dataString.c_str());
- xSemaphoreGive(xMutex);
- }
+ handleDataReceive(mac, incomingData);
}
[[noreturn]] void esp_loop() {
@@ -125,7 +103,7 @@ void on_data_recv(const uint8_t *mac, const uint8_t *incomingData, int len) {
}
[[noreturn]] void ESPNOWReceiveTask(void *parameter) {
- esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW, "ESPNOWReceiveTask started on core %d\n", xPortGetCoreID());
+ esp_log_write(ESP_LOG_INFO, TAG_ESPNOW, "ESPNOWReceiveTask started on core %d\n", xPortGetCoreID());
WiFi.mode(WIFI_STA);
@@ -164,6 +142,7 @@ void setup() {
// Set console baud rate
Serial.begin(115200);
delay(10);
+ WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // disable detector
try {
SDUtilities::setupSDCard(SD_MISO, SD_MOSI, SD_SCLK, SD_CS);
} catch (const SDSetupException &e) {
@@ -202,6 +181,54 @@ void setup() {
void loop() {
+ bool emptyQueue;
+
+ // check if queue is empty
+ xSemaphoreTake(xMutex, portMAX_DELAY);
+ emptyQueue = queue.empty();
+ xSemaphoreGive(xMutex);
+
+ // exit after 10 retries
+ int earlyExitCounter = 0;
+
+ // do while loop that takes one element of the queue each until the queue is empty
+ while (!emptyQueue) {
+ xSemaphoreTake(xMutex, portMAX_DELAY);
+ auto data = queue.front();
+ queue.pop();
+ xSemaphoreGive(xMutex);
+
+ // get mac address as string
+ String macAddress = getMacAddressAsString(data.mac);
+
+ for (const auto &dataPackage : data.data) {
+ // ignore padding messages
+ if (dataPackage.getMeasurementData().getErrorType() == ErrorType::NULL_MESSAGE) {
+ continue;
+ }
+
+ auto serverReadableString = dataPackageToServerReadableString(dataPackage, macAddress);
+
+ try {
+ SDUtilities::saveStringToSDCard(serverReadableString.c_str());
+ } catch (const std::exception &e) {
+ esp_log_write(ESP_LOG_ERROR, TAG_ESPNOW, "Failed to save data to SD card: %s", e.what());
+ }
+
+ // TODO: Try send or add to queue
+ }
+
+ if (earlyExitCounter > 10) {
+ break;
+ }
+ earlyExitCounter++;
+
+ // check if queue is empty
+ xSemaphoreTake(xMutex, portMAX_DELAY);
+ emptyQueue = queue.empty();
+ xSemaphoreGive(xMutex);
+ }
+
connectionManager.modemPowerOn();
delay(5000L);
@@ -276,3 +303,10 @@ void loop() {
delay(10000);
}
+void handleDataReceive(const uint8_t *mac, const uint8_t *incomingData) {
+ std::array<ClientDataPackage, 6> compressedDataPackage{};
+ memcpy(&compressedDataPackage, incomingData, sizeof(ClientDataPackage) * 6);
+ xSemaphoreTake(xMutex, portMAX_DELAY);
+ queue.emplace(QueueStruct{compressedDataPackage, {mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]}});
+ xSemaphoreGive(xMutex);
+}
\ No newline at end of file
--
GitLab