PX4FLOW光流模块DIY（含部分代码讲解）

暑假有时间整理一下以前做的东西，发发博客，既给网友们学习也方便自己交流。今天讲讲我两年前从github学习的PX4FLOW光流模块。
光流是视觉导航的重要部分。在运动检测和许多slam技术都使用到了光流，但是大部分还是处于理论阶段，在实际运用到的地方不多，我所知道的一个是电脑的鼠标使用到了光流，还有一个就是在飞行器的室内导航(因为室内基本接收不到GPS导航信号况且精度不够)。想知道光流原理的可以去百度必应找找，如果看不了那里的讲解可以试着理解我下面这段话，如果都理解不了劝你不要继续往下看。
我说说光流的基本原理：在连续捕获的图片流数据中找到当前帧图片中的具备某些特征的点，称其为特征点（特征点是比较好标定的一堆像素块：可以是3X3,4X4,8X8,9X9的像素块，一般都是取角点），标定其所在当前帧的像素的坐标（x1,y1），然后在下一帧图片中找到同一个特征点，记录像素坐标(x2,y2)，两帧图片之间的时间间隔是Δt。那么在这段时间间隔内的速度就是：x方向上：（x2-x1）/Δt，y方向上：（y2-y1）/Δt。
下面讲解PX4FLOW：
既然是制作实物首先还是需要制作一个实验用的硬件平台，如下是其使用到的一些芯片
主控MCU:STM32F407
传感器有摄像头：MT9V034,陀螺仪：L3G4200D,sonar声呐模块。
为了试验需要我使用了LCD显示图片，个人只想做视觉部分所以把陀螺仪和声呐部分都去除了下面是我设计的原理图

设计并制作好的PCB电路板：

下面开始编写代码，我只用了PX4FLOW的光流算法的代码，其余的摄像头驱动LCD驱动都是我自己参考各种手册写的。
Systick_Init();

Delay_us(1000);

USART1_Config(57600);
printf("USART1 Configure OK!\n");

Delay_us(50);

/* enable FPU on Cortex-M4F core */
SCB->CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2));


LCD_Init();

LED_Init();

MT9V034_Init();

DCMI_Configure();

DCMI_Start();
主函数中主要的初始化代码。
LCD和LED初始化我就不在这里累述了。野火原子都已经讲得很详细了。这里我将一下DCMI这个驱动，这是stm32f407自带的一个捕获并行像素数据的外设端口。同样我们使用库函数的话只用对结构体进行填充。下面是我的代码。

void DCMI_Configure(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
DCMI_InitTypeDef DCMI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;

//step 1 :Enable the clock for the DCMI and associated GPIOs

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC|RCC_AHB1Periph_GPIOE, ENABLE);
RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_DCMI,ENABLE);

//step 2 :DCMI pins configuration
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4|GPIO_Pin_6;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOA,&GPIO_InitStructure);

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7|GPIO_Pin_6|GPIO_Pin_8|GPIO_Pin_9;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOB,&GPIO_InitStructure);

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8/*|GPIO_Pin_9*/|GPIO_Pin_11;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOC,&GPIO_InitStructure);

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOE,&GPIO_InitStructure);

GPIO_PinA