Stamp UWB F Arduino 使用教程

1. 准备工作

环境配置：参考 Arduino IDE 上手教程完成 IDE 安装，并安装 Stamp C5 对应的板管理和驱动库。
使用到的驱动库：
- M5Stamp-UWB
使用到的硬件产品：
- Stamp-C5 DIP
- Stamp UWB F

2. 注意事项

注意

1. 为保证通信质量，请使用高 PSRR LDO 或电源纹波在 12mVpp 以内的电源给 Stamp UWB 供电。
2. UWB 测距至少需要 2 个设备，其中一个设备作为 Tag，另一个设备作为 Anchor。
3. 在进行整机设计时，请注意 FPC 排线及其连接器对模组天线性能的影响。FPC 排线不得经过模组 PCB 天线下方或天线净空区域，否则可能改变天线阻抗和辐射特性，降低射频性能并引入测距误差。建议将 FPC 排线布置在天线区域之外，并在整机装配完成后进行实际测试，以确保产品的通信距离和测距精度满足设计要求。

引脚兼容性

Stamp UWB 通过 SPI 接口通信。下方例程使用 Stamp C5 作为主控，使用其他主控或载板时，需要根据实际接线修改程序中的引脚定义。

Stamp C5 与 Stamp UWB F 的引脚连接如下：

Stamp UWB F 引脚	Stamp C5 GPIO
GP7	G23
IRQ	G0
WAKEUP	G24
RST	G25
MISO	G26
MOSI	G27
CS	G11
SCK	G12

3. 双边测距（DS-TWR）

3.1 Tag 端

cpp 
#include <Arduino.h>
#include <M5Stamp_UWB.h>

static constexpr uint32_t RANGE_INTERVAL_MS = 200;
static constexpr uint32_t LOG_INTERVAL      = 10;

M5Stamp_UWB uwb;
M5Stamp_UWBDSRangeConfig rangeConfig;
bool uwbReady        = false;
uint32_t lastRangeMs = 0;
uint32_t rangeCount  = 0;
uint32_t okCount     = 0;

static bool initUwb()
{
    // Stamp-C5 connections to the onboard QM33120 UWB transceiver.
    M5Stamp_UWBConfig config;
    config.pin_gp7    = 23;
    config.pin_irq    = 0;
    config.pin_wakeup = 24;
    config.pin_rst    = 25;
    config.pin_miso   = 26;
    config.pin_mosi   = 27;
    config.pin_cs     = 11;
    config.pin_sck    = 12;

    M5Stamp_UWBPHYConfig phy;
    phy.channel = M5Stamp_UWBChannel::Channel5;

    // Both devices must use the same network addresses and DS-TWR timing.
    rangeConfig.panId                          = 0xDECA;
    rangeConfig.initiatorAddress               = 0x0001;
    rangeConfig.responderAddress               = 0x0002;
    rangeConfig.responseRxAfterTxDelayUus      = 1500;
    rangeConfig.responseTxDelayUus             = 3000;
    rangeConfig.finalTxDelayUus                = 1800;
    rangeConfig.finalRxAfterResponseTxDelayUus = 500;
    rangeConfig.resultRxAfterFinalTxDelayUus   = 500;
    rangeConfig.rxTimeoutUus                   = 3000;
    rangeConfig.hostTimeoutMs                  = 100;
    rangeConfig.resultRepeatCount              = 3;
    rangeConfig.resultRepeatGapMs              = 3;

    if (!uwb.begin(config, phy)) {
        Serial.printf("UWB_BEGIN,result=FAIL,error=%s\n", uwb.lastErrorName());
        Serial.printf("UWB_RAW_ID,dev_id=0x%08lX\n", static_cast<unsigned long>(uwb.readRawDeviceId()));
        return false;
    }

    const uint32_t devId = uwb.deviceId();
    Serial.printf("UWB_ID,dev_id=0x%08lX,chip=%s\n", static_cast<unsigned long>(devId), uwb.chipName());
    if (devId != M5STAMP_UWB_QM33120_DEVICE_ID) {
        Serial.printf("UWB_ID,result=FAIL,expected=0xDECA0314\n");
        return false;
    }

    Serial.printf("UWB_CONFIG,result=OK,ch=%u,plen=%u,rate=6M8,tx_power=0x%08lX\n", static_cast<unsigned>(phy.channel),
                  static_cast<unsigned>(phy.preambleLength), static_cast<unsigned long>(phy.txPower));
    return true;
}

static void runRanging()
{
    // Start one DS-TWR exchange at the configured interval.
    if (millis() - lastRangeMs < RANGE_INTERVAL_MS) return;
    lastRangeMs = millis();

    const M5Stamp_UWBDSRangeResult result = uwb.requestDSRange(rangeConfig);
    rangeCount++;
    okCount += result.success;

    // Print accumulated statistics every LOG_INTERVAL attempts.
    if (rangeCount % LOG_INTERVAL) return;

    const uint32_t failCount = rangeCount - okCount;
    if (result.success) {
        Serial.printf(
            "DS_RANGE_STAT,count=%lu,ok=%lu,fail=%lu,last=OK,seq=%u,distance_mm=%ld,distance_m=%.3f,elapsed_ms=%lu\n",
            static_cast<unsigned long>(rangeCount), static_cast<unsigned long>(okCount),
            static_cast<unsigned long>(failCount), result.sequence, static_cast<long>(result.distanceMm),
            result.distanceM, static_cast<unsigned long>(result.elapsedMs));
    } else {
        Serial.printf("DS_RANGE_STAT,count=%lu,ok=%lu,fail=%lu,last=FAIL,seq=%u,error=%s\n",
                      static_cast<unsigned long>(rangeCount), static_cast<unsigned long>(okCount),
                      static_cast<unsigned long>(failCount), result.sequence, uwb.lastErrorName());
    }
}

