ArduPilot开源代码之AP_OpticalFlow_MSP

1. 源由

在《ArduPilot开源代码之OpticalFlow_backend》研读了抽象出来的通用代码部分，本文主要针对AP_OpticalFlow_MSP设备进行研读。

2. Library设计

设备相关的重要函数：

• init
• update
• handle_msp
• detect

class AP_OpticalFlow_MSP : public OpticalFlow_backend
{
public:
    /// 构造函数
    using OpticalFlow_backend::OpticalFlow_backend;

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

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

    // 从 MSP 获取更新
    void handle_msp(const MSP::msp_opflow_data_message_t &pkt) override;

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

private:

    uint64_t prev_frame_us;             // 上次调用 update 时消息的系统时间
    uint64_t latest_frame_us;           // 最近处理消息的系统时间
    Vector2l flow_sum;                  // 自上次调用 update 以来传感器的 flow_x 和 flow_y 值的总和
    uint16_t quality_sum;               // 自上次调用 update 以来传感器质量值的总和
    uint16_t count;                     // 自上次调用 update 以来的传感器读数次数
    Vector2f gyro_sum;                  // 自上次从光流传感器读取数据以来陀螺仪传感器值的总和
    uint16_t gyro_sum_count;            // 总和中的陀螺仪传感器值的次数
};

3. 重要例程

3.1 AP_OpticalFlow_MSP::init

该函数无具体的初始化内容。

    // initialise the sensor
    void init() override {}

3.2 AP_OpticalFlow_MSP::update

AP_OpticalFlow_MSP::update()
|
├── if (gyro_sum_count < 1000)       // 如果陀螺仪计数小于1000
|   ├── const Vector3f& gyro = AP::ahrs().get_gyro()       // 获取陀螺仪数据
|   ├── gyro_sum.x += gyro.x        // 累加陀螺仪 x 轴数据
|   ├── gyro_sum.y += gyro.y        // 累加陀螺仪 y 轴数据
|   └── gyro_sum_count++            // 增加陀螺仪数据计数
|
├── if (count == 0)                 // 如果没有读数
|   └── return                      // 直接返回，不更新状态
|
├── struct AP_OpticalFlow::OpticalFlow_state state {}    // 定义并初始化状态结构体
|
├── state.surface_quality = quality_sum / count           // 计算并记录表面质量
|
├── const float dt = (latest_frame_us - prev_frame_us) * 1.0e-6  // 计算时间间隔 dt
├── prev_frame_us = latest_frame_us                      // 更新上次帧的时间
|
├── if (is_positive(dt) && (dt < OPTFLOW_MSP_TIMEOUT_SEC))    // 检查时间间隔 dt 是否有效
|   ├── const float flow_scale_factor_x = 1.0f + 0.001f * _flowScaler().x     // 计算 x 轴流动缩放因子
|   ├── const float flow_scale_factor_y = 1.0f + 0.001f * _flowScaler().y     // 计算 y 轴流动缩放因子
|
|   ├── state.flowRate = { ((float)flow_sum.x / count) * flow_scale_factor_x * dt,  // 计算并记录 x 轴流动速率
|   |                        ((float)flow_sum.y / count) * flow_scale_factor_y * dt} // 计算并记录 y 轴流动速率
|
|   ├── state.bodyRate = { gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count }  // 计算并记录平均机体速率
|
|   ├── state.flowRate *= -1                            // 反转流动向量以对齐传感器方向
|
|   └── _applyYaw(state.flowRate)                       // 应用偏航角到流动速率
|
└── else                                                 // 如果时间间隔 dt 无效
    ├── state.flowRate.zero()                           // 将流动速率清零
    └── state.bodyRate.zero()                           // 将机体速率清零
|
├── _update_frontend(state)                             // 更新前端状态
|
├── flow_sum.zero()                                     // 重置本地缓冲区
├── quality_sum = 0
├── count = 0
|
└── gyro_sum.zero()                                     // 重置陀螺仪累加器
    └── gyro_sum_count = 0

3.3 AP_OpticalFlow_MSP::handle_msp

记录漂移x,y方向的总和，update函数定期处理。

// 处理 OPTICAL_FLOW 的 MSP 消息
void AP_OpticalFlow_MSP::handle_msp(const MSP::msp_opflow_data_message_t &pkt)
{
    // 记录消息接收的时间
    // 待办事项: 添加抖动校正
    latest_frame_us = AP_HAL::micros64();

    // 将传感器值累加到总和中
    flow_sum.x += pkt.motion_x;
    flow_sum.y += pkt.motion_y;
    quality_sum += (int)pkt.quality * 100 / 255;
    count++;
}

3.4 AP_OpticalFlow_MSP::detect

初始化AP_OpticalFlow_MSP类，并传入前台参数_frontend。详见：《ArduPilot开源飞控之AP_OpticalFlow》。

// detect the device
AP_OpticalFlow_MSP *AP_OpticalFlow_MSP::detect(AP_OpticalFlow &_frontend)
{
    // we assume msp messages will be sent into this driver
    return new AP_OpticalFlow_MSP(_frontend);
}

4. 总结

之前在研读《ArduPilot开源飞控之AP_OpticalFlow》出现了明显的错误，在进一步研读过程，发现并纠正错误。

随着Ardupilot研读的深入，逐步的发现其C++继承应用的方法，代码走读也就越来越容易。继续，保持，不断学习！！！

5. 参考资料

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