powerpods/main/esp_now_slave.c

#include "esp_now_slave.h"
#include "bosch456.h"
#include "cmd_led_ring.h"
#include "esp_now_comm.h"
#include "esp_now_core.h"
#include "esp_now_proto.h"
#include "board_input.h"
#include "led_ring.h"
#include "ota_espnow.h"
#include "ota_uart.h"
#include "pod_reboot.h"
#include "pod_settings.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h"
#include <string.h>

#ifndef POWERPOD_FW_VERSION
#define POWERPOD_FW_VERSION 1u
#endif

#define ESPNOW_HEARTBEAT_INTERVAL_MS 1000
#define SLAVE_MASTER_LOST_MS (ESPNOW_HEARTBEAT_INTERVAL_MS * 5)
#define ESPNOW_ACCEL_INTERVAL_MS 16
#define ESPNOW_BATTERY_INTERVAL_MS 30000
#define SLAVE_BATTERY_AFTER_JOIN_MS 150

static const char *TAG = "[ESPNOW_S]";

static bool s_joined;
static bool s_accel_stream_enabled;
static bool s_tap_notify_single;
static bool s_tap_notify_double;
static bool s_tap_notify_triple;
static uint8_t s_master_mac[ESP_NOW_ETH_ALEN];
static uint32_t s_last_discover_ms;

typedef enum {
  SLAVE_TX_SLAVE_INFO = 1,
  SLAVE_TX_BATTERY,
} slave_tx_op_t;

static QueueHandle_t s_tx_queue;

static bool from_joined_master(const uint8_t *master_mac) {
  return s_joined && esp_now_core_mac_equal(master_mac, s_master_mac);
}

static void fill_presence(alox_EspNowSlavePresence *presence) {
  const uint8_t *own = esp_now_core_own_mac();
  presence->network = esp_now_core_network();
  presence->version = POWERPOD_FW_VERSION;
  presence->slave_id = own[5];
  presence->available = true;
  presence->used = false;
  esp_now_proto_setup_presence_encode(presence, own);
}

static void send_presence(const uint8_t *dest_mac, alox_EspNowMessageType type) {
  alox_EspNowMessage msg = alox_EspNowMessage_init_zero;
  alox_EspNowSlavePresence *presence = NULL;

  msg.type = type;
  if (type == alox_EspNowMessageType_ESPNOW_SLAVE_INFO) {
    msg.which_payload = alox_EspNowMessage_slave_info_tag;
    presence = &msg.payload.slave_info;
  } else {
    msg.which_payload = alox_EspNowMessage_heartbeat_tag;
    presence = &msg.payload.heartbeat;
  }
  fill_presence(presence);
  esp_now_core_send(dest_mac, &msg);
}

static esp_err_t send_accel_sample(const uint8_t *dest_mac, uint32_t slave_id,
                                   int16_t x, int16_t y, int16_t z) {
  alox_EspNowMessage msg = alox_EspNowMessage_init_zero;
  msg.type = alox_EspNowMessageType_ESPNOW_ACCEL_SAMPLE;
  msg.which_payload = alox_EspNowMessage_accel_sample_tag;
  msg.payload.accel_sample.slave_id = slave_id;
  msg.payload.accel_sample.x = x;
  msg.payload.accel_sample.y = y;
  msg.payload.accel_sample.z = z;
  return esp_now_core_send(dest_mac, &msg);
}

static esp_err_t send_tap_event(const uint8_t *dest_mac, uint32_t slave_id,
                                uint32_t kind) {
  alox_EspNowMessage msg = alox_EspNowMessage_init_zero;
  msg.type = alox_EspNowMessageType_ESPNOW_TAP_EVENT;
  msg.which_payload = alox_EspNowMessage_tap_event_tag;
  msg.payload.tap_event.slave_id = slave_id;
  msg.payload.tap_event.kind = kind;
  return esp_now_core_send(dest_mac, &msg);
}

static esp_err_t send_battery_report(const uint8_t *dest_mac,
                                     const alox_EspNowBatteryReport *report) {
  if (report == NULL) {
    return ESP_ERR_INVALID_ARG;
  }
  alox_EspNowMessage msg = alox_EspNowMessage_init_zero;
  msg.type = alox_EspNowMessageType_ESPNOW_BATTERY_REPORT;
  msg.which_payload = alox_EspNowMessage_battery_report_tag;
  msg.payload.battery_report = *report;
  return esp_now_core_send(dest_mac, &msg);
}

