匿名无人机飞控代码整理6

最新推荐文章于 2024-06-15 14:41:53 发布

郑心怡呀

最新推荐文章于 2024-06-15 14:41:53 发布

阅读量1.2k

点赞数

分类专栏：电赛

本文链接：https://blog.csdn.net/qq_41382643/article/details/97685038

版权

电赛专栏收录该内容

31 篇文章 50 订阅

订阅专栏

色块追踪部分：
也就是轮询的这两个任务：

ANO_CBTracking_Task(20);
ANO_CBTracking_Ctrl_Task(20);

1、第一个是色块追踪的数据处理：

void ANO_CBTracking_Task(u8 dT_ms)
{

	ANO_CBTracking_Decoupling(&dT_ms,IMU_ROL,IMU_PIT);

	ANO_CBTracking_Calcu(&dT_ms,(s32)RELATIVE_HEIGHT_CM);
}

进去之后程序是这样的，第一个函数是色块跟踪解耦和，可以得到实际色块的偏移量（？偏移量不太懂，姑且认为是相邻帧的移动量，也就是速度？）
第二个是跟踪数据计算，参数有相对高度，可以计算出色块的移动速度，进而用以调整自身的移动速度。

2、第二个是色块跟踪控制任务：

void ANO_CBTracking_Ctrl_Task(u8 dT_ms)
{

	if(switchs.of_flow_on && switchs.opmv_on)
	{

		ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0]\
		= CBT_KF *ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[0]\
		+ CBT_KP *(ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0])\
		+ CBT_KD *ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0];
		
		ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1]\
		= CBT_KF *ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[1]\
		+ CBT_KP *(ano_opmv_cbt_ctrl.ground_pos_err_h_cm[1])\
		+ CBT_KD *ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[1];
	}
	else
	{
		ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0] = ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1] = 0;
	}

	Program_Ctrl_User_Set_HXYcmps(ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0],ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1]);
}

（查了下才知道\右斜杠是换行用的）
如果光流和openmv的开关打开，利用pid计算出xy的移动期望速度，
否则设为零。
最后的程控函数好像还有点东西：

void Program_Ctrl_User_Set_HXYcmps(float hx_vel_cmps,float hy_vel_cmps)
{
	
	pc_user.vel_cmps_set_h[0] = hx_vel_cmps;
	pc_user.vel_cmps_set_h[1] = hy_vel_cmps;

	length_limit(&pc_user.vel_cmps_set_h[0],&pc_user.vel_cmps_set_h[1],MAX_PC_XYVEL_CMPS,pc_user.vel_cmps_set_h);
}

用来控制速度，后面肯定还会遇到，先记住。。。