px4Flow程序main.c解读

程序下载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