ArduPilot开源代码之AP_OpticalFlow_CXOF

lida2003

于 2024-07-10 19:02:30 发布

阅读量218

点赞数 3

分类专栏： ArduPilot 文章标签： ardupilot 开源 c++

本文链接：https://blog.csdn.net/lida2003/article/details/140332015

版权

ArduPilot 专栏收录该内容

119 篇文章 86 订阅

订阅专栏

ArduPilot开源代码之AP_OpticalFlow_CXOF

1. 源由
2. Library设计
3. 重要例程
4. 总结
5. 参考资料

1. 源由

AP_OpticalFlow_CXOF是就是一个光流计，与前面传感模块：MATEKSYS Optical Flow & LIDAR 3901-L0X不一样。

MATEKSYS Optical Flow & LIDAR 3901-L0X不仅仅只有一个光流计，还有一个Lidar高度计。而本次研读的是基于CXOF串口协议的光流传感器。

2. Library设计

设备相关的重要函数：

init
update
detect

class AP_OpticalFlow_CXOF : public OpticalFlow_backend
{
public:
    /// 构造函数
    AP_OpticalFlow_CXOF(AP_OpticalFlow &_frontend, AP_HAL::UARTDriver *uart);

    // 初始化传感器
    void init() override;

    // 从传感器读取最新值并填充 x, y 和 totals
    void update(void) override;

    // 检测传感器是否可用
    static AP_OpticalFlow_CXOF *detect(AP_OpticalFlow &_frontend);

private:
    AP_HAL::UARTDriver *uart;           // 连接到光流传感器的 UART
    uint64_t last_frame_us;             // 上次从光流传感器接收到消息的系统时间
    uint8_t buf[10];                    // 从光流传感器接收到的字符缓冲区
    uint8_t buf_len;                    // 缓冲区中的字符数
    Vector2f gyro_sum;                  // 自上次从光流传感器接收到消息以来的陀螺仪传感器值的总和
    uint16_t gyro_sum_count;            // 陀螺仪传感器值总和中的数量
};

3. 重要例程

3.1 AP_OpticalFlow_CXOF::init

模块默认波特率19200 bps。

// initialise the sensor
void AP_OpticalFlow_CXOF::init()
{
    // sanity check uart
    if (uart == nullptr) {
        return;
    }

    // open serial port with baud rate of 19200
    uart->begin(19200);

    last_frame_us = AP_HAL::micros();
}

3.2 AP_OpticalFlow_CXOF::update

串口数据采用pull的方式读取，相对来说更占用CPU资源。

[AP_OpticalFlow_MSP](https://blog.csdn.net/lida2003/article/details/140328944)采用Interrupt方式比较节省资源，并且Matek光流计采用了115200 bps的波特率，速度更快，暂用CPU的时间更短。

AP_OpticalFlow_CXOF::update(void)
|
|-- 检查 UART
|   |-- if (uart == nullptr) { return; }
|
|-- 记录陀螺仪值
|   |-- if (gyro_sum_count < 1000)
|       |-- 获取陀螺仪值
|       |-- 累加到 gyro_sum
|       |-- gyro_sum_count++
|
|-- 初始化传感器值
|   |-- int32_t x_sum = 0
|   |-- int32_t y_sum = 0
|   |-- uint16_t qual_sum = 0
|   |-- uint16_t count = 0
|
|-- 读取串口缓冲区的可用字符
|   |-- int16_t nbytes = uart->available()
|   |-- while (nbytes-- > 0)
|       |-- 读取字符
|       |-- if (buf_len == 0)
|       |   |-- 检查是否为帧头
|       |-- else
|           |-- 添加字符到缓冲区
|           |-- if (buf_len >= CXOF_FRAME_LENGTH)
|               |-- 检查帧尾
|               |-- 解码数据包
|               |-- 累加 x_sum, y_sum, qual_sum 和 count
|               |-- 清空缓冲区
|
|-- 无读数则返回
|   |-- if (count == 0) { return; }
|
|-- 计算状态
|   |-- 创建 state 结构体
|   |-- 计算表面质量
|   |-- 计算 dt
|   |-- if (is_positive(dt) && (dt < CXOF_TIMEOUT_SEC))
|       |-- 计算流量值
|       |-- 复制到 state 结构体
|       |-- 复制机体速率到 state 结构体
|       |-- 调用 _applyYaw(state.flowRate)
|   |-- else
|       |-- 将流量值和机体速率置零
|
|-- 调用 _update_frontend(state)
|
|-- 重置陀螺仪和计数器
|   |-- gyro_sum.zero()
|   |-- gyro_sum_count = 0

3.3 AP_OpticalFlow_CXOF::detect

AP_OpticalFlow初始化的内容直接放到了detect函数实现。

// detect the device
AP_OpticalFlow_CXOF *AP_OpticalFlow_CXOF::detect(AP_OpticalFlow &_frontend)
{
    AP_SerialManager *serial_manager = AP::serialmanager().get_singleton();
    if (serial_manager == nullptr) {
        return nullptr;
    }

    // look for first serial driver with protocol defined as OpticalFlow
    AP_HAL::UARTDriver *uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_OpticalFlow, 0);
    if (uart == nullptr) {
        return nullptr;
    }

    // we have found a serial port so use it
    AP_OpticalFlow_CXOF *sensor = new AP_OpticalFlow_CXOF(_frontend, uart);
    return sensor;
}