无人机飞控PX4 中 DRONE CAN 相关代码解析与二次开发指南——光流和测距雷达实战例程

于 2025-07-31 10:30:21 发布
阅读量685 收藏 19
点赞数 22
CC 4.0 BY-SA版权
分类专栏： EDA设计 无人机 无人机硬件嵌入式开发 文章标签： 无人机 DRONE CAN PX4
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/bother3000/article/details/149798126

目录

  1. PX4 CAN 总线系统架构概述
  2. DRONE CAN 核心代码解析
  3. 工作原理详解
  4. 二次开发接口说明
  5. UML 软件工程图表
  6. 外部设备扩展指南
  7. 测距雷达扩展实现
  8. 光流传感器扩展实现
  9. 调试与测试方法
  10. 常见问题与解决方案

1. PX4 CAN 总线系统架构概述

CAN（Controller Area Network）总线是 PX4 自动驾驶系统中连接各类传感器和执行器的关键通信协议，具有高可靠性、实时性和抗干扰能力，非常适合无人机等嵌入式系统应用。

1.1 CAN 总线在 PX4 中的角色

功能描述优势
传感器数据采集连接 GPS、IMU、气压计、磁力计等减少布线，提高可靠性
执行器控制控制电调、舵机等执行机构实时性高，支持分布式控制
外部设备扩展连接测距雷达、光流、避障传感器等标准化接口，易于扩展
冗余设计支持双 CAN 总线配置提高系统容错能力

1.2 PX4 CAN 系统软件架构

PX4 的 CAN 系统采用分层设计，从上到下分为：

层级组件功能
应用层各类设备驱动、uORB 消息处理实现具体设备功能，处理传感器数据
协议层CAN 驱动、DRONE CAN 协议栈处理 CAN 消息的编码、解码和传输
硬件抽象层CAN 控制器驱动与具体硬件 CAN 控制器交互
硬件层CAN 控制器、收发器物理层和数据链路层实现

1.3 DRONE CAN 协议简介

DRONE CAN 是针对无人机应用优化的 CAN 总线协议，基于 CAN 2.0B 标准，定义了无人机系统中常用设备的通信规范。

特性说明
数据帧格式扩展帧 (29 位 ID) 为主，支持标准帧 (11 位 ID)
波特率默认 1Mbps，支持 500kbps 等其他速率
节点类型定义了传感器、执行器、控制器等节点类型
消息类型包含设备信息、传感器数据、控制指令等
即插即用支持自动识别和配置新接入的设备

2. DRONE CAN 核心代码解析

2.1 代码目录结构

PX4 中与 CAN 和 DRONE CAN 相关的代码主要分布在以下目录：

目录功能核心文件
drivers/canCAN 驱动核心实现can.h、can.cpp、can_dev.h
drivers/can/dronecanDRONE CAN 协议栈dronecan.h、dronecan.cpp、node.hpp
drivers/distance_sensor距离传感器驱动can_based_distance_sensor.cpp
drivers/flow光流传感器驱动can_flow.cpp
src/modules/uORB/topicsuORB 消息定义distance_sensor.msg、optical_flow.msg
src/lib/drivers设备驱动抽象device.h、sensor.h

2.2 CAN 驱动核心代码

can.h 和 can.cpp 实现了 CAN 总线的基本操作接口：

cpp

运行

// can.h 核心接口定义
class CANDriver : public Device
{
public:
    // 初始化CAN控制器
    virtual int init() = 0;
    
    // 发送CAN消息
    virtual int send(const can_message_t &msg, uint64_t timeout = 0) = 0;
    
    // 接收CAN消息
    virtual int receive(can_message_t &msg, uint64_t timeout = 0) = 0;
    
    // 设置CAN波特率
    virtual int set_bitrate(unsigned int bitrate) = 0;
    
    // 注册消息回调函数
    virtual void register_callback(CanCallback callback, void *user, uint32_t id, uint32_t mask) = 0;
};

can_message_t 结构体定义了 CAN 消息的格式：

cpp

运行

struct can_message_t {
    uint32_t id;         // CAN消息ID
    uint8_t  data[8];    // 数据内容
    uint8_t  len;        // 数据长度
    uint8_t  flags;      // 标志位(扩展帧/标准帧等)
    uint64_t timestamp;  // 时间戳
};

2.3 DRONE CAN 协议栈实现

dronecan.h 和 dronecan.cpp 实现了 DRONE CAN 协议的核心功能：

cpp

运行

// dronecan.h 核心类定义
class DroneCAN : public ModuleBase<DroneCAN>
{
public:
    // 初始化DRONE CAN节点
    bool init(const char *can_device_path);
    
    // 启动节点
    void start();
    
    // 发送DRONE CAN消息
    template <typename T>
    int send(const T &msg, uint8_t priority = CAN_PRIO_DEFAULT);
    
    // 注册消息回调
    template <typename T>
    void register_message_handler(DroneCANMessageHandler<T> handler, void *user_data = nullptr);
    
    // 处理接收到的CAN消息
    void handle_can_message(const can_message_t &msg);
    
    // 获取节点状态
    dronecan_node_status_t get_node_status() const;
};

DRONE CAN 消息处理采用注册回调的方式，当接收到特定类型的消息时，会自动调用相应的处理函数：

cpp

运行

// 消息处理示例
void handle_distance_sensor(const dronecan::messages::DistanceSensor &msg, 
                           const dronecan::TransferMetadata &meta,
                           void *user_data)
{
    // 处理距离传感器数据
    DistanceSensorData data;
    data.distance = msg.distance;
    data.timestamp = hrt_absolute_time();
    data.type = msg.sensor_type;
    
    // 发布到uORB
    publish_distance_sensor(data);
}

// 注册消息处理
dronecan.register_message_handler<dronecan::messages::DistanceSensor>(handle_distance_sensor, this);

2.4 设备驱动实现

以 CAN-based 距离传感器为例，其驱动实现结构如下：

cpp

运行

class CanBasedDistanceSensor : public DistanceSensor
{
public:
    CanBasedDistanceSensor(const char *can_device_path);
    ~CanBasedDistanceSensor() override;
    
    int init() override;
    int ioctl(unsigned int cmd, unsigned long arg) override;
    
private:
    // DRONE CAN节点指针
    DroneCAN *_dronecan;
    
    // 设备信息
    dronecan::NodeID _node_id;
    uint8_t _sensor_type;
    
    // 消息处理函数
    void handle_distance_message(const dronecan::messages::DistanceSensor &msg,
                                const dronecan::TransferMetadata &meta);
    
    // 工作线程
    static void work_trampoline(void *arg);
    void work();
    
    // 设备状态
    enum class State {
        INIT,
        WAITING_FOR_DEVICE,
        RUNNING,
        ERROR
    } _state;
};

3. 工作原理详解

3.1 CAN 总线初始化流程

PX4 系统启动时，CAN 总线的初始化流程如下：

步骤函数调用描述
1boardctl(BOARDCtl_INIT_CAN, 0)触发 CAN 初始化
2can_init()初始化 CAN 硬件和驱动
3can_start()启动 CAN 控制器
4DroneCAN::init()初始化 DRONE CAN 协议栈
5DroneCAN::start()启动 DRONE CAN 节点
6can_device_register()注册 CAN 设备驱动
7uORB::Publication::advertise()发布传感器数据到 uORB

