ArduPilot开源飞控之AP_OpticalFlow

1. 源由

光流传感器是使用地面纹理和可见特征来确定飞机地面速度的相机模块。

主要应用于室内定位，这里将光流传感应用的代码框架做一个整理。

2. 框架设计

2.1 启动代码

init_ardupilot
 └──> AP_OpticalFlow::init

2.2 任务代码 update

SCHED_TASK_CLASS(AP_OpticalFlow,          &copter.optflow,             update,         200, 160,  12),
 └──> AP_OpticalFlow::update

2.3 任务代码 handle_msg

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102)
 └──> GCS::update_receive
     └──> GCS_MAVLINK::update_receive
         └──> GCS_MAVLINK_Copter::packetReceived
             └──> GCS_MAVLINK_Copter::handleMessage
                 └──> GCS_MAVLINK::handle_common_message
                     └──> GCS_MAVLINK::handle_optical_flow
                         └──> AP_OpticalFlow::handle_msg

2.4 任务代码 handle_msp

AP_Vehicle::setup
 └──> AP_MSP::init
     └──> AP_MSP::loop  //thread_create
         └──> AP_MSP_Telem_Backend::process_incoming_data
             └──> AP_MSP_Telem_Backend::msp_process_received_command
                 └──> AP_MSP_Telem_Backend::msp_process_command
                     └──> AP_MSP_Telem_Backend::msp_process_sensor_command
                         └──> AP_MSP_Telem_Backend::msp_handle_opflow
                             └──> AP_OpticalFlow::handle_msp

2.5 任务代码 do_aux_function

Copter::init_ardupilot
 └──> RC_Channels::init
     └──> RC_Channels::init_aux_all
         └──> RC_Channel::init_aux
             └──> RC_Channel_Copter::init_aux_function
                 └──> RC_Channel::run_aux_function
                     └──> RC_Channel_Copter::do_aux_function
                         └──> RC_Channel::do_aux_function
                             ├──> AP_OpticalFlow::start_calibration
                             └──> AP_OpticalFlow::stop_calibration

3. 重要例程

3.1 AP_OpticalFlow

根据变量表进行初始化赋值。

AP_OpticalFlow::AP_OpticalFlow
 ├──> _singleton = this
 └──> AP_Param::setup_object_defaults(this, var_info)

3.2 init

支持以下协议的光流传感器：

1. AP_OpticalFlow_PX4Flow
2. AP_OpticalFlow_Pixart
3. AP_OpticalFlow_Onboard
4. AP_OpticalFlow_CXOF
5. AP_OpticalFlow_MAV
6. AP_OpticalFlow_HereFlow
7. AP_OpticalFlow_MSP
8. AP_OpticalFlow_UPFLOW
9. AP_OpticalFlow_SITL

AP_OpticalFlow::init
 ├──> _log_bit = log_bit
 │
 │  /********************************************************************************
 │   * Pre-check                                                                    *
 │   ********************************************************************************/
 │   // return immediately if not enabled or backend already created
 ├──> <(_type == Type::NONE) || (backend != nullptr)>
 │   └──> return
 │
 │  /********************************************************************************
 │   * Sensor detect or configure                                                   *
 │   ********************************************************************************/
 ├──> <case Type::PX4FLOW> <AP_OPTICALFLOW_PX4FLOW_ENABLED>
 │   └──> backend = AP_OpticalFlow_PX4Flow::detect(*this)
 ├──> <case Type::PIXART> <AP_OPTICALFLOW_PIXART_ENABLED>
 │   ├──> backend = AP_OpticalFlow_Pixart::detect("pixartflow", *this)
 │   └──> <backend == nullptr>
 │       └──> backend = AP_OpticalFlow_Pixart::detect("pixartPC15", *this)
 ├──> <case Type::BEBOP> <CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP>
 │   └──> backend = new AP_OpticalFlow_Onboard(*this)
 ├──> <case Type::CXOF> <AP_OPTICALFLOW_CXOF_ENABLED>
 │   └──> backend = AP_OpticalFlow_CXOF::detect(*this)
 ├──> <case Type::MAVLINK> <AP_OPTICALFLOW_MAV_ENABLED>
 │   └──> backend = AP_OpticalFlow_MAV::detect(*this)
 ├──> <case Type::UAVCAN> <AP_OPTICALFLOW_HEREFLOW_ENABLED>
 │   └──> backend = new AP_OpticalFlow_HereFlow(*this)
 ├──> <case Type::MSP> <HAL_MSP_OPTICALFLOW_ENABLED>
 │   └──> backend = AP_OpticalFlow_MSP::detect(*this)
 ├──> <case Type::UPFLOW> <AP_OPTICALFLOW_UPFLOW_ENABLED>
 │   └──> backend = AP_OpticalFlow_UPFLOW::detect(*this)
 ├──> <case Type::SITL> <AP_OPTICALFLOW_SITL_ENABLED>
 │   └──> backend = new AP_OpticalFlow_SITL(*this)
 │
 │  /********************************************************************************
 │   * Driver init                                                                  *
 │   ********************************************************************************/
 └──> <backend != nullptr>
     └──> backend->init()