void setup()
{
    Serial.begin(115200);
    delay(1000);
    Serial.printf("M5Stamp UWB DS-TWR TAG\n");
    Serial.printf("ROLE,mode=TAG\n");
    Serial.printf("TWR_MODE,mode=DS-TWR\n");
    uwbReady = initUwb();
    Serial.printf("TEST_START,result=%s\n", uwbReady ? "OK" : "FAIL");
}

void loop()
{
    if (uwbReady) runRanging();
    else delay(1000);
}

串口输出示例：

M5Stamp UWB DS-TWR TAG
ROLE,mode=TAG
TWR_MODE,mode=DS-TWR
UWB_ID,dev_id=0xDECA0314,chip=QM33120/DW3720
UWB_CONFIG,result=OK,ch=5,plen=128,rate=6M8,tx_power=0xFEFEFEFE
TEST_START,result=OK
DS_RANGE_STAT,count=10,ok=10,fail=0,last=OK,seq=10,distance_mm=164,distance_m=0.164,elapsed_ms=7
DS_RANGE_STAT,count=20,ok=20,fail=0,last=OK,seq=20,distance_mm=166,distance_m=0.166,elapsed_ms=7
DS_RANGE_STAT,count=30,ok=30,fail=0,last=OK,seq=30,distance_mm=171,distance_m=0.171,elapsed_ms=7
DS_RANGE_STAT,count=40,ok=40,fail=0,last=OK,seq=40,distance_mm=168,distance_m=0.168,elapsed_ms=7
DS_RANGE_STAT,count=50,ok=50,fail=0,last=OK,seq=50,distance_mm=153,distance_m=0.153,elapsed_ms=7
DS_RANGE_STAT,count=60,ok=60,fail=0,last=OK,seq=60,distance_mm=196,distance_m=0.196,elapsed_ms=7
DS_RANGE_STAT,count=70,ok=70,fail=0,last=OK,seq=70,distance_mm=172,distance_m=0.172,elapsed_ms=7
DS_RANGE_STAT,count=80,ok=80,fail=0,last=OK,seq=80,distance_mm=182,distance_m=0.182,elapsed_ms=7
DS_RANGE_STAT,count=90,ok=90,fail=0,last=OK,seq=90,distance_mm=160,distance_m=0.160,elapsed_ms=7
DS_RANGE_STAT,count=100,ok=100,fail=0,last=OK,seq=100,distance_mm=157,distance_m=0.157,elapsed_ms=7
DS_RANGE_STAT,count=110,ok=110,fail=0,last=OK,seq=110,distance_mm=210,distance_m=0.210,elapsed_ms=7
DS_RANGE_STAT,count=120,ok=120,fail=0,last=OK,seq=120,distance_mm=164,distance_m=0.164,elapsed_ms=7
DS_RANGE_STAT,count=130,ok=130,fail=0,last=OK,seq=130,distance_mm=174,distance_m=0.174,elapsed_ms=7 

3.2 Anchor 端

cpp 
#include <Arduino.h>
#include <M5Stamp_UWB.h>

static constexpr uint32_t LOG_INTERVAL = 20;

M5Stamp_UWB uwb;
M5Stamp_UWBDSRangeConfig rangeConfig;
bool uwbReady          = false;
uint32_t responseCount = 0;
uint32_t failCount     = 0;

static bool initUwb()
{
    // Stamp-C5 connections to the onboard QM33120 UWB transceiver.
    M5Stamp_UWBConfig config;
    config.pin_gp7    = 23;
    config.pin_irq    = 0;
    config.pin_wakeup = 24;
    config.pin_rst    = 25;
    config.pin_miso   = 26;
    config.pin_mosi   = 27;
    config.pin_cs     = 11;
    config.pin_sck    = 12;

    M5Stamp_UWBPHYConfig phy;
    phy.channel = M5Stamp_UWBChannel::Channel5;

    // Both devices must use the same network addresses and DS-TWR timing.
    rangeConfig.panId                          = 0xDECA;
    rangeConfig.initiatorAddress               = 0x0001;
    rangeConfig.responderAddress               = 0x0002;
    rangeConfig.responseRxAfterTxDelayUus      = 1500;
    rangeConfig.responseTxDelayUus             = 3000;
    rangeConfig.finalTxDelayUus                = 1800;
    rangeConfig.finalRxAfterResponseTxDelayUus = 500;
    rangeConfig.resultRxAfterFinalTxDelayUus   = 500;
    rangeConfig.rxTimeoutUus                   = 3000;
    rangeConfig.hostTimeoutMs                  = 100;
    rangeConfig.resultRepeatCount              = 3;
    rangeConfig.resultRepeatGapMs              = 3;

    if (!uwb.begin(config, phy)) {
        Serial.printf("UWB_BEGIN,result=FAIL,error=%s\n", uwb.lastErrorName());
        Serial.printf("UWB_RAW_ID,dev_id=0x%08lX\n", static_cast<unsigned long>(uwb.readRawDeviceId()));
        return false;
    }

    const uint32_t devId = uwb.deviceId();
    Serial.printf("UWB_ID,dev_id=0x%08lX,chip=%s\n", static_cast<unsigned long>(devId), uwb.chipName());
    if (devId != M5STAMP_UWB_QM33120_DEVICE_ID) {
        Serial.printf("UWB_ID,result=FAIL,expected=0xDECA0314\n");
        return false;
    }

    Serial.printf("UWB_CONFIG,result=OK,ch=%u,plen=%u,rate=6M8,tx_power=0x%08lX\n", static_cast<unsigned>(phy.channel),
                  static_cast<unsigned>(phy.preambleLength), static_cast<unsigned long>(phy.txPower));
    return true;
}