3.2 消息发送流程

DRONE CAN 消息发送的完整流程：

  1. 应用层准备数据（如传感器读数）
  2. 将数据封装为相应的 DRONE CAN 消息结构体
  3. 调用DroneCAN::send()方法发送消息
  4. 协议栈将消息编码为 CAN 帧格式
  5. 调用 CAN 驱动的send()方法发送 CAN 帧
  6. 硬件层将数据发送到 CAN 总线

3.3 消息接收流程

DRONE CAN 消息接收的完整流程：

  1. CAN 硬件接收到总线上的数据
  2. 触发中断，调用 CAN 驱动的中断处理函数
  3. CAN 驱动将数据封装为can_message_t结构体
  4. 调用 DRONE CAN 协议栈的消息处理函数
  5. 协议栈解析 CAN 帧，得到 DRONE CAN 消息
  6. 调用注册的消息回调函数处理消息
  7. 处理后的数据发布到 uORB 消息总线上

3.4 设备发现与配置

DRONE CAN 支持即插即用功能，设备发现与配置流程：

步骤描述相关消息
1新设备上电，发送节点状态消息NodeStatus
2主控节点接收到新节点消息-
3主控节点发送节点 ID 分配消息NodeIDAllocation
4新设备确认节点 IDNodeIDAllocationFeedback
5主控节点请求设备信息GetInfo
6新设备返回设备信息Info
7主控节点配置设备参数SetParameter
8设备确认参数配置ParameterValue
9设备开始正常工作，发送数据各类传感器数据消息

4. 二次开发接口说明

4.1 CAN 驱动接口

接口函数原型功能描述参数说明返回值
初始化 CANint can_init(const char *device, unsigned int bitrate)初始化指定 CAN 设备device: CAN 设备路径
bitrate: 波特率		0: 成功
-1: 失败
发送 CAN 消息int can_send(int fd, const can_message_t *msg, uint64_t timeout)发送 CAN 消息fd: CAN 设备文件描述符
msg: 要发送的消息
timeout: 超时时间		0: 成功
-1: 失败
接收 CAN 消息int can_recv(int fd, can_message_t *msg, uint64_t timeout)接收 CAN 消息fd: CAN 设备文件描述符
msg: 接收消息缓冲区
timeout: 超时时间		0: 成功
-1: 失败
注册消息过滤器int can_set_filter(int fd, uint32_t id, uint32_t mask)设置 CAN 消息过滤器fd: CAN 设备文件描述符
id: 消息 ID
mask: 掩码		0: 成功
-1: 失败

4.2 DRONE CAN 协议栈接口

接口函数原型功能描述参数说明返回值
初始化节点bool DroneCAN::init(const char *can_device_path)初始化 DRONE CAN 节点can_device_path: CAN 设备路径true: 成功
false: 失败
启动节点void DroneCAN::start()启动 DRONE CAN 节点--
发送消息template <typename T> int send(const T &msg, uint8_t priority)发送 DRONE CAN 消息msg: 要发送的消息对象
priority: 消息优先级		0: 成功
-1: 失败
注册消息处理template <typename T> void register_message_handler(DroneCANMessageHandler<T> handler, void *user_data)注册消息处理回调handler: 回调函数
user_data: 用户数据		-
获取节点 IDdronecan::NodeID get_node_id() const获取当前节点 ID-节点 ID
设置节点模式void set_node_mode(dronecan::NodeMode mode)设置节点工作模式mode: 节点模式-

4.3 设备驱动开发接口

接口函数原型功能描述适用场景
设备基类class Device所有设备的基类开发新设备驱动
传感器基类class Sensor传感器设备基类开发传感器驱动
距离传感器接口class DistanceSensor距离传感器抽象接口开发测距设备
光流传感器接口class OpticalFlow光流传感器抽象接口开发光流设备
uORB 发布接口uORB::Publication<T>发布消息到 uORB传感器数据发布
uORB 订阅接口uORB::Subscription<T>从 uORB 订阅消息接收控制指令

4.4 配置参数接口

PX4 提供了参数系统用于配置 CAN 设备，常用接口：

接口函数原型功能描述
定义参数PARAM_DEFINE_INT32(name, default_value)定义整数参数
查找参数param_t param_find(const char *name)查找参数 ID
获取参数int param_get(param_t param, void *val)获取参数值
设置参数int param_set(param_t param, const void *val)设置参数值
参数更新回调void param_changed(param_t param)参数变化时调用

常用 CAN 相关参数：

参数名描述取值范围默认值
CAN1_BITRATECAN1 总线波特率125000, 250000, 500000, 10000001000000
CAN2_BITRATECAN2 总线波特率125000, 250000, 500000, 10000001000000
CAN_DRV_LOG_LEVELCAN 驱动日志级别0-51
CAN_POWER_ENABLECAN 总线电源使能0-11
UAVCAN_ENABLEDRONE CAN 使能0-11

5. UML 软件工程图表

5.1 系统类图

5.2 消息处理时序图

5.3 设备初始化流程图

6. 外部设备扩展指南

6.1 扩展流程概述

为 PX4 添加新的 CAN 外部设备通常需要以下步骤：

步骤内容相关文件
1定义设备通信协议-
2实现 DRONE CAN 消息结构协议定义文件
3开发设备驱动drivers/目录下新建驱动文件
4实现 uORB 消息发布使用现有或新建 uORB 消息
5添加参数配置定义设备相关参数
6修改编译配置CMakeLists.txt
7测试与调试日志文件、mavlink 调试

6.2 设备协议设计原则

设计新设备的 DRONE CAN 通信协议时应遵循以下原则：

原则说明
兼容性尽量使用 DRONE CAN 标准消息类型
高效性数据格式紧凑，减少不必要的字段
实时性关键数据采用较高优先级
可靠性重要数据添加校验机制
可扩展性预留未来功能扩展的空间
一致性同类设备保持协议一致性

6.3 驱动开发模板

新 CAN 设备驱动的基本模板：

cpp

运行

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <drivers/device/device.h>
#include <lib/drivers/sensor.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/[topic_name].h>
#include <drivers/can/dronecan/dronecan.h>

class Can[DeviceType] : public [BaseClass]
{
public:
    Can[DeviceType](const char *can_device_path);
    ~Can[DeviceType]() override;
    
    int init() override;
    
    // 设备特定接口
    [device_specific_methods]
    
private:
    // DRONE CAN节点
    DroneCAN *_dronecan;
    
    // 设备信息
    dronecan::NodeID _node_id;
    char _device_name[32];
    
    // uORB发布器
    uORB::Publication<[topic_type]> _pub;
    
    // 消息处理回调
    void handle_message(const [dronecan_message_type] &msg, 
                       const dronecan::TransferMetadata &meta);
    
    // 工作线程
    void work();
    static void work_trampoline(void *arg);
    px4::WorkItem *_work_item;
    
    // 设备状态
    enum class State {
        INIT,
        WAITING_FOR_DEVICE,
        RUNNING,
        ERROR
    } _state;
    
    // 设备参数
    param_t _param1;
    param_t _param2;
};

