图片来源：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); // 速度运算
程序流程图
