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).
This commit is contained in:
salty
parent
66a4bbe25b
commit
a9e1b9fae5
@ -42,6 +42,9 @@
|
||||
#include <Arduino.h>
|
||||
#include <SPI.h>
|
||||
#include <math.h>
|
||||
#include <WiFi.h>
|
||||
#include <esp_now.h>
|
||||
#include <esp_wifi.h>
|
||||
#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();
|
||||
}
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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:<anchor_id>,<range_mm>,<rssi_dbm>\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 <Arduino.h>
|
||||
#include <SPI.h>
|
||||
#include <Wire.h>
|
||||
#include <math.h>
|
||||
#include "dw3000.h" // Makerfabs MaUWB_DW3000 library
|
||||
#include <WiFi.h>
|
||||
#include <esp_now.h>
|
||||
#include <esp_wifi.h>
|
||||
|
||||
#include "dw3000.h"
|
||||
|
||||
#include <Adafruit_GFX.h>
|
||||
#include <Adafruit_SSD1306.h>
|
||||
|
||||
/* ── 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();
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user