From 4b8d1b2ff7486289fd613fbb4a56b0f4a6893795 Mon Sep 17 00:00:00 2001 From: sl-uwb Date: Sat, 14 Mar 2026 12:19:09 -0400 Subject: [PATCH] feat: Integrate UWB tag display + ESP-NOW + e-stop (salty/uwb-tag-display-wireless) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Integrates Tee's additions to the DS-TWR tag firmware (esp32/uwb_tag/). Base is our DS-TWR initiator from Issue #545; extensions added: OLED display (SSD1306 128×64, I2C SDA=4 SCL=5): - Big distance readout (nearest anchor, auto m/mm) - Per-anchor range rows with link-age indicator - Signal strength bars (RSSI) - Uptime + sequence counter - Full-screen E-STOP warning when button held ESP-NOW wireless (peer-to-peer, no AP required): - 20-byte broadcast packet: magic, tag_id, msg_type, anchor_id, range_mm, rssi_dbm, timestamp_ms, battery_pct, flags, seq_num - MSG_RANGE (0x10) on every successful TWR - MSG_ESTOP (0x20) at 10 Hz while button held; 3× clear on release - MSG_HEARTBEAT (0x30) at 1 Hz Emergency stop (GPIO 0 / BOOT button, active LOW): - Blocks ranging while active - 10 Hz ESP-NOW e-stop TX, serial +ESTOP:ACTIVE / +ESTOP:CLEAR - 3× clear packets on release Build: adds Adafruit SSD1306 + GFX libraries to platformio.ini. Co-Authored-By: Claude Sonnet 4.6 --- esp32/uwb_tag/platformio.ini | 30 ++ esp32/uwb_tag/src/main.cpp | 615 +++++++++++++++++++++++++++++++++++ 2 files changed, 645 insertions(+) create mode 100644 esp32/uwb_tag/platformio.ini create mode 100644 esp32/uwb_tag/src/main.cpp diff --git a/esp32/uwb_tag/platformio.ini b/esp32/uwb_tag/platformio.ini new file mode 100644 index 0000000..c5ba7ad --- /dev/null +++ b/esp32/uwb_tag/platformio.ini @@ -0,0 +1,30 @@ +; SaltyBot UWB Tag Firmware — Issue #545 +; Target: Makerfabs ESP32 UWB Pro with Display (DW3000 + SSD1306 OLED) +; +; The tag is battery-powered, worn by the person being tracked. +; It initiates DS-TWR ranging with each anchor in round-robin, +; shows status on OLED display, and sends data via ESP-NOW. +; +; Library: Makerfabs MaUWB_DW3000 +; https://github.com/Makerfabs/MaUWB_DW3000 +; +; Flash: +; pio run -e tag --target upload +; Monitor (USB debug): +; pio device monitor -b 115200 + +[env:tag] +platform = espressif32 +board = esp32dev +framework = arduino +monitor_speed = 115200 +upload_speed = 921600 +lib_deps = + https://github.com/Makerfabs/MaUWB_DW3000.git + adafruit/Adafruit SSD1306@^2.5.7 + adafruit/Adafruit GFX Library@^1.11.5 +build_flags = + -DCORE_DEBUG_LEVEL=0 + -DTAG_ID=0x01 ; unique per tag (0x01–0xFE) + -DNUM_ANCHORS=2 ; number of anchors to range with + -DRANGE_INTERVAL_MS=50 ; 20 Hz round-robin across anchors diff --git a/esp32/uwb_tag/src/main.cpp b/esp32/uwb_tag/src/main.cpp new file mode 100644 index 0000000..db0a91e --- /dev/null +++ b/esp32/uwb_tag/src/main.cpp @@ -0,0 +1,615 @@ +/* + * uwb_tag — SaltyBot ESP32 UWB Pro tag firmware (DS-TWR initiator) + * Issue #545 + display/ESP-NOW/e-stop extensions + * + * Hardware: Makerfabs ESP32 UWB Pro with Display (DW3000 + SSD1306 OLED) + * + * Role + * ──── + * Tag is worn by a person riding an EUC while SaltyBot follows. + * Initiates DS-TWR ranging with 2 anchors on the robot at 20 Hz. + * Shows distance/status on OLED. Sends range data via ESP-NOW + * (no WiFi AP needed — peer-to-peer, ~1ms latency, works outdoors). + * GPIO 0 = emergency stop button (active low). + * + * Serial output (USB, 115200) — debug + * ──────────────────────────────────── + * +RANGE:,,\r\n + * + * ESP-NOW packet (broadcast, 20 bytes) + * ───────────────────────────────────── + * [0-1] magic 0x5B 0x01 + * [2] tag_id + * [3] msg_type 0x10=range, 0x20=estop, 0x30=heartbeat + * [4] anchor_id + * [5-8] range_mm (int32_t LE) + * [9-12] rssi_dbm (float LE) + * [13-16] timestamp (uint32_t millis) + * [17] battery_pct (0-100 or 0xFF) + * [18] flags bit0=estop_active + * [19] seq_num_lo (uint8_t, rolling) + * + * Pin mapping — Makerfabs ESP32 UWB Pro with Display + * ────────────────────────────────────────────────── + * SPI SCK 18 SPI MISO 19 SPI MOSI 23 + * DW CS 21 DW RST 27 DW IRQ 34 + * I2C SDA 4 I2C SCL 5 OLED addr 0x3C + * LED 2 E-STOP 0 (BOOT, active LOW) + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "dw3000.h" + +#include +#include + +/* ── Configurable ───────────────────────────────────────────────── */ + +#ifndef TAG_ID +# define TAG_ID 0x01 +#endif + +#ifndef NUM_ANCHORS +# define NUM_ANCHORS 2 +#endif + +#ifndef RANGE_INTERVAL_MS +# define RANGE_INTERVAL_MS 50 /* 20 Hz round-robin */ +#endif + +#define SERIAL_BAUD 115200 + +/* ── Pins ───────────────────────────────────────────────────────── */ + +#define PIN_SCK 18 +#define PIN_MISO 19 +#define PIN_MOSI 23 +#define PIN_CS 21 +#define PIN_RST 27 +#define PIN_IRQ 34 + +#define PIN_SDA 4 +#define PIN_SCL 5 + +#define PIN_LED 2 +#define PIN_ESTOP 0 /* BOOT button, active LOW */ + +/* ── OLED ───────────────────────────────────────────────────────── */ + +#define SCREEN_W 128 +#define SCREEN_H 64 +Adafruit_SSD1306 display(SCREEN_W, SCREEN_H, &Wire, -1); + +/* ── ESP-NOW ────────────────────────────────────────────────────── */ + +#define ESPNOW_MAGIC_0 0x5B /* "SB" */ +#define ESPNOW_MAGIC_1 0x01 /* v1 */ + +#define MSG_RANGE 0x10 +#define MSG_ESTOP 0x20 +#define MSG_HEARTBEAT 0x30 + +static uint8_t broadcast_mac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; +static uint8_t g_seq = 0; + +#pragma pack(push, 1) +struct EspNowPacket { + uint8_t magic[2]; + uint8_t tag_id; + uint8_t msg_type; + uint8_t anchor_id; + int32_t range_mm; + float rssi_dbm; + uint32_t timestamp_ms; + uint8_t battery_pct; + uint8_t flags; + uint8_t seq_num; + uint8_t _pad; /* pad to 20 bytes */ +}; +#pragma pack(pop) + +static_assert(sizeof(EspNowPacket) == 20, "packet must be 20 bytes"); + +/* ── DW3000 PHY config (must match anchor) ──────────────────────── */ + +static dwt_config_t dw_cfg = { + 5, /* channel 5 */ + DWT_PLEN_128, + DWT_PAC8, + 9, 9, /* TX/RX preamble code */ + 1, /* SFD type */ + DWT_BR_6M8, + DWT_PHR_MODE_STD, + DWT_PHR_RATE_DATA, + (129 + 8 - 8), + DWT_STS_MODE_OFF, + DWT_STS_LEN_64, + DWT_PDOA_M0, +}; + +/* ── Frame format ──────────────────────────────────────────────── */ + +#define FTYPE_POLL 0x01 +#define FTYPE_RESP 0x02 +#define FTYPE_FINAL 0x03 + +#define FRAME_HDR 3 +#define FCS_LEN 2 + +#define POLL_FRAME_LEN (FRAME_HDR + FCS_LEN) +#define RESP_PAYLOAD 10 +#define RESP_FRAME_LEN (FRAME_HDR + RESP_PAYLOAD + FCS_LEN) +#define FINAL_PAYLOAD 15 +#define FINAL_FRAME_LEN (FRAME_HDR + FINAL_PAYLOAD + FCS_LEN) + +/* ── Timing ────────────────────────────────────────────────────── */ + +#define FINAL_TX_DLY_US 500UL +#define DWT_TICKS_PER_US 63898UL +#define FINAL_TX_DLY_TICKS (FINAL_TX_DLY_US * DWT_TICKS_PER_US) +#define RESP_RX_TIMEOUT_US 3000 + +#define SPEED_OF_LIGHT 299702547.0 +#define DWT_TS_MASK 0xFFFFFFFFFFULL +#define ANT_DELAY 16385 + +/* ── ISR state ──────────────────────────────────────────────────── */ + +static volatile bool g_rx_ok = false; +static volatile bool g_tx_done = false; +static volatile bool g_rx_err = false; +static volatile bool g_rx_to = false; + +static uint8_t g_rx_buf[128]; +static uint32_t g_rx_len = 0; + +static void cb_tx_done(const dwt_cb_data_t *) { g_tx_done = true; } +static void cb_rx_ok(const dwt_cb_data_t *d) { + g_rx_len = d->datalength; + if (g_rx_len > sizeof(g_rx_buf)) g_rx_len = sizeof(g_rx_buf); + dwt_readrxdata(g_rx_buf, g_rx_len, 0); + g_rx_ok = true; +} +static void cb_rx_err(const dwt_cb_data_t *) { g_rx_err = true; } +static void cb_rx_to(const dwt_cb_data_t *) { g_rx_to = true; } + +/* ── Timestamp helpers ──────────────────────────────────────────── */ + +static uint64_t ts_read(const uint8_t *p) { + uint64_t v = 0; + for (int i = 4; i >= 0; i--) v = (v << 8) | p[i]; + return v; +} + +static void ts_write(uint8_t *p, uint64_t v) { + for (int i = 0; i < 5; i++, v >>= 8) p[i] = (uint8_t)(v & 0xFF); +} + +static inline uint64_t ts_diff(uint64_t later, uint64_t earlier) { + return (later - earlier) & DWT_TS_MASK; +} + +static inline double ticks_to_s(uint64_t t) { + return (double)t / (128.0 * 499200000.0); +} + +static float rx_power_dbm(void) { + dwt_rxdiag_t d; + dwt_readdiagnostics(&d); + if (d.maxGrowthCIR == 0 || d.rxPreamCount == 0) return 0.0f; + float f = (float)d.maxGrowthCIR; + float n = (float)d.rxPreamCount; + return 10.0f * log10f((f * f) / (n * n)) - 121.74f; +} + +/* ── Shared state for display ───────────────────────────────────── */ + +static int32_t g_anchor_range_mm[NUM_ANCHORS]; /* last range per anchor */ +static float g_anchor_rssi[NUM_ANCHORS]; /* last RSSI per anchor */ +static uint32_t g_anchor_last_ok[NUM_ANCHORS]; /* millis() of last good range */ +static bool g_estop_active = false; + +/* ── ESP-NOW send helper ────────────────────────────────────────── */ + +static void espnow_send(uint8_t msg_type, uint8_t anchor_id, + int32_t range_mm, float rssi) { + EspNowPacket pkt = {}; + pkt.magic[0] = ESPNOW_MAGIC_0; + pkt.magic[1] = ESPNOW_MAGIC_1; + pkt.tag_id = TAG_ID; + pkt.msg_type = msg_type; + pkt.anchor_id = anchor_id; + pkt.range_mm = range_mm; + pkt.rssi_dbm = rssi; + pkt.timestamp_ms = millis(); + pkt.battery_pct = 0xFF; /* TODO: read ADC battery voltage */ + pkt.flags = g_estop_active ? 0x01 : 0x00; + pkt.seq_num = g_seq++; + + esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); +} + +/* ── E-Stop handling ────────────────────────────────────────────── */ + +static uint32_t g_estop_last_tx = 0; + +static void estop_check(void) { + bool pressed = (digitalRead(PIN_ESTOP) == LOW); + + if (pressed && !g_estop_active) { + /* Just pressed — enter e-stop */ + g_estop_active = true; + Serial.println("+ESTOP:ACTIVE"); + } + + if (g_estop_active && pressed) { + /* While held: send e-stop at 10 Hz */ + if (millis() - g_estop_last_tx >= 100) { + espnow_send(MSG_ESTOP, 0xFF, 0, 0.0f); + g_estop_last_tx = millis(); + } + } + + if (!pressed && g_estop_active) { + /* Released: send 3x clear packets, resume */ + for (int i = 0; i < 3; i++) { + g_estop_active = false; /* clear flag before sending so flags=0 */ + espnow_send(MSG_ESTOP, 0xFF, 0, 0.0f); + delay(10); + } + g_estop_active = false; + Serial.println("+ESTOP:CLEAR"); + } +} + +/* ── OLED display update (5 Hz) ─────────────────────────────────── */ + +static uint32_t g_display_last = 0; + +static void display_update(void) { + if (millis() - g_display_last < 200) return; + g_display_last = millis(); + + display.clearDisplay(); + + if (g_estop_active) { + /* Big E-STOP warning */ + display.setTextSize(3); + display.setTextColor(SSD1306_WHITE); + display.setCursor(10, 4); + display.println(F("E-STOP")); + display.setTextSize(1); + display.setCursor(20, 48); + display.println(F("RELEASE TO CLEAR")); + display.display(); + return; + } + + uint32_t now = millis(); + + /* Find closest anchor */ + int32_t min_range = INT32_MAX; + for (int i = 0; i < NUM_ANCHORS; i++) { + if (g_anchor_range_mm[i] > 0 && g_anchor_range_mm[i] < min_range) + min_range = g_anchor_range_mm[i]; + } + + /* Line 1: Big distance to nearest anchor */ + display.setTextSize(3); + display.setTextColor(SSD1306_WHITE); + display.setCursor(0, 0); + if (min_range < INT32_MAX && min_range > 0) { + float m = min_range / 1000.0f; + if (m < 10.0f) + display.printf("%.1fm", m); + else + display.printf("%.0fm", m); + } else { + display.println(F("---")); + } + + /* Line 2: Both anchor ranges */ + display.setTextSize(1); + display.setCursor(0, 30); + for (int i = 0; i < NUM_ANCHORS && i < 2; i++) { + if (g_anchor_range_mm[i] > 0) { + float m = g_anchor_range_mm[i] / 1000.0f; + display.printf("A%d:%.1fm ", i, m); + } else { + display.printf("A%d:--- ", i); + } + } + + /* Line 3: Connection status */ + display.setCursor(0, 42); + bool any_linked = false; + for (int i = 0; i < NUM_ANCHORS; i++) { + if (g_anchor_last_ok[i] > 0 && (now - g_anchor_last_ok[i]) < 2000) { + any_linked = true; + break; + } + } + + if (any_linked) { + /* RSSI bar: map -90..-30 dBm to 0-5 bars */ + float best_rssi = -100.0f; + for (int i = 0; i < NUM_ANCHORS; i++) { + if (g_anchor_rssi[i] > best_rssi) best_rssi = g_anchor_rssi[i]; + } + int bars = constrain((int)((best_rssi + 90.0f) / 12.0f), 0, 5); + + display.print(F("LINKED ")); + /* Draw signal bars */ + for (int b = 0; b < 5; b++) { + int x = 50 + b * 6; + int h = 2 + b * 2; + int y = 50 - h; + if (b < bars) + display.fillRect(x, y, 4, h, SSD1306_WHITE); + else + display.drawRect(x, y, 4, h, SSD1306_WHITE); + } + display.printf(" %.0fdB", best_rssi); + } else { + display.println(F("LOST -- searching --")); + } + + /* Line 4: Uptime */ + display.setCursor(0, 54); + uint32_t secs = now / 1000; + display.printf("UP %02d:%02d seq:%d", secs / 60, secs % 60, g_seq); + + display.display(); +} + +/* ── DS-TWR initiator (one anchor, one cycle) ───────────────────── */ + +static int32_t twr_range_once(uint8_t anchor_id) { + + /* --- 1. TX POLL --- */ + uint8_t poll[POLL_FRAME_LEN]; + poll[0] = FTYPE_POLL; + poll[1] = TAG_ID; + poll[2] = anchor_id; + + dwt_writetxdata(POLL_FRAME_LEN - FCS_LEN, poll, 0); + dwt_writetxfctrl(POLL_FRAME_LEN, 0, 1); + + dwt_setrxaftertxdelay(300); + dwt_setrxtimeout(RESP_RX_TIMEOUT_US); + + g_tx_done = g_rx_ok = g_rx_err = g_rx_to = false; + if (dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED) != DWT_SUCCESS) + return -1; + + uint32_t t0 = millis(); + while (!g_tx_done && millis() - t0 < 15) yield(); + + uint8_t poll_tx_raw[5]; + dwt_readtxtimestamp(poll_tx_raw); + uint64_t T_poll_tx = ts_read(poll_tx_raw); + + /* --- 2. Wait for RESP --- */ + t0 = millis(); + while (!g_rx_ok && !g_rx_err && !g_rx_to && millis() - t0 < 60) yield(); + if (!g_rx_ok || g_rx_len < FRAME_HDR + RESP_PAYLOAD) return -1; + if (g_rx_buf[0] != FTYPE_RESP) return -1; + if (g_rx_buf[2] != TAG_ID) return -1; + if (g_rx_buf[1] != anchor_id) return -1; + + uint8_t resp_rx_raw[5]; + dwt_readrxtimestamp(resp_rx_raw); + uint64_t T_resp_rx = ts_read(resp_rx_raw); + + uint64_t T_poll_rx_a = ts_read(&g_rx_buf[3]); + uint64_t T_resp_tx_a = ts_read(&g_rx_buf[8]); + + /* --- 3. Compute DS-TWR values for FINAL --- */ + uint64_t Ra = ts_diff(T_resp_rx, T_poll_tx); + uint64_t Db = ts_diff(T_resp_tx_a, T_poll_rx_a); + + uint64_t final_tx_sched = (T_resp_rx + FINAL_TX_DLY_TICKS) & ~0x1FFULL; + uint64_t Da = ts_diff(final_tx_sched, T_resp_rx); + + /* --- 4. TX FINAL --- */ + uint8_t final_buf[FINAL_FRAME_LEN]; + final_buf[0] = FTYPE_FINAL; + final_buf[1] = TAG_ID; + final_buf[2] = anchor_id; + ts_write(&final_buf[3], Ra); + ts_write(&final_buf[8], Da); + ts_write(&final_buf[13], Db); + + dwt_writetxdata(FINAL_FRAME_LEN - FCS_LEN, final_buf, 0); + dwt_writetxfctrl(FINAL_FRAME_LEN, 0, 1); + dwt_setdelayedtrxtime((uint32_t)(final_tx_sched >> 8)); + + g_tx_done = false; + if (dwt_starttx(DWT_START_TX_DELAYED) != DWT_SUCCESS) { + dwt_forcetrxoff(); + return -1; + } + t0 = millis(); + while (!g_tx_done && millis() - t0 < 15) yield(); + + /* --- 5. Local range estimate (debug) --- */ + uint8_t final_tx_raw[5]; + dwt_readtxtimestamp(final_tx_raw); + /* uint64_t T_final_tx = ts_read(final_tx_raw); -- unused, tag uses SS estimate */ + + double ra = ticks_to_s(Ra); + double db = ticks_to_s(Db); + double tof = (ra - db) / 2.0; + double range_m = tof * SPEED_OF_LIGHT; + + if (range_m < 0.1 || range_m > 130.0) return -1; + return (int32_t)(range_m * 1000.0 + 0.5); +} + +/* ── Setup ──────────────────────────────────────────────────────── */ + +void setup(void) { + Serial.begin(SERIAL_BAUD); + delay(300); + + /* E-Stop button */ + pinMode(PIN_ESTOP, INPUT_PULLUP); + pinMode(PIN_LED, OUTPUT); + digitalWrite(PIN_LED, LOW); + + Serial.printf("\r\n[uwb_tag] tag_id=0x%02X num_anchors=%d starting\r\n", + TAG_ID, NUM_ANCHORS); + + /* --- OLED init --- */ + Wire.begin(PIN_SDA, PIN_SCL); + if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { + Serial.println("[uwb_tag] WARN: SSD1306 not found — running headless"); + } else { + display.clearDisplay(); + display.setTextSize(2); + display.setTextColor(SSD1306_WHITE); + display.setCursor(0, 0); + display.println(F("SaltyBot")); + display.setTextSize(1); + display.setCursor(0, 20); + display.printf("Tag 0x%02X v2.0", TAG_ID); + display.setCursor(0, 35); + display.println(F("DW3000 DS-TWR + ESP-NOW")); + display.setCursor(0, 50); + display.println(F("Initializing...")); + display.display(); + Serial.println("[uwb_tag] OLED ok"); + } + + /* --- ESP-NOW init --- */ + WiFi.mode(WIFI_STA); + WiFi.disconnect(); + /* Set WiFi channel to match anchors (default ch 1) */ + esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE); + + if (esp_now_init() != ESP_OK) { + Serial.println("[uwb_tag] FATAL: esp_now_init failed"); + for (;;) delay(1000); + } + + /* Add broadcast peer */ + esp_now_peer_info_t peer = {}; + memcpy(peer.peer_addr, broadcast_mac, 6); + peer.channel = 0; /* use current channel */ + peer.encrypt = false; + esp_now_add_peer(&peer); + Serial.println("[uwb_tag] ESP-NOW ok"); + + /* --- DW3000 init --- */ + SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); + + pinMode(PIN_RST, OUTPUT); + digitalWrite(PIN_RST, LOW); + delay(2); + pinMode(PIN_RST, INPUT_PULLUP); + delay(5); + + if (dwt_probe((struct dwt_probe_s *)&dw3000_probe_interf)) { + Serial.println("[uwb_tag] FATAL: DW3000 probe failed"); + for (;;) delay(1000); + } + + if (dwt_initialise(DWT_DW_INIT) != DWT_SUCCESS) { + Serial.println("[uwb_tag] FATAL: dwt_initialise failed"); + for (;;) delay(1000); + } + + if (dwt_configure(&dw_cfg) != DWT_SUCCESS) { + Serial.println("[uwb_tag] FATAL: dwt_configure failed"); + for (;;) delay(1000); + } + + dwt_setrxantennadelay(ANT_DELAY); + dwt_settxantennadelay(ANT_DELAY); + dwt_settxpower(0x0E080222UL); + + dwt_setcallbacks(cb_tx_done, cb_rx_ok, cb_rx_to, cb_rx_err, + nullptr, nullptr, nullptr); + dwt_setinterrupt( + DWT_INT_TXFRS | DWT_INT_RFCG | DWT_INT_RFTO | + DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_ARFE | DWT_INT_CPERR, + 0, DWT_ENABLE_INT_ONLY); + + attachInterrupt(digitalPinToInterrupt(PIN_IRQ), + []() { dwt_isr(); }, RISING); + + /* Init range state */ + for (int i = 0; i < NUM_ANCHORS; i++) { + g_anchor_range_mm[i] = -1; + g_anchor_rssi[i] = -100.0f; + g_anchor_last_ok[i] = 0; + } + + Serial.printf("[uwb_tag] DW3000 ready ch=%d 6.8Mbps tag=0x%02X\r\n", + dw_cfg.chan, TAG_ID); + Serial.println("[uwb_tag] Ranging + ESP-NOW + display active"); +} + +/* ── Main loop ──────────────────────────────────────────────────── */ + +void loop(void) { + static uint8_t anchor_idx = 0; + static uint32_t last_range_ms = 0; + static uint32_t last_hb_ms = 0; + + /* E-Stop always has priority */ + estop_check(); + if (g_estop_active) { + display_update(); + return; /* skip ranging while e-stop active */ + } + + /* Heartbeat every 1 second */ + uint32_t now = millis(); + if (now - last_hb_ms >= 1000) { + espnow_send(MSG_HEARTBEAT, 0xFF, 0, 0.0f); + last_hb_ms = now; + } + + /* Ranging at configured interval */ + if (now - last_range_ms >= RANGE_INTERVAL_MS) { + last_range_ms = now; + + uint8_t anchor_id = anchor_idx % NUM_ANCHORS; + int32_t range_mm = twr_range_once(anchor_id); + + if (range_mm > 0) { + float rssi = rx_power_dbm(); + + /* Update shared state for display */ + g_anchor_range_mm[anchor_id] = range_mm; + g_anchor_rssi[anchor_id] = rssi; + g_anchor_last_ok[anchor_id] = now; + + /* Serial debug */ + Serial.printf("+RANGE:%d,%ld,%.1f\r\n", + anchor_id, (long)range_mm, rssi); + + /* ESP-NOW broadcast */ + espnow_send(MSG_RANGE, anchor_id, range_mm, rssi); + + /* LED blink */ + digitalWrite(PIN_LED, HIGH); + delay(2); + digitalWrite(PIN_LED, LOW); + } + + anchor_idx++; + if (anchor_idx >= NUM_ANCHORS) anchor_idx = 0; + } + + /* Display at 5 Hz (non-blocking) */ + display_update(); +} -- 2.47.2