seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(uwb): replace ESP-NOW with DW1000 data frames + BLE GPS/IMU input

Browse Source 
- Tag: strip all ESP-NOW/WiFi code
- Tag: add BLE GPS characteristic (def3) for iPhone binary GPS writes
- Tag: add BLE Phone IMU characteristic (def4) for iPhone IMU writes
- Tag: transmit GPS/IMU/heartbeat via DW1000 data frames (v2 protocol)
- Tag: update OLED display with GPS speed/heading/fix indicators
- Tag: e-stop now sent via UWB data frames (3x for reliability)
- Anchor: strip all ESP-NOW/WiFi code
- Anchor: receive DW1000 data frames, forward to serial as +GPS/+PIMU/+TIMU/+ESTOP
- Protocol v2: magic {0x5B, 0x02}, msg types 0x60-0x64
This commit is contained in:
Salty Bead 2026-03-30 09:23:00 -04:00
parent 30a5212ba2
commit 5e591ab466
3 changed files with 1296 additions and 677 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

176
TASK.md Normal file
View File

@ -0,0 +1,176 @@
# Task: UWB Tag + Anchor Firmware Rewrite — GPS Breadcrumb Trail
## Context
SaltyBot is a self-balancing robot that follows a rider on an EUC (electric unicycle). The rider wears a UWB tag (ESP32 UWB Pro with Display). The robot has 2 UWB anchors. The rider's iPhone connects to the tag via BLE and streams GPS+IMU data. The tag relays this over UWB data frames to the anchors on the robot, which forward to the Jetson Orin via serial.
When the rider moves out of UWB range, the iPhone falls back to sending GPS over Tailscale/MQTT directly to Orin. The robot replays the exact breadcrumb path.
## Files to Modify
- `esp32/uwb_tag/src/main.cpp` — Tag firmware (wearable, ESP32 UWB Pro with Display)
- `esp32/uwb_anchor/src/main.cpp` — Anchor firmware (on robot, ESP32 UWB Pro, no display)
## Changes Required
### Both Files
1. **Remove ALL ESP-NOW code** — no more WiFi, esp_now, broadcast MAC, ESP-NOW packets, etc.
2. Replace ESP-NOW transport with **DW1000 data frames** for tag→anchor communication
### Tag Firmware (uwb_tag/src/main.cpp)
#### Strip
- All ESP-NOW includes, init, send, receive, packet structs
- WiFi mode/channel/disconnect calls
- `espnow_send()`, `espnow_send_imu()`, `espnow_rx_cb()`
#### Add: BLE GPS Input Service
New GATT characteristic for iPhone to write GPS data:
- **GPS Characteristic UUID:** `12345678-1234-5678-1234-56789abcdef3` (WRITE, WRITE_NR)
- iPhone writes a packed binary struct (not JSON — too slow at 5Hz):
```c
#pragma pack(push, 1)
struct GpsInput {
int32_t lat_e7; // latitude * 1e7 (nanodegrees)
int32_t lon_e7; // longitude * 1e7
int16_t alt_dm; // altitude in decimeters (±3276.7m range)
uint16_t speed_cmps; // ground speed in cm/s (0-655 m/s)
uint16_t heading_cd; // heading in centidegrees (0-35999)
uint8_t accuracy_dm; // horizontal accuracy in decimeters (0-25.5m)
uint8_t fix_type; // 0=none, 1=2D, 2=3D
uint32_t timestamp_ms; // iPhone monotonic timestamp
}; // 20 bytes total
#pragma pack(pop)
```
- On BLE write → store in `g_gps` global, set `g_gps_fresh = true`
- Also add an **iPhone IMU Characteristic UUID:** `12345678-1234-5678-1234-56789abcdef4` (WRITE, WRITE_NR)
```c
#pragma pack(push, 1)
struct PhoneImuInput {
int16_t ax_mg; // acceleration X in milli-g
int16_t ay_mg; // Y
int16_t az_mg; // Z
int16_t gx_cdps; // gyro X in centi-degrees/sec
int16_t gy_cdps; // Y
int16_t gz_cdps; // Z
int16_t mx_ut; // magnetometer X in micro-Tesla (for heading)
int16_t my_ut; // Y
int16_t mz_ut; // Z
uint32_t timestamp_ms; // iPhone monotonic timestamp
}; // 22 bytes
#pragma pack(pop)
```
#### Add: DW1000 Data Frame TX
After each GPS or IMU BLE write, transmit a DW1000 data frame to the anchors:
```c
#define UWB_MSG_GPS 0x60
#define UWB_MSG_PIMU 0x61 // Phone IMU
#define UWB_MSG_TIMU 0x62 // Tag onboard IMU (MPU6050)
#pragma pack(push, 1)
struct UwbDataFrame {
uint8_t magic[2]; // {0x5B, 0x02} — v2 protocol
uint8_t tag_id;
uint8_t msg_type; // UWB_MSG_GPS, UWB_MSG_PIMU, UWB_MSG_TIMU
uint8_t seq;
uint8_t payload[0]; // variable: GpsInput (20B) or ImuPayload (22B) or TagImu (14B)
};
#pragma pack(pop)
```
Use `DW1000.setData()` + `DW1000.startTransmit()` to send raw data frames between ranging cycles. The DW1000Ranging library handles TWR, but we can interleave data frames in between. Use a simple approach:
- After each successful TWR cycle in `newRange()`, if there's fresh GPS/IMU data, transmit a data frame
- Also send GPS at 5Hz and tag IMU at 10Hz on a timer if no ranging is happening
For the tag's own MPU6050 IMU, keep sending that too:
```c
#pragma pack(push, 1)
struct TagImuPayload {
int16_t ax_mg;
int16_t ay_mg;
int16_t az_mg;
int16_t gx_dps10;
int16_t gy_dps10;
int16_t gz_dps10;
uint8_t accel_mag_10;
uint8_t flags; // bit0: fall detected
}; // 14 bytes
#pragma pack(pop)
```
#### Update OLED Display
- Add GPS info line: lat/lon or speed+heading
- Show BLE data status (receiving from phone or not)
- Show "GPS: 3D" or "GPS: ---" indicator
#### Keep Everything Else
- UWB ranging (DW1000Ranging as tag/initiator)
- BLE config service (existing UUIDs ...def0, ...def1, ...def2)
- MPU6050 IMU reads + fall detection
- Power management (display sleep, DW1000 sleep, deep sleep)
- E-stop on fall detection (send via UWB data frame instead of ESP-NOW)
### Anchor Firmware (uwb_anchor/src/main.cpp)
#### Strip
- All ESP-NOW code (includes, init, rx callback, send, packet structs, ring buffer)
- WiFi mode/channel/disconnect
- `espnow_send_range()`, `espnow_rx_cb()`, `espnow_process()`, `g_other_*` tracking
#### Add: DW1000 Data Frame RX
- Register a DW1000 receive handler that catches data frames from the tag
- Parse UwbDataFrame header, then forward payload to Orin via serial:
```
+GPS:<tag_id>,<lat_e7>,<lon_e7>,<alt_dm>,<speed_cmps>,<heading_cd>,<accuracy_dm>,<fix>,<ts>\r\n
+PIMU:<tag_id>,<ax>,<ay>,<az>,<gx>,<gy>,<gz>,<mx>,<my>,<mz>,<ts>\r\n
+TIMU:<tag_id>,<ax>,<ay>,<az>,<gx>,<gy>,<gz>,<mag>,<flags>\r\n
+ESTOP:<tag_id>\r\n
```
#### Keep
- UWB ranging (DW1000Ranging as anchor/responder)
- AT command handler
- TDM slot system
- Serial output format for +RANGE lines
## DW1000 Data Frame Implementation Notes
The DW1000Ranging library uses the DW1000 for TWR (two-way ranging). To send data frames alongside ranging:
**Option A (simpler):** Use the DW1000's transmit-after-ranging capability. After a TWR exchange completes, the tag can immediately send a data frame. The DW1000 library exposes `DW1000.setData()` and `DW1000.startTransmit()`.
**Option B (more robust):** Use ESP-NOW on a separate WiFi channel just for data (not ranging). But we're removing ESP-NOW, so this is not an option.
**Option C (recommended):** Since DW1000Ranging uses SPI and manages the transceiver state, the safest approach is to **interleave data TX between ranging cycles**. After `newRange()` fires and a TWR cycle completes, queue one data frame for TX. The tag can check `DW1000Ranging.loop()` return or use a flag to know when a cycle is done.
Actually, the simplest reliable approach: **Use DW1000's user data field in ranging frames**. The DW1000Ranging library sends ranging frames that have a data payload area. We can embed our GPS/IMU data in the ranging frame itself. Check if `DW1000Ranging` exposes `setUserData()` or similar. If not, we can modify the ranging frame assembly.
If none of that works cleanly, fall back to **periodic standalone TX** using a timer. Between ranging cycles, send raw DW1000 frames. The anchors listen for both ranging responses and raw data frames (differentiated by frame type or header byte).
Pick whichever approach compiles cleanly with the existing DW1000/DW1000Ranging library in `../../lib/DW1000/`.
## Important Constraints
- ESP32 (not ESP32-S3) — standard Arduino + ESP-IDF
- DW1000 library is in `../../lib/DW1000/` (Makerfabs fork from mf_DW1000.zip)
- PlatformIO build, `pio run -e tag` / `pio run -e anchor0` / `pio run -e anchor1`
- Keep both files self-contained (no shared header files needed — duplicate structs is fine)
- Tag CS pin = 21 (display board), Anchor CS pin = 4 (non-display board)
- OLED is 128x64 SSD1306 on I2C (tag only)
- BLE + DW1000 SPI coexist fine on ESP32 (different peripherals)
- Max DW1000 frame payload is ~127 bytes — our packets are well under that
## Build Verification
After changes, verify the code compiles:
```bash
cd esp32/uwb_tag && pio run -e tag
cd esp32/uwb_anchor && pio run -e anchor0
```
(anchor0/anchor1 envs should exist in platformio.ini, or just `pio run`)
## Branch
Working on branch: `salty/uwb-tag-display-wireless`
Commit changes with a descriptive message when done.

