feat: bidirectional ESP-NOW — anchors share ranges with each other + tag

Anchors:
- Broadcast own range via ESP-NOW after each TWR cycle
- Receive other anchor's range via ESP-NOW, print as +RANGE on serial
- Either anchor's serial port gives Orin BOTH ranges (only need 1 USB/UART)

Tag:
- Receives ESP-NOW from both anchors
- Updates display with both A0 + A1 distances regardless of DW1000 pairing
- Solves the 'only sees A0' display issue

esp32/uwb_anchor/src/main.cpp

@@ -22,6 +22,7 @@
 
 #include <Arduino.h>
 #include <SPI.h>
+#include <string.h>
 #include <WiFi.h>
 #include <esp_now.h>
 #include <esp_wifi.h>
@@ -90,6 +91,32 @@ static void IRAM_ATTR espnow_rx_cb(const esp_now_recv_info_t *info, const uint8_
     g_en_head = next;
 }
 
+/* ── ESP-NOW TX: broadcast own range to other anchor + tag ──────── */
+
+static uint8_t broadcast_mac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+static uint8_t g_seq = 0;
+
+/* 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));
+}
+
 /* ── AT command handler ─────────────────────────────────────────── */
 
 static char g_at_buf[64];
@@ -132,6 +159,19 @@ static void espnow_process(void) {
             continue;
         }
 
+        /* 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;
+        }
+
         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);
@@ -151,6 +191,9 @@ 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) {
@@ -174,7 +217,13 @@ void setup(void) {
     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 rx ok");
+        /* 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");
     }

esp32/uwb_tag/src/main.cpp

@@ -95,6 +95,25 @@ struct EspNowPacket {
 };
 #pragma pack(pop)
 
+/* ── ESP-NOW RX: receive range data from anchors ────────────────── */
+
+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 *pkt = (const EspNowPacket *)data;
+    if (pkt->magic[0] != ESPNOW_MAGIC_0 || pkt->magic[1] != ESPNOW_MAGIC_1) return;
+
+    /* Only process anchor range broadcasts (anchor_id 0 or 1) */
+    if (pkt->msg_type == MSG_RANGE && pkt->anchor_id < NUM_ANCHORS) {
+        int idx = pkt->anchor_id;
+        g_anchor_range_mm[idx] = pkt->range_mm;
+        g_anchor_rssi[idx]     = pkt->rssi_dbm;
+        g_anchor_last_ok[idx]  = millis();
+    }
+}
+
+/* ── ESP-NOW TX ─────────────────────────────────────────────────── */
+
 static void espnow_send(uint8_t msg_type, uint8_t anchor_id,
                          int32_t range_mm, float rssi) {
     EspNowPacket pkt = {};
@@ -363,7 +382,8 @@ void setup(void) {
         peer.channel = 0;
         peer.encrypt = false;
         esp_now_add_peer(&peer);
-        Serial.println("[uwb_tag] ESP-NOW tx ok");
+        esp_now_register_recv_cb(espnow_rx_cb);
+        Serial.println("[uwb_tag] ESP-NOW tx+rx ok");
     } else {
         Serial.println("[uwb_tag] WARN: ESP-NOW failed");
     }