px4flow源码分析

Flow.c

计算光流用的是SAD块匹配算法

 

第一部分是生成直方图，第二部分是根据直方图来进行位移向量的计算。

 

外部的j、i的for循环是采样点的循环，内部的jj、ii的循环是对于一个小邻域的采样。采样点是从image1找的，之后根据采样点的坐标在image2的winmin到winmax的一个小矩形中找一个最相似的点。

 

PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD变量用来判断对于特征点采样的质量要求，越大越严苛。表示是否当前这个点可以作为计算光流的点。（compute_diff函数计算了梯度）

 

如果符合要求，则根据该采样点的坐标，在第二张图的一个区域内，计算sad(sum of absolute difference)也就是对于两张图的差异的绝对值的和进行计算（两个8*8的块进行匹配）。值越小说明越相似，

 

PARAM_BOTTOM_FLOW_VALUE_THRESHOLD则是对于相似度的一个阈值判断，如果不满足这个条件，说明两张图片几乎找不到对应点，那么就没戏了，接着看下一个采样点。

 

如果很幸运，image1中找到了可以用的采样点，而且在image2中也找到了对应的点，那么可以计算x和y方向的差值，或者说是一个移动向量，分别计算x和y两个方向的移动，这样就能够获得移动的向量，其中两个方向x和y移动的多少则记录在一个直方图的数组中。

 

这里还有一个优化——双线性插值，subdirs记录亚像素级，也就是半个像素点的移动

 

如果是PARAM_BOTTOM_FLOW_HIST_FILTER（？？什么意思），那么就是对于hist的处理，如果不是，那么就是对于dirs和subdirs的计算，求和然后处以有效的采样点个数

 

最后融合gyro的信息进行调整

 

 

 

 

Sonar.c &sonar_mode_filter.c

sonar.c是一个声呐接口程序

void UART4_IRQHandler(void):中断处理程序

里面有一个mode filter被注释掉了

// the mode filter turned out to be more problematic（使用mode filter比用声呐的原始值更有问题）

// than using the raw value of the sonar

//insert_sonar_value_and_get_mode_value(valid_data / SONAR_SCALE);

 

 

卡尔曼滤波：

预测值：

x_pred = x_post + dt * v_pred;

v_pred = v_post;

测量值：

x_new = sonar_mode

算出此时的x和v的最优值：（根据滤波估计方程）

x_post = x_pred + global_data.param[PARAM_SONAR_KALMAN_L1] * (x_new - x_pred);

PARAM_SONAR_KALMAN_L1  default:0.8461

v_post = v_pred + global_data.param[PARAM_SONAR_KALMAN_L2] * (x_new - x_pred);

PARAM_SONAR_KALMAN_L2  default:6.2034

 

 

 

Main.c

主要是while(1)的那个循环

前面是一些准备工作，得到陀螺仪的数据，计算焦距，得到声呐的数据

从计算光流开始：

/*

 * real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z

 * x / f = X / Z

 * y / f = Y / Z

 */

但我感觉实际上用到的是

v/f=V/Z

v——flow.c中得到的光流：pixel_flow_x，pixel_flow_y

V——实际的米制速度：new_velocity_x，new_velocity_y

Z——由声呐得到的距地面距离：sonar_distance_filtered

 

之后就是传数据了：

I2C：velocity_x_sum/valid_frame_count

     velocity_y_sum/valid_frame_count

所有的速度相加，除以有效的帧的个数（不知道为什么要算这个）

serial mavlink  + usb mavlink：串口是MAVLINK_COMM_0  USB是MAVLINK_COMM_2

mavlink_msg_optical_flow_send

mavlink_msg_optical_flow_rad_send

 

如果FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_LPOS])，会发送不包括航向的位置的粗略估计

/* rough local position estimate for unit testing */

lpos.x += ground_distance*accumulated_flow_x;

lpos.y += ground_distance*accumulated_flow_y;

lpos.z = -ground_distance;

 

如果是需要准确的位置估计的话，还是要用飞控里的ESF2，LPE或者INAV那三种算法把。