Getting Ack-Response time down to <15ms

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) {

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;
 }

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;

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;
 }

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;

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(":", "");

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

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 =

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