static void runResponder()
{
    // Wait for a Poll/Final exchange and return the DS-TWR distance result.
    const M5Stamp_UWBDSResponderResult result = uwb.respondDSRange(rangeConfig);
    if (!result.success) {
        // Idle receive timeouts are expected and are not counted as failures.
        if (result.error == M5Stamp_UWBError::RxTimeout) return;
        if (++failCount % LOG_INTERVAL == 0) {
            Serial.printf("DS_RESP_STAT,count=%lu,fail=%lu,last=FAIL,error=%s\n",
                          static_cast<unsigned long>(responseCount), static_cast<unsigned long>(failCount),
                          uwb.lastErrorName());
        }
        return;
    }

    // Print accumulated statistics every LOG_INTERVAL responses.
    if (++responseCount % LOG_INTERVAL == 0) {
        Serial.printf(
            "DS_RESP_STAT,count=%lu,fail=%lu,last=OK,seq=%u,requester=0x%X,distance_mm=%ld,distance_m=%.3f,elapsed_ms=%lu\n",
            static_cast<unsigned long>(responseCount), static_cast<unsigned long>(failCount), result.sequence,
            result.requester, static_cast<long>(result.distanceMm), result.distanceM,
            static_cast<unsigned long>(result.elapsedMs));
    }
}

void setup()
{
    Serial.begin(115200);
    delay(1000);
    Serial.printf("M5Stamp UWB DS-TWR ANCHOR\n");
    Serial.printf("ROLE,mode=ANCHOR\n");
    Serial.printf("TWR_MODE,mode=DS-TWR\n");
    uwbReady = initUwb();
    Serial.printf("TEST_START,result=%s\n", uwbReady ? "OK" : "FAIL");
}

void loop()
{
    if (uwbReady) runResponder();
    else delay(1000);
}

串口输出示例：

M5Stamp UWB DS-TWR ANCHOR
ROLE,mode=ANCHOR
TWR_MODE,mode=DS-TWR
UWB_ID,dev_id=0xDECA0314,chip=QM33120/DW3720
UWB_CONFIG,result=OK,ch=5,plen=128,rate=6M8,tx_power=0xFEFEFEFE
TEST_START,result=OK
DS_RESP_STAT,count=9500,fail=0,last=OK,seq=7,requester=0x1,distance_mm=162,distance_m=0.162,elapsed_ms=98
DS_RESP_STAT,count=9520,fail=0,last=OK,seq=27,requester=0x1,distance_mm=185,distance_m=0.185,elapsed_ms=93
DS_RESP_STAT,count=9540,fail=0,last=OK,seq=47,requester=0x1,distance_mm=181,distance_m=0.181,elapsed_ms=98
DS_RESP_STAT,count=9560,fail=0,last=OK,seq=67,requester=0x1,distance_mm=167,distance_m=0.167,elapsed_ms=98
DS_RESP_STAT,count=9580,fail=0,last=OK,seq=87,requester=0x1,distance_mm=193,distance_m=0.193,elapsed_ms=98
DS_RESP_STAT,count=9600,fail=0,last=OK,seq=107,requester=0x1,distance_mm=166,distance_m=0.166,elapsed_ms=98
DS_RESP_STAT,count=9620,fail=0,last=OK,seq=127,requester=0x1,distance_mm=164,distance_m=0.164,elapsed_ms=98 

4. 单边测距（SS-TWR）

注意

此测距方式最终计算距离时只使用了 Tag 端的时间戳，因此测距精度受 Tag 端的时钟影响较大，误差较大，不推荐使用此方式。若需要更高精度的测距结果，请使用双边测距（DS-TWR）方式。

4.1 Tag 端

cpp 
#include <Arduino.h>
#include <M5Stamp_UWB.h>

static constexpr uint32_t RANGE_INTERVAL_MS = 200;
static constexpr uint32_t LOG_INTERVAL      = 10;

M5Stamp_UWB uwb;
M5Stamp_UWBRangeConfig rangeConfig;
bool uwbReady        = false;
uint32_t lastRangeMs = 0;
uint32_t rangeCount  = 0;
uint32_t okCount     = 0;

