saltylab-firmware/TASK.md

# 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.