static void reset_join(void) {
  s_joined = false;
  s_accel_stream_enabled = false;
  memset(s_master_mac, 0, sizeof(s_master_mac));
  s_last_discover_ms = 0;
  if (s_tx_queue != NULL) {
    xQueueReset(s_tx_queue);
  }
}

static void queue_tx(slave_tx_op_t op) {
  if (s_tx_queue == NULL) {
    return;
  }
  if (xQueueSend(s_tx_queue, &op, 0) != pdTRUE) {
    ESP_LOGW(TAG, "tx queue full (op=%d)", (int)op);
  }
}

static void send_battery_to_master(void) {
  if (!s_joined) {
    return;
  }

  board_lipo_reading_t reading;
  board_input_read_lipo(&reading);

  alox_EspNowBatteryReport report = alox_EspNowBatteryReport_init_zero;
  report.client_id = esp_now_core_own_mac()[5];
  report.lipo1_valid = reading.lipo1_valid;
  report.lipo2_valid = reading.lipo2_valid;
  report.lipo1_mv = reading.lipo1_mv;
  report.lipo2_mv = reading.lipo2_mv;

  esp_err_t err = send_battery_report(s_master_mac, &report);
  if (err != ESP_OK) {
    ESP_LOGW(TAG, "battery report send failed id=%lu: %s",
             (unsigned long)report.client_id, esp_err_to_name(err));
  } else {
    ESP_LOGI(TAG, "battery report sent id=%lu L1=%s %lu mV L2=%s %lu mV",
             (unsigned long)report.client_id,
             report.lipo1_valid ? "ok" : "n/a",
             (unsigned long)report.lipo1_mv,
             report.lipo2_valid ? "ok" : "n/a",
             (unsigned long)report.lipo2_mv);
  }
}

esp_err_t esp_now_comm_send_ota_status(const uint8_t master_mac[CLIENT_MAC_LEN],
                                       uint32_t status, uint32_t bytes_written,
                                       uint32_t error) {
  if (master_mac == NULL || esp_now_core_is_master()) {
    return ESP_ERR_INVALID_STATE;
  }
  alox_EspNowMessage msg = alox_EspNowMessage_init_zero;
  msg.type = alox_EspNowMessageType_ESPNOW_OTA_STATUS;
  msg.which_payload = alox_EspNowMessage_ota_status_tag;
  msg.payload.ota_status.status = status;
  msg.payload.ota_status.bytes_written = bytes_written;
  msg.payload.ota_status.error = error;
  return esp_now_core_send_wait(master_mac, &msg);
}

bool esp_now_comm_get_master_mac(uint8_t mac_out[CLIENT_MAC_LEN]) {
  return esp_now_slave_get_master_mac(mac_out);
}

bool esp_now_slave_get_master_mac(uint8_t mac_out[CLIENT_MAC_LEN]) {
  if (mac_out == NULL || !s_joined) {
    return false;
  }
  memcpy(mac_out, s_master_mac, CLIENT_MAC_LEN);
  return true;
}

static void tx_task(void *param) {
  (void)param;
  slave_tx_op_t op;

  ESP_LOGI(TAG, "deferred tx task ready");

  while (1) {
    if (xQueueReceive(s_tx_queue, &op, portMAX_DELAY) != pdTRUE) {
      continue;
    }
    if (!s_joined) {
      continue;
    }
    switch (op) {
    case SLAVE_TX_SLAVE_INFO:
      send_presence(s_master_mac, alox_EspNowMessageType_ESPNOW_SLAVE_INFO);
      break;
    case SLAVE_TX_BATTERY:
      vTaskDelay(pdMS_TO_TICKS(SLAVE_BATTERY_AFTER_JOIN_MS));
      send_battery_to_master();
      break;
    default:
      break;
    }
  }
}

static void handle_unicast_test(const uint8_t *master_mac,
                                const alox_EspNowUnicastTest *test) {
  char mac_str[18];
  esp_now_core_mac_to_str(master_mac, mac_str, sizeof(mac_str));
  ESP_LOGI(TAG, "UNICAST TEST OK from master %s seq=%lu (joined=%d)", mac_str,
           (unsigned long)test->seq, (int)s_joined);
}

static void handle_restart(const uint8_t *master_mac,
                           const alox_EspNowRestart *req) {
  const uint8_t *own = esp_now_core_own_mac();
  uint32_t my_id = own[5];

  if (req->client_id != 0 && req->client_id != my_id) {
    return;
  }
  if (s_joined && !esp_now_core_mac_equal(master_mac, s_master_mac)) {
    return;
  }

  char mac_str[18];
  esp_now_core_mac_to_str(master_mac, mac_str, sizeof(mac_str));
  ESP_LOGI(TAG, "RESTART from master %s (id=%lu)", mac_str, (unsigned long)my_id);
  pod_schedule_restart();
}