static bool initUwb()
{
    // Stamp-C5 connections to the onboard QM33120 UWB transceiver.
    M5Stamp_UWBConfig config;
    config.pin_gp7    = 23;
    config.pin_irq    = 0;
    config.pin_wakeup = 24;
    config.pin_rst    = 25;
    config.pin_miso   = 26;
    config.pin_mosi   = 27;
    config.pin_cs     = 11;
    config.pin_sck    = 12;

    M5Stamp_UWBPHYConfig phy;
    phy.channel = M5Stamp_UWBChannel::Channel5;

    // Both devices must use the same PAN ID, addresses, and response timing.
    rangeConfig.panId                     = 0xDECA;
    rangeConfig.initiatorAddress          = 0x0001;
    rangeConfig.responderAddress          = 0x0002;
    rangeConfig.responseRxAfterTxDelayUus = 500;
    rangeConfig.responseTxDelayUus        = 3000;
    rangeConfig.rxTimeoutUus              = 6000;
    rangeConfig.hostTimeoutMs             = 100;

    if (!uwb.begin(config, phy)) {
        Serial.printf("UWB_BEGIN,result=FAIL,error=%s\n", uwb.lastErrorName());
        Serial.printf("UWB_RAW_ID,dev_id=0x%08lX\n", static_cast<unsigned long>(uwb.readRawDeviceId()));
        return false;
    }

    const uint32_t devId = uwb.deviceId();
    Serial.printf("UWB_ID,dev_id=0x%08lX,chip=%s\n", static_cast<unsigned long>(devId), uwb.chipName());
    if (devId != M5STAMP_UWB_QM33120_DEVICE_ID) {
        Serial.printf("UWB_ID,result=FAIL,expected=0xDECA0314\n");
        return false;
    }

    Serial.printf("UWB_CONFIG,result=OK,ch=%u,plen=%u,rate=6M8,tx_power=0x%08lX\n", static_cast<unsigned>(phy.channel),
                  static_cast<unsigned>(phy.preambleLength), static_cast<unsigned long>(phy.txPower));
    return true;
}

static void runRanging()
{
    // Start one SS-TWR exchange at the configured interval.
    if (millis() - lastRangeMs < RANGE_INTERVAL_MS) return;
    lastRangeMs = millis();

    const M5Stamp_UWBRangeResult result = uwb.requestRange(rangeConfig);
    rangeCount++;
    okCount += result.success;

    // Print accumulated statistics every LOG_INTERVAL attempts.
    if (rangeCount % LOG_INTERVAL) return;

    const uint32_t failCount = rangeCount - okCount;
    if (result.success) {
        Serial.printf(
            "SS_RANGE_STAT,count=%lu,ok=%lu,fail=%lu,last=OK,seq=%u,distance_mm=%ld,distance_m=%.3f,elapsed_ms=%lu\n",
            static_cast<unsigned long>(rangeCount), static_cast<unsigned long>(okCount),
            static_cast<unsigned long>(failCount), result.sequence, static_cast<long>(result.distanceMm),
            result.distanceM, static_cast<unsigned long>(result.elapsedMs));
    } else {
        Serial.printf("SS_RANGE_STAT,count=%lu,ok=%lu,fail=%lu,last=FAIL,seq=%u,error=%s\n",
                      static_cast<unsigned long>(rangeCount), static_cast<unsigned long>(okCount),
                      static_cast<unsigned long>(failCount), result.sequence, uwb.lastErrorName());
    }
}

void setup()
{
    Serial.begin(115200);
    delay(1000);
    Serial.printf("M5Stamp UWB SS-TWR TAG\n");
    Serial.printf("ROLE,mode=TAG\n");
    Serial.printf("TWR_MODE,mode=SS-TWR\n");
    uwbReady = initUwb();
    Serial.printf("TEST_START,result=%s\n", uwbReady ? "OK" : "FAIL");
}

void loop()
{
    if (uwbReady) runRanging();
    else delay(1000);
}

串口输出示例：

M5Stamp UWB SS-TWR TAG
ROLE,mode=TAG
TWR_MODE,mode=SS-TWR
UWB_ID,dev_id=0xDECA0314,chip=QM33120/DW3720
UWB_CONFIG,result=OK,ch=5,plen=128,rate=6M8,tx_power=0xFEFEFEFE
TEST_START,result=OK
SS_RANGE_STAT,count=10,ok=10,fail=0,last=OK,seq=10,distance_mm=2270,distance_m=2.270,elapsed_ms=4
SS_RANGE_STAT,count=20,ok=20,fail=0,last=OK,seq=20,distance_mm=2282,distance_m=2.282,elapsed_ms=4
SS_RANGE_STAT,count=30,ok=30,fail=0,last=OK,seq=30,distance_mm=2275,distance_m=2.275,elapsed_ms=4
SS_RANGE_STAT,count=40,ok=40,fail=0,last=OK,seq=40,distance_mm=2251,distance_m=2.251,elapsed_ms=4
SS_RANGE_STAT,count=50,ok=50,fail=0,last=OK,seq=50,distance_mm=2265,distance_m=2.265,elapsed_ms=4
SS_RANGE_STAT,count=60,ok=60,fail=0,last=OK,seq=60,distance_mm=2301,distance_m=2.301,elapsed_ms=4
SS_RANGE_STAT,count=70,ok=70,fail=0,last=OK,seq=70,distance_mm=2301,distance_m=2.301,elapsed_ms=4
SS_RANGE_STAT,count=80,ok=80,fail=0,last=OK,seq=80,distance_mm=2291,distance_m=2.291,elapsed_ms=4
SS_RANGE_STAT,count=90,ok=90,fail=0,last=OK,seq=90,distance_mm=2282,distance_m=2.282,elapsed_ms=4
SS_RANGE_STAT,count=100,ok=100,fail=0,last=OK,seq=100,distance_mm=2312,distance_m=2.312,elapsed_ms=4 