// 构造函数
Can[DeviceType]::Can[DeviceType](const char *can_device_path)
    : [BaseClass]("Can[DeviceType]", "", [DeviceType]_DEVICE_PATH),
      _node_id(0),
      _state(State::INIT),
      _work_item(nullptr)
{
    // 初始化代码
}

// 析构函数
Can[DeviceType]::~Can[DeviceType]()
{
    if (_work_item) {
        delete _work_item;
    }
}

// 初始化函数
int Can[DeviceType]::init()
{
    // 初始化DRONE CAN
    _dronecan = DroneCAN::get_instance(_can_device_path);
    if (!_dronecan) {
        PX4_ERR("Failed to get DroneCAN instance");
        return -1;
    }
    
    // 注册消息回调
    _dronecan->register_message_handler<[dronecan_message_type]>(
        [this](const [dronecan_message_type] &msg, const dronecan::TransferMetadata &meta) {
            this->handle_message(msg, meta);
        }, this);
    
    // 初始化工作线程
    _work_item = new px4::WorkItem(this, &Can[DeviceType]::work);
    
    // 查找参数
    _param1 = param_find("[PARAM_NAME1]");
    _param2 = param_find("[PARAM_NAME2]");
    
    _state = State::WAITING_FOR_DEVICE;
    return 0;
}

// 消息处理函数
void Can[DeviceType]::handle_message(const [dronecan_message_type] &msg, 
                                    const dronecan::TransferMetadata &meta)
{
    // 处理接收到的消息
    _node_id = meta.source_node_id;
    _state = State::RUNNING;
    
    // 处理数据...
}

// 工作函数
void Can[DeviceType]::work()
{
    switch (_state) {
        case State::INIT:
            // 初始化状态处理
            break;
            
        case State::WAITING_FOR_DEVICE:
            // 等待设备连接
            break;
            
        case State::RUNNING:
            // 运行状态处理，发布数据
            [topic_type] data = {};
            // 填充数据...
            _pub.publish(data);
            break;
            
        case State::ERROR:
            // 错误状态处理
            break;
    }
}

// 驱动注册
extern "C" __EXPORT int can_[device_type]_start(const char *can_device_path)
{
    // 检查设备是否已存在
    if (device_exists("[device_path]")) {
        PX4_ERR("Device already exists");
        return -1;
    }
    
    // 创建驱动实例
    Can[DeviceType] *dev = new Can[DeviceType](can_device_path);
    if (!dev) {
        PX4_ERR("Failed to allocate driver");
        return -1;
    }
    
    // 初始化驱动
    if (dev->init() != 0) {
        PX4_ERR("Failed to initialize driver");
        delete dev;
        return -1;
    }
    
    PX4_INFO("CAN [DeviceType] driver started");
    return 0;
}

7. 测距雷达扩展实现

7.1 测距雷达 DRONE CAN 协议定义

7.1.1 设备信息消息
字段类型描述单位范围
sensor_typeuint8_t传感器类型-0-255
min_distancefloat最小测量距离m0-100
max_distancefloat最大测量距离m0-1000
orientationuint8_t安装方向-0-15
update_ratefloat数据更新率Hz0-1000
7.1.2 距离数据消息
字段类型描述单位范围
distancefloat测量距离m0-1000
signal_qualityuint8_t信号质量%0-100
timestampuint32_t时间戳ms0-4294967295
flagsuint8_t状态标志-0-255
7.1.3 配置消息
字段类型描述单位范围
update_ratefloat数据更新率Hz0-1000
field_of_viewfloat视场角度0-180
modeuint8_t工作模式-0-3
filter_strengthuint8_t滤波强度-0-10
enablebool使能状态-0-1
7.1.4 消息 ID 定义
消息类型消息 ID优先级传输方向
设备信息0x1001低传感器→飞控
距离数据0x1002中传感器→飞控
配置消息0x1003中飞控→传感器
配置确认0x1004中传感器→飞控
设备状态0x1005低传感器→飞控
命令消息0x1006高飞控→传感器

7.2 测距雷达驱动实现

7.2.1 驱动头文件 (can_rangefinder.h)

cpp

运行

#ifndef DRIVERS_CAN_RANGFINDER_H_
#define DRIVERS_CAN_RANGFINDER_H_

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <drivers/device/device.h>
#include <lib/drivers/rangefinder/rangefinder.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/distance_sensor.h>
#include <drivers/can/dronecan/dronecan.h>
#include <drivers/can/dronecan/messages/rangefinder.hpp>

class CanRangefinder : public Rangefinder
{
public:
    CanRangefinder(const char *can_device_path);
    ~CanRangefinder() override;

    int init() override;
    int ioctl(unsigned int cmd, unsigned long arg) override;

    void print_info() override;

private:
    DroneCAN *_dronecan;

    dronecan::NodeID _node_id{0};
    char _vendor_name[32]{"Unknown"};
    char _model_name[32]{"Unknown"};

    uORB::Publication<distance_sensor_s> _pub{ORB_ID(distance_sensor)};

    // 传感器参数
    float _min_distance{0.1f};
    float _max_distance{10.0f};
    float _fov{30.0f};
    int _orientation{distance_sensor_s::ROTATION_DOWNWARD};

    // 状态变量
    bool _initialized{false};
    hrt_abstime _last_measurement_time{0};
    uint32_t _consecutive_failures{0};

    // 消息处理函数
    void handle_device_info(const dronecan::messages::rangefinder::DeviceInfo &msg,
                           const dronecan::TransferMetadata &meta);
                           
    void handle_distance_measurement(const dronecan::messages::rangefinder::DistanceMeasurement &msg,
                                    const dronecan::TransferMetadata &meta);
                                    
    void handle_config_ack(const dronecan::messages::rangefinder::ConfigAck &msg,
                          const dronecan::TransferMetadata &meta);

    // 工作线程
    void work();
    static void work_trampoline(void *arg);
    px4::WorkItem *_work_item{nullptr};

    // 发送配置到传感器
    int send_config();

    // 参数处理
    static int param_update(int param_id, void *arg);
    param_t _param_orientation;
    param_t _param_update_rate;
    param_t _param_filter_strength;
};

#endif /* DRIVERS_CAN_RANGFINDER_H_ */
7.2.2 驱动实现文件 (can_rangefinder.cpp)

cpp

运行

#include "can_rangefinder.h"
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <parameters/param.h>

CanRangefinder::CanRangefinder(const char *can_device_path)
    : Rangefinder("CAN_Rangefinder", can_device_path)
{
    _work_item = new px4::WorkItem(this, &CanRangefinder::work);
}

CanRangefinder::~CanRangefinder()
{
    if (_work_item) {
        delete _work_item;
    }
}