static void handle_battery_query(const uint8_t *master_mac,
                                 const alox_EspNowBatteryQuery *query) {
  uint32_t my_id = esp_now_core_own_mac()[5];
  if (query->client_id != 0 && query->client_id != my_id) {
    return;
  }
  if (s_joined && !esp_now_core_mac_equal(master_mac, s_master_mac)) {
    return;
  }
  send_battery_to_master();
}

static void handle_led_ring(const uint8_t *master_mac,
                            const alox_EspNowLedRing *msg) {
  uint32_t my_id = esp_now_core_own_mac()[5];
  if (msg->client_id != 0 && msg->client_id != my_id) {
    return;
  }
  if (s_joined && !esp_now_core_mac_equal(master_mac, s_master_mac)) {
    return;
  }

  alox_LedRingProgressRequest req = alox_LedRingProgressRequest_init_zero;
  req.mode = msg->mode;
  req.progress = msg->progress;
  req.digit = msg->digit;
  req.r = msg->r;
  req.g = msg->g;
  req.b = msg->b;
  req.intensity = msg->intensity;
  req.blink_ms = msg->blink_ms;
  req.blink_count = msg->blink_count;

  char mac_str[18];
  esp_now_core_mac_to_str(master_mac, mac_str, sizeof(mac_str));
  ESP_LOGI(TAG, "LED_RING mode %lu from master %s (id=%lu)",
           (unsigned long)req.mode, mac_str, (unsigned long)my_id);
  cmd_led_ring_apply(&req);
}

static void handle_find_me(const uint8_t *master_mac, const alox_EspNowFindMe *req) {
  uint32_t my_id = esp_now_core_own_mac()[5];
  if (req->client_id != 0 && req->client_id != my_id) {
    return;
  }
  if (s_joined && !esp_now_core_mac_equal(master_mac, s_master_mac)) {
    return;
  }
  char mac_str[18];
  esp_now_core_mac_to_str(master_mac, mac_str, sizeof(mac_str));
  ESP_LOGI(TAG, "FIND_ME from master %s (id=%lu)", mac_str, (unsigned long)my_id);
  led_ring_find_me();
}

static void handle_accel_stream(const uint8_t *master_mac,
                                const alox_EspNowAccelStream *cfg) {
  uint32_t my_id = esp_now_core_own_mac()[5];
  if (cfg->client_id != 0 && cfg->client_id != my_id) {
    return;
  }
  if (s_joined && !esp_now_core_mac_equal(master_mac, s_master_mac)) {
    return;
  }
  s_accel_stream_enabled = cfg->enable;
  char mac_str[18];
  esp_now_core_mac_to_str(master_mac, mac_str, sizeof(mac_str));
  ESP_LOGI(TAG, "accel stream %s from master %s (id=%lu)",
           cfg->enable ? "on" : "off", mac_str, (unsigned long)my_id);
}

static void handle_tap_notify(const uint8_t *master_mac,
                              const alox_EspNowTapNotify *cfg) {
  uint32_t my_id = esp_now_core_own_mac()[5];
  if (cfg->client_id != 0 && cfg->client_id != my_id) {
    return;
  }
  if (s_joined && !esp_now_core_mac_equal(master_mac, s_master_mac)) {
    return;
  }
  s_tap_notify_single = cfg->single;
  s_tap_notify_double = cfg->double_tap;
  s_tap_notify_triple = cfg->triple;
  char mac_str[18];
  esp_now_core_mac_to_str(master_mac, mac_str, sizeof(mac_str));
  ESP_LOGI(TAG,
           "tap notify single=%d double=%d triple=%d from master %s (id=%lu)",
           cfg->single, cfg->double_tap, cfg->triple, mac_str,
           (unsigned long)my_id);
}