4.2 Anchor 端

cpp 
#include <Arduino.h>
#include <M5Stamp_UWB.h>

static constexpr uint32_t LOG_INTERVAL = 20;

M5Stamp_UWB uwb;
M5Stamp_UWBRangeConfig rangeConfig;
bool uwbReady         = false;
uint32_t responseCount = 0;
uint32_t failCount     = 0;

static bool initUwb()
{
    // Stamp-C5 connections to the onboard QM33120 UWB transceiver.
    M5Stamp_UWBConfig config;
    config.pin_gp7    = 23;
    config.pin_irq    = 0;
    config.pin_wakeup = 24;
    config.pin_rst    = 25;
    config.pin_miso   = 26;
    config.pin_mosi   = 27;
    config.pin_cs     = 11;
    config.pin_sck    = 12;

    M5Stamp_UWBPHYConfig phy;
    phy.channel = M5Stamp_UWBChannel::Channel5;

    // Both devices must use the same PAN ID, addresses, and response timing.
    rangeConfig.panId                     = 0xDECA;
    rangeConfig.initiatorAddress          = 0x0001;
    rangeConfig.responderAddress          = 0x0002;
    rangeConfig.responseRxAfterTxDelayUus = 500;
    rangeConfig.responseTxDelayUus        = 3000;
    rangeConfig.rxTimeoutUus              = 6000;
    rangeConfig.hostTimeoutMs             = 100;

    if (!uwb.begin(config, phy)) {
        Serial.printf("UWB_BEGIN,result=FAIL,error=%s\n", uwb.lastErrorName());
        Serial.printf("UWB_RAW_ID,dev_id=0x%08lX\n", static_cast<unsigned long>(uwb.readRawDeviceId()));
        return false;
    }

    const uint32_t devId = uwb.deviceId();
    Serial.printf("UWB_ID,dev_id=0x%08lX,chip=%s\n", static_cast<unsigned long>(devId), uwb.chipName());
    if (devId != M5STAMP_UWB_QM33120_DEVICE_ID) {
        Serial.printf("UWB_ID,result=FAIL,expected=0xDECA0314\n");
        return false;
    }

    Serial.printf("UWB_CONFIG,result=OK,ch=%u,plen=%u,rate=6M8,tx_power=0x%08lX\n", static_cast<unsigned>(phy.channel),
                  static_cast<unsigned>(phy.preambleLength), static_cast<unsigned long>(phy.txPower));
    return true;
}

static void runResponder()
{
    // Wait for a Poll frame and send the delayed SS-TWR Response frame.
    const M5Stamp_UWBResponderResult result = uwb.respondRange(rangeConfig);
    if (!result.success) {
        // Idle receive timeouts are expected and are not counted as failures.
        if (result.error == M5Stamp_UWBError::RxTimeout) return;
        if (++failCount % LOG_INTERVAL == 0) {
            Serial.printf("SS_RESP_STAT,count=%lu,fail=%lu,last=FAIL,error=%s\n",
                          static_cast<unsigned long>(responseCount), static_cast<unsigned long>(failCount),
                          uwb.lastErrorName());
        }
        return;
    }

    // Print accumulated statistics every LOG_INTERVAL responses.
    if (++responseCount % LOG_INTERVAL == 0) {
        Serial.printf("SS_RESP_STAT,count=%lu,fail=%lu,last=OK,seq=%u,requester=0x%X,elapsed_ms=%lu\n",
                      static_cast<unsigned long>(responseCount), static_cast<unsigned long>(failCount), result.sequence,
                      result.requester, static_cast<unsigned long>(result.elapsedMs));
    }
}

void setup()
{
    Serial.begin(115200);
    delay(1000);
    Serial.printf("M5Stamp UWB SS-TWR ANCHOR\n");
    Serial.printf("ROLE,mode=ANCHOR\n");
    Serial.printf("TWR_MODE,mode=SS-TWR\n");
    uwbReady = initUwb();
    Serial.printf("TEST_START,result=%s\n", uwbReady ? "OK" : "FAIL");
}

void loop()
{
    if (uwbReady) runResponder();
    else delay(1000);
}

串口输出示例：

M5Stamp UWB SS-TWR ANCHOR
ROLE,mode=ANCHOR
TWR_MODE,mode=SS-TWR
UWB_ID,dev_id=0xDECA0314,chip=QM33120/DW3720
UWB_CONFIG,result=OK,ch=5,plen=128,rate=6M8,tx_power=0xFEFEFEFE
TEST_START,result=OK
SS_RESP_STAT,count=1260,fail=0,last=OK,seq=12,requester=0x1,elapsed_ms=98
SS_RESP_STAT,count=1280,fail=0,last=OK,seq=32,requester=0x1,elapsed_ms=98
SS_RESP_STAT,count=1300,fail=0,last=OK,seq=52,requester=0x1,elapsed_ms=93
SS_RESP_STAT,count=1320,fail=0,last=OK,seq=72,requester=0x1,elapsed_ms=98
SS_RESP_STAT,count=1340,fail=0,last=OK,seq=92,requester=0x1,elapsed_ms=98 