int CanRangefinder::init()
{
    // 获取DRONE CAN实例
    _dronecan = DroneCAN::get_instance(_device_path);
    if (!_dronecan) {
        PX4_ERR("Failed to get DroneCAN instance for %s", _device_path);
        return PX4_ERROR;
    }

    // 注册消息回调
    _dronecan->register_message_handler<dronecan::messages::rangefinder::DeviceInfo>(
        [this](const dronecan::messages::rangefinder::DeviceInfo &msg, const dronecan::TransferMetadata &meta) {
            this->handle_device_info(msg, meta);
        }, this);

    _dronecan->register_message_handler<dronecan::messages::rangefinder::DistanceMeasurement>(
        [this](const dronecan::messages::rangefinder::DistanceMeasurement &msg, const dronecan::TransferMetadata &meta) {
            this->handle_distance_measurement(msg, meta);
        }, this);

    _dronecan->register_message_handler<dronecan::messages::rangefinder::ConfigAck>(
        [this](const dronecan::messages::rangefinder::ConfigAck &msg, const dronecan::TransferMetadata &meta) {
            this->handle_config_ack(msg, meta);
        }, this);

    // 查找参数
    _param_orientation = param_find("RNGFND_CAN_ORIENT");
    _param_update_rate = param_find("RNGFND_CAN_RATE");
    _param_filter_strength = param_find("RNGFND_CAN_FILT");

    // 设置参数更新回调
    param_set_callback(_param_orientation, this, &CanRangefinder::param_update);
    param_set_callback(_param_update_rate, this, &CanRangefinder::param_update);
    param_set_callback(_param_filter_strength, this, &CanRangefinder::param_update);

    // 加载参数
    param_update(0, this);

    // 启动工作线程
    _work_item->ScheduleOnInterval(100000); // 10Hz

    _initialized = true;
    return PX4_OK;
}

int CanRangefinder::ioctl(unsigned int cmd, unsigned long arg)
{
    switch (cmd) {
        case RANGEFINDERIOC_SET_ORIENTATION:
            _orientation = arg;
            send_config();
            return PX4_OK;

        case RANGEFINDERIOC_GET_INFO: {
            struct rangefinder_info_s info = {};
            strncpy(info.vendor_name, _vendor_name, sizeof(info.vendor_name) - 1);
            strncpy(info.model_name, _model_name, sizeof(info.model_name) - 1);
            info.min_distance = _min_distance;
            info.max_distance = _max_distance;
            info.fov = _fov;
            info.orientation = _orientation;
            return copy_to_user((void *)arg, &info, sizeof(info)) ? PX4_ERROR : PX4_OK;
        }

        default:
            return Rangefinder::ioctl(cmd, arg);
    }
}

void CanRangefinder::print_info()
{
    PX4_INFO("CAN Rangefinder:");
    PX4_INFO("  Device Path: %s", _device_path);
    PX4_INFO("  Node ID: %d", (int)_node_id);
    PX4_INFO("  Vendor: %s", _vendor_name);
    PX4_INFO("  Model: %s", _model_name);
    PX4_INFO("  Min Distance: %.2f m", _min_distance);
    PX4_INFO("  Max Distance: %.2f m", _max_distance);
    PX4_INFO("  Field of View: %.1f degrees", _fov);
    PX4_INFO("  Orientation: %d", _orientation);
    PX4_INFO("  Last Measurement: %lld us ago", hrt_elapsed_time(&_last_measurement_time));
    PX4_INFO("  Status: %s", _initialized ? "Initialized" : "Not initialized");
}

void CanRangefinder::handle_device_info(const dronecan::messages::rangefinder::DeviceInfo &msg,
                                       const dronecan::TransferMetadata &meta)
{
    _node_id = meta.source_node_id;
    strncpy(_vendor_name, msg.vendor_name.c_str(), sizeof(_vendor_name) - 1);
    strncpy(_model_name, msg.model_name.c_str(), sizeof(_model_name) - 1);
    
    _min_distance = msg.min_distance;
    _max_distance = msg.max_distance;
    _fov = msg.field_of_view;
    
    PX4_INFO("Discovered CAN rangefinder: %s %s (Node ID: %d)",
             _vendor_name, _model_name, (int)_node_id);
    
    // 收到设备信息后发送配置
    send_config();
}

void CanRangefinder::handle_distance_measurement(const dronecan::messages::rangefinder::DistanceMeasurement &msg,
                                                const dronecan::TransferMetadata &meta)
{
    _node_id = meta.source_node_id;
    _last_measurement_time = hrt_absolute_time();
    _consecutive_failures = 0;

    // 检查数据有效性
    if (msg.distance < 0 || msg.distance > _max_distance * 1.2f) {
        PX4_DEBUG("Invalid distance measurement: %.2f m", (double)msg.distance);
        _consecutive_failures++;
        return;
    }

    // 发布距离传感器数据
    distance_sensor_s data = {};
    data.timestamp = hrt_absolute_time();
    data.min_distance = _min_distance;
    data.max_distance = _max_distance;
    data.current_distance = msg.distance;
    data.signal_quality = msg.signal_quality;
    data.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
    data.orientation = _orientation;
    data.covariance = 0.01f; // 1cm 方差
    data.timestamp_sample = data.timestamp;

    _pub.publish(data);
    
    // 更新基类数据
    set_data(data.current_distance, data.timestamp);
}

void CanRangefinder::handle_config_ack(const dronecan::messages::rangefinder::ConfigAck &msg,
                                      const dronecan::TransferMetadata &meta)
{
    if (msg.success) {
        PX4_INFO("Rangefinder config applied successfully");
    } else {
        PX4_ERR("Rangefinder config failed: %s", msg.error_message.c_str());
    }
}

int CanRangefinder::send_config()
{
    if (!_dronecan || _node_id == 0) {
        return PX4_ERROR;
    }

    // 获取当前参数值
    int32_t orientation = _orientation;
    param_get(_param_orientation, &orientation);
    _orientation = orientation;

    float update_rate = 10.0f;
    param_get(_param_update_rate, &update_rate);

    int32_t filter_strength = 5;
    param_get(_param_filter_strength, &filter_strength);

    // 创建配置消息
    dronecan::messages::rangefinder::Config cfg;
    cfg.update_rate = update_rate;
    cfg.field_of_view = _fov; // 保持传感器默认FOV
    cfg.orientation = _orientation;
    cfg.filter_strength = filter_strength;
    cfg.enable = true;

    // 发送配置消息到指定节点
    return _dronecan->send(cfg, CAN_PRIO_NORMAL, _node_id);
}

void CanRangefinder::work()
{
    if (!_initialized) {
        return;
    }

    // 检查设备是否在线
    if (hrt_elapsed_time(&_last_measurement_time) > 5000000) { // 5秒无数据
        if (_consecutive_failures < 5) {
            _consecutive_failures++;
            PX4_WARN("No data from CAN rangefinder (count: %d)", _consecutive_failures);
        }
    }

    // 如果已发现设备但未配置，尝试发送配置
    if (_node_id != 0 && hrt_elapsed_time(&_last_measurement_time) < 5000000) {
        static hrt_abstime last_config_send = 0;
        if (hrt_elapsed_time(&last_config_send) > 1000000) { // 每1秒发送一次配置
            send_config();
            last_config_send = hrt_absolute_time();
        }
    }
}

int CanRangefinder::param_update(int param_id, void *arg)
{
    CanRangefinder *dev = (CanRangefinder *)arg;
    if (dev) {
        dev->send_config();
    }
    return 0;
}

