diff --git a/esp32/uwb_anchor/src/main.cpp b/esp32/uwb_anchor/src/main.cpp index c9ae1ed..3587bb0 100644 --- a/esp32/uwb_anchor/src/main.cpp +++ b/esp32/uwb_anchor/src/main.cpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -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"); } diff --git a/esp32/uwb_tag/src/main.cpp b/esp32/uwb_tag/src/main.cpp index 9b63296..1dd6a93 100644 --- a/esp32/uwb_tag/src/main.cpp +++ b/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"); }