5. 多 Anchor 测距（1 个 Tag 与多个 Anchor）

下方例程适用于 1 个 Tag 与多个 Anchor 的测距场景。Tag 会按照 ANCHOR_SHORT_ADDRS 中的地址依次发起 DS-TWR 测距，Anchor 只响应发往自身 ANCHOR_SHORT_ADDR 的请求，Tag 最终输出与各 Anchor 之间的距离，不进行二维或三维坐标解算。请为每个 Anchor 设置唯一的短地址，并将所有 Anchor 地址填写到 Tag 端的地址列表中；Tag 与所有 Anchor 必须使用相同的 PAN ID、UWB 信道、Tag 地址和 DS-TWR 时序参数。Tag 在切换 Anchor 前会等待一段时间，测距失败时会对当前 Anchor 重试，以降低残留帧或帧冲突的影响。若要实现二维或三维定位，还需要预先确定各 Anchor 的坐标，并在上层程序中增加多边测量定位算法。

5.1 Tag 端

cpp 
#include <Arduino.h>
#include <M5Stamp_UWB.h>

static constexpr uint32_t RANGE_INTERVAL_MS    = 1000;
static constexpr uint32_t ANCHOR_SWITCH_GAP_MS = 30;
static constexpr uint8_t ANCHOR_RETRY_COUNT    = 3;

static constexpr uint16_t PAN_ID = 0xDECA;
static constexpr uint16_t TAG_SHORT_ADDR = 0x0001;
static constexpr M5Stamp_UWBChannel UWB_CHANNEL = M5Stamp_UWBChannel::Channel5;

// Add or remove Anchor addresses as needed. Every Anchor must be unique.
static constexpr uint16_t ANCHOR_SHORT_ADDRS[] = {0x0002, 0x0003, 0x0004};
static constexpr size_t ANCHOR_COUNT = sizeof(ANCHOR_SHORT_ADDRS) / sizeof(ANCHOR_SHORT_ADDRS[0]);

M5Stamp_UWB uwb;
M5Stamp_UWBDSRangeConfig rangeConfig;
bool uwbReady        = false;
uint32_t lastRangeMs = 0;
uint32_t rangeCount  = 0;
uint32_t okCount     = 0;

static void setRangeConfig(uint16_t anchorAddress)
{
    // Both devices must use the same network addresses and DS-TWR timing.
    rangeConfig.panId                          = PAN_ID;
    rangeConfig.initiatorAddress               = TAG_SHORT_ADDR;
    rangeConfig.responderAddress               = anchorAddress;
    rangeConfig.responseRxAfterTxDelayUus      = 1500;
    rangeConfig.responseTxDelayUus             = 3000;
    rangeConfig.finalTxDelayUus                = 1800;
    rangeConfig.finalRxAfterResponseTxDelayUus = 500;
    rangeConfig.resultRxAfterFinalTxDelayUus   = 500;
    rangeConfig.rxTimeoutUus                   = 3000;
    rangeConfig.hostTimeoutMs                  = 100;
    rangeConfig.resultRepeatCount              = 3;
    rangeConfig.resultRepeatGapMs              = 3;
}

static bool initUwb()
{
    M5Stamp_UWBConfig config;
    config.pin_gp7    = 23;
    config.pin_irq    = 0;
    config.pin_wakeup = 24;
    config.pin_rst    = 25;
    config.pin_miso   = 26;
    config.pin_mosi   = 27;
    config.pin_cs     = 11;
    config.pin_sck    = 12;

    M5Stamp_UWBPHYConfig phy;
    phy.channel = UWB_CHANNEL;

    if (!uwb.begin(config, phy)) {
        Serial.printf("UWB_BEGIN,result=FAIL,error=%s\n", uwb.lastErrorName());
        Serial.printf("UWB_RAW_ID,dev_id=0x%08lX\n",
                      static_cast<unsigned long>(uwb.readRawDeviceId()));
        return false;
    }

    const uint32_t devId = uwb.deviceId();
    Serial.printf("UWB_ID,dev_id=0x%08lX,chip=%s\n",
                  static_cast<unsigned long>(devId), uwb.chipName());

    if (devId != M5STAMP_UWB_QM33120_DEVICE_ID) {
        Serial.printf("UWB_ID,result=FAIL,expected=0xDECA0314\n");
        return false;
    }

    Serial.printf("UWB_CONFIG,result=OK,ch=%u,plen=%u,rate=6M8,tx_power=0x%08lX\n",
                  static_cast<unsigned>(phy.channel),
                  static_cast<unsigned>(phy.preambleLength),
                  static_cast<unsigned long>(phy.txPower));
    return true;
}

static M5Stamp_UWBDSRangeResult rangeAnchor(uint16_t anchorAddress)
{
    M5Stamp_UWBDSRangeResult result;

    for (uint8_t attempt = 0; attempt < ANCHOR_RETRY_COUNT; ++attempt) {
        setRangeConfig(anchorAddress);
        result = uwb.requestDSRange(rangeConfig);

        if (result.success) return result;

        if (attempt + 1 < ANCHOR_RETRY_COUNT) {
            // Clear stale frames before retrying the same Anchor.
            delay(ANCHOR_SWITCH_GAP_MS);
        }
    }

    return result;
}

