Ardupilot — AP_OpticalFlow代码梳理-阿里云开发者社区

Ardupilot — AP_OpticalFlow代码梳理

2024-05-17 21

版权

本文内容由阿里云实名注册用户自发贡献，版权归原作者所有，阿里云开发者社区不拥有其著作权，亦不承担相应法律责任。具体规则请查看《阿里云开发者社区用户服务协议》和《阿里云开发者社区知识产权保护指引》。如果您发现本社区中有涉嫌抄袭的内容，填写侵权投诉表单进行举报，一经查实，本社区将立刻删除涉嫌侵权内容。

简介： Ardupilot — AP_OpticalFlow代码梳理

前言

故事的开始，要从参数 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() 函数调用。

void Copter::setup()
{
    // Load the default values of variables listed in var_info[]s
    AP_Param::setup_sketch_defaults();
 
    // setup storage layout for copter
    StorageManager::set_layout_copter();
 
    init_ardupilot();
 
    // initialise the main loop scheduler
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}

2 system.cpp

2.1 void Copter::init_ardupilot()

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

void Copter::init_ardupilot()
{
    ...
 
    // init the optical flow sensor
    init_optflow();
 
    ...
}

3 sensors.cpp

3.1 void Copter::init_optflow()

初始化光流传感器。

// initialise optical flow sensor
void Copter::init_optflow()
{
#if OPTFLOW == ENABLED
    // initialise optical flow sensor
    optflow.init(MASK_LOG_OPTFLOW);
#endif      // OPTFLOW == ENABLED
}

3.2 对象optflow说明

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

    // Optical flow sensor
#if OPTFLOW == ENABLED
    OpticalFlow optflow;
#endif

4 OpticalFlow.cpp

4.1 void OpticalFlow::init(uint32_t log_bit)

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

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

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

void OpticalFlow::init(uint32_t log_bit)
{
     _log_bit = log_bit;
 
    // return immediately if not enabled or backend already created
    if ((_type == (int8_t)OpticalFlowType::NONE) || (backend != nullptr)) {
        return;
    }
 
    switch ((OpticalFlowType)_type.get()) {
    case OpticalFlowType::NONE:
        break;
    case OpticalFlowType::PX4FLOW:
        backend = AP_OpticalFlow_PX4Flow::detect(*this);
        break;
    case OpticalFlowType::PIXART:
        backend = AP_OpticalFlow_Pixart::detect("pixartflow", *this);
        if (backend == nullptr) {
            backend = AP_OpticalFlow_Pixart::detect("pixartPC15", *this);
        }
        break;
    case OpticalFlowType::BEBOP:
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
        backend = new AP_OpticalFlow_Onboard(*this);
#endif
        break;
    case OpticalFlowType::CXOF:
        backend = AP_OpticalFlow_CXOF::detect(*this);
        break;
    case OpticalFlowType::MAVLINK:
        backend = AP_OpticalFlow_MAV::detect(*this);
        break;
    case OpticalFlowType::UAVCAN:
#if HAL_WITH_UAVCAN
        backend = new AP_OpticalFlow_HereFlow(*this);
#endif
        break;
    case OpticalFlowType::SITL:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        backend = new AP_OpticalFlow_SITL(*this);
#endif
        break;
    }
 
    if (backend != nullptr) {
        backend->init();
    }
}

5 AP_OpticalFlow_Pixart.cpp

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

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

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

// detect the device
AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(const char *devname, OpticalFlow &_frontend)
{
    AP_OpticalFlow_Pixart *sensor = new AP_OpticalFlow_Pixart(devname, _frontend);
    if (!sensor) {
        return nullptr;
    }
    if (!sensor->setup_sensor()) {
        delete sensor;
        return nullptr;
    }
    return sensor;
}

5.2 bool AP_OpticalFlow_Pixart::setup_sensor(void)

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

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

// setup the device
bool AP_OpticalFlow_Pixart::setup_sensor(void)
{
    ...
    // check product ID
    uint8_t id1 = reg_read(PIXART_REG_PRODUCT_ID);
    uint8_t id2;
    if (id1 == 0x3f) {
        id2 = reg_read(PIXART_REG_INV_PROD_ID);
    } else {
        id2 = reg_read(PIXART_REG_INV_PROD_ID2);
    }
    debug("id1=0x%02x id2=0x%02x ~id1=0x%02x\n", id1, id2, uint8_t(~id1));
    if (id1 == 0x3F && id2 == uint8_t(~id1)) {
        model = PIXART_3900;
    } else if (id1 == 0x49 && id2 == uint8_t(~id1)) {
        model = PIXART_3901;
    } else {
        debug("Not a recognised device\n");
        return false;
    }
 
    if (model == PIXART_3900) {
        srom_download();
 
        id = reg_read(PIXART_REG_SROM_ID);
        if (id != srom_id) {
            debug("Pixart: bad SROM ID: 0x%02x\n", id);
            return false;
        }
 
        reg_write(PIXART_REG_SROM_EN, 0x15);
        hal.scheduler->delay(10);
 
        crc = reg_read16u(PIXART_REG_DOUT_L);
        if (crc != 0xBEEF) {
            debug("Pixart: bad SROM CRC: 0x%04x\n", crc);
            return false;
        }
    }
 
    if (model == PIXART_3900) {
        load_configuration(init_data_3900, ARRAY_SIZE(init_data_3900));
    } else {
        load_configuration(init_data_3901_1, ARRAY_SIZE(init_data_3901_1));
        hal.scheduler->delay(100);
        load_configuration(init_data_3901_2, ARRAY_SIZE(init_data_3901_2));
    }
 
    hal.scheduler->delay(50);
 
    debug("Pixart %s ready\n", model==PIXART_3900?"3900":"3901");
 
    integral.last_frame_us = AP_HAL::micros();
 
    _dev->register_periodic_callback(2000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Pixart::timer, void));
    return true;
}

