Stamp UWB F Arduino チュートリアル

1. 準備

環境設定： Arduino IDE はじめにガイドを参照して IDE のインストールを完了し、Stamp C5 用のボードマネージャとドライバをインストールしてください。
使用するライブラリ：
- M5Stamp-UWB
使用するハードウェア：
- Stamp-C5 DIP
- Stamp UWB F

2. 注意事項

注意

1. 通信品質を確保するため、Stamp UWB には高 PSRR の LDO、またはリップルが 12mVpp 以下の電源を使用してください。
2. UWB 測距には最低 2 台のデバイスが必要です。片方を Tag、もう片方を Anchor として使用します。
3. 製品全体を設計する際は、FPC ケーブルとそのコネクタがモジュールのアンテナ性能に与える影響に注意してください。FPC ケーブルをモジュールの PCB アンテナの下やアンテナのクリアランス領域に通さないでください。アンテナのインピーダンスや放射特性が変化し、RF 性能の低下や測距誤差が生じる可能性があります。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 との距離を出力しますが、2 次元または 3 次元の座標計算は行いません。各 Anchor には一意のショートアドレスを設定し、すべての Anchor のアドレスを Tag 側のアドレスリストに追加してください。Tag とすべての Anchor では、同じ PAN ID、UWB チャネル、Tag アドレス、および DS-TWR タイミングパラメータを使用する必要があります。Tag は Anchor を切り替える前に短い待ち時間を設け、測距に失敗した場合は現在の Anchor に対して再試行することで、残留フレームやフレーム衝突の影響を抑えます。2 次元または 3 次元の測位を行うには、あらかじめ各 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 

6. コンパイルと書き込み

デバイスポートを選択し、上記のいずれかのサンプルを Arduino IDE に貼り付け、Arduino IDE の左上にあるコンパイル／アップロードボタンをクリックして、スケッチのコンパイルと書き込みが完了するのを待ちます。

Next 1. Arduino環境設定 - 1. Arduino IDEインストール

Page Tools

PDF

1. Arduino環境設定

2. デバイス＆サンプル

Tab5

Arduino Nesso N1

Atom VoiceS3R

Atom-Lite / Atom-Matrix

AtomS3 / AtomS3-Lite

AtomS3U

AtomS3R

Basic/Gray

Capsule

Cardputer / -Adv

Chain DualKey

Core2/Core2 For AWS

CoreInk

CoreS3

StackChan