static void handle_accel_deadzone(const uint8_t *master_mac,
                                  const alox_EspNowAccelDeadzone *cfg) {
  uint32_t my_id = esp_now_core_own_mac()[5];
  if (cfg->client_id != 0 && cfg->client_id != my_id) {
    return;
  }
  if (s_joined && !esp_now_core_mac_equal(master_mac, s_master_mac)) {
    return;
  }
  char mac_str[18];
  esp_now_core_mac_to_str(master_mac, mac_str, sizeof(mac_str));
  ESP_LOGI(TAG,
           "accel deadzone from master %s: %lu LSB id=%lu (sensor %s)", mac_str,
           (unsigned long)cfg->deadzone, (unsigned long)my_id,
           bma456_is_ready() ? "ok" : "not installed");
  bma456_set_accel_deadzone(cfg->deadzone);
  if (pod_settings_save_accel_deadzone(cfg->deadzone) != ESP_OK) {
    ESP_LOGW(TAG, "deadzone %lu applied but not saved to NVS",
             (unsigned long)cfg->deadzone);
  }
}

static void handle_discover(const uint8_t *sender_mac,
                            const alox_EspNowDiscover *discover) {
  if (discover->network != esp_now_core_network()) {
    return;
  }

  uint32_t now = esp_now_core_now_ms();

  if (s_joined) {
    if (!esp_now_core_mac_equal(sender_mac, s_master_mac)) {
      return;
    }
    if ((now - s_last_discover_ms) <= SLAVE_MASTER_LOST_MS) {
      s_last_discover_ms = now;
      return;
    }
    ESP_LOGW(TAG, "master lost, rejoining");
    reset_join();
  }

  memcpy(s_master_mac, sender_mac, ESP_NOW_ETH_ALEN);
  s_joined = true;
  s_last_discover_ms = now;
  esp_now_core_ensure_peer(sender_mac);

  char mac_str[18];
  esp_now_core_mac_to_str(sender_mac, mac_str, sizeof(mac_str));
  ESP_LOGI(TAG, "joined network %u, master %s", (unsigned)discover->network,
           mac_str);

  queue_tx(SLAVE_TX_SLAVE_INFO);
  queue_tx(SLAVE_TX_BATTERY);
}

static void check_master_timeout(void) {
  if (!s_joined || s_last_discover_ms == 0) {
    return;
  }
  uint32_t now = esp_now_core_now_ms();
  if ((now - s_last_discover_ms) > SLAVE_MASTER_LOST_MS) {
    ESP_LOGW(TAG, "no master discover for %u ms, reconnecting",
             (unsigned)(now - s_last_discover_ms));
    reset_join();
  }
}

static void accel_stream_task(void *param) {
  (void)param;
  const uint8_t *own = esp_now_core_own_mac();

  ESP_LOGI(TAG, "accel stream task (interval %u ms)",
           (unsigned)ESPNOW_ACCEL_INTERVAL_MS);

  while (1) {
    vTaskDelay(pdMS_TO_TICKS(ESPNOW_ACCEL_INTERVAL_MS));
    if (!s_joined || !s_accel_stream_enabled || !bma456_is_ready()) {
      continue;
    }
    int16_t x = 0;
    int16_t y = 0;
    int16_t z = 0;
    if (bma456_read_accel(&x, &y, &z) != ESP_OK) {
      continue;
    }
    (void)send_accel_sample(s_master_mac, own[5], x, y, z);
  }
}

static void on_bma456_tap(bma456_tap_kind_t kind, void *ctx) {
  (void)ctx;
  if (!s_joined) {
    return;
  }
  bool enabled = false;
  switch (kind) {
  case BMA456_TAP_SINGLE:
    enabled = s_tap_notify_single;
    break;
  case BMA456_TAP_DOUBLE:
    enabled = s_tap_notify_double;
    break;
  case BMA456_TAP_TRIPLE:
    enabled = s_tap_notify_triple;
    break;
  default:
    return;
  }
  if (!enabled) {
    return;
  }
  (void)send_tap_event(s_master_mac, esp_now_core_own_mac()[5], (uint32_t)kind);
}

static void heartbeat_task(void *param) {
  (void)param;
  uint32_t last_battery_ms = 0;

  ESP_LOGI(TAG, "heartbeat task (interval %u ms)",
           (unsigned)ESPNOW_HEARTBEAT_INTERVAL_MS);

  while (1) {
    vTaskDelay(pdMS_TO_TICKS(ESPNOW_HEARTBEAT_INTERVAL_MS));
    check_master_timeout();
    if (!s_joined) {
      last_battery_ms = 0;
      continue;
    }
    send_presence(s_master_mac, alox_EspNowMessageType_ESPNOW_HEARTBEAT);
    uint32_t now = esp_now_core_now_ms();
    if (last_battery_ms == 0 ||
        (now - last_battery_ms) >= ESPNOW_BATTERY_INTERVAL_MS) {
      send_battery_to_master();
      last_battery_ms = now;
    }
  }
}

