esp32/uwb_anchor/src/main.cpp

@@ -27,6 +27,8 @@
  *   AT+RANGE_ADDR=<hex_addr>   → pair with specific tag (filter others)
  *   AT+RANGE_ADDR=             → clear pairing (accept all tags)
  *   AT+ID?                     → returns +ID:<anchor_id>
+ *   AT+PEER_RANGE=<id>         → inter-anchor DS-TWR (for auto-calibration)
+ *                                → +PEER_RANGE:<my>,<peer>,<mm>,<rssi>
  *
  * 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=<peer_anchor_id>
+         * Response: +PEER_RANGE:<my_id>,<peer_id>,<range_mm>,<rssi_dbm>
+         *       or: +PEER_RANGE:ERR,<peer_id>,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");
     }