extern "C" __EXPORT int can_rangefinder_start(const char *can_device_path)
{
    if (!can_device_path) {
        PX4_ERR("Missing CAN device path");
        return PX4_ERROR;
    }

    // 检查设备是否已存在
    if (device_exists("can/rangefinder")) {
        PX4_ERR("Device already exists");
        return PX4_ERROR;
    }

    // 创建驱动实例
    CanRangefinder *dev = new CanRangefinder(can_device_path);
    if (!dev) {
        PX4_ERR("Failed to allocate driver");
        return PX4_ERROR;
    }

    // 初始化驱动
    if (dev->init() != PX4_OK) {
        PX4_ERR("Failed to initialize driver");
        delete dev;
        return PX4_ERROR;
    }

    // 注册设备
    if (dev->register_class_devname("can/rangefinder") != PX4_OK) {
        PX4_ERR("Failed to register device");
        delete dev;
        return PX4_ERROR;
    }

    PX4_INFO("CAN rangefinder driver started on %s", can_device_path);
    return PX4_OK;
}

extern "C" __EXPORT int can_rangefinder_stop()
{
    // 查找并删除设备
    Device *dev = device_find("can/rangefinder");
    if (!dev) {
        PX4_ERR("Device not found");
        return PX4_ERROR;
    }

    delete dev;
    PX4_INFO("CAN rangefinder driver stopped");
    return PX4_OK;
}

extern "C" __EXPORT int can_rangefinder_main(int argc, char *argv[])
{
    if (argc == 2 && strcmp(argv[1], "start") == 0) {
        const char *can_device = argc > 2 ? argv[2] : "/dev/can0";
        return can_rangefinder_start(can_device);
    } else if (argc == 1 && strcmp(argv[0], "stop") == 0) {
        return can_rangefinder_stop();
    } else if (argc == 1 && strcmp(argv[0], "status") == 0) {
        Device *dev = device_find("can/rangefinder");
        if (!dev) {
            PX4_ERR("Device not running");
            return PX4_ERROR;
        }

        CanRangefinder *rf = (CanRangefinder *)dev;
        rf->print_info();
        return PX4_OK;
    }

    PX4_INFO("Usage: can_rangefinder {start|stop|status} [can_device]");
    return PX4_ERROR;
}

7.3 测距雷达参数配置

参数名描述取值范围默认值
RNGFND_CAN_ORIENT传感器安装方向0-2325 (向下)
RNGFND_CAN_RATE数据更新率1-10010 Hz
RNGFND_CAN_FILT滤波强度0-105
RNGFND_CAN_MIN最小距离0.01-100.1 m
RNGFND_CAN_MAX最大距离0.1-100010 m
RNGFND_CAN_FOV视场角1-18030 度
RNGFND_CAN_MODE工作模式0-30 (默认)

7.4 初始化与应用代码

7.4.1 驱动注册与启动

在 CMakeLists.txt 中添加：

cmake

px4_add_module(
    MODULE drivers__can_rangefinder
    MAIN can_rangefinder
    SRCS
        can_rangefinder.cpp
    DEPENDS
        platforms__common
        drivers__device
        drivers__can__dronecan
    )

在启动脚本中添加（例如 posix-configs/px4fmu_common/init.d/rc.sensors）：

bash

# 启动CAN测距雷达
if can_rangefinder start /dev/can0; then
    echo "Started CAN rangefinder on /dev/can0"
else
    echo "Failed to start CAN rangefinder on /dev/can0"
fi
7.4.2 应用层使用示例

cpp

运行

#include <uORB/Subscription.hpp>
#include <uORB/topics/distance_sensor.h>

class RangefinderUser
{
public:
    RangefinderUser() : _distance_sub(ORB_ID(distance_sensor)) {}
    
    void update()
    {
        distance_sensor_s dist;
        if (_distance_sub.update(&dist)) {
            // 检查数据是否有效
            if (dist.current_distance >= dist.min_distance && 
                dist.current_distance <= dist.max_distance) {
                
                // 使用距离数据
                PX4_INFO("Distance: %.2f m", (double)dist.current_distance);
                
                // 根据安装方向处理数据
                switch (dist.orientation) {
                    case distance_sensor_s::ROTATION_DOWNWARD:
                        handle_downward_distance(dist.current_distance);
                        break;
                    case distance_sensor_s::ROTATION_FRONT:
                        handle_front_distance(dist.current_distance);
                        break;
                    // 处理其他方向...
                }
            } else {
                PX4_WARN("Distance out of range: %.2f m", (double)dist.current_distance);
            }
        }
    }
    
private:
    uORB::Subscription<distance_sensor_s> _distance_sub;
    
    void handle_downward_distance(float distance)
    {
        // 处理下视距离数据，例如用于定高
    }
    
    void handle_front_distance(float distance)
    {
        // 处理前视距离数据，例如用于避障
    }
};

8. 光流传感器扩展实现

8.1 光流传感器 DRONE CAN 协议定义

8.1.1 设备信息消息
字段类型描述单位范围
resolution_xuint16_tX 方向分辨率像素0-65535
resolution_yuint16_tY 方向分辨率像素0-65535
max_flow_ratefloat最大流速像素 / 秒0-10000
update_ratefloat更新率Hz0-1000
fov_xfloatX 方向视场角度0-180
fov_yfloatY 方向视场角度0-180
8.1.2 光流数据消息
字段类型描述单位范围
flow_xfloatX 方向流速像素 / 秒-10000-10000
flow_yfloatY 方向流速像素 / 秒-10000-10000
flow_x_integralfloatX 方向积分流速像素-1e6-1e6
flow_y_integralfloatY 方向积分流速像素-1e6-1e6
ground_distancefloat地面距离m0-100
qualityuint8_t数据质量%0-100
timestampuint32_t时间戳ms0-4294967295
flagsuint8_t状态标志-0-255
8.1.3 配置消息
字段类型描述单位范围
update_ratefloat数据更新率Hz0-1000
gainuint8_t增益设置-0-100
enable_gyrobool使能陀螺仪融合-0-1
enable_lpfbool使能低通滤波-0-1
lpf_cutofffloat低通滤波截止频率Hz0-100
orientationuint8_t安装方向-0-15
8.1.4 消息 ID 定义
消息类型消息 ID优先级传输方向
设备信息0x1101低传感器→飞控
光流数据0x1102高传感器→飞控
配置消息0x1103中飞控→传感器
配置确认0x1104中传感器→飞控
设备状态0x1105低传感器→飞控
陀螺仪数据0x1106高传感器→飞控

8.2 光流传感器驱动实现

8.2.1 驱动头文件 (can_optical_flow.h)

cpp

运行

#ifndef DRIVERS_CAN_OPTICAL_FLOW_H_
#define DRIVERS_CAN_OPTICAL_FLOW_H_

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <drivers/device/device.h>
#include <lib/drivers/optical_flow/optical_flow.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <drivers/can/dronecan/dronecan.h>
#include <drivers/can/dronecan/messages/optical_flow.hpp>

class CanOpticalFlow : public OpticalFlow
{
public:
    CanOpticalFlow(const char *can_device_path);
    ~CanOpticalFlow() override;

    int init() override;
    int ioctl(unsigned int cmd, unsigned long arg) override;

    void print_info() override;

private:
    DroneCAN *_dronecan;

    dronecan::NodeID _node_id{0};
    char _vendor_name[32]{"Unknown"};
    char _model_name[32]{"Unknown"};

    uORB::Publication<optical_flow_s> _pub{ORB_ID(optical_flow)};
    uORB::Subscription<vehicle_angular_velocity_s> _ang_vel_sub{ORB_ID(vehicle_angular_velocity)};

