【UAV】光流传感器代码分析

【UAV】光流传感器代码分析

图片来源：STM32四轴 学生 航模四轴 光流定点悬停 无人机DIY开源四轴开发板

文章目录

• 光流分析
• • 光流数据结构体
  • 光流输出数据结构体
  • void Flow_Receive(u8 data) // 串口1解析光流模块数据
  • void Pixel_Flow_Fix(float dT) // mini光流数据滤波融合，dT 设定了 0.006
  • void Flow_Pos_Controler(float dt) // 位置定点控制器 光流
• 程序流程图

---

光流分析 光流数据结构体

// 光流数据结构体
struct _flow_
{
	float flow_x;			// 光流原始数据
	float flow_y;			// 光流原始数据
	float flow_x_i;
	float flow_y_i;
	
	u8 qual;				// SQUAL 环境质量
	u8 ok;
	float flow_High;
	u8 ssi;
	u8 ssi_cnt;
//	u8 err;
};

struct _flow_ mini

光流输出数据结构体

// 光流输出数据结构体
struct _pixel_flow_
{
	float x_i;					// x轴积分原始值
	float y_i;					// y轴积分原始值
	float fix_x_i;				// x轴积分滤波值
	float fix_y_i;				// y轴积分滤波值
	float ang_x;			
	float ang_y;		
	float gyr_x;			
	float gyr_y;		
	float out_x_i;				// x轴积分输出值
	float out_y_i;				// y轴积分输出值
	float out_x_i_o;			
	float out_y_i_o;		
	
	float dx;		
	float dy;		
	
	float x;					// x轴速度原始值
	float y;					// y轴速度原始值
	float fix_x;				// x轴速度融合值
	float fix_y;				// y轴速度融合值
	
	float loc_x;
	float loc_y;
	float loc_xs;
	float loc_ys;
	
	float fix_High;
};

struct _pixel_flow_ pixel_flow;		//光流输出数据结构体

_ f l o w _ ⇒ m i n i _flow_ Rightarrow mini _flow_⇒mini

_ p i x e l _ f l o w _ ⇒ p i x e l _ f l o w _pixel_flow_ Rightarrow pixel_flow _pixel_flow_⇒pixel_flow

---

void Flow_Receive(u8 data) // 串口1解析光流模块数据

这个函数主要是把原始光流数据读取到 mini.flow_x, mni.flow_y 中，并累加到 mini.flow_x_i, mini.flow_y_i 变量中。

void Flow_Receive(u8 data) 		// 串口1解析光流模块数据
mini.flow_x = ((s16)(*(RxBuffer+3)<<8)|*(RxBuffer+2));			// 得到光流测量的 x 方向数据
mini.flow_y = ((s16)(*(RxBuffer+5)<<8)|*(RxBuffer+4));			// 得到光流测量的 y 方向数据

mini.flow_x_i += mini.flow_x ;
mini.flow_y_i += mini.flow_y ;

---

void Pixel_Flow_Fix(float dT) // mini光流数据滤波融合，dT 设定了 0.006

这个函数最后更新了两个变量 pixel_flow.loc_x, pixel_flow.loc_y 和 pixel_flow.loc_xs, pixel_flow.loc_ys。

void Pixel_Flow_Fix(float dT)		// mini光流数据滤波融合，dT 设定了 0.006
// 积分位移值单位转换为：厘米
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;

---

void Flow_Pos_Controler(float dt) // 位置定点控制器 光流

了解此光流到控制的函数之前，先明确存储数据的变量名称：
外环 Flow_PosPid_x, Flow_PosPid_y
内环 Flow_SpeedPid_x, Flow_SpeedPid_y

这里通过光流位置变量 pixel_flow.loc_x 达到更新内环输出 Flow_SpeedPid_x.desired 的目的。

注意，这里需要结合源代码来看，因为还牵扯到控制逻辑

// 记录位置 一次
Flow_PosPid_x.desired = pixel_flow.loc_x;						// 记录位置 // 刷新位置期望	
Flow_PosPid_y.desired = pixel_flow.loc_y;						// 记录位置 // 刷新位置期望

// 外环位置控制
Flow_PosPid_x.measured = pixel_flow.loc_x;							// 实时位置反馈
pidUpdate(&Flow_PosPid_x,dt);										// 位置运算PID
Flow_PosPid_y.measured = pixel_flow.loc_y;							// 实时位置反馈
pidUpdate(&Flow_PosPid_y,dt);										// 位置运算PID

// 内环期望
Flow_SpeedPid_x.desired = LIMIT(Flow_PosPid_x.out,-1000,1000);		// 位置PID输出给速度期望
Flow_SpeedPid_y.desired = LIMIT(Flow_PosPid_y.out,-1000,1000);		// 位置PID输出给速度期望

// 内环
Flow_SpeedPid_x.measured = pixel_flow.loc_x;							// 速度反馈
pidUpdate(&Flow_SpeedPid_x,dt);											// 速度运算
Flow_SpeedPid_y.measured = pixel_flow.loc_y;							// 速度反馈
pidUpdate(&Flow_SpeedPid_y,dt);											// 速度运算

---

程序流程图