static void runRanging()
{
    if (millis() - lastRangeMs < RANGE_INTERVAL_MS) return;
    lastRangeMs = millis();

    // Range each Anchor in sequence so their responses do not collide.
    for (size_t i = 0; i < ANCHOR_COUNT; ++i) {
        const uint16_t anchorAddress = ANCHOR_SHORT_ADDRS[i];
        const M5Stamp_UWBDSRangeResult result = rangeAnchor(anchorAddress);

        rangeCount++;
        okCount += result.success;

        const uint32_t failCount = rangeCount - okCount;

        if (result.success) {
            Serial.printf(
                "MULTI_ANCHOR_RANGE,count=%lu,ok=%lu,fail=%lu,anchor=0x%04X,last=OK,seq=%u,distance_mm=%ld,distance_m=%.3f,elapsed_ms=%lu\n",
                static_cast<unsigned long>(rangeCount),
                static_cast<unsigned long>(okCount),
                static_cast<unsigned long>(failCount),
                static_cast<unsigned>(anchorAddress),
                result.sequence,
                static_cast<long>(result.distanceMm),
                result.distanceM,
                static_cast<unsigned long>(result.elapsedMs));
        } else {
            Serial.printf(
                "MULTI_ANCHOR_RANGE,count=%lu,ok=%lu,fail=%lu,anchor=0x%04X,last=FAIL,seq=%u,error=%s\n",
                static_cast<unsigned long>(rangeCount),
                static_cast<unsigned long>(okCount),
                static_cast<unsigned long>(failCount),
                static_cast<unsigned>(anchorAddress),
                result.sequence,
                uwb.lastErrorName());
        }

        if (i + 1 < ANCHOR_COUNT) {
            // Wait for the current Anchor to finish repeated Result frames.
            delay(ANCHOR_SWITCH_GAP_MS);
        }
    }
}

void setup()
{
    Serial.begin(115200);
    delay(1000);

    Serial.printf("M5Stamp UWB DS-TWR MULTI-ANCHOR TAG\n");
    Serial.printf("ROLE,mode=TAG\n");
    Serial.printf("TWR_MODE,mode=DS-TWR\n");

    uwbReady = initUwb();

    Serial.printf("TEST_START,result=%s,anchors=%u\n",
                  uwbReady ? "OK" : "FAIL",
                  static_cast<unsigned>(ANCHOR_COUNT));
}

void loop()
{
    if (uwbReady) runRanging();
    else delay(1000);
}

串口输出示例：

M5Stamp UWB DS-TWR MULTI-ANCHOR TAG
ROLE,mode=TAG
TWR_MODE,mode=DS-TWR
UWB_ID,dev_id=0xDECA0314,chip=QM33120/DW3720
UWB_CONFIG,result=OK,ch=5,plen=128,rate=6M8,tx_power=0xFEFEFEFE
TEST_START,result=OK,anchors=3
MULTI_ANCHOR_RANGE,count=1,ok=1,fail=0,anchor=0x0002,last=OK,seq=1,distance_mm=1014,distance_m=1.014,elapsed_ms=7
MULTI_ANCHOR_RANGE,count=2,ok=2,fail=0,anchor=0x0003,last=OK,seq=2,distance_mm=478,distance_m=0.478,elapsed_ms=7
MULTI_ANCHOR_RANGE,count=3,ok=3,fail=0,anchor=0x0004,last=OK,seq=3,distance_mm=703,distance_m=0.703,elapsed_ms=7
MULTI_ANCHOR_RANGE,count=4,ok=4,fail=0,anchor=0x0002,last=OK,seq=4,distance_mm=977,distance_m=0.977,elapsed_ms=7
MULTI_ANCHOR_RANGE,count=5,ok=5,fail=0,anchor=0x0003,last=OK,seq=5,distance_mm=233,distance_m=0.233,elapsed_ms=7
MULTI_ANCHOR_RANGE,count=6,ok=6,fail=0,anchor=0x0004,last=OK,seq=6,distance_mm=758,distance_m=0.758,elapsed_ms=7
MULTI_ANCHOR_RANGE,count=7,ok=7,fail=0,anchor=0x0002,last=OK,seq=7,distance_mm=1009,distance_m=1.009,elapsed_ms=7
MULTI_ANCHOR_RANGE,count=8,ok=8,fail=0,anchor=0x0003,last=OK,seq=8,distance_mm=55,distance_m=0.055,elapsed_ms=7
MULTI_ANCHOR_RANGE,count=9,ok=9,fail=0,anchor=0x0004,last=OK,seq=9,distance_mm=679,distance_m=0.679,elapsed_ms=7 

5.2 Anchor 端

cpp 
#include <Arduino.h>
#include <M5Stamp_UWB.h>

static constexpr uint32_t LOG_INTERVAL = 20;

static constexpr uint16_t PAN_ID = 0xDECA;
static constexpr uint16_t TAG_SHORT_ADDR = 0x0001;

// Change this value on every Anchor. Do not reuse an address.
static constexpr uint16_t ANCHOR_SHORT_ADDR = 0x0002;

static constexpr M5Stamp_UWBChannel UWB_CHANNEL = M5Stamp_UWBChannel::Channel5;