    // 传感器参数
    uint16_t _resolution_x{0};
    uint16_t _resolution_y{0};
    float _fov_x{0.0f};
    float _fov_y{0.0f};
    float _max_flow_rate{0.0f};
    int _orientation{0};

    // 状态变量
    bool _initialized{false};
    hrt_abstime _last_measurement_time{0};
    uint32_t _consecutive_failures{0};
    float _last_ground_distance{0.5f};

    // 积分变量
    float _flow_x_integral{0.0f};
    float _flow_y_integral{0.0f};
    hrt_abstime _last_integral_time{0};

    // 消息处理函数
    void handle_device_info(const dronecan::messages::optical_flow::DeviceInfo &msg,
                           const dronecan::TransferMetadata &meta);
                           
    void handle_flow_measurement(const dronecan::messages::optical_flow::FlowMeasurement &msg,
                                const dronecan::TransferMetadata &meta);
                                
    void handle_gyro_data(const dronecan::messages::optical_flow::GyroData &msg,
                         const dronecan::TransferMetadata &meta);
                         
    void handle_config_ack(const dronecan::messages::optical_flow::ConfigAck &msg,
                          const dronecan::TransferMetadata &meta);

    // 工作线程
    void work();
    static void work_trampoline(void *arg);
    px4::WorkItem *_work_item{nullptr};

    // 发送配置到传感器
    int send_config();

    // 参数处理
    static int param_update(int param_id, void *arg);
    param_t _param_orientation;
    param_t _param_update_rate;
    param_t _param_gain;
    param_t _param_lpf_cutoff;
};

#endif /* DRIVERS_CAN_OPTICAL_FLOW_H_ */
8.2.2 驱动实现文件 (can_optical_flow.cpp)

cpp

运行

#include "can_optical_flow.h"
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <parameters/param.h>
#include <mathlib/mathlib.h>

CanOpticalFlow::CanOpticalFlow(const char *can_device_path)
    : OpticalFlow("CAN_OpticalFlow", can_device_path)
{
    _work_item = new px4::WorkItem(this, &CanOpticalFlow::work);
}

CanOpticalFlow::~CanOpticalFlow()
{
    if (_work_item) {
        delete _work_item;
    }
}

int CanOpticalFlow::init()
{
    // 获取DRONE CAN实例
    _dronecan = DroneCAN::get_instance(_device_path);
    if (!_dronecan) {
        PX4_ERR("Failed to get DroneCAN instance for %s", _device_path);
        return PX4_ERROR;
    }

    // 注册消息回调
    _dronecan->register_message_handler<dronecan::messages::optical_flow::DeviceInfo>(
        [this](const dronecan::messages::optical_flow::DeviceInfo &msg, const dronecan::TransferMetadata &meta) {
            this->handle_device_info(msg, meta);
        }, this);

    _dronecan->register_message_handler<dronecan::messages::optical_flow::FlowMeasurement>(
        [this](const dronecan::messages::optical_flow::FlowMeasurement &msg, const dronecan::TransferMetadata &meta) {
            this->handle_flow_measurement(msg, meta);
        }, this);

    _dronecan->register_message_handler<dronecan::messages::optical_flow::GyroData>(
        [this](const dronecan::messages::optical_flow::GyroData &msg, const dronecan::TransferMetadata &meta) {
            this->handle_gyro_data(msg, meta);
        }, this);

    _dronecan->register_message_handler<dronecan::messages::optical_flow::ConfigAck>(
        [this](const dronecan::messages::optical_flow::ConfigAck &msg, const dronecan::TransferMetadata &meta) {
            this->handle_config_ack(msg, meta);
        }, this);

    // 查找参数
    _param_orientation = param_find("FLOW_CAN_ORIENT");
    _param_update_rate = param_find("FLOW_CAN_RATE");
    _param_gain = param_find("FLOW_CAN_GAIN");
    _param_lpf_cutoff = param_find("FLOW_CAN_LPF");

    // 设置参数更新回调
    param_set_callback(_param_orientation, this, &CanOpticalFlow::param_update);
    param_set_callback(_param_update_rate, this, &CanOpticalFlow::param_update);
    param_set_callback(_param_gain, this, &CanOpticalFlow::param_update);
    param_set_callback(_param_lpf_cutoff, this, &CanOpticalFlow::param_update);

    // 加载参数
    param_update(0, this);

    // 启动工作线程
    _work_item->ScheduleOnInterval(50000); // 20Hz

    _last_integral_time = hrt_absolute_time();
    _initialized = true;
    return PX4_OK;
}

int CanOpticalFlow::ioctl(unsigned int cmd, unsigned long arg)
{
    switch (cmd) {
        case OPTICAL_FLOW_IOC_SET_ORIENTATION:
            _orientation = arg;
            send_config();
            return PX4_OK;

        case OPTICAL_FLOW_IOC_GET_INFO: {
            struct optical_flow_info_s info = {};
            strncpy(info.vendor_name, _vendor_name, sizeof(info.vendor_name) - 1);
            strncpy(info.model_name, _model_name, sizeof(info.model_name) - 1);
            info.resolution_x = _resolution_x;
            info.resolution_y = _resolution_y;
            info.fov_x = _fov_x;
            info.fov_y = _fov_y;
            info.max_flow_rate = _max_flow_rate;
            info.orientation = _orientation;
            return copy_to_user((void *)arg, &info, sizeof(info)) ? PX4_ERROR : PX4_OK;
        }

        default:
            return OpticalFlow::ioctl(cmd, arg);
    }
}

void CanOpticalFlow::print_info()
{
    PX4_INFO("CAN Optical Flow:");
    PX4_INFO("  Device Path: %s", _device_path);
    PX4_INFO("  Node ID: %d", (int)_node_id);
    PX4_INFO("  Vendor: %s", _vendor_name);
    PX4_INFO("  Model: %s", _model_name);
    PX4_INFO("  Resolution: %dx%d pixels", _resolution_x, _resolution_y);
    PX4_INFO("  Field of View: %.1fx%.1f degrees", _fov_x, _fov_y);
    PX4_INFO("  Max Flow Rate: %.0f pixels/s", _max_flow_rate);
    PX4_INFO("  Orientation: %d", _orientation);
    PX4_INFO("  Last Measurement: %lld us ago", hrt_elapsed_time(&_last_measurement_time));
    PX4_INFO("  Last Ground Distance: %.2f m", _last_ground_distance);
    PX4_INFO("  Status: %s", _initialized ? "Initialized" : "Not initialized");
}

void CanOpticalFlow::handle_device_info(const dronecan::messages::optical_flow::DeviceInfo &msg,
                                       const dronecan::TransferMetadata &meta)
{
    _node_id = meta.source_node_id;
    strncpy(_vendor_name, msg.vendor_name.c_str(), sizeof(_vendor_name) - 1);
    strncpy(_model_name, msg.model_name.c_str(), sizeof(_model_name) - 1);
    
    _resolution_x = msg.resolution_x;
    _resolution_y = msg.resolution_y;
    _fov_x = msg.fov_x;
    _fov_y = msg.fov_y;
    _max_flow_rate = msg.max_flow_rate;
    
    PX4_INFO("Discovered CAN optical flow: %s %s (Node ID: %d)",
             _vendor_name, _model_name, (int)_node_id);
    
    // 收到设备信息后发送配置
    send_config();
}

