文章目录
前言
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 |
|
1 Copter.cpp
1.1 void Copter::setup()
此函数仅在启动时调用一次。用于初始化一些必要的任务。此函数由 HAL 中的 main() 函数调用。
void Copter::setup()
{// Load the default values of variables listed in var_info[]sAP_Param::setup_sketch_defaults();// setup storage layout for copterStorageManager::set_layout_copter();init_ardupilot();// initialise the main loop schedulerscheduler.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 sensorinit_optflow();...
}3 sensors.cpp
3.1 void Copter::init_optflow()
初始化光流传感器。
// initialise optical flow sensor
void Copter::init_optflow()
{
#if OPTFLOW == ENABLED// initialise optical flow sensoroptflow.init(MASK_LOG_OPTFLOW);
#endif // OPTFLOW == ENABLED
}
3.2 对象optflow说明
在 Copter.h 文件中，我们用 OpticalFlow 类定义了 optflow 对象。
// Optical flow sensor
#if OPTFLOW == ENABLEDOpticalFlow optflow;
#endif4 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 createdif ((_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_BEBOPbackend = new AP_OpticalFlow_Onboard(*this);
#endifbreak;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_UAVCANbackend = new AP_OpticalFlow_HereFlow(*this);
#endifbreak;case OpticalFlowType::SITL:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITLbackend = new AP_OpticalFlow_SITL(*this);
#endifbreak;}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 IDuint8_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 0static uint32_t last_print_ms;static int fd = -1;if (fd == -1) {fd = open("/dev/ttyACM0", O_WRONLY);}// used for debuggingstatic 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(®, 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 == ENABLEDSCHED_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);
}
