PX4 飞控 UWB 集成实战：SR150 兼容与 DW3000 开发全解析（含 NuttX 驱动）（下）

原创于 2025-08-31 07:15:00 发布 · 1.5k 阅读

47 ·

CC 4.0 BY-SA版权

文章标签：

#无人机 #PX4 #UWB #DW3000 #SR150 #室内定位

3.2 NuttX 驱动开发：DW3000 的底层控制

3.2.1 头文件dw3000.h补充定义

接前文，完成 DW3000 关键枚举与数据结构定义，重点体现 TDoA 模式特性：

// 2. 工作模式枚举

enum dw3000_mode {

DW3000_MODE_SLEEP = 0x00, // 休眠模式

DW3000_MODE_STANDBY = 0x01, // 待机模式

DW3000_MODE_TWR = 0x02, // TWR模式（点对点测距）

DW3000_MODE_TDOA_TAG = 0x03, // TDoA标签模式（无人机端，接收锚点信号）

DW3000_MODE_TDOA_ANCHOR = 0x04 // TDoA锚点模式（固定端，发送同步信号）

};

// 3. 频段枚举（对应REG_FREQ，支持频道5/9）

enum dw3000_freq {

DW3000_FREQ_CH5 = 0x05, // 6.5 GHz（带宽500MHz，中心频率6489.6MHz）

DW3000_FREQ_CH9 = 0x09 // 8.0 GHz（带宽500MHz，中心频率7987.2MHz）

};

// 4. TDoA锚点信息结构体（存储单个锚点的ID、坐标、时间戳）

struct dw3000_anchor_info {

uint64_t anchor_id; // 锚点唯一ID（64位，从锚点广播中获取）

float x; // 锚点X坐标（单位：m，需提前校准）

float y; // 锚点Y坐标（单位：m）

float z; // 锚点Z坐标（单位：m，2D定位时设为0）

uint32_t rx_timestamp; // 标签接收该锚点信号的时间戳（单位：ns）

bool valid; // 锚点数据是否有效（true=有效）

};

// 5. TDoA定位结果结构体（标签的3D坐标与质量）

struct dw3000_tdoa_position {

float x; // 标签X坐标（无人机位置，单位：m）

float y; // 标签Y坐标（单位：m）

float z; // 标签Z坐标（单位：m）

float accuracy; // 定位精度（单位：m，越小越准）

uint8_t anchor_count; // 参与定位的有效锚点数量（≥3才有效）

uint64_t timestamp; // 定位结果时间戳（单位：us，来自PX4系统时间）

};

// 6. 设备私有数据结构（存储DW3000状态与TDoA相关数据）

struct dw3000_dev_s {

struct spi_dev_s *spi; // SPI设备句柄

uint32_t cs_pin; // SPI片选引脚

uint32_t int_pin; // 中断引脚

uint32_t rst_pin; // 复位引脚

uint32_t xtal_in_pin; // 外部时钟输入引脚（可选）

enum dw3000_mode mode; // 当前工作模式（标签/锚点）

enum dw3000_freq freq; // 当前频段

struct dw3000_anchor_info anchors[8]; // 最多支持8个锚点

struct dw3000_tdoa_position tdoa_pos; // 最新TDoA定位结果

sem_t sem; // 信号量（保护数据访问）

bool data_ready; // 定位数据就绪标志（中断触发）

uint8_t anchor_max; // 最大锚点数量（配置值，通常3~8）

};

// 7. 驱动IOCTL命令（用于配置TDoA参数）

#define DW3000_IOCTL_SET_MODE _IOWR(0xDB, 0x01, enum dw3000_mode)

#define DW3000_IOCTL_SET_FREQ _IOWR(0xDB, 0x02, enum dw3000_freq)

#define DW3000_IOCTL_ADD_ANCHOR _IOWR(0xDB, 0x03, struct dw3000_anchor_info)

#define DW3000_IOCTL_GET_TDOA_POS _IOWR(0xDB, 0x04, struct dw3000_tdoa_position)

// 8. 驱动函数声明

int dw3000_init(FAR struct spi_dev_s *spi, uint32_t cs_pin, uint32_t int_pin,

uint32_t rst_pin, uint32_t xtal_in_pin);

int dw3000_register(const char *devpath, struct spi_dev_s *spi,

uint32_t cs_pin, uint32_t int_pin, uint32_t rst_pin,

uint32_t xtal_in_pin);

static int dw3000_spi_write_reg(FAR struct dw3000_dev_s *dev, uint16_t reg_addr,

FAR const uint8_t *data, uint8_t len);

static int dw3000_spi_read_reg(FAR struct dw3000_dev_s *dev, uint16_t reg_addr,

FAR uint8_t *data, uint8_t len);

关键差异点：

DW3000 寄存器地址为 16 位（SR150 为 8 位），需在 SPI 通信中适配；

新增dw3000_anchor_info存储锚点坐标与时间戳，支持多锚点 TDoA 计算；

IOCTL 命令增加ADD_ANCHOR（添加锚点）和GET_TDOA_POS（获取 TDoA 位置），适配 TDoA 模式需求。

3.2.2 SPI 通信实现：适配 16 位寄存器地址

DW3000 的 SPI 读写需先发送 16 位寄存器地址（高 8 位 + 低 8 位），再传输数据，且读操作需在地址最高位设为 1，实现如下：

// dw3000.c 中SPI通信函数

#include "dw3000.h"

#include <nuttx/clock.h>