void CanOpticalFlow::handle_flow_measurement(const dronecan::messages::optical_flow::FlowMeasurement &msg,
                                            const dronecan::TransferMetadata &meta)
{
    _node_id = meta.source_node_id;
    _last_measurement_time = hrt_absolute_time();
    _consecutive_failures = 0;

    // 更新地面距离
    if (msg.ground_distance > 0.01f) { // 有效距离
        _last_ground_distance = msg.ground_distance;
    }

    // 计算时间差
    const hrt_abstime now = hrt_absolute_time();
    const float dt = (now - _last_integral_time) / 1e6f;
    _last_integral_time = now;

    // 更新积分
    _flow_x_integral += msg.flow_x * dt;
    _flow_y_integral += msg.flow_y * dt;

    // 限制积分值范围
    _flow_x_integral = math::constrain(_flow_x_integral, -1e6f, 1e6f);
    _flow_y_integral = math::constrain(_flow_y_integral, -1e6f, 1e6f);

    // 获取机体角速度用于补偿
    vehicle_angular_velocity_s ang_vel{};
    _ang_vel_sub.copy(&ang_vel);

    // 发布光流数据
    optical_flow_s data = {};
    data.timestamp = now;
    data.sensor_id = _node_id;
    data.flow_x = msg.flow_x;
    data.flow_y = msg.flow_y;
    data.flow_x_integral = _flow_x_integral;
    data.flow_y_integral = _flow_y_integral;
    data.ground_distance = _last_ground_distance;
    data.quality = msg.quality;
    data.gyro_x = ang_vel.xyz[0];
    data.gyro_y = ang_vel.xyz[1];
    data.gyro_z = ang_vel.xyz[2];
    data.timestamp_gyro = ang_vel.timestamp;
    data.orientation = _orientation;

    _pub.publish(data);
    
    // 更新基类数据
    set_data(data.flow_x, data.flow_y, data.quality, data.ground_distance, data.timestamp);
}

void CanOpticalFlow::handle_gyro_data(const dronecan::messages::optical_flow::GyroData &msg,
                                     const dronecan::TransferMetadata &meta)
{
    // 如果光流传感器内置陀螺仪，处理其数据
    // 这里可以将数据发布到相应的uORB主题或用于内部补偿
}

void CanOpticalFlow::handle_config_ack(const dronecan::messages::optical_flow::ConfigAck &msg,
                                      const dronecan::TransferMetadata &meta)
{
    if (msg.success) {
        PX4_INFO("Optical flow config applied successfully");
    } else {
        PX4_ERR("Optical flow config failed: %s", msg.error_message.c_str());
    }
}

int CanOpticalFlow::send_config()
{
    if (!_dronecan || _node_id == 0) {
        return PX4_ERROR;
    }

    // 获取当前参数值
    int32_t orientation = _orientation;
    param_get(_param_orientation, &orientation);
    _orientation = orientation;

    float update_rate = 50.0f;
    param_get(_param_update_rate, &update_rate);

    int32_t gain = 50;
    param_get(_param_gain, &gain);

    float lpf_cutoff = 20.0f;
    param_get(_param_lpf_cutoff, &lpf_cutoff);

    // 创建配置消息
    dronecan::messages::optical_flow::Config cfg;
    cfg.update_rate = update_rate;
    cfg.gain = gain;
    cfg.enable_gyro = true;
    cfg.enable_lpf = true;
    cfg.lpf_cutoff = lpf_cutoff;
    cfg.orientation = _orientation;

    // 发送配置消息到指定节点
    return _dronecan->send(cfg, CAN_PRIO_NORMAL, _node_id);
}

void CanOpticalFlow::work()
{
    if (!_initialized) {
        return;
    }

    // 检查设备是否在线
    if (hrt_elapsed_time(&_last_measurement_time) > 5000000) { // 5秒无数据
        if (_consecutive_failures < 5) {
            _consecutive_failures++;
            PX4_WARN("No data from CAN optical flow (count: %d)", _consecutive_failures);
        }
    }

    // 如果已发现设备但未配置，尝试发送配置
    if (_node_id != 0 && hrt_elapsed_time(&_last_measurement_time) < 5000000) {
        static hrt_abstime last_config_send = 0;
        if (hrt_elapsed_time(&last_config_send) > 1000000) { // 每1秒发送一次配置
            send_config();
            last_config_send = hrt_absolute_time();
        }
    }
}

int CanOpticalFlow::param_update(int param_id, void *arg)
{
    CanOpticalFlow *dev = (CanOpticalFlow *)arg;
    if (dev) {
        dev->send_config();
    }
    return 0;
}

extern "C" __EXPORT int can_optical_flow_start(const char *can_device_path)
{
    if (!can_device_path) {
        PX4_ERR("Missing CAN device path");
        return PX4_ERROR;
    }

    // 检查设备是否已存在
    if (device_exists("can/optical_flow")) {
        PX4_ERR("Device already exists");
        return PX4_ERROR;
    }

    // 创建驱动实例
    CanOpticalFlow *dev = new CanOpticalFlow(can_device_path);
    if (!dev) {
        PX4_ERR("Failed to allocate driver");
        return PX4_ERROR;
    }

    // 初始化驱动
    if (dev->init() != PX4_OK) {
        PX4_ERR("Failed to initialize driver");
        delete dev;
        return PX4_ERROR;
    }

    // 注册设备
    if (dev->register_class_devname("can/optical_flow") != PX4_OK) {
        PX4_ERR("Failed to register device");
        delete dev;
        return PX4_ERROR;
    }

    PX4_INFO("CAN optical flow driver started on %s", can_device_path);
    return PX4_OK;
}

extern "C" __EXPORT int can_optical_flow_stop()
{
    // 查找并删除设备
    Device *dev = device_find("can/optical_flow");
    if (!dev) {
        PX4_ERR("Device not found");
        return PX4_ERROR;
    }

    delete dev;
    PX4_INFO("CAN optical flow driver stopped");
    return PX4_OK;
}

extern "C" __EXPORT int can_optical_flow_main(int argc, char *argv[])
{
    if (argc >= 2 && strcmp(argv[1], "start") == 0) {
        const char *can_device = argc > 2 ? argv[2] : "/dev/can0";
        return can_optical_flow_start(can_device);
    } else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
        return can_optical_flow_stop();
    } else if (argc == 2 && strcmp(argv[1], "status") == 0) {
        Device *dev = device_find("can/optical_flow");
        if (!dev) {
            PX4_ERR("Device not running");
            return PX4_ERROR;
        }

        CanOpticalFlow *flow = (CanOpticalFlow *)dev;
        flow->print_info();
        return PX4_OK;
    }

    PX4_INFO("Usage: can_optical_flow {start|stop|status} [can_device]");
    return PX4_ERROR;
}

8.3 光流传感器参数配置

参数名描述取值范围默认值
FLOW_CAN_ORIENT传感器安装方向0-2325 (向下)
FLOW_CAN_RATE数据更新率1-20050 Hz
FLOW_CAN_GAIN传感器增益0-10050
FLOW_CAN_LPF低通滤波截止频率1-10020 Hz
FLOW_CAN_MAX最大流速100-100005000 像素 / 秒
FLOW_CAN_SCALE流速缩放因子0.001-101.0
FLOW_CAN_GYRO使能陀螺仪融合0-11

