PV3953L1光流定点激光定高四轴悬停模块说明书

PV3953L1光流定点激光定高四轴悬停模块说明书

视频效果：

接线标识：V：3.3V ；T：发送；R：接收：G：GND

注意：电源电压不能接5V，接了就烧模块！

2、模块通讯协议
（1）本模块通过串口向上位机发送数据
（2）数据特性
a、8位数据位，无校验位，1位停止位，波特率设置为19200bps。
b、数据频率：约66fps帧率向主机发送数据。
c、其他：如果连续两帧位移数据太小，会输出0，然后在足够位移时候在输出数据。

3、协议说明：
包头 字节数 光流数据 高度数据 和校验 环境质量 结束标识符
0XFE 0X04 DATA0 DATA1 DATA2 DATA3 DATA4 DATA5 SUM SQUAL 0XAA

a、包头为固定的两个字节：0XFE（包头）、0X04（字节数）。
b、光流X数据：DATA0、DATA1是mini.flow_x低8位和高8位
既：mini.flow_x= (s16)((DATA1 )<<8)|(DATA0 );
b、光流Y数据：DATA2、DATA3是mini.flow_y低8位和高8位
既：mini.flow_y= (s16)((DATA3 )<<8)|(DATA2 );
c、高度Z数据：DATA4、DATA5是mini.flow_High低8位和高8位
既：mini.flow_High= (s16)((DATA5 )<<8)|(DATA4 );
d、倒数第三位是校验字节，是DATA0~DATA5六个字节累加值，
既：sum=DATA0 +DATA1+DATA2 +DATA3 +DATA4 +DATA5 ；
e、倒数第二个是光流地面环境质量值：数值越大表示地面环境质量越高；
f、最后一个字节是包结束标识符：0XAA；

4、数据的接收和解析，这点就不详解了，根据通信协议编写代码就行，比较简单直接上代码吧。

void Flow_Receive(u8 data) //串口1解析光流模块数据
{
static u8 RxBuffer[32];
static u8 _data_cnt = 0;
static u8 state = 0;
u8 sum = 0;
static u8 fault_cnt;

switch(state)
{
	case 0:
		if(data==0xFE)  //包头
		{
			state=1;
			RxBuffer[_data_cnt++]=data;
		}else state = 0;
	break;
	case 1:
		if(data==0x04)
		{
			state=2;
			RxBuffer[_data_cnt++]=data;
		}else state = 0;
	break;
	case 2:
		RxBuffer[_data_cnt++]=data;
		if(_data_cnt==11)
		{
			state = 0;
			_data_cnt = 0;
			sum =  (RxBuffer[2] + RxBuffer[3] + RxBuffer[4] + RxBuffer[5]+ RxBuffer[6] + RxBuffer[7]);
			if((0xAA == data) && (sum == RxBuffer[8])) 
			{					
				//读取原始数据
				mini.flow_x = ((s16)(*(RxBuffer+3)<<8)|*(RxBuffer+2));
				mini.flow_y = ((s16)(*(RxBuffer+5)<<8)|*(RxBuffer+4));
				mini.qual = *(RxBuffer+9);
				mini.flow_High=((s16)(*(RxBuffer+7)<<8)|*(RxBuffer+6));
				mini.flow_x_i += mini.flow_x ;
				mini.flow_y_i += mini.flow_y ;
				
				//判断光流数据是否有效
				if(mini.qual<25)
				{
						fault_cnt++;													
						if(fault_cnt>60)	//连续60次异常标定为无效
						{
							fault_cnt = 60;
							mini.ok = 0;
						}
				}
				else 
				{
						fault_cnt=0;
						mini.ok = 1;
				}
			}
		}
	break;
	default:
	break;
}

}

先来看看光流输出的原始数据曲线吧，测试条件：模块离桌面20厘米左右，镜头正对着桌面，手持来回平移，数据输出范围大概在±30：

5、光流数据的处理（由于这个光流输出的数据是没有经过惯性融合的，所以必须进行处理才能使用）。

第1步，对积分位移进行一个简单的低通滤波。
pixel_flow.fix_x_i += (mini.flow_x_i - pixel_flow.fix_x_i) 0.2;
pixel_flow.fix_y_i += (mini.flow_y_i - pixel_flow.fix_y_i) 0.2;
第2步，用姿态角去补偿积分位移（#define angle_to_rad 0.0174f //角度转弧度）。
pixel_flow.ang_x += (600.0ftan(imu_data.rolangle_to_rad) - pixel_flow.ang_x) 0.2;
pixel_flow.ang_y += (600.0ftan(imu_data.pit*angle_to_rad) - pixel_flow.ang_y) *0.2;
pixel_flow.out_x_i = pixel_flow.fix_x_i - pixel_flow.ang_x;
pixel_flow.out_y_i = pixel_flow.fix_y_i - pixel_flow.ang_y;
注意：600.0f是一个重要参数！需要慢慢调，调到怎么样才算合格呢？

当飞机在原地摇晃时，pixel_flow.fix_x_i 和 pixel_flow.ang_x 的曲线变化幅度几乎一样即可，两者相减互相抵消，达到补偿的作用。或者这么说：pixel_flow.fix_y_i 和 pixel_flow.ang_y 的曲线变化幅度几乎一样即可（特别注意，飞机在原地晃时积分位移数据会缓慢地增大或者减小，这是积分飘移现象，是正常的）。

飞机在原地摇晃时，pixel_flow.fix_x_i 和 pixel_flow.ang_x 的曲线图：

用姿态角去补偿积分位移的作用是：当飞机在原地摇晃时，使积分位移尽量保持不变。为什么要使积分位移尽量保持不变呢？很简单，因为飞机在在原地摇晃，位移没变。

飞机在原地摇晃时，姿态角补偿后的积分位移曲线图：

第3步，对积分位移进行微分处理，得到速度。

//求微分速度
pixel_flow.x = (pixel_flow.out_x_i - pixel_flow.out_x_i_o)/dT;
pixel_flow.out_x_i_o= pixel_flow.out_x_i;
pixel_flow.y = (pixel_flow.out_y_i - pixel_flow.out_y_i_o)/dT;
pixel_flow.out_y_i_o = pixel_flow.out_y_i;

//低通滤波
pixel_flow.fix_x += ( pixel_flow.x - pixel_flow.fix_x ) * 0.1f;
pixel_flow.fix_y += ( pixel_flow.y - pixel_flow.fix_y ) * 0.1f;

有的人看到这里可能会迷惑了，为什么要微分求速度而不是直接使用光流的输出作为速度呢？原因是：这个光流模块的输出的数据严格的说不是速度，而是连续两帧画面的相对位移，单位是像素。

飞机在一个水平面内来回平移的速度曲线图：

第4步，把数据单位转换为厘米。

//式中HIGH为实际高度，单位：米
cpi = ((50*0.01f) / 11.914f) *2.54f ;
pixel_flow.fix_High=cpi;
//积分位移值单位转换为：厘米
pixel_flow.loc_x = pixel_flow.out_x_i * cpi;
pixel_flow.loc_y = pixel_flow.out_y_i * cpi;

//微分速度值单位转换为：厘米/秒
pixel_flow.loc_xs = pixel_flow.fix_x * cpi;
pixel_flow.loc_ys = pixel_flow.fix_y * cpi;

至此，光流数据的处理算是基本完成了，有了积分位移和微分速度这两个参数，就可以进行水平面内的位置环和速度环的串级PID控制了。

6、方向安装图

注意：方向安装不对会导致定点的方向乱飞！

7、模块尺寸图（单位MM）