feat(uwb): replace ESP-NOW with DW1000 data frames + BLE GPS/IMU input
- 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
parent
30a5212ba2
commit
5e591ab466
176
TASK.md
Normal file
176
TASK.md
Normal 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.
|
||||
@ -7,10 +7,13 @@
|
||||
*
|
||||
* Serial output to Jetson (115200):
|
||||
* +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:
|
||||
* +ESPNOW:<tag_id>,<msg_type>,<range_mm>,<rssi>,<batt>,<flags>,<seq>\r\n
|
||||
* +ESTOP:<tag_id>\r\n (priority)
|
||||
* Receives DW1000 data frames from tag (v2 protocol, magic 0x5B 0x02).
|
||||
* Ranging via DW1000Ranging library (TWR responder).
|
||||
*
|
||||
* AT commands (host → anchor):
|
||||
* AT+ID? → +ID:<anchor_id>
|
||||
@ -23,10 +26,8 @@
|
||||
#include <Arduino.h>
|
||||
#include <SPI.h>
|
||||
#include <string.h>
|
||||
#include <WiFi.h>
|
||||
#include <esp_now.h>
|
||||
#include <esp_wifi.h>
|
||||
#include "DW1000Ranging.h"
|
||||
#include "DW1000.h"
|
||||
|
||||
/* ── Config ─────────────────────────────────────────────────────── */
|
||||
|
||||
@ -36,7 +37,6 @@
|
||||
|
||||
#define SERIAL_BAUD 115200
|
||||
|
||||
/* Unique DW1000 address per anchor (last 2 bytes differ) */
|
||||
#if ANCHOR_ID == 0
|
||||
static char ANCHOR_ADDR[] = "86:17:5B:D5:A9:9A:E2:00";
|
||||
#else
|
||||
@ -52,72 +52,83 @@ static char ANCHOR_ADDR[] = "86:17:5B:D5:A9:9A:E2:01";
|
||||
#define PIN_RST 27
|
||||
#define PIN_IRQ 34
|
||||
|
||||
/* ── ESP-NOW packet format (shared with tag) ───────────────────── */
|
||||
/* ── UWB Data Frame Protocol (shared with tag) ──────────────────── */
|
||||
|
||||
#define ESPNOW_MAGIC_0 0x5B
|
||||
#define ESPNOW_MAGIC_1 0x01
|
||||
#define MSG_RANGE 0x10
|
||||
#define MSG_ESTOP 0x20
|
||||
#define MSG_HEARTBEAT 0x30
|
||||
#define MSG_IMU 0x40
|
||||
#define MSG_FALL 0x50
|
||||
#define UWB_MAGIC_0 0x5B
|
||||
#define UWB_MAGIC_1 0x02
|
||||
|
||||
#define UWB_MSG_GPS 0x60
|
||||
#define UWB_MSG_PIMU 0x61
|
||||
#define UWB_MSG_TIMU 0x62
|
||||
#define UWB_MSG_ESTOP 0x63
|
||||
#define UWB_MSG_HB 0x64
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct EspNowPacket {
|
||||
struct UwbFrameHeader {
|
||||
uint8_t magic[2];
|
||||
uint8_t tag_id;
|
||||
uint8_t msg_type;
|
||||
uint8_t anchor_id;
|
||||
int32_t range_mm;
|
||||
float rssi_dbm;
|
||||
uint8_t seq;
|
||||
};
|
||||
|
||||
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;
|
||||
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 seq_num;
|
||||
uint8_t _pad;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
/* ISR ring buffer for ESP-NOW */
|
||||
#define ESPNOW_Q_SIZE 8
|
||||
static EspNowPacket g_enq[ESPNOW_Q_SIZE];
|
||||
static volatile int g_en_head = 0, g_en_tail = 0;
|
||||
/* ── DW1000 data frame RX buffer ────────────────────────────────── */
|
||||
|
||||
static void IRAM_ATTR espnow_rx_cb(const esp_now_recv_info_t *info, const uint8_t *data, int len) {
|
||||
if (len < (int)sizeof(EspNowPacket)) return;
|
||||
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;
|
||||
}
|
||||
#define UWB_RX_BUF_SIZE 64
|
||||
#define UWB_RX_Q_SIZE 8
|
||||
|
||||
/* ── 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};
|
||||
static uint8_t g_seq = 0;
|
||||
/* Last seen sequence per tag to detect duplicates */
|
||||
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;
|
||||
static float g_other_rssi = -100.0f;
|
||||
static uint32_t g_other_last_ms = 0;
|
||||
|
||||
static void espnow_send_range(int32_t range_mm, float rssi, uint16_t tag_addr) {
|
||||
EspNowPacket pkt = {};
|
||||
pkt.magic[0] = ESPNOW_MAGIC_0;
|
||||
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));
|
||||
}
|
||||
/*
|
||||
* DW1000 RX callback for data frames.
|
||||
* Called from ISR context — keep it fast, just copy to ring buffer.
|
||||
*
|
||||
* Note: The DW1000Ranging library handles TWR frames internally.
|
||||
* We register a separate handler for non-ranging frames via DW1000 low-level API.
|
||||
* However, since DW1000Ranging manages the receiver state, we intercept
|
||||
* data frames by polling in loop() between ranging cycles.
|
||||
*/
|
||||
|
||||
/* ── 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) {
|
||||
while (g_en_tail != g_en_head) {
|
||||
const EspNowPacket &pkt = (const EspNowPacket &)g_enq[g_en_tail];
|
||||
g_en_tail = (g_en_tail + 1) % ESPNOW_Q_SIZE;
|
||||
static void process_uwb_data(const uint8_t *data, uint16_t len) {
|
||||
if (len < sizeof(UwbFrameHeader)) return;
|
||||
|
||||
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;
|
||||
const UwbFrameHeader *hdr = (const UwbFrameHeader *)data;
|
||||
if (hdr->magic[0] != UWB_MAGIC_0 || hdr->magic[1] != UWB_MAGIC_1) return;
|
||||
|
||||
uint8_t tag = hdr->tag_id;
|
||||
uint8_t mtype = hdr->msg_type;
|
||||
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
|
||||
&& pkt.anchor_id < 2) {
|
||||
g_other_range_mm = pkt.range_mm;
|
||||
g_other_rssi = pkt.rssi_dbm;
|
||||
g_other_last_ms = millis();
|
||||
/* Print as +RANGE so Orin sees both anchors from either serial port */
|
||||
Serial.printf("+RANGE:%d,%02X,%ld,%.1f\r\n",
|
||||
pkt.anchor_id, pkt.tag_id,
|
||||
(long)pkt.range_mm, pkt.rssi_dbm);
|
||||
continue;
|
||||
/*
|
||||
* Try to read a data frame from DW1000.
|
||||
* Called between ranging cycles when the DW1000 might have buffered
|
||||
* a non-ranging frame. We briefly switch to receive mode, check for
|
||||
* data, then let ranging resume.
|
||||
*
|
||||
* This is a cooperative approach: the DW1000Ranging library uses
|
||||
* the transceiver for TWR, and we steal idle slots to listen for
|
||||
* data frames from the tag.
|
||||
*/
|
||||
static uint8_t g_uwb_rx_buf[UWB_RX_BUF_SIZE];
|
||||
|
||||
static void uwb_check_data_rx(void) {
|
||||
/* Check if DW1000 received a frame that DW1000Ranging didn't consume */
|
||||
if (!DW1000.isReceiveDone()) return;
|
||||
|
||||
uint16_t len = DW1000.getDataLength();
|
||||
if (len < sizeof(UwbFrameHeader) || len > UWB_RX_BUF_SIZE) {
|
||||
/* Not our frame or too large — let ranging handle it */
|
||||
return;
|
||||
}
|
||||
|
||||
/* Forward IMU and FALL packets to Jetson serial */
|
||||
if (pkt.msg_type == MSG_IMU || pkt.msg_type == MSG_FALL) {
|
||||
const uint8_t *raw = (const uint8_t *)&pkt;
|
||||
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;
|
||||
}
|
||||
DW1000.getData(g_uwb_rx_buf, len);
|
||||
|
||||
Serial.printf("+ESPNOW:%d,%02X,%ld,%.1f,%d,%02X,%d\r\n",
|
||||
pkt.tag_id, pkt.msg_type, (long)pkt.range_mm,
|
||||
pkt.rssi_dbm, pkt.battery_pct, pkt.flags, pkt.seq_num);
|
||||
/* 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",
|
||||
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) {
|
||||
@ -232,26 +289,10 @@ static void inactiveDevice(DW1000Device *device) {
|
||||
void setup(void) {
|
||||
Serial.begin(SERIAL_BAUD);
|
||||
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 */
|
||||
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 */
|
||||
/* DW1000 ranging */
|
||||
SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI);
|
||||
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);
|
||||
|
||||
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 ───────────────────────────────────────────────────────── */
|
||||
|
||||
/*
|
||||
* Time-division multiplexing: 2 anchors share the same channel.
|
||||
* Each anchor only responds during its time slot to avoid RF collisions.
|
||||
* Slot duration = 50ms → each anchor gets 10 Hz effective rate.
|
||||
* Anchor 0 responds during even slots, anchor 1 during odd slots.
|
||||
* TDM: 2 anchors share the same channel.
|
||||
* Each anchor only responds during its time slot.
|
||||
* Slot = 50ms → each anchor gets 10 Hz effective rate.
|
||||
* Between slots, check for UWB data frames.
|
||||
*/
|
||||
#define SLOT_MS 50
|
||||
#define NUM_ANCHORS_TOTAL 2
|
||||
|
||||
void loop(void) {
|
||||
serial_poll();
|
||||
espnow_process();
|
||||
|
||||
uint32_t slot = (millis() / SLOT_MS) % NUM_ANCHORS_TOTAL;
|
||||
if (slot == ANCHOR_ID) {
|
||||
DW1000Ranging.loop();
|
||||
}
|
||||
|
||||
/* Check for UWB data frames from tag between ranging slots */
|
||||
uwb_check_data_rx();
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Loading…
x
Reference in New Issue
Block a user