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