程序下载https://github.com/PX4/Flow
主程序在Flow/src/modules/flow/main.c
进入主程序int main(void)
首先__enable_irq();启用IRQ中断,此函数通过清除程序状态寄存器中的I位来启用中断请求。只能在特权模式下执行
然后是初始化以及加载一些参数
global_data_reset_param_defaults();
global_data_reset();
...
board_led_rgb( 0, 0,255, 4);
/*使能FPU浮点运算*/
SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2));
//设置CP10完全访问和设置CP11完全访问
if (SysTick_Config(SystemCoreClock / 100000))
/*将定时器设置为每10微秒触发一次中断*/
{
/* capture clock error 捕获时钟错误*/
LEDOn(LED_ERR);
while (1);
}
/* 初始化 */
USBD_Init( &USB_OTG_dev,
USB_OTG_FS_CORE_ID,
...
&USR_cb);
/*初始化MAVLink*/
communication_init();
enable_image_capture();//启用图像捕获
/* 配置陀螺仪*/
gyro_config();
/*初始化和清除快速图像缓冲区 */
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
...
}
...初始化其他配置
while (1)从这里进入程序主循环,会去调用其他函数
PROBE_1(false);
uavcan_run();//UAVCAN是一种轻量级协议,还不知道这里做了什么
PROBE_1(true);
probe应该是驱动吧,个人猜测这里是发布或者接受了消息
然后判断buffer_reset_needed= =1 必要时重置光流缓冲器
if(buffer_reset_needed)
{
buffer_reset_needed = 0;
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
delay(500);
continue;
}
查代码发现是在communication.c 中handle_mavlink_message()有四种情况会重置光流缓存器
1. i == PARAM_SENSOR_POSITION)//当手动控制位置
2. i == PARAM_IMAGE_LOW_LIGHT || i == PARAM_IMAGE_ROW_NOISE_CORR|| i == PARAM_IMAGE_TEST_PATTERN)
//处理低光模式和噪声校正
3. i == PARAM_VIDEO_ONLY//手工校准
4 i == PARAM_SHTR_W_1 || i == PARAM_SHTR_W_2 || i == PARAM_SHTR_W_TOT || i == PARAM_EXPOSURE_MAX || i == PARAM_HDR || i == PARAM_AEC || i == PARAM_AGC || i == PARAM_BRIGHT || i == PARAM_GAIN_MAX
//快门/曝光参数到这些值时
接下来是校准,直到灯闪成功
if(FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY]))
{
while(FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY]))
{
dcmi_restart_calibration_routine();//校准图像采集程序重启
...
communication_parameter_send();//发送一条低优先级参数消息
LEDToggle(LED_COM);//切换所选LED。
}
dcmi_restart_calibration_routine();//校准图像采集程序重启
LEDOff(LED_COM);
}
从参数PARAM_IMAGE_WIDTH和PARAM_IMAGE_HEIGHT设定图像大小 image_size
然后读取当前的陀螺仪数据gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor,&gyro_temp);
在进行转换
计算焦距
focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f;
//原始焦距:12毫米像素大小:6毫米,已启用4像素合并
获取声纳数据distance_validsonar_read(&sonar_distance_filtered, &sonar_distance_raw)当测量距离大于阈值时置零
当位置到最底了,计算光流if (FLOAT_EQ_INT(global_data.param[PARAM_SENSOR_POSITION], BOTTOM))
将最近的图像复制到更快的RAM dma_copy_image_buffers
计算光流 qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);
compute_flow函数在flow.c文件中其作用是 计算从Image1到Image2的像素流
*从旧图像(image1)中搜索新图像(image2)中最多64个像素的相应位置,并计算所有偏移的平均值。其算法就是整个程序的核心了,可以参考这篇文章https://blog.csdn.net/qq_42237381/article/details/88821175
接下来就是超声波结合得到最终地面的速度
float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f);
float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f);
实点P(X,Y,Z),图像平面投影P(X,Y,Z),焦距F,到场景Z的距离x / f = X / Z