307
esp32/uwb_anchor/src/main.cpp
View File

@ -7,10 +7,13 @@
* *
* Serial output to Jetson (115200): * Serial output to Jetson (115200):
* +RANGE:<anchor_id>,<tag_short_addr>,<range_mm>,<rssi_dbm>\r\n * +RANGE:<anchor_id>,<tag_short_addr>,<range_mm>,<rssi_dbm>\r\n
* +GPS:<tag_id>,<lat_e7>,<lon_e7>,<alt_dm>,<speed_cmps>,<heading_cd>,<accuracy_dm>,<fix>,<ts>\r\n
* +PIMU:<tag_id>,<ax>,<ay>,<az>,<gx>,<gy>,<gz>,<mx>,<my>,<mz>,<ts>\r\n
* +TIMU:<tag_id>,<ax>,<ay>,<az>,<gx>,<gy>,<gz>,<mag>,<flags>\r\n
* +ESTOP:<tag_id>,<flags>\r\n
* *
* Also receives ESP-NOW packets from tag and forwards: * Receives DW1000 data frames from tag (v2 protocol, magic 0x5B 0x02).
* +ESPNOW:<tag_id>,<msg_type>,<range_mm>,<rssi>,<batt>,<flags>,<seq>\r\n * Ranging via DW1000Ranging library (TWR responder).
* +ESTOP:<tag_id>\r\n (priority)
* *
* AT commands (host → anchor): * AT commands (host → anchor):
* AT+ID? → +ID:<anchor_id> * AT+ID? → +ID:<anchor_id>
@ -23,10 +26,8 @@
#include <Arduino.h> #include <Arduino.h>
#include <SPI.h> #include <SPI.h>
#include <string.h> #include <string.h>
#include <WiFi.h>
#include <esp_now.h>
#include <esp_wifi.h>
#include "DW1000Ranging.h" #include "DW1000Ranging.h"
#include "DW1000.h"
/* ── Config ─────────────────────────────────────────────────────── */ /* ── Config ─────────────────────────────────────────────────────── */
@ -36,7 +37,6 @@
#define SERIAL_BAUD 115200 #define SERIAL_BAUD 115200
/* Unique DW1000 address per anchor (last 2 bytes differ) */
#if ANCHOR_ID == 0 #if ANCHOR_ID == 0
static char ANCHOR_ADDR[] = "86:17:5B:D5:A9:9A:E2:00"; static char ANCHOR_ADDR[] = "86:17:5B:D5:A9:9A:E2:00";
#else #else
@ -52,72 +52,83 @@ static char ANCHOR_ADDR[] = "86:17:5B:D5:A9:9A:E2:01";
#define PIN_RST 27 #define PIN_RST 27
#define PIN_IRQ 34 #define PIN_IRQ 34
/* ── ESP-NOW packet format (shared with tag) ───────────────────── */ /* ── UWB Data Frame Protocol (shared with tag) ──────────────────── */
#define ESPNOW_MAGIC_0 0x5B #define UWB_MAGIC_0 0x5B
#define ESPNOW_MAGIC_1 0x01 #define UWB_MAGIC_1 0x02
#define MSG_RANGE 0x10
#define MSG_ESTOP 0x20 #define UWB_MSG_GPS 0x60
#define MSG_HEARTBEAT 0x30 #define UWB_MSG_PIMU 0x61
#define MSG_IMU 0x40 #define UWB_MSG_TIMU 0x62
#define MSG_FALL 0x50 #define UWB_MSG_ESTOP 0x63
#define UWB_MSG_HB 0x64
#pragma pack(push, 1) #pragma pack(push, 1)
struct EspNowPacket { struct UwbFrameHeader {
uint8_t magic[2]; uint8_t magic[2];
uint8_t tag_id; uint8_t tag_id;
uint8_t msg_type; uint8_t msg_type;
uint8_t anchor_id; uint8_t seq;
int32_t range_mm; };
float rssi_dbm;
struct GpsInput {
int32_t lat_e7;
int32_t lon_e7;
int16_t alt_dm;
uint16_t speed_cmps;
uint16_t heading_cd;
uint8_t accuracy_dm;
uint8_t fix_type;
uint32_t timestamp_ms; uint32_t timestamp_ms;
uint8_t battery_pct; };
struct PhoneImuInput {
int16_t ax_mg;
int16_t ay_mg;
int16_t az_mg;
int16_t gx_cdps;
int16_t gy_cdps;
int16_t gz_cdps;
int16_t mx_ut;
int16_t my_ut;
int16_t mz_ut;
uint32_t timestamp_ms;
};
struct TagImuPayload {
int16_t ax_mg;
int16_t ay_mg;
int16_t az_mg;
int16_t gx_dps10;
int16_t gy_dps10;
int16_t gz_dps10;
uint8_t accel_mag_10;
uint8_t flags; uint8_t flags;
uint8_t seq_num;
uint8_t _pad;
}; };
#pragma pack(pop) #pragma pack(pop)
/* ISR ring buffer for ESP-NOW */ /* ── DW1000 data frame RX buffer ────────────────────────────────── */
#define ESPNOW_Q_SIZE 8
static EspNowPacket g_enq[ESPNOW_Q_SIZE];
static volatile int g_en_head = 0, g_en_tail = 0;
static void IRAM_ATTR espnow_rx_cb(const esp_now_recv_info_t *info, const uint8_t *data, int len) { #define UWB_RX_BUF_SIZE 64
if (len < (int)sizeof(EspNowPacket)) return; #define UWB_RX_Q_SIZE 8
const EspNowPacket *p = (const EspNowPacket *)data;
if (p->magic[0] != ESPNOW_MAGIC_0 || p->magic[1] != ESPNOW_MAGIC_1) return;
int next = (g_en_head + 1) % ESPNOW_Q_SIZE;
if (next == g_en_tail) return;
memcpy(&g_enq[g_en_head], p, sizeof(EspNowPacket));
g_en_head = next;
}
/* ── ESP-NOW TX: broadcast own range to other anchor + tag ──────── */ static uint8_t g_rx_q[UWB_RX_Q_SIZE][UWB_RX_BUF_SIZE];
static uint16_t g_rx_q_len[UWB_RX_Q_SIZE];
static volatile int g_rx_head = 0, g_rx_tail = 0;
static uint8_t broadcast_mac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; /* Last seen sequence per tag to detect duplicates */
static uint8_t g_seq = 0; static uint8_t g_last_seq[256] = {}; /* indexed by tag_id */
static bool g_seq_seen[256] = {};
/* Track the other anchor's range (received via ESP-NOW) */ /*
static int32_t g_other_range_mm = -1; * DW1000 RX callback for data frames.
static float g_other_rssi = -100.0f; * Called from ISR context — keep it fast, just copy to ring buffer.
static uint32_t g_other_last_ms = 0; *
* Note: The DW1000Ranging library handles TWR frames internally.
static void espnow_send_range(int32_t range_mm, float rssi, uint16_t tag_addr) { * We register a separate handler for non-ranging frames via DW1000 low-level API.
EspNowPacket pkt = {}; * However, since DW1000Ranging manages the receiver state, we intercept
pkt.magic[0] = ESPNOW_MAGIC_0; * data frames by polling in loop() between ranging cycles.
pkt.magic[1] = ESPNOW_MAGIC_1; */
pkt.tag_id = (uint8_t)(tag_addr & 0xFF);
pkt.msg_type = MSG_RANGE;
pkt.anchor_id = ANCHOR_ID;
pkt.range_mm = range_mm;
pkt.rssi_dbm = rssi;
pkt.timestamp_ms = millis();
pkt.battery_pct = 0xFF;
pkt.flags = 0x00;
pkt.seq_num = g_seq++;
esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt));
}
/* ── AT command handler ─────────────────────────────────────────── */ /* ── AT command handler ─────────────────────────────────────────── */
@ -146,58 +157,107 @@ static void serial_poll(void) {
} }
} }
/* ── ESP-NOW processing ─────────────────────────────────────────── */ /* ── Process received UWB data frames ────────────────────────────── */
static void espnow_process(void) { static void process_uwb_data(const uint8_t *data, uint16_t len) {
while (g_en_tail != g_en_head) { if (len < sizeof(UwbFrameHeader)) return;
const EspNowPacket &pkt = (const EspNowPacket &)g_enq[g_en_tail];
g_en_tail = (g_en_tail + 1) % ESPNOW_Q_SIZE;
if (pkt.msg_type == MSG_ESTOP) { const UwbFrameHeader *hdr = (const UwbFrameHeader *)data;
if (pkt.flags & 0x01) if (hdr->magic[0] != UWB_MAGIC_0 || hdr->magic[1] != UWB_MAGIC_1) return;
Serial.printf("+ESTOP:%d\r\n", pkt.tag_id);
else uint8_t tag = hdr->tag_id;
Serial.printf("+ESTOP_CLEAR:%d\r\n", pkt.tag_id); uint8_t mtype = hdr->msg_type;
continue; uint8_t seq = hdr->seq;
const uint8_t *payload = data + sizeof(UwbFrameHeader);
uint16_t plen = len - sizeof(UwbFrameHeader);
/* Dedup: skip if we've seen this exact seq from this tag */
if (g_seq_seen[tag] && g_last_seq[tag] == seq) return;
g_last_seq[tag] = seq;
g_seq_seen[tag] = true;
switch (mtype) {
case UWB_MSG_GPS: {
if (plen < sizeof(GpsInput)) return;
const GpsInput *gps = (const GpsInput *)payload;
Serial.printf("+GPS:%d,%ld,%ld,%d,%u,%u,%u,%u,%lu\r\n",
tag,
(long)gps->lat_e7, (long)gps->lon_e7,
gps->alt_dm, gps->speed_cmps, gps->heading_cd,
gps->accuracy_dm, gps->fix_type,
(unsigned long)gps->timestamp_ms);
break;
}
case UWB_MSG_PIMU: {
if (plen < sizeof(PhoneImuInput)) return;
const PhoneImuInput *pi = (const PhoneImuInput *)payload;
Serial.printf("+PIMU:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%lu\r\n",
tag,
pi->ax_mg, pi->ay_mg, pi->az_mg,
pi->gx_cdps, pi->gy_cdps, pi->gz_cdps,
pi->mx_ut, pi->my_ut, pi->mz_ut,
(unsigned long)pi->timestamp_ms);
break;
}
case UWB_MSG_TIMU: {
if (plen < sizeof(TagImuPayload)) return;
const TagImuPayload *ti = (const TagImuPayload *)payload;
Serial.printf("+TIMU:%d,%d,%d,%d,%d,%d,%d,%u,%u\r\n",
tag,
ti->ax_mg, ti->ay_mg, ti->az_mg,
ti->gx_dps10, ti->gy_dps10, ti->gz_dps10,
ti->accel_mag_10, ti->flags);
break;
}
case UWB_MSG_ESTOP: {
if (plen < sizeof(TagImuPayload)) {
/* Minimal e-stop with just flags byte */
uint8_t flags = (plen >= 1) ? payload[plen - 1] : 0x01;
Serial.printf("+ESTOP:%d,%u\r\n", tag, flags);
} else {
const TagImuPayload *ep = (const TagImuPayload *)payload;
Serial.printf("+ESTOP:%d,%u\r\n", tag, ep->flags);
} }
break;
}
case UWB_MSG_HB: {
/* Heartbeat — just note tag is alive */
Serial.printf("+HB:%d\r\n", tag);
break;
}
default:
Serial.printf("+UWB_UNK:%d,0x%02X,%d\r\n", tag, mtype, plen);
break;
}
}
/* If this is a range from the OTHER anchor, track it and print as +RANGE */ /*
if (pkt.msg_type == MSG_RANGE && pkt.anchor_id != ANCHOR_ID * Try to read a data frame from DW1000.
&& pkt.anchor_id < 2) { * Called between ranging cycles when the DW1000 might have buffered
g_other_range_mm = pkt.range_mm; * a non-ranging frame. We briefly switch to receive mode, check for
g_other_rssi = pkt.rssi_dbm; * data, then let ranging resume.
g_other_last_ms = millis(); *
/* Print as +RANGE so Orin sees both anchors from either serial port */ * This is a cooperative approach: the DW1000Ranging library uses
Serial.printf("+RANGE:%d,%02X,%ld,%.1f\r\n", * the transceiver for TWR, and we steal idle slots to listen for
pkt.anchor_id, pkt.tag_id, * data frames from the tag.
(long)pkt.range_mm, pkt.rssi_dbm); */
continue; static uint8_t g_uwb_rx_buf[UWB_RX_BUF_SIZE];
}
/* Forward IMU and FALL packets to Jetson serial */ static void uwb_check_data_rx(void) {
if (pkt.msg_type == MSG_IMU || pkt.msg_type == MSG_FALL) { /* Check if DW1000 received a frame that DW1000Ranging didn't consume */
const uint8_t *raw = (const uint8_t *)&pkt; if (!DW1000.isReceiveDone()) return;
Serial.printf("+%s:%d,",
pkt.msg_type == MSG_FALL ? "FALL" : "IMU",
pkt.tag_id);
/* Forward as hex for easy parsing: ax,ay,az,gx,gy,gz,mag */
int16_t ax = (int16_t)(raw[4] | (raw[5] << 8));
int16_t ay = (int16_t)(raw[6] | (raw[7] << 8));
int16_t az = (int16_t)(raw[8] | (raw[9] << 8));
int16_t gx = (int16_t)(raw[10] | (raw[11] << 8));
int16_t gy = (int16_t)(raw[12] | (raw[13] << 8));
int16_t gz = (int16_t)(raw[14] | (raw[15] << 8));
uint8_t mag = raw[16];
Serial.printf("%.3f,%.3f,%.3f,%.1f,%.1f,%.1f,%.1f\r\n",
ax/1000.0f, ay/1000.0f, az/1000.0f,
gx/10.0f, gy/10.0f, gz/10.0f,
mag/10.0f);
continue;
}
Serial.printf("+ESPNOW:%d,%02X,%ld,%.1f,%d,%02X,%d\r\n", uint16_t len = DW1000.getDataLength();
pkt.tag_id, pkt.msg_type, (long)pkt.range_mm, if (len < sizeof(UwbFrameHeader) || len > UWB_RX_BUF_SIZE) {
pkt.rssi_dbm, pkt.battery_pct, pkt.flags, pkt.seq_num); /* Not our frame or too large — let ranging handle it */
return;
}
DW1000.getData(g_uwb_rx_buf, len);
/* Quick magic check — only process v2 data frames */
if (g_uwb_rx_buf[0] == UWB_MAGIC_0 && g_uwb_rx_buf[1] == UWB_MAGIC_1) {
process_uwb_data(g_uwb_rx_buf, len);
} }
} }
@ -214,9 +274,6 @@ static void newRange(void) {
Serial.printf("+RANGE:%d,%04X,%ld,%.1f\r\n", Serial.printf("+RANGE:%d,%04X,%ld,%.1f\r\n",
ANCHOR_ID, tag_addr, (long)range_mm, rxPow); ANCHOR_ID, tag_addr, (long)range_mm, rxPow);
/* Broadcast own range via ESP-NOW → other anchor + tag receive it */
espnow_send_range(range_mm, rxPow, tag_addr);
} }
static void newBlink(DW1000Device *device) { static void newBlink(DW1000Device *device) {
@ -232,26 +289,10 @@ static void inactiveDevice(DW1000Device *device) {
void setup(void) { void setup(void) {
Serial.begin(SERIAL_BAUD); Serial.begin(SERIAL_BAUD);
delay(300); delay(300);
Serial.printf("\r\n[uwb_anchor] id=%d addr=%s starting\r\n", ANCHOR_ID, ANCHOR_ADDR); Serial.printf("\r\n[uwb_anchor] id=%d addr=%s v2 (UWB data frames)\r\n",
ANCHOR_ID, ANCHOR_ADDR);
/* ESP-NOW receiver */ /* DW1000 ranging */
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);
/* Add broadcast peer for TX */
esp_now_peer_info_t peer = {};
memcpy(peer.peer_addr, broadcast_mac, 6);
peer.channel = 0;
peer.encrypt = false;
esp_now_add_peer(&peer);
Serial.println("[uwb_anchor] ESP-NOW rx+tx ok");
} else {
Serial.println("[uwb_anchor] WARN: ESP-NOW failed");
}
/* DW1000 */
SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI); SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI);
DW1000Ranging.initCommunication(PIN_RST, PIN_CS, PIN_IRQ); DW1000Ranging.initCommunication(PIN_RST, PIN_CS, PIN_IRQ);
@ -262,26 +303,28 @@ void setup(void) {
DW1000Ranging.startAsAnchor(ANCHOR_ADDR, DW1000.MODE_LONGDATA_RANGE_LOWPOWER, false); DW1000Ranging.startAsAnchor(ANCHOR_ADDR, DW1000.MODE_LONGDATA_RANGE_LOWPOWER, false);
Serial.printf("[uwb_anchor] DW1000 ready id=%d MODE_LONGDATA_RANGE_LOWPOWER\r\n", ANCHOR_ID); Serial.printf("[uwb_anchor] DW1000 ready id=%d MODE_LONGDATA_RANGE_LOWPOWER\r\n", ANCHOR_ID);
Serial.println("[uwb_anchor] Listening for tags..."); Serial.println("[uwb_anchor] Listening for tags + UWB data frames...");
} }
/* ── Loop ───────────────────────────────────────────────────────── */ /* ── Loop ───────────────────────────────────────────────────────── */
/* /*
* Time-division multiplexing: 2 anchors share the same channel. * TDM: 2 anchors share the same channel.
* Each anchor only responds during its time slot to avoid RF collisions. * Each anchor only responds during its time slot.
* Slot duration = 50ms → each anchor gets 10 Hz effective rate. * Slot = 50ms → each anchor gets 10 Hz effective rate.
* Anchor 0 responds during even slots, anchor 1 during odd slots. * Between slots, check for UWB data frames.
*/ */
#define SLOT_MS 50 #define SLOT_MS 50
#define NUM_ANCHORS_TOTAL 2 #define NUM_ANCHORS_TOTAL 2
void loop(void) { void loop(void) {
serial_poll(); serial_poll();
espnow_process();
uint32_t slot = (millis() / SLOT_MS) % NUM_ANCHORS_TOTAL; uint32_t slot = (millis() / SLOT_MS) % NUM_ANCHORS_TOTAL;
if (slot == ANCHOR_ID) { if (slot == ANCHOR_ID) {
DW1000Ranging.loop(); DW1000Ranging.loop();
} }
/* Check for UWB data frames from tag between ranging slots */
uwb_check_data_rx();
} }

1490
esp32/uwb_tag/src/main.cpp
View File

File diff suppressed because it is too large Load Diff