71 lines
2.4 KiB
C
71 lines
2.4 KiB
C
#include "client_registry.h"
|
|
#include "cmd_espnow_echo_ping.h"
|
|
#include "esp_log.h"
|
|
#include "esp_now_comm.h"
|
|
#include "uart_cmd.h"
|
|
|
|
static const char *TAG = "[ECHO_PING]";
|
|
|
|
static void reply(bool success, uint32_t client_id, uint64_t timestamp_us,
|
|
uint32_t esp_rtt_us) {
|
|
alox_UartMessage response;
|
|
uart_cmd_init_response(&response, alox_MessageType_ESPNOW_ECHO_PING,
|
|
alox_UartMessage_espnow_echo_ping_response_tag);
|
|
response.payload.espnow_echo_ping_response.success = success;
|
|
response.payload.espnow_echo_ping_response.client_id = client_id;
|
|
response.payload.espnow_echo_ping_response.timestamp_us = timestamp_us;
|
|
response.payload.espnow_echo_ping_response.esp_rtt_us = esp_rtt_us;
|
|
uart_cmd_send(&response, TAG);
|
|
}
|
|
|
|
static void handle_espnow_echo_ping(const uint8_t *data, size_t len) {
|
|
alox_UartMessage uart_msg;
|
|
|
|
if (uart_cmd_decode(data, len, &uart_msg) != ESP_OK) {
|
|
ESP_LOGW(TAG, "decode failed");
|
|
reply(false, 0, 0, 0);
|
|
return;
|
|
}
|
|
|
|
const alox_EspNowEchoPingRequest *req = UART_CMD_REQ(
|
|
&uart_msg, alox_UartMessage_espnow_echo_ping_request_tag,
|
|
espnow_echo_ping_request);
|
|
if (req == NULL || req->client_id == 0) {
|
|
ESP_LOGW(TAG, "need client_id in request");
|
|
reply(false, 0, 0, 0);
|
|
return;
|
|
}
|
|
|
|
const client_info_t *client = client_registry_find_by_id(req->client_id);
|
|
if (client == NULL) {
|
|
ESP_LOGW(TAG, "client id %lu not in registry",
|
|
(unsigned long)req->client_id);
|
|
reply(false, req->client_id, 0, 0);
|
|
return;
|
|
}
|
|
|
|
ESP_LOGI(TAG, "UART request client_id=%lu host_ts=%llu",
|
|
(unsigned long)req->client_id,
|
|
(unsigned long long)req->timestamp_us);
|
|
|
|
esp_now_echo_ping_result_t ping_result = {0};
|
|
esp_err_t err =
|
|
esp_now_comm_echo_ping(client->mac, req->timestamp_us, &ping_result);
|
|
if (err != ESP_OK) {
|
|
ESP_LOGW(TAG, "echo ping to id=%lu failed: %s", (unsigned long)req->client_id,
|
|
esp_err_to_name(err));
|
|
reply(false, req->client_id, 0, 0);
|
|
return;
|
|
}
|
|
|
|
ESP_LOGI(TAG, "UART reply client_id=%lu host_ts=%llu esp_rtt_us=%lu",
|
|
(unsigned long)req->client_id,
|
|
(unsigned long long)ping_result.echoed_us,
|
|
(unsigned long)ping_result.esp_rtt_us);
|
|
reply(true, req->client_id, ping_result.echoed_us, ping_result.esp_rtt_us);
|
|
}
|
|
|
|
void cmd_espnow_echo_ping_register(void) {
|
|
uart_cmd_register(alox_MessageType_ESPNOW_ECHO_PING, handle_espnow_echo_ping);
|
|
}
|