// 写DW3000寄存器：reg_addr=16位地址，data=数据，len=数据长度

static int dw3000_spi_write_reg(FAR struct dw3000_dev_s *dev, uint16_t reg_addr,

FAR const uint8_t *data, uint8_t len) {

int ret;

uint8_t tx_buf[len + 2]; // 2字节地址 + len字节数据

// 1. 构造16位地址（大端模式，写操作：最高位清0）

tx_buf[0] = (reg_addr >> 8) & 0x7F; // 高8位，最高位0表示写

tx_buf[1] = reg_addr & 0xFF; // 低8位

// 2. 填充数据

memcpy(&tx_buf[2], data, len);

// 3. SPI总线锁定与通信

SPI_LOCK(dev->spi, true);

gpio_set_value(dev->cs_pin, false); // 拉低CS选中设备

// 4. 发送地址+数据（SPI速率20MHz，适配PX4飞控SPI控制器上限）

ret = SPI_SEND(dev->spi, tx_buf, len + 2);

// 5. 释放设备与总线

gpio_set_value(dev->cs_pin, true);

SPI_LOCK(dev->spi, false);

return ret;

}

// 读DW3000寄存器：reg_addr=16位地址，data=接收缓冲区，len=数据长度

static int dw3000_spi_read_reg(FAR struct dw3000_dev_s *dev, uint16_t reg_addr,

FAR uint8_t *data, uint8_t len) {

int ret;

uint8_t tx_buf[2]; // 2字节地址（读操作：最高位1）

// 1. 构造16位地址（大端模式，读操作：最高位1）

tx_buf[0] = ((reg_addr >> 8) & 0x7F) | 0x80; // 高8位，最高位1表示读

tx_buf[1] = reg_addr & 0xFF; // 低8位

SPI_LOCK(dev->spi, true);

gpio_set_value(dev->cs_pin, false);

// 2. 发送读地址

ret = SPI_SEND(dev->spi, tx_buf, 2);

if (ret < 0) {

goto out;

}

// 3. 读取寄存器数据（需等待DW3000准备，延迟1us）

nxsig_usleep(1);

ret = SPI_RECV(dev->spi, data, len);

out:

gpio_set_value(dev->cs_pin, true);

SPI_LOCK(dev->spi, false);

return ret;

}

关键注意事项：

DW3000 要求 SPI 通信为大端模式（地址高 8 位先传），与 SR150 的小端模式不同，需严格遵循；

读数据前需添加 1us 延迟，等待 DW3000 从寄存器中取出数据（硬件时序要求）；

SPI 速率设为 20MHz（PX4 飞控 STM32F7 SPI 控制器上限），低于 DW3000 的 38MHz 最大值，确保通信稳定。

3.2.3 设备初始化：支持 TDoA 模式与外部时钟

DW3000 初始化需额外配置 TDoA 参数（锚点数量、同步周期）和外部时钟（可选），步骤如下：

// dw3000.c 中初始化函数

