ArduPilot开源代码之AP_OpticalFlow_CXOF

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;
}

4. 总结

CXOF是一种光流计协议,但是协议使用的方式来看,效率偏低,并不推荐。

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计

相关推荐

  1. ArduPilot源代码AP_MSP

    2024-07-11 02:24:02       6 阅读
  2. ArduPilot源代码AP_OpticalFlow_MSP

    2024-07-11 02:24:02       7 阅读
  3. ArduPilot源代码AP_OpticalFlow_UPFLOW

    2024-07-11 02:24:02       4 阅读
  4. ArduPilot源代码AP_OpticalFlow_CXOF

    2024-07-11 02:24:02       7 阅读
  5. ArduPilot源代码OpticalFlow_backend

    2024-07-11 02:24:02       6 阅读
  6. ArduPilot源代码AP_AHRS_View

    2024-07-11 02:24:02       4 阅读
  7. ArduPilot源代码AP_DAL_GPS

    2024-07-11 02:24:02       5 阅读
  8. ArduPilot源代码AP_DAL_RangeFinder

    2024-07-11 02:24:02       1 阅读
  9. ArduPilot源代码AP_DAL研读系列

    2024-07-11 02:24:02       0 阅读
  10. ArduPilot开源飞控AP_VisualOdom

    2024-07-11 02:24:02       4 阅读

最近更新

  1. android audio 相机按键音:(一)资源加载与替换

    2024-07-11 02:24:02       0 阅读
  2. 使用 Jenkins 实现持续集成和持续部署(CI/CD)

    2024-07-11 02:24:02       0 阅读

热门阅读

  1. QT实现WebSocket通信

    2024-07-11 02:24:02       7 阅读
  2. Text2SQL提问中包括时间的实战方案

    2024-07-11 02:24:02       9 阅读
  3. 进程与线程的区别

    2024-07-11 02:24:02       8 阅读
  4. HTTP有哪些请求方式?

    2024-07-11 02:24:02       7 阅读
  5. 笔记

    2024-07-11 02:24:02       7 阅读
  6. 代码随想录Day76(图论Part11)

    2024-07-11 02:24:02       5 阅读
  7. 优雅下线的艺术:Eureka服务管理深度解析

    2024-07-11 02:24:02       7 阅读
  8. 【LeetCode】12. 小张刷题计划

    2024-07-11 02:24:02       4 阅读
  9. samout 最新版本state 逐层控制加速收敛

    2024-07-11 02:24:02       6 阅读
  10. 【个人笔记】跨域问题

    2024-07-11 02:24:02       5 阅读
  11. webpack 打包配置

    2024-07-11 02:24:02       4 阅读
  12. 人类历史时间轴

    2024-07-11 02:24:02       7 阅读