Added ECHO cmd and fixed some timing issues

main/esp_now_master.c

@@ -5,8 +5,10 @@
 #include "esp_now_proto.h"
 #include "board_input.h"
 #include "ota_espnow.h"
+#include "ota_session.h"
 #include "ota_uart.h"
 #include "esp_log.h"
+#include "esp_timer.h"
 #include "freertos/FreeRTOS.h"
 #include "freertos/idf_additions.h"
 #include <string.h>
@@ -51,6 +53,66 @@ static esp_err_t send_unicast_test(const uint8_t *dest_mac, uint32_t seq) {
   return esp_now_core_send(dest_mac, &msg);
 }
 
+#define ESPNOW_ECHO_PING_TIMEOUT_MS 500
+#define ECHO_PING_TAG "[ECHO_PING]"
+
+static SemaphoreHandle_t s_echo_pong_sem;
+static bool s_echo_waiting;
+static uint64_t s_echo_expect_ts;
+static uint32_t s_echo_esp_rtt_us;
+static uint8_t s_echo_expect_mac[ESP_NOW_ETH_ALEN];
+
+static esp_err_t echo_ping_init(void) {
+  if (s_echo_pong_sem != NULL) {
+    return ESP_OK;
+  }
+  s_echo_pong_sem = xSemaphoreCreateBinary();
+  if (s_echo_pong_sem == NULL) {
+    return ESP_ERR_NO_MEM;
+  }
+  return ESP_OK;
+}
+
+static esp_err_t send_echo_ping(const uint8_t *dest_mac, uint64_t host_timestamp_us,
+                                uint64_t master_time_us) {
+  alox_EspNowMessage msg = alox_EspNowMessage_init_zero;
+  msg.type = alox_EspNowMessageType_ESPNOW_ECHO_PING;
+  msg.which_payload = alox_EspNowMessage_echo_ping_tag;
+  msg.payload.echo_ping.host_timestamp_us = host_timestamp_us;
+  msg.payload.echo_ping.master_time_us = master_time_us;
+  char mac_str[18];
+  esp_now_core_mac_to_str(dest_mac, mac_str, sizeof(mac_str));
+  ESP_LOGI(ECHO_PING_TAG, "ESP-NOW PING send to %s host_ts=%llu master_time_us=%llu",
+           mac_str, (unsigned long long)host_timestamp_us,
+           (unsigned long long)master_time_us);
+  return esp_now_core_send_fast(dest_mac, &msg);
+}
+
+static void echo_ping_on_pong(const uint8_t mac[ESP_NOW_ETH_ALEN],
+                              const alox_EspNowEchoPong *pong) {
+  if (pong == NULL || !s_echo_waiting) {
+    return;
+  }
+  if (!esp_now_core_mac_equal(mac, s_echo_expect_mac)) {
+    return;
+  }
+  if (pong->host_timestamp_us != s_echo_expect_ts) {
+    return;
+  }
+  int64_t now_us = esp_timer_get_time();
+  s_echo_esp_rtt_us =
+      (uint32_t)(now_us - (int64_t)pong->master_time_us);
+  char mac_str[18];
+  esp_now_core_mac_to_str(mac, mac_str, sizeof(mac_str));
+  ESP_LOGI(ECHO_PING_TAG,
+           "ESP-NOW PONG recv from %s host_ts=%llu master_time_us=%llu "
+           "recv_time_us=%lld esp_rtt_us=%lu",
+           mac_str, (unsigned long long)pong->host_timestamp_us,
+           (unsigned long long)pong->master_time_us, (long long)now_us,
+           (unsigned long)s_echo_esp_rtt_us);
+  xSemaphoreGive(s_echo_pong_sem);
+}
+
 static esp_err_t send_find_me(const uint8_t *dest_mac, uint32_t client_id) {
   alox_EspNowMessage msg = alox_EspNowMessage_init_zero;
   msg.type = alox_EspNowMessageType_ESPNOW_FIND_ME;
@@ -119,7 +181,7 @@ static esp_err_t send_ota_payload(const uint8_t *dest_mac, uint32_t seq,
   msg.payload.ota_payload.seq = seq;
   msg.payload.ota_payload.data.size = len;
   memcpy(msg.payload.ota_payload.data.bytes, data, len);
-  return esp_now_core_send_wait(dest_mac, &msg);
+  return esp_now_core_send_reliable(dest_mac, &msg);
 }
 
 static esp_err_t send_ota_end(const uint8_t *dest_mac) {
@@ -224,6 +286,48 @@ esp_err_t esp_now_comm_send_unicast_test(const uint8_t mac[CLIENT_MAC_LEN],
   return err;
 }
 
+esp_err_t esp_now_comm_echo_ping(const uint8_t mac[CLIENT_MAC_LEN],
+                                 uint64_t timestamp_us,
+                                 esp_now_echo_ping_result_t *result) {
+  if (mac == NULL || !esp_now_core_is_master()) {
+    return ESP_ERR_INVALID_STATE;
+  }
+  if (echo_ping_init() != ESP_OK) {
+    return ESP_ERR_NO_MEM;
+  }
+
+  xSemaphoreTake(s_echo_pong_sem, 0);
+  s_echo_waiting = true;
+  s_echo_expect_ts = timestamp_us;
+  s_echo_esp_rtt_us = 0;
+  memcpy(s_echo_expect_mac, mac, ESP_NOW_ETH_ALEN);
+
+  uint64_t master_time_us = (uint64_t)esp_timer_get_time();
+  esp_err_t err = send_echo_ping(mac, timestamp_us, master_time_us);
+  if (err != ESP_OK) {
+    s_echo_waiting = false;
+    return err;
+  }
+
+  if (xSemaphoreTake(s_echo_pong_sem,
+                     pdMS_TO_TICKS(ESPNOW_ECHO_PING_TIMEOUT_MS)) != pdTRUE) {
+    s_echo_waiting = false;
+    char mac_str[18];
+    esp_now_core_mac_to_str(mac, mac_str, sizeof(mac_str));
+    ESP_LOGW(ECHO_PING_TAG,
+             "ESP-NOW PONG timeout to %s host_ts=%llu",
+             mac_str, (unsigned long long)timestamp_us);
+    return ESP_ERR_TIMEOUT;
+  }
+
+  s_echo_waiting = false;
+  if (result != NULL) {
+    result->echoed_us = timestamp_us;
+    result->esp_rtt_us = s_echo_esp_rtt_us;
+  }
+  return ESP_OK;
+}
+
 esp_err_t esp_now_comm_send_accel_stream(const uint8_t mac[CLIENT_MAC_LEN],
                                          uint32_t client_id, bool enable) {
   if (mac == NULL || !esp_now_core_is_master()) {
@@ -415,6 +519,12 @@ void esp_now_master_on_recv(const esp_now_recv_info_t *info, const uint8_t *data
     return;
   }
 
+  if (msg.which_payload == alox_EspNowMessage_echo_pong_tag) {
+    esp_now_core_ensure_peer(info->src_addr);
+    echo_ping_on_pong(info->src_addr, &msg.payload.echo_pong);
+    return;
+  }
+
   if (msg.type == alox_EspNowMessageType_ESPNOW_BATTERY_REPORT &&
       msg.which_payload != alox_EspNowMessage_battery_report_tag) {
     ESP_LOGW(TAG, "BATTERY_REPORT type but which=%u", msg.which_payload);
@@ -439,7 +549,10 @@ static void discover_task(void *param) {
            (unsigned)esp_now_core_wifi_channel());
 
   while (1) {
-    esp_now_core_send(ESPNOW_BCAST, &msg);
+    msg.payload.discover.master_ota_pending = ota_session_busy();
+    if (!ota_espnow_distribution_active()) {
+      esp_now_core_send_fast(ESPNOW_BCAST, &msg);
+    }
     vTaskDelay(pdMS_TO_TICKS(ESPNOW_DISCOVER_INTERVAL_MS));
   }
 }