5.3 void AP_OpticalFlow_Pixart::timer(void)

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

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

void AP_OpticalFlow_Pixart::timer(void)
{
    if (AP_HAL::micros() - last_burst_us < 500) {
        return;
    }
    motion_burst();
    last_burst_us = AP_HAL::micros();
 
    uint32_t dt_us = last_burst_us - integral.last_frame_us;
    float dt = dt_us * 1.0e-6;
    const Vector3f &gyro = AP::ahrs_navekf().get_gyro();
 
    {
        WITH_SEMAPHORE(_sem);
 
        integral.sum.x += burst.delta_x;
        integral.sum.y += burst.delta_y;
        integral.sum_us += dt_us;
        integral.last_frame_us = last_burst_us;
        integral.gyro += Vector2f(gyro.x, gyro.y) * dt;
    }
 
#if 0
    static uint32_t last_print_ms;
    static int fd = -1;
    if (fd == -1) {
        fd = open("/dev/ttyACM0", O_WRONLY);
    }
    // used for debugging
    static int32_t sum_x;
    static int32_t sum_y;
    sum_x += burst.delta_x;
    sum_y += burst.delta_y;
 
    uint32_t now = AP_HAL::millis();
    if (now - last_print_ms >= 100 && (sum_x != 0 || sum_y != 0)) {
        last_print_ms = now;
        dprintf(fd, "Motion: %d %d obs:0x%02x squal:%u rds:%u maxr:%u minr:%u sup:%u slow:%u\n",
               (int)sum_x, (int)sum_y, (unsigned)burst.squal, (unsigned)burst.rawdata_sum, (unsigned)burst.max_raw,
               (unsigned)burst.max_raw, (unsigned)burst.min_raw, (unsigned)burst.shutter_upper, (unsigned)burst.shutter_lower);
        sum_x = sum_y = 0;
    }
#endif
}

5.4 void AP_OpticalFlow_Pixart::motion_burst(void)

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

void AP_OpticalFlow_Pixart::motion_burst(void)
{
    uint8_t *b = (uint8_t *)&burst;
 
    burst.delta_x = 0;
    burst.delta_y = 0;
 
    _dev->set_chip_select(true);
    uint8_t reg = model==PIXART_3900?PIXART_REG_MOT_BURST:PIXART_REG_MOT_BURST2;
 
    _dev->transfer(&reg, 1, nullptr, 0);
    hal.scheduler->delay_microseconds(150);
 
    for (uint8_t i=0; i<sizeof(burst); i++) {
        _dev->transfer(nullptr, 0, &b[i], 1);
        if (i == 0 && (burst.motion & 0x80) == 0) {
            // no motion, save some bus bandwidth
            _dev->set_chip_select(false);
            return;
        }
    }
    _dev->set_chip_select(false);
}

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

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

const AP_Scheduler::Task Copter::scheduler_tasks[] = {
...
 
#if OPTFLOW == ENABLED
    SCHED_TASK_CLASS(OpticalFlow,          &copter.optflow,             update,         200, 160),
#endif
...
}

5.6 void AP_OpticalFlow_Pixart::update(void)

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

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

// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_Pixart::update(void)
{
    uint32_t now = AP_HAL::millis();
    if (now - last_update_ms < 100) {
        return;
    }
    last_update_ms = now;
 
    struct OpticalFlow::OpticalFlow_state state;
    state.surface_quality = burst.squal;
 
    if (integral.sum_us > 0) {
        WITH_SEMAPHORE(_sem);
 
        const Vector2f flowScaler = _flowScaler();
        float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
        float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
        float dt = integral.sum_us * 1.0e-6;
 
        state.flowRate = Vector2f(integral.sum.x * flowScaleFactorX,
                                  integral.sum.y * flowScaleFactorY);
        state.flowRate *= flow_pixel_scaling / dt;
 
        // we only apply yaw to flowRate as body rate comes from AHRS
        _applyYaw(state.flowRate);
 
        state.bodyRate = integral.gyro / dt;
 
        integral.sum.zero();
        integral.sum_us = 0;
        integral.gyro.zero();
    } else {
        state.flowRate.zero();
        state.bodyRate.zero();
    }
 
    // copy results to front end
    _update_frontend(state);
}

文章标签：

传感器