diff --git a/esp32/uwb_anchor/src/main.cpp b/esp32/uwb_anchor/src/main.cpp index e022ee2..1f8c5df 100644 --- a/esp32/uwb_anchor/src/main.cpp +++ b/esp32/uwb_anchor/src/main.cpp @@ -27,6 +27,8 @@ * AT+RANGE_ADDR= → pair with specific tag (filter others) * AT+RANGE_ADDR= → clear pairing (accept all tags) * AT+ID? → returns +ID: + * AT+PEER_RANGE= → inter-anchor DS-TWR (for auto-calibration) + * → +PEER_RANGE:,,, * * Pin mapping — Makerfabs ESP32 UWB Pro * ────────────────────────────────────── @@ -188,6 +190,116 @@ static float rx_power_dbm(void) { return 10.0f * log10f((f * f) / (n * n)) - 121.74f; } +/* ── Peer-anchor ranging (initiator role, for auto-calibration) ─── */ + +/* Timeout waiting for peer's RESP during inter-anchor ranging */ +#define PEER_RX_RESP_TIMEOUT_US 3500 +/* Block up to this many ms waiting for the interrupt flags */ +#define PEER_WAIT_MS 20 + +/* Initiate a single DS-TWR exchange toward peer anchor `peer_id`. + * Returns range in mm (>=0) on success, or -1 on timeout/error. + * RSSI is stored in *rssi_out if non-null. + * + * Exchange: + * This anchor → peer POLL + * peer → This RESP (carries T_poll_rx, T_resp_tx) + * This anchor → peer FINAL (carries Ra, Da) + * This side computes its own range estimate from Ra/Da. + */ +static int32_t peer_range_once(uint8_t peer_id, float *rssi_out) { + /* ── Reset interrupt flags ── */ + g_tx_done = g_rx_ok = g_rx_err = g_rx_to = false; + + /* ── Build POLL frame ── */ + uint8_t poll_buf[FRAME_HDR + FCS_LEN] = { + FTYPE_POLL, + (uint8_t)ANCHOR_ID, + peer_id, + }; + + dwt_writetxdata(sizeof(poll_buf), poll_buf, 0); + dwt_writetxfctrl(sizeof(poll_buf), 0, 1); + dwt_setrxtimeout(PEER_RX_RESP_TIMEOUT_US); + dwt_rxenable(DWT_START_RX_IMMEDIATE); + if (dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED) != DWT_SUCCESS) + return -1; + + /* Wait for TX done */ + uint32_t t0 = millis(); + while (!g_tx_done && !g_rx_err && !g_rx_to) { + if (millis() - t0 > (uint32_t)PEER_WAIT_MS) return -1; + } + if (g_rx_err || g_rx_to) return -1; + g_tx_done = false; + + /* Capture T_poll_tx */ + uint64_t T_poll_tx; + dwt_readtxtimestamp((uint8_t *)&T_poll_tx); + T_poll_tx &= DWT_TS_MASK; + + /* Wait for RESP */ + t0 = millis(); + while (!g_rx_ok && !g_rx_err && !g_rx_to) { + if (millis() - t0 > (uint32_t)PEER_WAIT_MS) return -1; + } + if (!g_rx_ok) return -1; + + /* Validate RESP */ + if (g_rx_len < (uint32_t)(FRAME_HDR + RESP_PAYLOAD + FCS_LEN)) return -1; + if (g_rx_buf[0] != FTYPE_RESP) return -1; + if (g_rx_buf[1] != peer_id) return -1; + if (g_rx_buf[2] != (uint8_t)ANCHOR_ID) return -1; + + uint64_t T_resp_rx; + dwt_readrxtimestamp((uint8_t *)&T_resp_rx); + T_resp_rx &= DWT_TS_MASK; + + /* Extract peer timestamps from RESP payload */ + const uint8_t *pl = g_rx_buf + FRAME_HDR; + uint64_t T_poll_rx_peer = ts_read(pl); + uint64_t T_resp_tx_peer = ts_read(pl + 5); + + /* DS-TWR Ra, Da */ + uint64_t Ra = ts_diff(T_resp_rx, T_poll_tx); + uint64_t Da = ts_diff(T_resp_tx_peer, T_poll_rx_peer); + + g_rx_ok = g_rx_err = g_rx_to = false; + + /* ── Build FINAL frame ── */ + uint8_t final_buf[FRAME_HDR + FINAL_PAYLOAD + FCS_LEN]; + final_buf[0] = FTYPE_FINAL; + final_buf[1] = (uint8_t)ANCHOR_ID; + final_buf[2] = peer_id; + ts_write(final_buf + FRAME_HDR, Ra); + ts_write(final_buf + FRAME_HDR + 5, Da); + ts_write(final_buf + FRAME_HDR + 10, (uint64_t)0); /* Db placeholder */ + + dwt_setrxtimeout(0); + dwt_writetxdata(sizeof(final_buf), final_buf, 0); + dwt_writetxfctrl(sizeof(final_buf), 0, 1); + if (dwt_starttx(DWT_START_TX_IMMEDIATE) != DWT_SUCCESS) return -1; + + t0 = millis(); + while (!g_tx_done && !g_rx_err) { + if (millis() - t0 > (uint32_t)PEER_WAIT_MS) return -1; + } + g_tx_done = false; + + /* Simplified one-sided range estimate: tof = Ra - Da/2 */ + double tof_s = ticks_to_s(Ra) - ticks_to_s(Da) / 2.0; + if (tof_s < 0.0) tof_s = 0.0; + int32_t range_mm = (int32_t)(tof_s * SPEED_OF_LIGHT * 1000.0); + + if (rssi_out) *rssi_out = rx_power_dbm(); + + /* Re-enable normal RX for tag ranging */ + dwt_setrxtimeout(0); + dwt_rxenable(DWT_START_RX_IMMEDIATE); + + return range_mm; +} + /* ── AT command handler ─────────────────────────────────────────── */ static char g_at_buf[64]; @@ -212,6 +324,23 @@ static void at_dispatch(const char *cmd) { g_paired_tag_id = (uint8_t)strtoul(v, nullptr, 0); Serial.printf("+OK:PAIRED=0x%02X\r\n", g_paired_tag_id); } + + } else if (strncmp(cmd, "AT+PEER_RANGE=", 14) == 0) { + /* Inter-anchor ranging for calibration. + * Usage: AT+PEER_RANGE= + * Response: +PEER_RANGE:,,, + * or: +PEER_RANGE:ERR,,TIMEOUT + */ + uint8_t peer_id = (uint8_t)strtoul(cmd + 14, nullptr, 0); + float rssi = 0.0f; + int32_t mm = peer_range_once(peer_id, &rssi); + if (mm >= 0) { + Serial.printf("+PEER_RANGE:%d,%d,%ld,%.1f\r\n", + ANCHOR_ID, peer_id, mm, (double)rssi); + } else { + Serial.printf("+PEER_RANGE:ERR,%d,TIMEOUT\r\n", peer_id); + } + } else { Serial.println("+ERR:UNKNOWN_CMD"); }