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 |
|
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;
#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 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(®, 1, nullptr, 0);
hal.scheduler->delay_microseconds(150);
for (uint8_t i=0; itransfer(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);
}