int dw3000_init(FAR struct spi_dev_s *spi, uint32_t cs_pin, uint32_t int_pin,

uint32_t rst_pin, uint32_t xtal_in_pin) {

struct dw3000_dev_s *dev;

int ret;

uint8_t init_data[4];

uint8_t chip_id[4]; // 读取芯片ID验证设备

// 1. 分配设备内存

dev = kmm_zalloc(sizeof(struct dw3000_dev_s));

if (dev == NULL) {

PX4_ERR("DW3000 dev memory alloc failed");

return -ENOMEM;

}

// 2. 初始化硬件参数

dev->spi = spi;

dev->cs_pin = cs_pin;

dev->int_pin = int_pin;

dev->rst_pin = rst_pin;

dev->xtal_in_pin = xtal_in_pin;

dev->anchor_max = 4; // 默认支持4个锚点（可通过IOCTL修改）

dev->mode = DW3000_MODE_STANDBY;

dev->freq = DW3000_FREQ_CH9; // 默认使用频道9（避5G干扰）

// 3. 配置GPIO引脚

// 3.1 CS引脚：推挽输出，初始高

gpio_configure_pin(cs_pin, GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_DRIVE_HIGH);

gpio_set_value(cs_pin, true);

// 3.2 复位引脚：拉低100ms复位

gpio_configure_pin(rst_pin, GPIO_OUTPUT | GPIO_PUSHPULL);

gpio_set_value(rst_pin, false);

nxsig_usleep(100000); // 100ms复位

gpio_set_value(rst_pin, true);

nxsig_usleep(100000); // 等待100ms启动

// 3.3 外部时钟（可选）：若xtal_in_pin有效，配置为32MHz输出

if (xtal_in_pin != 0) {

gpio_configure_pin(xtal_in_pin, GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_DRIVE_HIGH);

// 启用PX4定时器生成32MHz时钟（需底层驱动支持）

ret = px4_timer_configure(xtal_in_pin, 32000000);

if (ret != 0) {

PX4_WARN("External 32MHz clock config failed");

} else {

PX4_INFO("DW3000 using external 32MHz clock");

}

// 3.4 中断引脚：下降沿触发（DW3000中断为低电平有效）

gpio_configure_pin(int_pin, GPIO_INPUT | GPIO_INT_FALLING);

ret = irq_attach(gpio_pin2irq(int_pin), dw3000_int_handler, dev);

if (ret < 0) {

PX4_ERR("DW3000 irq attach failed: %d", ret);

goto fail;

}

irq_enable(gpio_pin2irq(int_pin));

// 4. 验证芯片ID（DW3000芯片ID固定为0xDECA0130）

ret = dw3000_spi_read_reg(dev, DW3000_REG_ID, chip_id, 4);

if (ret < 0 || (chip_id[0] != 0xDE || chip_id[1] != 0xCA ||

chip_id[2] != 0x01 || chip_id[3] != 0x30)) {

PX4_ERR("DW3000 chip ID invalid: 0x%02X%02X%02X%02X",

chip_id[0], chip_id[1], chip_id[2], chip_id[3]);

ret = -ENODEV;

goto fail;

}

PX4_INFO("DW3000 chip ID verified: 0xDECA0130");

// 5. 配置工作模式（默认TDoA标签模式）

init_data[0] = DW3000_MODE_TDOA_TAG;

ret = dw3000_spi_write_reg(dev, DW3000_REG_MODE, init_data, 1);

if (ret < 0) {

PX4_ERR("Set TDOA tag mode failed");

goto fail;

}

// 6. 配置频段（频道9，8GHz）

init_data[0] = DW3000_FREQ_CH9;

ret = dw3000_spi_write_reg(dev, DW3000_REG_FREQ, init_data, 1);

if (ret < 0) {

PX4_ERR("Set frequency failed");

goto fail;

}

// 7. 配置TDoA参数（锚点同步周期100ms，定位频率10Hz）

init_data[0] = 0x0A; // 同步周期：100ms（0x0A * 10ms）

init_data[1] = dev->anchor_max; // 最大锚点数量

ret = dw3000_spi_write_reg(dev, DW3000_REG_TDOA_CFG, init_data, 2);

if (ret < 0) {

PX4_ERR("TDoA config failed");

goto fail;

}

// 8. 使能中断（数据就绪+锚点同步中断）

init_data[0] = 0x03; // 0x01=数据就绪，0x02=锚点同步

ret = dw3000_spi_write_reg(dev, DW3000_REG_INT_EN, init_data, 1);

if (ret < 0) {

PX4_ERR("Interrupt enable failed");

goto fail;

}

// 9. 初始化信号量与数据标志

nxsem_init(&dev->sem, 0, 1);

dev->data_ready = false;

memset(dev->anchors, 0, sizeof(dev->anchors));

// 10. 保存设备私有数据到SPI

SPI_SETPRIV(spi, dev);

return 0;

fail:

irq_detach(gpio_pin2irq(int_pin));

kmm_free(dev);

return ret;

}

TDoA 关键配置：

同步周期设为 100ms（对应定位频率 10Hz），可通过DW3000_REG_TDOA_CFG调整，周期越小定位越频繁但功耗越高；

外部时钟配置需依赖 PX4 定时器驱动（如 STM32 的 TIM8），生成 32MHz 方波，可将测距精度提升 10%（从 ±10cm 到 ±9cm）；

芯片 ID 验证是必要步骤，避免与其他 UWB 芯片（如 DW1000）混淆，DW3000 固定 ID 为0xDECA0130。

3.2.4 中断处理：TDoA 锚点数据接收与解析

DW3000 中断包含 “数据就绪” 和 “锚点同步” 两类，需在中断服务函数中区分处理，解析锚点时间戳并暂存：

// dw3000.c 中中断服务函数

static int dw3000_int_handler(int irq, FAR void *context, FAR void *arg) {

struct dw3000_dev_s *dev = (struct dw3000_dev_s *)arg;

uint8_t int_stat;

uint8_t anchor_data[12]; // 单个锚点数据：8字节ID + 4字节时间戳

// 1. 读取中断状态

dw3000_spi_read_reg(dev, DW3000_REG_INT_STAT, &int_stat, 1);

// 2. 处理锚点同步中断（仅标签模式有效）

if (int_stat & 0x02 && dev->mode == DW3000_MODE_TDOA_TAG) {

// 2.1 读取当前锚点数据（最多8个，循环存储）

static uint8_t anchor_idx = 0;

ret = dw3000_spi_read_reg(dev, DW3000_REG_TDOA_DATA + anchor_idx * 12,

anchor_data, 12);

if (ret < 0) {

PX4_WARN("Read anchor data failed");

goto clear_int;

}

// 2.2 解析锚点ID（64位）与时间戳（32位，ns）

struct dw3000_anchor_info *anchor = &dev->anchors[anchor_idx];

anchor->anchor_id = (uint64_t)anchor_data[0] << 56 |

(uint64_t)anchor_data[1] << 48 |

(uint64_t)anchor_data[2] << 40 |

(uint64_t)anchor_data[3] << 32 |

(uint64_t)anchor_data[4] << 24 |

(uint64_t)anchor_data[5] << 16 |

(uint64_t)anchor_data[6] << 8 |

(uint64_t)anchor_data[7];

anchor->rx_timestamp = (uint32_t)anchor_data[8] << 24 |

(uint32_t)anchor_data[9] << 16 |

(uint32_t)anchor_data[10] << 8 |

(uint32_t)anchor_data[11];

anchor->valid = true; // 标记锚点数据有效

// 2.3 循环更新锚点索引

anchor_idx = (anchor_idx + 1) % dev->anchor_max;

}

// 3. 处理数据就绪中断（TDoA定位结果计算完成）

if (int_stat & 0x01) {

// 3.1 读取TDoA定位结果（x/y/z/accuracy）

uint8_t tdoa_data[16];

ret = dw3000_spi_read_reg(dev, DW3000_REG_TDOA_DATA + 0x80, tdoa_data, 16);

if (ret < 0) {

PX4_WARN("Read TDoA data failed");

goto clear_int;

}

// 3.2 解析定位结果（浮点数，小端模式）

nxsem_wait(&dev->sem);

memcpy(&dev->tdoa_pos.x, &tdoa_data[0], 4);

memcpy(&dev->tdoa_pos.y, &tdoa_data[4], 4);

memcpy(&dev->tdoa_pos.z, &tdoa_data[8], 4);

memcpy(&dev->tdoa_pos.accuracy, &tdoa_data[12], 4);

// 3.3 统计有效锚点数量

dev->tdoa_pos.anchor_count = 0;

for (int i = 0; i < dev->anchor_max; i++) {

if (dev->anchors[i].valid) {

dev->tdoa_pos.anchor_count++;

}

// 3.4 记录时间戳

dev->tdoa_pos.timestamp = px4_clock_get_usec();

dev->data_ready = true;

nxsem_post(&dev->sem);

}

clear_int:

// 4. 清除中断标志（写1清除）

uint8_t clear_data = int_stat;

dw3000_spi_write_reg(dev, DW3000_REG_INT_STAT, &clear_data, 1);

return OK;

}

TDoA 数据解析要点：

锚点数据格式为 “8 字节 ID + 4 字节时间戳”，需按大端模式解析（DW3000 硬件输出格式）；

有效锚点数量需≥3 才能生成可靠的 TDoA 定位结果，否则accuracy会设为 0（无效）；

定位结果为 32 位浮点数（小端模式），与 PX4 的浮点数存储格式一致，可直接 memcpy 解析。

3.2.5 字符设备接口与驱动注册

DW3000 的字符设备接口需支持 TDoA 锚点配置与定位结果读取，file_operations实现如下：

// dw3000.c 中字符设备接口

static int dw3000_open(FAR struct file *filep) {

// 打开时将设备切换为待机模式

FAR struct inode *inode = filep->f_inode;

FAR struct dw3000_dev_s *dev = inode->i_private;

uint8_t standby_mode = DW3000_MODE_STANDBY;

dw3000_spi_write_reg(dev, DW3000_REG_MODE, &standby_mode, 1);

return OK;

}

static int dw3000_close(FAR struct file *filep) {

// 关闭时切换为休眠模式，降低功耗

FAR struct inode *inode = filep->f_inode;

FAR struct dw3000_dev_s *dev = inode->i_private;

uint8_t sleep_mode = DW3000_MODE_SLEEP;

dw3000_spi_write_reg(dev, DW3000_REG_MODE, &sleep_mode, 1);

return OK;

}

static ssize_t dw3000_read(FAR struct file *filep, FAR char *buf, size_t len) {

// 仅支持读取TDoA定位结果（len需等于结构体大小）

if (len != sizeof(struct dw3000_tdoa_position)) {

return -EINVAL;

}

FAR struct inode *inode = filep->f_inode;

FAR struct dw3000_dev_s *dev = inode->i_private;

struct dw3000_tdoa_position pos;

int ret;

// 等待数据就绪（超时500ms）

uint32_t timeout = 500;

while (!dev->data_ready && timeout-- > 0) {

nxsig_usleep(1000);

}

if (!dev->data_ready) {

return -ETIMEDOUT;

}

// 读取定位结果

nxsem_wait(&dev->sem);

pos = dev->tdoa_pos;

dev->data_ready = false;

nxsem_post(&dev->sem);

// 拷贝到应用层

ret = memcpy(buf, &pos, sizeof(pos));

return (ret == OK) ? sizeof(pos) : -EIO;

}

static int dw3000_ioctl(FAR struct file *filep, int cmd, unsigned long arg) {

FAR struct inode *inode = filep->f_inode;

FAR struct dw3000_dev_s *dev = inode->i_private;

int ret = OK;

switch (cmd) {

// 切换工作模式（标签/锚点/TWR）

case DW3000_IOCTL_SET_MODE: {

enum dw3000_mode mode = (enum dw3000_mode)arg;

if (mode < DW3000_MODE_SLEEP || mode > DW3000_MODE_TDOA_ANCHOR) {

return -EINVAL;

}

dev->mode = mode;

uint8_t mode_data = mode;

ret = dw3000_spi_write_reg(dev, DW3000_REG_MODE, &mode_data, 1);

break;

}

// 配置频段（频道5/9）

case DW3000_IOCTL_SET_FREQ: {

enum dw3000_freq freq = (enum dw3000_freq)arg;

if (freq != DW3000_FREQ_CH5 && freq != DW3000_FREQ_CH9) {

return -EINVAL;

}

dev->freq = freq;

uint8_t freq_data = freq;

ret = dw3000_spi_write_reg(dev, DW3000_REG_FREQ, &freq_data, 1);

break;

}

// 添加锚点坐标（需提前校准锚点位置）

case DW3000_IOCTL_ADD_ANCHOR: {

struct dw3000_anchor_info *anchor = (struct dw3000_anchor_info *)arg;

// 查找空锚点位置

for (int i = 0; i < dev->anchor_max; i++) {

if (!dev->anchors[i].valid) {

nxsem_wait(&dev->sem);

dev->anchors[i] = *anchor;

nxsem_post(&dev->sem);

goto add_anchor_done;

}

ret = -ENOSPC; // 锚点数量已满

add_anchor_done:

break;

}

// 主动获取TDoA定位结果（非阻塞）

case DW3000_IOCTL_GET_TDOA_POS: {

nxsem_wait(&dev->sem);

memcpy((struct dw3000_tdoa_position *)arg, &dev->tdoa_pos,

sizeof(struct dw3000_tdoa_position));

nxsem_post(&dev->sem);

break;

}

default:

ret = -ENOTTY;

break;

}

return ret;

}

// 定义file_operations结构体

static const struct file_operations dw3000_fops = {

.open = dw3000_open,

.close = dw3000_close,

.read = dw3000_read,

.ioctl = dw3000_ioctl,

.write = NULL,

.seek = NULL,

};

// 驱动注册函数：创建/dev/uwb1设备节点（区分SR150的/dev/uwb0）

int dw3000_register(const char *devpath, struct spi_dev_s *spi,

uint32_t cs_pin, uint32_t int_pin, uint32_t rst_pin,

uint32_t xtal_in_pin) {

int ret;

struct dw3000_dev_s *dev;

ret = dw3000_init(spi, cs_pin, int_pin, rst_pin, xtal_in_pin);

if (ret < 0) {

return ret;

}

dev = SPI_GETPRIV(spi);

ret = register_chardev(0, devpath, &dw3000_fops, dev);

if (ret < 0) {

kmm_free(dev);

return ret;

}

PX4_INFO("DW3000 UWB driver registered at %s", devpath);

return OK;

}

3.2.6 编译配置：Kconfig 与 Make.defs

与 SR150 类似，需添加 DW3000 的编译配置文件，确保 PX4 编译系统识别：

1. Kconfig 文件（src/drivers/uwb/qorvo_dw3000/Kconfig）

config DRIVER_UWB_QORVO_DW3000

bool "Qorvo DW3000 UWB Driver"

default n

depends on SPI && PX4_PLATFORM_COMMON

help

Enable support for the Qorvo DW3000 UWB chip (TDoA/TWR mode).

Requires SPI bus support and PX4 platform common libraries.

2. Make.defs 文件（src/drivers/uwb/qorvo_dw3000/Make.defs）

ifeq ($(CONFIG_DRIVER_UWB_QORVO_DW3000),y)

CSRCS += dw3000.c

endif

3.3 PX4 应用层开发：DW3000 TDoA 数据处理与 uORB 发布

DW3000 的 PX4 应用层需完成 “锚点配置→TDoA 数据读取→uORB 发布→EKF2 融合”，核心文件为dw3000_uwb.cpp。

3.3.1 定义 uORB 主题：uwb_tdoa_position.msg

TDoA 定位结果包含 3D 坐标与锚点信息，需创建专用 uORB 主题：

# msg/uwb_tdoa_position.msg

float32 x # 无人机X坐标（单位：m，东北天坐标系）

float32 y # 无人机Y坐标（单位：m）

float32 z # 无人机Z坐标（单位：m）

float32 accuracy # 定位精度（单位：m，越小越准）

uint8 anchor_count # 参与定位的有效锚点数量（≥3有效）

uint64 timestamp # 时间戳（单位：us，来自hrt_absolute_time()）

bool valid # 定位结果是否有效（true=有效）

# 元数据：队列长度4，确保数据不丢失

uint8 ORB_QUEUE_LENGTH = 4

添加到msg/CMakeLists.txt：

set(msg_files

# 其他msg文件...

uwb_tdoa_position.msg

)

3.3.2 应用层代码实现：dw3000_uwb.cpp

核心功能：初始化 DW3000 为 TDoA 标签模式、添加预校准锚点、循环读取定位数据并发布 uORB：

// src/drivers/uwb/qorvo_dw3000/dw3000_uwb.cpp

#include <px4_platform_common/px4_config.h>

#include <px4_platform_common/posix.h>

#include <px4_platform_common/log.h>

#include <drivers/drv_hrt.h>

#include <uORB/topics/uwb_tdoa_position.h>

#include "dw3000.h"

// 外部C函数声明（NuttX驱动）

extern "C" {

int dw3000_register(const char *devpath, void *spi,

uint32_t cs_pin, uint32_t int_pin, uint32_t rst_pin,

uint32_t xtal_in_pin);

}

// 预校准的锚点坐标（示例：4个锚点，东北天坐标系）

const struct dw3000_anchor_info DEFAULT_ANCHORS[] = {

{0xDECA013000000001, 0.0f, 0.0f, 0.0f, 0, true}, // 锚点1：(0,0,0)

{0xDECA013000000002, 10.0f, 0.0f, 0.0f, 0, true}, // 锚点2：(10,0,0)

{0xDECA013000000003, 10.0f, 10.0f, 0.0f, 0, true}, // 锚点3：(10,10,0)

{0xDECA013000000004, 0.0f, 10.0f, 0.0f, 0, true} // 锚点4：(0,10,0)

};

class DW3000UWB

{

public:

DW3000UWB() : _dev_fd(-1) {}

~DW3000UWB() { if (_dev_fd >= 0) close(_dev_fd); }

// 初始化：注册驱动+配置锚点

int init() {

// 1. 初始化SPI2总线（Pixhawk 4扩展SPI）

void *spi_dev = px4_spi_bus_initialize(2);

if (spi_dev == nullptr) {

PX4_ERR("SPI2 bus init failed");

return -1;

}

// 2. 注册DW3000驱动（设备节点：/dev/uwb1）

int ret = dw3000_register("/dev/uwb1", spi_dev,

PX4_GPIO_PB12, // CS引脚：PB12（与SR150复用，需分时复用）

PX4_GPIO_PC4, // INT引脚：PC4（需修改为不同引脚，避免冲突）

PX4_GPIO_PC5, // RST引脚：PC5

PX4_GPIO_PC6); // 外部时钟引脚：PC6

if (ret != 0) {

PX4_ERR("DW3000 register failed: %d", ret);

return ret;

}

// 3. 打开设备文件

_dev_fd = open("/dev/uwb1", O_RDONLY);

if (_dev_fd < 0) {

PX4_ERR("Open /dev/uwb1 failed: %d", errno);

return -errno;

}

// 4. 配置为TDoA标签模式

enum dw3000_mode tag_mode = DW3000_MODE_TDOA_TAG;

ret = ioctl(_dev_fd, DW3000_IOCTL_SET_MODE, (unsigned long)tag_mode);

if (ret != 0) {

PX4_ERR("Set TDoA tag mode failed: %d", ret);

return ret;

}

// 5. 配置为频道9（8GHz，避干扰）

enum dw3000_freq freq_ch9 = DW3000_FREQ_CH9;

ret = ioctl(_dev_fd, DW3000_IOCTL_SET_FREQ, (unsigned long)freq_ch9);

if (ret != 0) {

PX4_ERR("Set frequency failed: %d", ret);

return ret;

}

// 6. 添加预校准锚点

for (size_t i = 0; i < sizeof(DEFAULT_ANCHORS)/sizeof(DEFAULT_ANCHORS[0]); i++) {

ret = ioctl(_dev_fd, DW3000_IOCTL_ADD_ANCHOR,

(unsigned long)&DEFAULT_ANCHORS[i]);

if (ret != 0) {

PX4_WARN("Add anchor %d failed: %d", i+1, ret);

} else {

PX4_INFO("Add anchor %d: ID=0x%016lx, (%0.1f, %0.1f, %0.1f)",

i+1, DEFAULT_ANCHORS[i].anchor_id,

DEFAULT_ANCHORS[i].x, DEFAULT_ANCHORS[i].y, DEFAULT_ANCHORS[i].z);

}

PX4_INFO("DW3000 UWB initialized as TDoA tag");

return 0;

}

// 循环运行：读取TDoA数据并发布uORB

void run() {

struct dw3000_tdoa_position tdoa_pos;

struct uwb_tdoa_position_s orb_pos;

memset(&orb_pos, 0, sizeof(orb_pos));

while (!_should_exit) {

// 1. 读取TDoA定位结果（阻塞，超时500ms）

ssize_t len = read(_dev_fd, &tdoa_pos, sizeof(tdoa_pos));

if (len != sizeof(tdoa_pos)) {

PX4_WARN("Read TDoA data failed: len=%d, errno=%d", len, errno);

px4_usleep(10000); // 10ms重试

continue;

}

// 2. 验证定位结果有效性（锚点≥3且精度<1m）

orb_pos.valid = (tdoa_pos.anchor_count >= 3) && (tdoa_pos.accuracy < 1.0f);

if (!orb_pos.valid) {

PX4_WARN("TDoA invalid: anchors=%d, accuracy=%.2fm",

tdoa_pos.anchor_count, tdoa_pos.accuracy);

continue;

}

// 3. 填充uORB数据（转换为东北天坐标系）

orb_pos.x = tdoa_pos.x;

orb_pos.y = tdoa_pos.y;

orb_pos.z = tdoa_pos.z;

orb_pos.accuracy = tdoa_pos.accuracy;

orb_pos.anchor_count = tdoa_pos.anchor_count;

orb_pos.timestamp = hrt_absolute_time();

// 4. 发布uORB主题

if (!_orb_pub.initialized()) {

_orb_pub.advertise();

}

_orb_pub.publish(orb_pos);

// 5. 打印调试信息（1Hz）

static uint64_t last_print = 0;

if (hrt_elapsed_time(&last_print) > 1000000) {

PX4_INFO("UWB TDoA: (%.2f, %.2f, %.2f)m, acc=%.2fm, anchors=%d",

orb_pos.x, orb_pos.y, orb_pos.z,

orb_pos.accuracy, orb_pos.anchor_count);

last_print = hrt_absolute_time();

}

// 6. 控制循环频率（10Hz，与TDoA同步周期一致）

px4_usleep(100000);

}

void stop() { _should_exit = true; }

private:

int _dev_fd; // 设备文件描述符

bool _should_exit = false; // 退出标志

uORB::Publication<uwb_tdoa_position_s> _orb_pub{ORB_ID(uwb_tdoa_position)};

};

// PX4模块入口函数

extern "C" __EXPORT int dw3000_uwb_main(int argc, char *argv[]) {

DW3000UWB uwb;

if (argc == 2 && strcmp(argv[1], "start") == 0) {

int ret = uwb.init();

if (ret == 0) {

uwb.run();

}

return ret;

} else if (argc == 2 && strcmp(argv[1], "stop") == 0) {

uwb.stop();

return 0;

} else if (argc == 2 && strcmp(argv[1], "status") == 0) {

PX4_INFO("DW3000 UWB status: running (TDoA tag mode)");

return 0;

}

PX4_INFO("Usage: dw3000_uwb {start|stop|status}");

return 1;

}

关键注意事项：

若 SR150 与 DW3000 共用 SPI 总线，需通过片选引脚分时复用（同一时间仅一个设备激活），避免 SPI 冲突；

锚点坐标需提前用激光测距仪校准，误差≤5cm，否则会导致 TDoA 定位漂移；

定位结果有效性判断：锚点数量≥3 且精度 < 1m，过滤无效数据（如遮挡导致锚点丢失）。

3.4 定位数据融合：UWB 与 EKF2 集成

PX4 的 EKF2（扩展卡尔曼滤波）是传感器融合核心，需配置 EKF2 使用 DW3000 的 UWB 数据作为定位源，补充或替换 GPS。

3.4.1 EKF2 配置参数

通过 QGroundControl 或 PX4 Shell 修改 EKF2 参数，启用 UWB 融合：

参数名	含义	推荐值	说明
EKF2_AID_MASK	辅助定位源掩码	8	8 = 启用 UWB（需确保固件支持 UWB 掩码）
EKF2_UWB_POS_X	UWB 原点 X 偏移（东北天坐标系）	0.0	若 UWB 原点与 GPS 原点一致，设为 0
EKF2_UWB_POS_Y	UWB 原点 Y 偏移	0.0	同上
EKF2_UWB_POS_Z	UWB 原点 Z 偏移	0.0	同上
EKF2_UWB_VAR_POS	UWB 位置方差（可信度，越小权重越高）	0.01	对应 UWB 精度 ±0.1m，方差 = 精度 ²
EKF2_UWB_RATE	UWB 数据更新频率（Hz）	10	与 DW3000 的 TDoA 同步周期一致
EKF2_POS_SRC	主要位置源	2	2=UWB（0=GPS，1 = 视觉，2=UWB）

表 3-2：EKF2 UWB 融合配置参数

配置步骤：

连接 QGroundControl，进入 “参数” 页面，搜索上述参数；

修改EKF2_AID_MASK为 8，启用 UWB 辅助；

若 UWB 坐标系与 GPS 一致，EKF2_UWB_POS_X/Y/Z设为 0；

EKF2_UWB_VAR_POS设为 0.01（对应 ±0.1m 精度）；

重启 PX4 飞控，使参数生效。

3.4.2 EKF2 UWB 数据订阅与融合逻辑

需在 EKF2 源码中添加uwb_tdoa_position主题的订阅，修改src/modules/ekf2/EKF2.cpp：

// EKF2.cpp 中添加UWB订阅

#include <uORB/topics/uwb_tdoa_position.h>

// 在EKF2类中添加订阅者

class EKF2 : public ModuleBase<EKF2>

{

// 其他成员...

private:

uORB::Subscription _uwb_tdoa_sub{ORB_ID(uwb_tdoa_position)};

struct uwb_tdoa_position_s _uwb_tdoa_data;

};

// 在run()函数中读取UWB数据并融合

void EKF2::run()

{

// 其他数据读取...

// 读取UWB TDoA数据

if (_uwb_tdoa_sub.update(&_uwb_tdoa_data)) {

// 仅使用有效数据

if (_uwb_tdoa_data.valid) {

// 将UWB数据转换为EKF2的东北天坐标系

Vector3f uwb_pos(_uwb_tdoa_data.x, _uwb_tdoa_data.y, _uwb_tdoa_data.z);

// 添加原点偏移

uwb_pos(0) += _params.ekf2_uwb_pos_x;

uwb_pos(1) += _params.ekf2_uwb_pos_y;

uwb_pos(2) += _params.ekf2_uwb_pos_z;

// 设置位置方差

float uwb_var = _params.ekf2_uwb_var_pos;

// 调用EKF融合接口

_ekf.setExternalPositionMeasurement(uwb_pos, uwb_var, _uwb_tdoa_data.timestamp);

}

// 其他融合逻辑...

}

融合逻辑说明：

EKF2 通过setExternalPositionMeasurement接口接收 UWB 位置数据，自动与 IMU、GPS（若存在）融合；

位置方差uwb_var决定 UWB 数据的权重：方差越小，EKF2 越信任 UWB 数据；

若 GPS 信号丢失（如室内），EKF2 自动切换为 UWB 作为主要定位源，确保定位连续性。

3.5 测试验证与问题排查

DW3000 的测试需重点验证 TDoA 多锚点定位精度、EKF2 融合效果及抗干扰能力。

3.5.1 测试环境与设备

测试环节	设备 / 工具	用途
锚点部署	4 个 DWM3000 模块（锚点）+ 1 个标签	构建 10m×10m 的 TDoA 定位区域
精度测试	激光测距仪（±1mm）、坐标纸	对比 UWB 定位与实际位置误差
数据监控	QGroundControl、PX4 Shell	查看 UWB 数据、EKF2 融合结果
抗干扰测试	Wi-Fi 路由器（5GHz）、蓝牙设备	测试多无线信号干扰下的定位稳定性
多无人机测试	2 架 Pixhawk 4 无人机	验证编队飞行中 UWB 的协同定位能力

表 3-3：DW3000 测试环境与设备

3.5.2 核心测试步骤与预期结果

步骤 1：锚点部署与校准

在 10m×10m 区域内，按正方形部署 4 个锚点，坐标分别为 (0,0,0)、(10,0,0)、(10,10,0)、(0,10,0)；
用激光测距仪测量相邻锚点距离，确保实际误差≤5cm；
在dw3000_uwb.cpp中更新DEFAULT_ANCHORS的坐标，重新编译固件。

步骤 2：驱动与 uORB 测试

烧录固件，执行dw3000_uwb start启动驱动；

执行uorb top，查看uwb_tdoa_position主题：

刷新率：10Hz；
anchor_count：4；
accuracy：≤0.1m；

在 QGroundControl 的 “Mavlink Inspector” 中查看UWB_TDOA_POSITION消息，确认数据与uorb top一致。

步骤 3：定位精度测试

将无人机（标签）放在已知坐标点（如 (5,5,0)），记录 UWB 输出的x/y/z；
计算误差：误差=√[(UWBx-实际x)² + (UWB y-实际y)²]，预期误差≤0.1m；
移动无人机到多个点（如 (2,3,0)、(8,7,0)），重复测试，平均误差≤0.1m。

步骤 4：EKF2 融合测试

配置 EKF2 参数（参考表 3-2），重启飞控；

执行ekf2 status，查看定位源：

室外 GPS 正常：pos source: GPS + UWB；
室内 GPS 丢失：pos source: UWB；

记录 EKF2 输出的local_position（东北天坐标系），与 UWB 数据对比，误差≤0.15m（融合 IMU 后）。

步骤 5：抗干扰测试

在测试区域内开启 5GHz Wi-Fi 路由器（信道 36）和多个蓝牙设备；

持续 1 小时记录 UWB 的accuracy和anchor_count：

预期anchor_count保持；
accuracy波动≤0.05m；

移动无人机，观察定位轨迹是否平滑，无明显跳变。

3.5.3 常见问题与解决方案

问题现象	可能原因	解决方案
anchor_count < 3	锚点未同步；射频遮挡；锚点 ID 错误	1. 检查锚点电源；2. 移除遮挡；3. 核对锚点 ID
accuracy > 0.2m	锚点坐标校准误差大；外部时钟未启用	1. 重新校准锚点坐标；2. 启用 32MHz 外部时钟
EKF2 不使用 UWB 数据	EKF2_AID_MASK未设为 8；UWB 数据无效	1. 修改EKF2_AID_MASK为 8；2. 确保valid=true
定位结果跳变（>0.5m）	SPI 通信干扰；锚点同步周期不匹配	1. 缩短 SPI 线缆；2. 统一锚点与标签的同步周期（100ms）
驱动启动失败（open /dev/uwb1 failed）	片选引脚冲突；SPI 总线被占用	1. 为 DW3000 分配独立 INT 引脚；2. 确保 SPI2 未被其他设备使用

表 3-4：DW3000 常见问题与解决方案

3.6 多无人机编队扩展：UWB 协同定位

DW3000 的 TDoA 模式支持多标签（多无人机）同时定位，只需在每个无人机上部署 DW3000 标签，共享同一组锚点：

3.6.1 多标签配置

为每个无人机分配唯一的标签 ID（在驱动中通过DW3000_REG_ID配置）；

所有标签使用相同的锚点坐标和同步周期（100ms）；

锚点支持最多 32 个标签同时接入（Qorvo 硬件限制）。

3.6.2 协同定位测试

部署 2 架无人机，分别启动dw3000_uwb start；

在地面站查看两架无人机的 UWB 位置，计算相对距离：

实际相对距离：用激光测距仪测量；
UWB 相对距离：√[(x1-x2)² + (y1-y2)²]；
预期相对误差≤0.15m；

控制两架无人机保持 1m 相对距离飞行，UWB 相对距离波动≤0.08m。

第四部分：SR150 与 DW3000 的 PX4 选型与对比

在实际项目中，需根据应用场景选择合适的 UWB 芯片，以下从多维度对比两者在 PX4 中的适配特性。

4.1 核心特性对比

对比维度	NXP SR150	Qorvo DW3000	适用场景
定位算法	TWR + 3D AoA（角度测量）	TWR + TDoA（多锚点定位）	SR150：避障、点对点跟拍；DW3000：大范围定位
定位精度	距离 ±0.1m，角度 ±3°	距离 ±0.1m，相对定位 ±0.05m	SR150：需角度感知；DW3000：需多机协同
有效范围	单锚点≤50m（视距）	多锚点≤100m（视距）	SR150：中短距；DW3000：中长距
锚点数量	1 个（AoA）	≥3 个（TDoA）	SR150：简单场景；DW3000：复杂场景
功耗	接收 22mA，休眠 < 0.3μA	接收 24mA，休眠 < 0.5μA	SR150：电池供电无人机；DW3000：外接电源
苹果 U1 兼容	支持（固件 v3.14.0+）	不支持	SR150：苹果生态配件；DW3000：工业 / 编队
PX4 集成复杂度	低（无需多锚点）	高（需锚点部署与校准）	SR150：快速原型；DW3000：量产项目

表 4-1：SR150 与 DW3000 在 PX4 中的核心特性对比

4.2 选型建议

应用场景	推荐芯片	选型理由
室内单无人机巡检	SR150	无需多锚点，低功耗，支持角度避障
苹果生态无人机配件	SR150	兼容 U1 芯片，可与 iPhone/iPad 协同
多无人机编队飞行	DW3000	TDoA 支持多标签，相对定位精度高
工业仓储大范围定位	DW3000	多锚点覆盖广，抗干扰能力强
低成本原型验证	SR150	集成简单，开发周期短
高可靠性量产项目	DW3000	工业级稳定性，支持大规模锚点部署