3.3 update

光流数据更新及校准。

AP_OpticalFlow::update
 │
 │  /********************************************************************************
 │   * Pre-check, exit immediately if not enabled                                   *
 │   ********************************************************************************/
 ├──> <!enabled()>
 │   └──> return
 │
 │  /********************************************************************************
 │   * Driver update                                                                *
 │   ********************************************************************************/
 ├──> <backend != nullptr>
 │   └──> backend->update()
 │
 │   // only healthy if the data is less than 0.5s old
 ├──> _flags.healthy = (AP_HAL::millis() - _last_update_ms < 500)
 │
 │  /********************************************************************************
 │   * Driver calibration                                                           *
 │   ********************************************************************************/
 └──> <AP_OPTICALFLOW_CALIBRATOR_ENABLED>
     │   // update calibrator and save resulting scaling
     └──> <_calibrator != nullptr> <calibrator->update()>
         │   // apply new calibration values
         ├──> const Vector2f new_scaling = _calibrator->get_scalars()
         ├──> const float flow_scalerx_as_multiplier = (1.0 + (_flowScalerX * 0.001)) * new_scaling.x
         ├──> const float flow_scalery_as_multiplier = (1.0 + (_flowScalerY * 0.001)) * new_scaling.y
         ├──> _flowScalerX.set_and_save_ifchanged((flow_scalerx_as_multiplier - 1.0) * 1000.0)
         ├──> _flowScalerY.set_and_save_ifchanged((flow_scalery_as_multiplier - 1.0) * 1000.0)
         ├──> _flowScalerX.notify()
         ├──> _flowScalerY.notify()
         └──> GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FlowCal: FLOW_FXSCALER=%d, FLOW_FYSCALER=%d", (int)_flowScalerX, (int)_flowScalerY)

3.4 handle_msg

处理MAVLink消息。

AP_OpticalFlow::handle_msg
 │
 │  /********************************************************************************
 │   * Pre-check, exit immediately if not enabled                                   *
 │   ********************************************************************************/
 ├──> <!enabled()>
 │   └──> return
 │
 │  /********************************************************************************
 │   * Handle mavlink message                                                       *
 │   ********************************************************************************/
 └──> <backend != nullptr>
     └──> backend->handle_msg(msg)

3.5 handle_msp

处理MSP消息。

AP_OpticalFlow::handle_msp
 │
 │  /********************************************************************************
 │   * Pre-check, exit immediately if not enabled                                   *
 │   ********************************************************************************/
 ├──> <!enabled()>
 │   └──> return
 │
 │  /********************************************************************************
 │   * Handle msp message                                                           *
 │   ********************************************************************************/
 └──> <backend != nullptr>
     └──> backend->handle_msp(pkt)

3.6 start_calibration

开始校准。

void AP_OpticalFlow::start_calibration
 ├──> <_calibrator == nullptr>
 │   ├──> _calibrator = new AP_OpticalFlow_Calibrator()
 │   └──> <_calibrator == nullptr>
 │       ├──> GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "FlowCal: failed to start")
 │       └──> return
 └──> <_calibrator != nullptr>
     └──> _calibrator->start()

3.7 stop_calibration

停止校准。

AP_OpticalFlow::stop_calibration
 └──> <_calibrator != nullptr>
     └──> _calibrator->stop()

4. 总结

支持9种光流传感器：

enum class Type {
    NONE = 0,
    PX4FLOW = 1,
    PIXART = 2,
    BEBOP = 3,
    CXOF = 4,
    MAVLINK = 5,
    UAVCAN = 6,
    MSP = 7,
    UPFLOW = 8,
    SITL = 10,
};

具体实现，详见以下对应驱动类：

1. AP_OpticalFlow_PX4Flow
2. AP_OpticalFlow_Pixart
3. AP_OpticalFlow_Onboard
4. AP_OpticalFlow_CXOF
5. AP_OpticalFlow_MAV
6. AP_OpticalFlow_HereFlow
7. AP_OpticalFlow_MSP
8. AP_OpticalFlow_UPFLOW
9. AP_OpticalFlow_SITL

5. 参考资料

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