void esp_now_slave_on_recv(const esp_now_recv_info_t *info, const uint8_t *data,
                           int len) {
  if (info == NULL || data == NULL || len <= 0) {
    return;
  }

  alox_EspNowMessage msg = alox_EspNowMessage_init_zero;
  if (esp_now_proto_decode(data, (size_t)len, &msg) != ESP_OK) {
    ESP_LOGW(TAG, "decode failed (%d bytes)", len);
    return;
  }

  if (from_joined_master(info->src_addr)) {
    esp_now_core_ensure_peer(info->src_addr);
  }

  if (ota_uart_is_active()) {
    switch (msg.which_payload) {
    case alox_EspNowMessage_ota_start_tag:
    case alox_EspNowMessage_ota_payload_tag:
    case alox_EspNowMessage_ota_end_tag:
      if (!from_joined_master(info->src_addr)) {
        break;
      }
      if (msg.which_payload == alox_EspNowMessage_ota_start_tag) {
        ota_espnow_slave_on_start(info->src_addr, &msg.payload.ota_start);
      } else if (msg.which_payload == alox_EspNowMessage_ota_payload_tag) {
        ota_espnow_slave_on_payload(info->src_addr, &msg.payload.ota_payload);
      } else {
        ota_espnow_slave_on_end(info->src_addr);
      }
      break;
    default:
      break;
    }
    return;
  }

  switch (msg.which_payload) {
  case alox_EspNowMessage_discover_tag:
    handle_discover(info->src_addr, &msg.payload.discover);
    break;
  case alox_EspNowMessage_unicast_test_tag:
    if (from_joined_master(info->src_addr)) {
      handle_unicast_test(info->src_addr, &msg.payload.unicast_test);
    }
    break;
  case alox_EspNowMessage_accel_deadzone_tag:
    if (from_joined_master(info->src_addr)) {
      handle_accel_deadzone(info->src_addr, &msg.payload.accel_deadzone);
    }
    break;
  case alox_EspNowMessage_accel_stream_tag:
    if (from_joined_master(info->src_addr)) {
      handle_accel_stream(info->src_addr, &msg.payload.accel_stream);
    }
    break;
  case alox_EspNowMessage_tap_notify_tag:
    if (from_joined_master(info->src_addr)) {
      handle_tap_notify(info->src_addr, &msg.payload.tap_notify);
    }
    break;
  case alox_EspNowMessage_battery_query_tag:
    if (from_joined_master(info->src_addr)) {
      handle_battery_query(info->src_addr, &msg.payload.battery_query);
    }
    break;
  case alox_EspNowMessage_led_ring_tag:
    if (from_joined_master(info->src_addr)) {
      handle_led_ring(info->src_addr, &msg.payload.led_ring);
    }
    break;
  case alox_EspNowMessage_find_me_tag:
    if (from_joined_master(info->src_addr)) {
      handle_find_me(info->src_addr, &msg.payload.find_me);
    }
    break;
  case alox_EspNowMessage_restart_tag:
    if (from_joined_master(info->src_addr)) {
      handle_restart(info->src_addr, &msg.payload.restart);
    }
    break;
  default:
    ESP_LOGW(TAG, "unhandled which=%u type=%u", msg.which_payload,
             (unsigned)msg.type);
    break;
  }
}

esp_err_t esp_now_slave_start(void) {
  reset_join();

  s_tx_queue = xQueueCreate(4, sizeof(slave_tx_op_t));
  if (s_tx_queue == NULL) {
    ESP_LOGE(TAG, "failed to create tx queue");
    return ESP_ERR_NO_MEM;
  }
  if (xTaskCreate(tx_task, "espnow_stx", 4096, NULL, 5, NULL) != pdPASS) {
    ESP_LOGE(TAG, "failed to create tx task");
    return ESP_FAIL;
  }
  if (xTaskCreate(heartbeat_task, "espnow_hb", 4096, NULL, 4, NULL) != pdPASS) {
    ESP_LOGE(TAG, "failed to create heartbeat task");
    return ESP_FAIL;
  }
  if (xTaskCreate(accel_stream_task, "espnow_accel", 4096, NULL, 5, NULL) !=
      pdPASS) {
    ESP_LOGE(TAG, "failed to create accel stream task");
    return ESP_FAIL;
  }
  ota_espnow_slave_init();
  bma456_set_tap_handler(on_bma456_tap, NULL);
  return ESP_OK;
}