From a9e1b9fae568605e1ff284e6a09393bb996bd8e3 Mon Sep 17 00:00:00 2001 From: salty Date: Sat, 14 Mar 2026 12:12:31 -0400 Subject: [PATCH] feat: add OLED display, ESP-NOW wireless, and E-Stop to UWB tag Tag firmware (esp32/uwb_tag): - SSD1306 128x64 OLED: shows distance, anchor ranges, RSSI bars, link status - ESP-NOW broadcast: sends range/heartbeat/estop packets (20 bytes, peer-to-peer) - Emergency stop: GPIO 0 (BOOT), active LOW, 10Hz TX while held, 3x clear on release - Display updates at 5Hz, ranging still at 20Hz round-robin - Added Adafruit SSD1306 + GFX lib_deps to platformio.ini Anchor firmware (esp32/uwb_anchor): - ESP-NOW receiver: captures tag packets via ISR ring buffer - Forwards to Jetson serial as +ESPNOW: and +ESTOP: lines - E-STOP packets get priority immediate output - Zero impact on existing TWR ranging loop For Makerfabs ESP32 UWB Pro with Display (DW3000 chip). --- esp32/uwb_anchor/src/main.cpp | 88 +++++++ esp32/uwb_tag/platformio.ini | 7 +- esp32/uwb_tag/src/main.cpp | 476 ++++++++++++++++++++++++++-------- 3 files changed, 457 insertions(+), 114 deletions(-) diff --git a/esp32/uwb_anchor/src/main.cpp b/esp32/uwb_anchor/src/main.cpp index e022ee2..eb484d6 100644 --- a/esp32/uwb_anchor/src/main.cpp +++ b/esp32/uwb_anchor/src/main.cpp @@ -42,6 +42,9 @@ #include #include #include +#include +#include +#include #include "dw3000.h" // Makerfabs MaUWB_DW3000 library /* ── Configurable ───────────────────────────────────────────────── */ @@ -52,6 +55,47 @@ #define SERIAL_BAUD 115200 +/* ── ESP-NOW packet format (shared with tag firmware) ──────────── */ + +#define ESPNOW_MAGIC_0 0x5B +#define ESPNOW_MAGIC_1 0x01 +#define MSG_RANGE 0x10 +#define MSG_ESTOP 0x20 +#define MSG_HEARTBEAT 0x30 + +#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; +}; +#pragma pack(pop) + +/* Ring buffer for received ESP-NOW packets (ISR → main loop) */ +#define ESPNOW_QUEUE_SIZE 8 +static volatile EspNowPacket g_espnow_queue[ESPNOW_QUEUE_SIZE]; +static volatile int g_espnow_head = 0; +static volatile int g_espnow_tail = 0; + +static void IRAM_ATTR espnow_rx_cb(const uint8_t *mac, const uint8_t *data, int len) { + if (len < (int)sizeof(EspNowPacket)) return; + const EspNowPacket *pkt = (const EspNowPacket *)data; + if (pkt->magic[0] != ESPNOW_MAGIC_0 || pkt->magic[1] != ESPNOW_MAGIC_1) return; + + int next = (g_espnow_head + 1) % ESPNOW_QUEUE_SIZE; + if (next == g_espnow_tail) return; /* queue full, drop */ + g_espnow_queue[g_espnow_head] = *pkt; + g_espnow_head = next; +} + /* ── Pin map (Makerfabs ESP32 UWB Pro) ─────────────────────────── */ #define PIN_SCK 18 @@ -353,6 +397,37 @@ static void twr_cycle(void) { Serial.println(g_last_range_line); } +/* ── ESP-NOW packet processing (main loop context) ──────────────── */ + +static void espnow_process(void) { + while (g_espnow_tail != g_espnow_head) { + const EspNowPacket &pkt = (const EspNowPacket &)g_espnow_queue[g_espnow_tail]; + g_espnow_tail = (g_espnow_tail + 1) % ESPNOW_QUEUE_SIZE; + + /* E-STOP gets priority line */ + if (pkt.msg_type == MSG_ESTOP) { + if (pkt.flags & 0x01) { + Serial.printf("+ESTOP:%d\r\n", pkt.tag_id); + } else { + Serial.printf("+ESTOP_CLEAR:%d\r\n", pkt.tag_id); + } + continue; + } + + /* Forward all other packets */ + Serial.printf("+ESPNOW:%d,%02X,%d,%ld,%.1f,%lu,%d,%02X,%d\r\n", + pkt.tag_id, + pkt.msg_type, + pkt.anchor_id, + (long)pkt.range_mm, + pkt.rssi_dbm, + (unsigned long)pkt.timestamp_ms, + pkt.battery_pct, + pkt.flags, + pkt.seq_num); + } +} + /* ── Arduino setup ──────────────────────────────────────────────── */ void setup(void) { @@ -361,6 +436,18 @@ void setup(void) { Serial.printf("\r\n[uwb_anchor] anchor_id=%d starting\r\n", ANCHOR_ID); + /* --- ESP-NOW receiver init --- */ + WiFi.mode(WIFI_STA); + WiFi.disconnect(); + esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE); + + if (esp_now_init() == ESP_OK) { + esp_now_register_recv_cb(espnow_rx_cb); + Serial.println("[uwb_anchor] ESP-NOW receiver ok"); + } else { + Serial.println("[uwb_anchor] WARN: ESP-NOW init failed — tag relay disabled"); + } + SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); /* Hardware reset */ @@ -409,5 +496,6 @@ void setup(void) { void loop(void) { serial_poll(); + espnow_process(); /* forward tag ESP-NOW packets to Jetson via serial */ twr_cycle(); } diff --git a/esp32/uwb_tag/platformio.ini b/esp32/uwb_tag/platformio.ini index 590eee0..c5ba7ad 100644 --- a/esp32/uwb_tag/platformio.ini +++ b/esp32/uwb_tag/platformio.ini @@ -1,8 +1,9 @@ ; SaltyBot UWB Tag Firmware — Issue #545 -; Target: Makerfabs ESP32 UWB Pro (DW3000 chip) +; 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. +; 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 @@ -20,6 +21,8 @@ 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) diff --git a/esp32/uwb_tag/src/main.cpp b/esp32/uwb_tag/src/main.cpp index 6d79e15..db0a91e 100644 --- a/esp32/uwb_tag/src/main.cpp +++ b/esp32/uwb_tag/src/main.cpp @@ -1,65 +1,72 @@ /* * uwb_tag — SaltyBot ESP32 UWB Pro tag firmware (DS-TWR initiator) - * Issue #545 + * Issue #545 + display/ESP-NOW/e-stop extensions * - * Hardware: Makerfabs ESP32 UWB Pro (DW3000 chip), battery-powered + * Hardware: Makerfabs ESP32 UWB Pro with Display (DW3000 + SSD1306 OLED) * * Role * ──── - * Tag is worn by the person being tracked. It initiates DS-TWR - * ranging with each anchor on the robot in round-robin fashion. - * The anchors independently compute and report the range to the - * Jetson Orin (via their own USB serial AT+RANGE output). + * 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). * - * Protocol: Double-Sided TWR (DS-TWR) — tag as initiator - * ──────────────────────────────────────────────────────── - * Tag → POLL (msg_type 0x01) to anchor[i] - * Anchor → RESP (msg_type 0x02) with T_poll_rx, T_resp_tx - * Tag → FINAL (msg_type 0x03) with Ra, Da, Db timestamps - * - * The anchor side computes the range and reports it to the Jetson. - * The tag also computes range locally for debug/LED feedback. - * - * Serial output (USB, 115200) — debug only - * ───────────────────────────────────────── + * Serial output (USB, 115200) — debug + * ──────────────────────────────────── * +RANGE:,,\r\n - * (same format as anchor, useful for bench-testing with USB connected) * - * Pin mapping — Makerfabs ESP32 UWB Pro - * ────────────────────────────────────── - * SPI SCK 18 SPI MISO 19 SPI MOSI 23 - * DW CS 21 DW RST 27 DW IRQ 34 - * LED 2 (onboard, blinks on each successful range) + * 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) * - * Build - * ────── - * pio run -e tag --target upload - * Edit -DTAG_ID= and -DNUM_ANCHORS= in platformio.ini per deployment. + * 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 "dw3000.h" // Makerfabs MaUWB_DW3000 library +#include +#include +#include + +#include "dw3000.h" + +#include +#include /* ── Configurable ───────────────────────────────────────────────── */ #ifndef TAG_ID -# define TAG_ID 0x01 /* unique 8-bit address per tag */ +# define TAG_ID 0x01 #endif #ifndef NUM_ANCHORS -# define NUM_ANCHORS 2 /* anchors to range with (0..N-1) */ +# define NUM_ANCHORS 2 #endif #ifndef RANGE_INTERVAL_MS -# define RANGE_INTERVAL_MS 50 /* ms between ranging attempts (20 Hz) */ +# define RANGE_INTERVAL_MS 50 /* 20 Hz round-robin */ #endif #define SERIAL_BAUD 115200 -#define PIN_LED 2 -/* ── Pin map (Makerfabs ESP32 UWB Pro) ─────────────────────────── */ +/* ── Pins ───────────────────────────────────────────────────────── */ #define PIN_SCK 18 #define PIN_MISO 19 @@ -68,14 +75,55 @@ #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, /* TX preamble code */ - 9, /* RX preamble code */ + 9, 9, /* TX/RX preamble code */ 1, /* SFD type */ DWT_BR_6M8, DWT_PHR_MODE_STD, @@ -86,7 +134,7 @@ static dwt_config_t dw_cfg = { DWT_PDOA_M0, }; -/* ── Frame format (shared with anchor firmware) ─────────────────── */ +/* ── Frame format ──────────────────────────────────────────────── */ #define FTYPE_POLL 0x01 #define FTYPE_RESP 0x02 @@ -95,25 +143,17 @@ static dwt_config_t dw_cfg = { #define FRAME_HDR 3 #define FCS_LEN 2 -/* POLL: header only */ -#define POLL_FRAME_LEN (FRAME_HDR + FCS_LEN) - -/* RESP: T_poll_rx(5) + T_resp_tx(5) */ -#define RESP_PAYLOAD 10 -#define RESP_FRAME_LEN (FRAME_HDR + RESP_PAYLOAD + FCS_LEN) - -/* FINAL: Ra(5) + Da(5) + Db(5) */ -#define FINAL_PAYLOAD 15 -#define FINAL_FRAME_LEN (FRAME_HDR + FINAL_PAYLOAD + FCS_LEN) +#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 ────────────────────────────────────────────────────── */ -/* Tag TX turnaround from resp_rx to final_tx: 500 µs */ #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) - -/* Timeout waiting for RESP after POLL */ #define RESP_RX_TIMEOUT_US 3000 #define SPEED_OF_LIGHT 299702547.0 @@ -169,22 +209,168 @@ static float rx_power_dbm(void) { 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) ───────────────────── */ -/* - * Returns range in mm, or -1 on failure. - * - * DS-TWR tag side: - * 1. TX POLL to anchor_id - * 2. RX RESP from anchor → extract T_poll_rx_a, T_resp_tx_a - * Record: T_poll_tx, T_resp_rx - * 3. Compute: - * Ra = T_resp_rx − T_poll_tx (tag round-trip) - * Da = T_final_tx − T_resp_rx (tag turnaround) - * Db = T_resp_tx_a − T_poll_rx_a (anchor turnaround, from RESP) - * 4. TX FINAL with Ra, Da, Db — anchor uses these to compute range - * 5. (Optional) compute range locally from RESP timestamps for debug - */ static int32_t twr_range_once(uint8_t anchor_id) { /* --- 1. TX POLL --- */ @@ -194,7 +380,7 @@ static int32_t twr_range_once(uint8_t anchor_id) { poll[2] = anchor_id; dwt_writetxdata(POLL_FRAME_LEN - FCS_LEN, poll, 0); - dwt_writetxfctrl(POLL_FRAME_LEN, 0, 1 /*ranging*/); + dwt_writetxfctrl(POLL_FRAME_LEN, 0, 1); dwt_setrxaftertxdelay(300); dwt_setrxtimeout(RESP_RX_TIMEOUT_US); @@ -203,11 +389,9 @@ static int32_t twr_range_once(uint8_t anchor_id) { if (dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED) != DWT_SUCCESS) return -1; - /* Wait for TX done */ uint32_t t0 = millis(); while (!g_tx_done && millis() - t0 < 15) yield(); - /* Read T_poll_tx */ uint8_t poll_tx_raw[5]; dwt_readtxtimestamp(poll_tx_raw); uint64_t T_poll_tx = ts_read(poll_tx_raw); @@ -217,27 +401,24 @@ static int32_t twr_range_once(uint8_t anchor_id) { 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; /* dst must be us */ - if (g_rx_buf[1] != anchor_id) return -1; /* src must be target anchor */ + if (g_rx_buf[2] != TAG_ID) return -1; + if (g_rx_buf[1] != anchor_id) return -1; - /* Read T_resp_rx */ uint8_t resp_rx_raw[5]; dwt_readrxtimestamp(resp_rx_raw); uint64_t T_resp_rx = ts_read(resp_rx_raw); - /* Extract anchor-side timestamps from RESP payload */ - uint64_t T_poll_rx_a = ts_read(&g_rx_buf[3]); /* anchor received poll */ - uint64_t T_resp_tx_a = ts_read(&g_rx_buf[8]); /* anchor sent response */ + 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 to embed in FINAL --- */ - uint64_t Ra = ts_diff(T_resp_rx, T_poll_tx); /* tag round-trip */ - uint64_t Db = ts_diff(T_resp_tx_a, T_poll_rx_a); /* anchor turnaround*/ + /* --- 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); - /* Compute T_final_tx: resp_rx + turnaround, aligned to 512-tick grid */ uint64_t final_tx_sched = (T_resp_rx + FINAL_TX_DLY_TICKS) & ~0x1FFULL; - uint64_t Da = ts_diff(final_tx_sched, T_resp_rx); /* tag turnaround */ + uint64_t Da = ts_diff(final_tx_sched, T_resp_rx); - /* --- 4. TX FINAL with Ra, Da, Db --- */ + /* --- 4. TX FINAL --- */ uint8_t final_buf[FINAL_FRAME_LEN]; final_buf[0] = FTYPE_FINAL; final_buf[1] = TAG_ID; @@ -247,30 +428,24 @@ static int32_t twr_range_once(uint8_t anchor_id) { ts_write(&final_buf[13], Db); dwt_writetxdata(FINAL_FRAME_LEN - FCS_LEN, final_buf, 0); - dwt_writetxfctrl(FINAL_FRAME_LEN, 0, 1 /*ranging*/); + dwt_writetxfctrl(FINAL_FRAME_LEN, 0, 1); dwt_setdelayedtrxtime((uint32_t)(final_tx_sched >> 8)); g_tx_done = false; - int tx_ret = dwt_starttx(DWT_START_TX_DELAYED); - if (tx_ret != DWT_SUCCESS) { + 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 computation (debug) --- */ - /* Read actual T_final_tx */ + /* --- 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); - uint64_t Da_actual = ts_diff(T_final_tx, T_resp_rx); + /* uint64_t T_final_tx = ts_read(final_tx_raw); -- unused, tag uses SS estimate */ - /* Single-sided estimate from tag's perspective (anchor will do DS-TWR) */ double ra = ticks_to_s(Ra); - double da = ticks_to_s(Da_actual); double db = ticks_to_s(Db); - /* For local display use simplified estimate: tof ≈ (Ra - Db) / 2 */ double tof = (ra - db) / 2.0; double range_m = tof * SPEED_OF_LIGHT; @@ -284,15 +459,57 @@ 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); - /* Hardware reset */ pinMode(PIN_RST, OUTPUT); digitalWrite(PIN_RST, LOW); delay(2); @@ -300,7 +517,7 @@ void setup(void) { delay(5); if (dwt_probe((struct dwt_probe_s *)&dw3000_probe_interf)) { - Serial.println("[uwb_tag] FATAL: DW3000 probe failed — check SPI wiring"); + Serial.println("[uwb_tag] FATAL: DW3000 probe failed"); for (;;) delay(1000); } @@ -328,36 +545,71 @@ void setup(void) { attachInterrupt(digitalPinToInterrupt(PIN_IRQ), []() { dwt_isr(); }, RISING); - Serial.printf("[uwb_tag] DW3000 ready ch=%d 6.8Mbps tag_id=0x%02X\r\n", + /* 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] Starting ranging..."); + Serial.println("[uwb_tag] Ranging + ESP-NOW + display active"); } -/* ── Main loop — round-robin across anchors ─────────────────────── */ +/* ── Main loop ──────────────────────────────────────────────────── */ void loop(void) { - static uint8_t anchor_idx = 0; + 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_range_ms < RANGE_INTERVAL_MS) { - yield(); - return; - } - 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(); - Serial.printf("+RANGE:%d,%ld,%.1f\r\n", anchor_id, (long)range_mm, rssi); - /* Brief LED blink */ - digitalWrite(PIN_LED, HIGH); - delay(2); - digitalWrite(PIN_LED, LOW); + if (now - last_hb_ms >= 1000) { + espnow_send(MSG_HEARTBEAT, 0xFF, 0, 0.0f); + last_hb_ms = now; } - anchor_idx++; - if (anchor_idx >= NUM_ANCHORS) anchor_idx = 0; + /* 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(); }