M5Stamp_UWB uwb;
M5Stamp_UWBDSRangeConfig rangeConfig;
bool uwbReady      = false;
uint32_t responseCount = 0;
uint32_t failCount     = 0;

static void setRangeConfig()
{
    // Both devices must use the same network addresses and DS-TWR timing.
    rangeConfig.panId                          = PAN_ID;
    rangeConfig.initiatorAddress               = TAG_SHORT_ADDR;
    rangeConfig.responderAddress               = ANCHOR_SHORT_ADDR;
    rangeConfig.responseRxAfterTxDelayUus      = 1500;
    rangeConfig.responseTxDelayUus             = 3000;
    rangeConfig.finalTxDelayUus                = 1800;
    rangeConfig.finalRxAfterResponseTxDelayUus = 500;
    rangeConfig.resultRxAfterFinalTxDelayUus   = 500;
    rangeConfig.rxTimeoutUus                   = 3000;
    rangeConfig.hostTimeoutMs                  = 100;
    rangeConfig.resultRepeatCount              = 3;
    rangeConfig.resultRepeatGapMs              = 3;
}

static bool initUwb()
{
    M5Stamp_UWBConfig config;
    config.pin_gp7    = 23;
    config.pin_irq    = 0;
    config.pin_wakeup = 24;
    config.pin_rst    = 25;
    config.pin_miso   = 26;
    config.pin_mosi   = 27;
    config.pin_cs     = 11;
    config.pin_sck    = 12;

    M5Stamp_UWBPHYConfig phy;
    phy.channel = UWB_CHANNEL;

    if (!uwb.begin(config, phy)) {
        Serial.printf("UWB_BEGIN,result=FAIL,error=%s\n", uwb.lastErrorName());
        Serial.printf("UWB_RAW_ID,dev_id=0x%08lX\n",
                      static_cast<unsigned long>(uwb.readRawDeviceId()));
        return false;
    }

    const uint32_t devId = uwb.deviceId();
    Serial.printf("UWB_ID,dev_id=0x%08lX,chip=%s\n",
                  static_cast<unsigned long>(devId), uwb.chipName());

    if (devId != M5STAMP_UWB_QM33120_DEVICE_ID) {
        Serial.printf("UWB_ID,result=FAIL,expected=0xDECA0314\n");
        return false;
    }

    Serial.printf("UWB_CONFIG,result=OK,ch=%u,plen=%u,rate=6M8,tx_power=0x%08lX\n",
                  static_cast<unsigned>(phy.channel),
                  static_cast<unsigned>(phy.preambleLength),
                  static_cast<unsigned long>(phy.txPower));
    return true;
}

static void runResponder()
{
    setRangeConfig();

    const M5Stamp_UWBDSResponderResult result = uwb.respondDSRange(rangeConfig);

    if (!result.success) {
        // Other Anchors also hear Polls addressed to them; ignore those frames.
        if (result.error == M5Stamp_UWBError::RxTimeout ||
            result.error == M5Stamp_UWBError::RangeFrameMismatch) {
            return;
        }

        if (++failCount % LOG_INTERVAL == 0) {
            Serial.printf(
                "MULTI_ANCHOR_RESP,count=%lu,fail=%lu,last=FAIL,anchor=0x%X,error=%s\n",
                static_cast<unsigned long>(responseCount),
                static_cast<unsigned long>(failCount),
                static_cast<unsigned>(ANCHOR_SHORT_ADDR),
                uwb.lastErrorName());
        }
        return;
    }

    if (++responseCount % LOG_INTERVAL == 0) {
        Serial.printf(
            "MULTI_ANCHOR_RESP,count=%lu,fail=%lu,last=OK,anchor=0x%X,seq=%u,requester=0x%X,distance_mm=%ld,distance_m=%.3f,elapsed_ms=%lu\n",
            static_cast<unsigned long>(responseCount),
            static_cast<unsigned long>(failCount),
            static_cast<unsigned>(ANCHOR_SHORT_ADDR),
            result.sequence,
            result.requester,
            static_cast<long>(result.distanceMm),
            result.distanceM,
            static_cast<unsigned long>(result.elapsedMs));
    }
}

void setup()
{
    Serial.begin(115200);
    delay(1000);

    Serial.printf("M5Stamp UWB DS-TWR ANCHOR\n");
    Serial.printf("ROLE,mode=ANCHOR\n");
    Serial.printf("TWR_MODE,mode=DS-TWR\n");
    Serial.printf("ANCHOR_ADDR,addr=0x%04X\n",
                  static_cast<unsigned>(ANCHOR_SHORT_ADDR));

    uwbReady = initUwb();

    Serial.printf("TEST_START,result=%s\n",
                  uwbReady ? "OK" : "FAIL");
}

void loop()
{
    if (uwbReady) runResponder();
    else delay(1000);
}

串口输出示例：

M5Stamp UWB DS-TWR ANCHOR
ROLE,mode=ANCHOR
TWR_MODE,mode=DS-TWR
ANCHOR_ADDR,addr=0x0002
UWB_ID,dev_id=0xDECA0314,chip=QM33120/DW3720
UWB_CONFIG,result=OK,ch=5,plen=128,rate=6M8,tx_power=0xFEFEFEFE
TEST_START,result=OK 