• Ardupilot — AP_OpticalFlow代码梳理

    文章目录

    前言

    1 Copter.cpp

    1.1 void Copter::setup()

    2 system.cpp

    2.1 void Copter::init_ardupilot()

    3 sensors.cpp

    3.1 void Copter::init_optflow()

    3.2 对象optflow说明

    4 OpticalFlow.cpp

    4.1 void OpticalFlow::init(uint32_t log_bit)

    5 AP_OpticalFlow_Pixart.cpp

    5.1 AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(...)

    5.2 bool AP_OpticalFlow_Pixart::setup_sensor(void)

    5.3 void AP_OpticalFlow_Pixart::timer(void)

    5.4 void AP_OpticalFlow_Pixart::motion_burst(void)

    5.5 SCHED_TASK_CLASS(OpticalFlow,          &copter.optflow,             update,         200, 160),

    5.6 void AP_OpticalFlow_Pixart::update(void)

    前言

    故事的开始，要从参数 FLOW_TYPE 说起。

    FLOW_TYPE：光学流量传感器类型

    RebootRequired

    Values

    True

    Value

    Meaning

    0

    None

    1

    PX4Flow

    2

    Pixart

    3

    Bebop

    4

    CXOF

    5

    MAVLink

    6

    UAVCAN

    1 Copter.cpp

    1.1 void Copter::setup()

    此函数仅在启动时调用一次。用于初始化一些必要的任务。此函数由 HAL 中的 main() 函数调用。

    1. void Copter::setup()
    2. {
    3.     // Load the default values of variables listed in var_info[]s
    4.     AP_Param::setup_sketch_defaults();
    5.  
    6.     // setup storage layout for copter
    7.     StorageManager::set_layout_copter();
    8.  
    9.     init_ardupilot();
    10.  
    11.     // initialise the main loop scheduler
    12.     scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
    13. }

    2 system.cpp

    2.1 void Copter::init_ardupilot()

    init_ardupilot() 函数将处理空中重启所需的一切。稍后将确定飞行器是否真的在地面上，并在这种情况下处理地面启动。

    1. void Copter::init_ardupilot()
    2. {
    3.     ...
    4.  
    5.     // init the optical flow sensor
    6.     init_optflow();
    7.  
    8.     ...
    9. }

    3 sensors.cpp

    3.1 void Copter::init_optflow()

    初始化光流传感器。

    1. // initialise optical flow sensor
    2. void Copter::init_optflow()
    3. {
    4. #if OPTFLOW == ENABLED
    5.     // initialise optical flow sensor
    6.     optflow.init(MASK_LOG_OPTFLOW);
    7. #endif      // OPTFLOW == ENABLED
    8. }

    3.2 对象optflow说明

    在 Copter.h 文件中，我们用 OpticalFlow 类定义了 optflow 对象。

    1.     // Optical flow sensor
    2. #if OPTFLOW == ENABLED
    3.     OpticalFlow optflow;
    4. #endif

    4 OpticalFlow.cpp

    4.1 void OpticalFlow::init(uint32_t log_bit)

    所以，我们在跳转 init() 这个成员函数的时候，跳转到 OpticalFlow 类的 init() 函数。

    根据参数 FLOW_TYPE，选择不同的光流传感器进行检测。

    本例以 Pixart（2）作为示例。

    1. void OpticalFlow::init(uint32_t log_bit)
    2. {
    3.      _log_bit = log_bit;
    4.  
    5.     // return immediately if not enabled or backend already created
    6.     if ((_type == (int8_t)OpticalFlowType::NONE) || (backend != nullptr)) {
    7.         return;
    8.     }
    9.  
    10.     switch ((OpticalFlowType)_type.get()) {
    11.     case OpticalFlowType::NONE:
    12.         break;
    13.     case OpticalFlowType::PX4FLOW:
    14.         backend = AP_OpticalFlow_PX4Flow::detect(*this);
    15.         break;
    16.     case OpticalFlowType::PIXART:
    17.         backend = AP_OpticalFlow_Pixart::detect("pixartflow", *this);
    18.         if (backend == nullptr) {
    19.             backend = AP_OpticalFlow_Pixart::detect("pixartPC15", *this);
    20.         }
    21.         break;
    22.     case OpticalFlowType::BEBOP:
    23. #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
    24.         backend = new AP_OpticalFlow_Onboard(*this);
    25. #endif
    26.         break;
    27.     case OpticalFlowType::CXOF:
    28.         backend = AP_OpticalFlow_CXOF::detect(*this);
    29.         break;
    30.     case OpticalFlowType::MAVLINK:
    31.         backend = AP_OpticalFlow_MAV::detect(*this);
    32.         break;
    33.     case OpticalFlowType::UAVCAN:
    34. #if HAL_WITH_UAVCAN
    35.         backend = new AP_OpticalFlow_HereFlow(*this);
    36. #endif
    37.         break;
    38.     case OpticalFlowType::SITL:
    39. #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    40.         backend = new AP_OpticalFlow_SITL(*this);
    41. #endif
    42.         break;
    43.     }
    44.  
    45.     if (backend != nullptr) {
    46.         backend->init();
    47.     }
    48. }

    5 AP_OpticalFlow_Pixart.cpp

    5.1 AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(...)

    首先会根据传入的参数，new 一个 AP_OpticalFlow_Pixart 类对象。

    然后，检测光流传感器的产品 ID，再配置传感器（sensor->setup_sensor()）。

    1. // detect the device
    2. AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(const char *devname, OpticalFlow &_frontend)
    3. {
    4.     AP_OpticalFlow_Pixart *sensor = new AP_OpticalFlow_Pixart(devname, _frontend);
    5.     if (!sensor) {
    6.         return nullptr;
    7.     }
    8.     if (!sensor->setup_sensor()) {
    9.         delete sensor;
    10.         return nullptr;
    11.     }
    12.     return sensor;
    13. }

    5.2 bool AP_OpticalFlow_Pixart::setup_sensor(void)

    先读取光流传感器的产品 ID，识别光流传感器类型。再根据类型的不同，写入不同的参数配置光流传感器。

    最后，注册光流传感器的周期运行函数 timer()。

    1. // setup the device
    2. bool AP_OpticalFlow_Pixart::setup_sensor(void)
    3. {
    4.     ...
    5.     // check product ID
    6.     uint8_t id1 = reg_read(PIXART_REG_PRODUCT_ID);
    7.     uint8_t id2;
    8.     if (id1 == 0x3f) {
    9.         id2 = reg_read(PIXART_REG_INV_PROD_ID);
    10.     } else {
    11.         id2 = reg_read(PIXART_REG_INV_PROD_ID2);
    12.     }
    13.     debug("id1=0x%02x id2=0x%02x ~id1=0x%02x\n", id1, id2, uint8_t(~id1));
    14.     if (id1 == 0x3F && id2 == uint8_t(~id1)) {
    15.         model = PIXART_3900;
    16.     } else if (id1 == 0x49 && id2 == uint8_t(~id1)) {
    17.         model = PIXART_3901;
    18.     } else {
    19.         debug("Not a recognised device\n");
    20.         return false;
    21.     }
    22.  
    23.     if (model == PIXART_3900) {
    24.         srom_download();
    25.  
    26.         id = reg_read(PIXART_REG_SROM_ID);
    27.         if (id != srom_id) {
    28.             debug("Pixart: bad SROM ID: 0x%02x\n", id);
    29.             return false;
    30.         }
    31.  
    32.         reg_write(PIXART_REG_SROM_EN, 0x15);
    33.         hal.scheduler->delay(10);
    34.  
    35.         crc = reg_read16u(PIXART_REG_DOUT_L);
    36.         if (crc != 0xBEEF) {
    37.             debug("Pixart: bad SROM CRC: 0x%04x\n", crc);
    38.             return false;
    39.         }
    40.     }
    41.  
    42.     if (model == PIXART_3900) {
    43.         load_configuration(init_data_3900, ARRAY_SIZE(init_data_3900));
    44.     } else {
    45.         load_configuration(init_data_3901_1, ARRAY_SIZE(init_data_3901_1));
    46.         hal.scheduler->delay(100);
    47.         load_configuration(init_data_3901_2, ARRAY_SIZE(init_data_3901_2));
    48.     }
    49.  
    50.     hal.scheduler->delay(50);
    51.  
    52.     debug("Pixart %s ready\n", model==PIXART_3900?"3900":"3901");
    53.  
    54.     integral.last_frame_us = AP_HAL::micros();
    55.  
    56.     _dev->register_periodic_callback(2000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Pixart::timer, void));
    57.     return true;
    58. }

    5.3 void AP_OpticalFlow_Pixart::timer(void)

    每 2ms 调用一次 timer() 函数，读取光流传感器的测量值。

    可以将 #if 0 #endif 屏蔽，打开调试信息。

    1. void AP_OpticalFlow_Pixart::timer(void)
    2. {
    3.     if (AP_HAL::micros() - last_burst_us < 500) {
    4.         return;
    5.     }
    6.     motion_burst();
    7.     last_burst_us = AP_HAL::micros();
    8.  
    9.     uint32_t dt_us = last_burst_us - integral.last_frame_us;
    10.     float dt = dt_us * 1.0e-6;
    11.     const Vector3f &gyro = AP::ahrs_navekf().get_gyro();
    12.  
    13.     {
    14.         WITH_SEMAPHORE(_sem);
    15.  
    16.         integral.sum.x += burst.delta_x;
    17.         integral.sum.y += burst.delta_y;
    18.         integral.sum_us += dt_us;
    19.         integral.last_frame_us = last_burst_us;
    20.         integral.gyro += Vector2f(gyro.x, gyro.y) * dt;
    21.     }
    22.  
    23. #if 0
    24.     static uint32_t last_print_ms;
    25.     static int fd = -1;
    26.     if (fd == -1) {
    27.         fd = open("/dev/ttyACM0", O_WRONLY);
    28.     }
    29.     // used for debugging
    30.     static int32_t sum_x;
    31.     static int32_t sum_y;
    32.     sum_x += burst.delta_x;
    33.     sum_y += burst.delta_y;
    34.  
    35.     uint32_t now = AP_HAL::millis();
    36.     if (now - last_print_ms >= 100 && (sum_x != 0 || sum_y != 0)) {
    37.         last_print_ms = now;
    38.         dprintf(fd, "Motion: %d %d obs:0x%02x squal:%u rds:%u maxr:%u minr:%u sup:%u slow:%u\n",
    39.                (int)sum_x, (int)sum_y, (unsigned)burst.squal, (unsigned)burst.rawdata_sum, (unsigned)burst.max_raw,
    40.                (unsigned)burst.max_raw, (unsigned)burst.min_raw, (unsigned)burst.shutter_upper, (unsigned)burst.shutter_lower);
    41.         sum_x = sum_y = 0;
    42.     }
    43. #endif
    44. }

    5.4 void AP_OpticalFlow_Pixart::motion_burst(void)

    通过 SPI 读取光流传感器运动值。

    1. void AP_OpticalFlow_Pixart::motion_burst(void)
    2. {
    3.     uint8_t *b = (uint8_t *)&burst;
    4.  
    5.     burst.delta_x = 0;
    6.     burst.delta_y = 0;
    7.  
    8.     _dev->set_chip_select(true);
    9.     uint8_t reg = model==PIXART_3900?PIXART_REG_MOT_BURST:PIXART_REG_MOT_BURST2;
    10.  
    11.     _dev->transfer(®, 1, nullptr, 0);
    12.     hal.scheduler->delay_microseconds(150);
    13.  
    14.     for (uint8_t i=0; i<sizeof(burst); i++) {
    15.         _dev->transfer(nullptr, 0, &b[i], 1);
    16.         if (i == 0 && (burst.motion & 0x80) == 0) {
    17.             // no motion, save some bus bandwidth
    18.             _dev->set_chip_select(false);
    19.             return;
    20.         }
    21.     }
    22.     _dev->set_chip_select(false);
    23. }

    5.5 SCHED_TASK_CLASS(OpticalFlow,          &copter.optflow,             update,         200, 160),

    在 Copter.cpp 文件的周期任务列表中，注册了调用频率为 200Hz 的光流传感器 update() 函数。

    1. const AP_Scheduler::Task Copter::scheduler_tasks[] = {
    2. ...
    3.  
    4. #if OPTFLOW == ENABLED
    5.     SCHED_TASK_CLASS(OpticalFlow,          &copter.optflow,             update,         200, 160),
    6. #endif
    7. ...
    8. }

    5.6 void AP_OpticalFlow_Pixart::update(void)

    根据 5.1 中的返回的 AP_OpticalFlow_Pixart 对象，调用 AP_OpticalFlow_Pixart 类中的 update() 函数。

    更新 - 从传感器读取最新数值，并填入 x、y 和总数。

    1. // update - read latest values from sensor and fill in x,y and totals.
    2. void AP_OpticalFlow_Pixart::update(void)
    3. {
    4.     uint32_t now = AP_HAL::millis();
    5.     if (now - last_update_ms < 100) {
    6.         return;
    7.     }
    8.     last_update_ms = now;
    9.  
    10.     struct OpticalFlow::OpticalFlow_state state;
    11.     state.surface_quality = burst.squal;
    12.  
    13.     if (integral.sum_us > 0) {
    14.         WITH_SEMAPHORE(_sem);
    15.  
    16.         const Vector2f flowScaler = _flowScaler();
    17.         float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
    18.         float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
    19.         float dt = integral.sum_us * 1.0e-6;
    20.  
    21.         state.flowRate = Vector2f(integral.sum.x * flowScaleFactorX,
    22.                                   integral.sum.y * flowScaleFactorY);
    23.         state.flowRate *= flow_pixel_scaling / dt;
    24.  
    25.         // we only apply yaw to flowRate as body rate comes from AHRS
    26.         _applyYaw(state.flowRate);
    27.  
    28.         state.bodyRate = integral.gyro / dt;
    29.  
    30.         integral.sum.zero();
    31.         integral.sum_us = 0;
    32.         integral.gyro.zero();
    33.     } else {
    34.         state.flowRate.zero();
    35.         state.bodyRate.zero();
    36.     }
    37.  
    38.     // copy results to front end
    39.     _update_frontend(state);
    40. }
  • 相关阅读:
    ES6（ECMASript 6 新特性---数值扩展，对象扩展，模块化）
    Simulink与Arduino烧录配置
    鞋业的数字化转型：3D建模与3D打印
    2020年9月大学英语六级作文
    strcmp函数详解：字符串比较的利器
    数据结构和算法——图结构
    1.MidBook项目经验之MybatisPlus
    软件测试 - 基础（软件测试的生命周期、测试报告、bug的级别、与开发人员产生争执的调解方式）
    BUUCTF 不一样的flag 1
    【修复】centos定时任务python top不能输出
  • 原文地址：https://blog.csdn.net/qq_20016593/article/details/132723033