8.4 初始化与应用代码

8.4.1 驱动注册与启动

在 CMakeLists.txt 中添加：

cmake

px4_add_module(
    MODULE drivers__can_optical_flow
    MAIN can_optical_flow
    SRCS
        can_optical_flow.cpp
    DEPENDS
        platforms__common
        drivers__device
        drivers__can__dronecan
    )

在启动脚本中添加（例如 posix-configs/px4fmu_common/init.d/rc.sensors）：

bash

# 启动CAN光流传感器
if can_optical_flow start /dev/can0; then
    echo "Started CAN optical flow on /dev/can0"
else
    echo "Failed to start CAN optical flow on /dev/can0"
fi
8.4.2 应用层使用示例

cpp

运行

#include <uORB/Subscription.hpp>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>

class OpticalFlowUser
{
public:
    OpticalFlowUser() : 
        _flow_sub(ORB_ID(optical_flow)),
        _distance_sub(ORB_ID(distance_sensor)) {}
    
    void update()
    {
        optical_flow_s flow;
        if (_flow_sub.update(&flow)) {
            // 检查数据质量
            if (flow.quality > 50) { // 质量大于50%
                // 获取地面距离（如果光流未提供）
                float ground_distance = flow.ground_distance;
                if (ground_distance < 0.01f) {
                    distance_sensor_s dist;
                    if (_distance_sub.update(&dist) && 
                        dist.orientation == distance_sensor_s::ROTATION_DOWNWARD) {
                        ground_distance = dist.current_distance;
                    }
                }
                
                // 计算机体坐标系下的速度 (m/s)
                if (ground_distance > 0.1f) { // 有效距离
                    float velocity_x = calculate_velocity(flow.flow_x, ground_distance, flow.fov_x, flow.resolution_x);
                    float velocity_y = calculate_velocity(flow.flow_y, ground_distance, flow.fov_y, flow.resolution_y);
                    
                    PX4_INFO("Velocity: x=%.2f m/s, y=%.2f m/s", 
                             (double)velocity_x, (double)velocity_y);
                             
                    // 使用速度数据，例如用于位置控制
                    use_velocity_data(velocity_x, velocity_y);
                }
            } else {
                PX4_WARN("Low flow quality: %d%%", flow.quality);
            }
        }
    }
    
private:
    uORB::Subscription<optical_flow_s> _flow_sub;
    uORB::Subscription<distance_sensor_s> _distance_sub;
    
    // 将像素流速转换为米/秒
    float calculate_velocity(float flow_rate, float distance, float fov, uint16_t resolution)
    {
        if (resolution == 0 || distance < 0.01f) {
            return 0.0f;
        }
        
        // 计算每像素对应的弧度
        float rad_per_pixel = (fov * M_PI / 180.0f) / resolution;
        
        // 转换为弧度/秒
        float angular_velocity = flow_rate * rad_per_pixel;
        
        // 转换为米/秒
        return angular_velocity * distance;
    }
    
    void use_velocity_data(float vx, float vy)
    {
        // 使用速度数据进行位置控制或其他用途
    }
};

9. 调试与测试方法

9.1 日志查看

命令描述常用选项
logger on启动日志记录-
logger off停止日志记录-
log show查看日志内容-f can 筛选 CAN 相关日志
log dump <topic>导出指定主题日志distance_sensor, optical_flow
dmesg查看系统启动日志-w 实时监控

9.2 CAN 总线调试工具

工具描述常用命令
canutilsCAN 总线调试工具集candump can0, cansend can0 123#112233
uorb top查看 uORB 消息频率-
uorb info <topic>查看指定 uORB 主题信息uorb info distance_sensor
param show <pattern>查看参数param show CAN_*
param set <name> <value>设置参数param set CAN1_BITRATE 1000000

9.3 测试流程

9.3.1 测距雷达测试流程
  1. 连接硬件并上电
  2. 检查 CAN 总线状态：canstatus can0
  3. 启动驱动：can_rangefinder start /dev/can0
  4. 检查驱动状态：can_rangefinder status
  5. 查看 uORB 消息：uorb top distance_sensor
  6. 移动目标物，观察距离变化：log dump distance_sensor
  7. 测试不同距离下的测量精度
  8. 测试传感器在不同光照条件下的表现
  9. 测试传感器在不同材质表面的表现
9.3.2 光流传感器测试流程
  1. 连接硬件并上电
  2. 检查 CAN 总线状态：canstatus can0
  3. 启动驱动：can_optical_flow start /dev/can0
  4. 检查驱动状态：can_optical_flow status
  5. 查看 uORB 消息：uorb top optical_flow
  6. 移动传感器，观察流速变化：log dump optical_flow
  7. 测试不同光照条件下的表现
  8. 测试不同纹理表面的表现
  9. 测试不同高度下的表现
  10. 测试不同运动速度下的表现

10. 常见问题与解决方案

10.1 硬件连接问题

问题可能原因解决方案
无 CAN 通信1. 接线错误
2. 电源问题
3. 终端电阻缺失		1. 检查 CAN_H 和 CAN_L 是否接反
2. 检查传感器供电电压和电流
3. 在总线两端添加 120Ω 终端电阻
通信不稳定1. 线缆过长
2. 电磁干扰
3. 接触不良		1. 缩短 CAN 总线长度
2. 使用屏蔽线
3. 检查连接器是否牢固
设备不被识别1. 设备未上电
2. 波特率不匹配
3. 设备故障		1. 检查设备电源
2. 确认波特率设置一致
3. 更换设备测试

10.2 驱动问题

问题可能原因解决方案
驱动无法启动1. CAN 设备不存在
2. 权限问题
3. 驱动代码错误		1. 确认 CAN 设备路径正确
2. 检查文件权限
3. 查看系统日志，修复代码错误
无数据输出1. 设备未正确初始化
2. 消息过滤器设置错误
3. 设备未发送数据		1. 检查设备初始化流程
2. 检查 CAN 过滤器设置
3. 使用 candump 确认总线上是否有数据
数据频率低1. 设备配置问题
2. 工作线程周期过长
3. 总线负载过高		1. 调整设备更新率参数
2. 减小工作线程周期
3. 检查总线上的其他设备

10.3 数据问题

问题可能原因解决方案
数据噪声大1. 传感器本身噪声
2. 滤波参数不当
3. 安装不牢固		1. 检查传感器规格
2. 增大滤波强度
3. 确保传感器安装牢固
数据偏移1. 校准问题
2. 安装倾斜
3. 参数设置错误		1. 执行传感器校准
2. 调整传感器安装角度
3. 检查相关参数设置
数据不更新1. 传感器停止工作
2. 通信中断
3. 驱动崩溃		1. 检查传感器电源
2. 检查 CAN 总线连接
3. 重启驱动，查看日志

通过以上详细解析和实现指南，开发者可以深入理解 PX4 中 DRONE CAN 的工作原理，并成功扩展支持新的测距雷达和光流传感器设备。这些扩展将增强无人机的环境感知能力，为自主导航和避障等功能提供关键数据支持。

评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值