程序员经验分享-hwhale

匿名飞控码STM32版代码整理之Ano_AttCtrl.c

2023-05-16

代码

#include “Ano_AttCtrl.h”
#include “Ano_Imu.h”
#include “Drv_icm20602.h”
#include “Ano_MagProcess.h”
#include “Drv_spl06.h”
#include “Ano_MotionCal.h”
#include “Ano_FlightCtrl.h”
#include “Ano_LocCtrl.h”
#include “Ano_MotorCtrl.h”
#include “Ano_FlyCtrl.h”
#include “Ano_Sensor_Basic.h”
#include “Ano_ProgramCtrl_User.h”

//角度环控制参数
_PID_arg_st arg_2[VEC_RPY] ;

//角速度环控制参数
_PID_arg_st arg_1[VEC_RPY] ;

//角度环控制数据
_PID_val_st val_2[VEC_RPY];

//角速度环控制数据
_PID_val_st val_1[VEC_RPY];

/角度环PID参数初始化/

void Att_2level_PID_Init()
{

arg_2[ROL].kp = Ano_Parame.set.pid_att_2level[ROL][KP];
arg_2[ROL].ki = Ano_Parame.set.pid_att_2level[ROL][KI];
arg_2[ROL].kd_ex = Ano_Parame.set.pid_att_2level[ROL][KD];
arg_2[ROL].kd_fb = Ano_Parame.set.pid_att_2level[ROL][KD];
arg_2[ROL].k_ff = 0.0f;

arg_2[PIT].kp = Ano_Parame.set.pid_att_2level[PIT][KP];
arg_2[PIT].ki = Ano_Parame.set.pid_att_2level[PIT][KI];
arg_2[PIT].kd_ex = Ano_Parame.set.pid_att_2level[PIT][KD];
arg_2[PIT].kd_fb = Ano_Parame.set.pid_att_2level[PIT][KD];
arg_2[PIT].k_ff = 0.0f;

arg_2[YAW].kp = Ano_Parame.set.pid_att_2level[YAW][KP];
arg_2[YAW].ki = Ano_Parame.set.pid_att_2level[YAW][KI];
arg_2[YAW].kd_ex = Ano_Parame.set.pid_att_2level[YAW][KD];
arg_2[YAW].kd_fb = Ano_Parame.set.pid_att_2level[YAW][KD];
arg_2[YAW].k_ff = 0.0f;

}

#define CTRL_1_KI_START 0.f

/角速度环PID参数初始化/

void Att_1level_PID_Init()
{

arg_1[ROL].kp = Ano_Parame.set.pid_att_1level[ROL][KP];
arg_1[ROL].ki = Ano_Parame.set.pid_att_1level[ROL][KI];
arg_1[ROL].kd_ex = 0;//0.000f   ;
arg_1[ROL].kd_fb = Ano_Parame.set.pid_att_1level[ROL][KD];
arg_1[ROL].k_ff = 0.0f;

arg_1[PIT].kp = Ano_Parame.set.pid_att_1level[PIT][KP];
arg_1[PIT].ki = Ano_Parame.set.pid_att_1level[PIT][KI];
arg_1[PIT].kd_ex = 0;//0.000f   ;
arg_1[PIT].kd_fb = Ano_Parame.set.pid_att_1level[PIT][KD];
arg_1[PIT].k_ff = 0.0f;

arg_1[YAW].kp = Ano_Parame.set.pid_att_1level[YAW][KP];
arg_1[YAW].ki = Ano_Parame.set.pid_att_1level[YAW][KI];
arg_1[YAW].kd_ex = 0;//0.00f   ;
arg_1[YAW].kd_fb = Ano_Parame.set.pid_att_1level[YAW][KD];
arg_1[YAW].k_ff = 0.00f;

#if (MOTOR_ESC_TYPE == 2) //电调带刹车
#define DIFF_GAIN 0.3f

arg_1[ROL].kd_fb = arg_1[ROL].kd_fb *DIFF_GAIN;
arg_1[PIT].kd_fb = arg_1[PIT].kd_fb *DIFF_GAIN;

#elif (MOTOR_ESC_TYPE == 1) //电调不带刹车
#define DIFF_GAIN 1.0f

arg_1[ROL].kd_fb = arg_1[ROL].kd_fb *DIFF_GAIN;
arg_1[PIT].kd_fb = arg_1[PIT].kd_fb *DIFF_GAIN;

#endif
}

//设置角速度环的Ki
void Set_Att_1level_Ki(u8 mode)
{

if(mode == 0)
{
	arg_1[ROL].ki = arg_1[PIT].ki = 0;
}
else if(mode == 1)
{
	arg_1[ROL].ki = Ano_Parame.set.pid_att_1level[ROL][KI];
	arg_1[PIT].ki = Ano_Parame.set.pid_att_1level[PIT][KI];
}
else 
{
	arg_1[ROL].ki = arg_1[PIT].ki = CTRL_1_KI_START;
}

}
//设置角度环的Ki
void Set_Att_2level_Ki(u8 mode)
{

if(mode == 0)
{
	arg_2[ROL].ki = arg_2[PIT].ki = 0;
}
else
{
	arg_2[ROL].ki = Ano_Parame.set.pid_att_2level[ROL][KI];
	arg_2[PIT].ki = Ano_Parame.set.pid_att_2level[PIT][KI];
}

}

_att_2l_ct_st att_2l_ct;

static s32 max_yaw_speed,set_yaw_av_tmp;

#define POS_V_DAMPING 0.02f
static float exp_rol_tmp,exp_pit_tmp;

/角度环控制/
void Att_2level_Ctrl(float dT_s,s16 *CH_N)
{

//将位置环的角度信息(期望角度的负数)储存到角度环的期望角度
exp_rol_tmp = - loc_ctrl_1.out[Y];
exp_pit_tmp = - loc_ctrl_1.out[X];

//如果飞行模式为自稳模式ATT_STAB
if(flag.flight_mode == ATT_STAB)
{
	//若位置环算出的期望横滚角加上期望的横滚角的修正的绝对值小于5
	if(ABS(exp_rol_tmp + att_2l_ct.exp_rol_adj) < 5)
	{
			/*积分微调*/
		att_2l_ct.exp_rol_adj += 0.2f *exp_rol_tmp *dT_s;
		att_2l_ct.exp_rol_adj = LIMIT(att_2l_ct.exp_rol_adj,-1,1);
	}
	//若位置环算出的俯仰角加上期望的俯仰角偏差的绝对值小于5
	if(ABS(exp_pit_tmp + att_2l_ct.exp_pit_adj) < 5)
	{
			/*积分微调*/
		att_2l_ct.exp_pit_adj += 0.2f *exp_pit_tmp *dT_s;
		att_2l_ct.exp_pit_adj = LIMIT(att_2l_ct.exp_pit_adj,-1,1);
	}
}

else//在非自稳模式下，不需要期望角的调整参数
{
	att_2l_ct.exp_rol_adj = 
	att_2l_ct.exp_pit_adj = 0;
}

*/ **********************以上部分就是说：
在自稳模式下，如果期望的角度很小且与调整参数之间相差小于5，则增大调整参数；
期望角度 = 期望角度 + 角度调整参数
在非自稳模式下，期望角度 = 期望角度

*****************************/								

/*正负参考ANO坐标参考方向*/
//对期望角度赋值
att_2l_ct.exp_rol = exp_rol_tmp + att_2l_ct.exp_rol_adj;// + POS_V_DAMPING *imu_data.h_acc[Y];
att_2l_ct.exp_pit = exp_pit_tmp + att_2l_ct.exp_pit_adj;// + POS_V_DAMPING *imu_data.h_acc[X];

/*期望角度限幅*/
att_2l_ct.exp_rol = LIMIT(att_2l_ct.exp_rol,-MAX_ANGLE,MAX_ANGLE);
att_2l_ct.exp_pit = LIMIT(att_2l_ct.exp_pit,-MAX_ANGLE,MAX_ANGLE);

//
//如果速度模式为3
if(flag.speed_mode == 3)
{
	//设置飞控的最大航向角速度=250-----------给设置偏航角速度限幅用的
	max_yaw_speed = MAX_SPEED_YAW;
}
//如果速度模式为2
else if(flag.speed_mode == 2 )
{
	//设置飞控的最大航向角速度=220
	max_yaw_speed = 220;
}
else 
{
	//设置飞控的最大航向角速度=200
	max_yaw_speed = 200;
}
//
fc_stv.yaw_pal_limit = max_yaw_speed;

/*摇杆量转换为YAW期望角速度 + 程控期望角速度*/
set_yaw_av_tmp = (s32)(0.0023f *my_deadzone(CH_N[CH_YAW],0,65) *max_yaw_speed) + (-program_ctrl.yaw_pal_dps) + pc_user.pal_dps_set;

/*最大YAW角速度限幅*/
set_yaw_av_tmp = LIMIT(set_yaw_av_tmp ,-max_yaw_speed,max_yaw_speed);

/*没有起飞，复位*/
if(flag.taking_off==0||(flag.locking))
{
	att_2l_ct.exp_rol = att_2l_ct.exp_pit = set_yaw_av_tmp = 0;
	att_2l_ct.exp_yaw = att_2l_ct.fb_yaw;
}
/*限制误差增大*/
if(att_2l_ct.yaw_err>90)
{
	//当偏航角度误差过大时，且此时偏航角速度有值，则直接令偏航角速度=0（不然偏差会越来越大）
	if(set_yaw_av_tmp>0)
	{
		set_yaw_av_tmp = 0;
	}
}
else if(att_2l_ct.yaw_err<-90)
{
	//当偏航角度误差过大时，且此时偏航角速度有值，则直接令偏航角速度=0（不然偏差会越来越大）----------可以优化一下，调用绝对值函数就可以用一条语句解决
	if(set_yaw_av_tmp<0)
	{
		set_yaw_av_tmp = 0;
	}
}	

//增量限幅      
//将摇杆量转换的偏航角速度
att_1l_ct.set_yaw_speed += LIMIT((set_yaw_av_tmp - att_1l_ct.set_yaw_speed),-30,30);

/*设置期望YAW角度*/
att_2l_ct.exp_yaw += att_1l_ct.set_yaw_speed *dT_s;

/*限制为+-180度*/
if(att_2l_ct.exp_yaw<-180) att_2l_ct.exp_yaw += 360;
else if(att_2l_ct.exp_yaw>180) att_2l_ct.exp_yaw -= 360;	

/*计算YAW角度误差*/
att_2l_ct.yaw_err = (att_2l_ct.exp_yaw - att_2l_ct.fb_yaw);
/*限制为+-180度*/
if(att_2l_ct.yaw_err<-180) att_2l_ct.yaw_err += 360;
else if(att_2l_ct.yaw_err>180) att_2l_ct.yaw_err -= 360;



	/*赋值反馈角度值---------------------------imu测量值作为反馈*/
	att_2l_ct.fb_yaw = imu_data.yaw ;
		
	att_2l_ct.fb_rol = (imu_data.rol ) ;
	att_2l_ct.fb_pit = (imu_data.pit ) ;
		

PID_calculate( dT_s,            //周期（单位：秒）
									0 ,				//前馈值
									att_2l_ct.exp_rol ,				//期望值（设定值）
									att_2l_ct.fb_rol ,			//反馈值（）
									&arg_2[ROL], //PID参数结构体
									&val_2[ROL],	//PID数据结构体-------------------------------代表副翼
                  5,//积分误差限幅
									5 *flag.taking_off			//integration limit，积分限幅
									 )	;
									
PID_calculate( dT_s,            //周期（单位：秒）
									0 ,				//前馈值
									att_2l_ct.exp_pit ,				//期望值（设定值）
									att_2l_ct.fb_pit ,			//反馈值（）
									&arg_2[PIT], //PID参数结构体
									&val_2[PIT],	//PID数据结构体-------------------------代表升降
                  5,//积分误差限幅
									5 *flag.taking_off		//integration limit，积分限幅
									 )	;

PID_calculate( dT_s,            //周期（单位：秒）
									0 ,				//前馈值
									att_2l_ct.yaw_err ,				//期望值（设定值）
									0 ,			//反馈值（）
									&arg_2[YAW], //PID参数结构体
									&val_2[YAW],	//PID数据结构体-------------------------------代表方向
                  5,//积分误差限幅
									5 *flag.taking_off			//integration limit，积分限幅
									 )	;
									 
									 //经过PID调节后的结果（角度环的输出）保存在val_2[i].out中

}

_att_1l_ct_st att_1l_ct;
static float ct_val[4];

/角速度环控制/
void Att_1level_Ctrl(float dT_s)
{
改变控制参数任务（最小控制周期内）
ctrl_parameter_change_task();//改变控制参数，-------------应该是改变pid的参数

	/*目标角速度赋值--------------------期望角速度赋值*/
	 for(u8 i = 0;i<3;i++)
	{
		att_1l_ct.exp_angular_velocity[i] = val_2[i].out;// val_2[i].out;//目标速度赋值就是遥控器传过去给到角度环的值，从角度环输出到角速度环就是目标的角速度，也就是你想要控制飞机怎么去飞的一个值
		//val_2[i]就是二环				 val_2[i].out就是姿态角度环的输出
		/*
			对于给定的状态，可以是静止也可以是前进。如果是静止，那么姿态角度为0，如果是前进，俯仰角保持一定的、
		角度。总之，为了满足给定的状态必须保持一个一定的姿态，也就是我们期望达到的姿态，也就是期望的角度。
		如果我们能测的当前的角度，就可以通过pid求出我们需要多大的角速度。
		*/
	}

	/*目标角速度限幅（没有考虑到YAW角）*/
	att_1l_ct.exp_angular_velocity[ROL] = LIMIT(att_1l_ct.exp_angular_velocity[ROL],-MAX_ROLLING_SPEED,MAX_ROLLING_SPEED);
	att_1l_ct.exp_angular_velocity[PIT] = LIMIT(att_1l_ct.exp_angular_velocity[PIT],-MAX_ROLLING_SPEED,MAX_ROLLING_SPEED);


/*反馈角速度赋值-----------------------就是陀螺仪测得的当前角速度值*/
att_1l_ct.fb_angular_velocity[ROL] = ( sensor.Gyro_deg[X] );
att_1l_ct.fb_angular_velocity[PIT] = (-sensor.Gyro_deg[Y] );
att_1l_ct.fb_angular_velocity[YAW] = (-sensor.Gyro_deg[Z] );


/*PID计算*/

for(u8 i = 0;i<3;i++)//这个函数就是调用PID函数来进行各个角速度的计算得到输出值，也就是控制电机的转速的值
{

	PID_calculate( dT_s,            //周期（单位：秒）
									0,				//前馈值
									att_1l_ct.exp_angular_velocity[i],				//期望值（设定值）
									att_1l_ct.fb_angular_velocity[i],			//反馈值（测量值）
									&arg_1[i], //PID参数结构体
									&val_1[i],	//PID数据结构体
                200,//积分误差限幅
									CTRL_1_INTE_LIM *flag.taking_off			//integration limit，积分幅度限幅
									 )	; 


 ct_val[i] = (val_1[i].out);

}

/*赋值，最终比例调节*/
mc.ct_val_rol =                   FINAL_P *ct_val[ROL];//FINAL_P点击输出量比例：0.5
mc.ct_val_pit = X_PROPORTION_X_Y *FINAL_P *ct_val[PIT];//X_PROPORTION_X_Y：1.0f
mc.ct_val_yaw =                   FINAL_P *ct_val[YAW];
/*输出量限幅*/
mc.ct_val_rol = LIMIT(mc.ct_val_rol,-1000,1000);
mc.ct_val_pit = LIMIT(mc.ct_val_pit,-1000,1000);
mc.ct_val_yaw = LIMIT(mc.ct_val_yaw,-400,400);

}

_rolling_flag_st rolling_flag;

总结

该源文件主要写了两个函数：角速度环和角度环。构成了双环姿态控制器。
其中：角速度环是内环；角度环是外环。
角度环：利用PID求解期望角速度

  1. 期望的YAW角度
    摇杆量转换的角速度 * 周期 + 当前角度(位置环的输出)
  2. 期望的ROL/PIT角度
    在自稳模式下：期望角度(位置环的输出) + 修正参数
    非自稳模式下：期望角度 = 期望角度(位置环的输出)
  3. 反馈角度值
    就是陀螺仪的测量值
  4. PID求解出期望的角速度

角速度环：也是利用PID求解出电机控制量
就是角度环的输出
2. 反馈角速度值
就是陀螺仪测量的角速度
3. 利用PID求解出角速度环的输出，即电机控制量
所以，根据以上的分析我们可以知道，通过角度环与角速度环，最终得到的是对于电机的控制量。且对于角度环与角速度环的输入就是周期与飞行模式。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

匿名飞控码STM32版代码整理之Ano_AttCtrl.c 的相关文章

  • 基于迁移深度学习的遥感图像场景分类

    前述 根据语义特征对遥感图像场景进行分类是一项具有挑战性的任务 因为遥感图像场景的类内变化较大 xff0c 而类间变化有时却较小 不同的物体会以不同的尺度和方向出现在同一类场景中 xff0c 而同样的物体也可能出现在不同的场景里 理论上 x

  • reStructuredText文档图片表格等自动编号以及名称修改

    number figures 61 True numfig 61 True numfig secnum depth 61 1 numfig format 61 39 figure 39 39 图 s 39 39 code block 39

  • Robocup 2D仿真足球机器人环境搭建（Ubuntu 16.04）

    现在很多高校的学生都在搞RoboCup2D仿真足球机器人 xff0c 而平台的搭建便是一大问题 本人也在搭建环境中遇到各种奇奇怪怪的问题 xff0c 有些找了许多网页 xff0c 下面是我对环境搭建 中遇到的问题以及解决方法的总结 希望能帮

  • Robocup 2D仿真足球机器人环境搭建快速搭建

    虽然之前已经写了Robocup 2D仿真足球机器人环境搭建 xff0c 但是后面我找到了更快搭建环境的方法 而且现在很多团队都是在agent底层代码的基础上编写的 之前的方法配置的环境运行agent球队代码会出现错误 通过找资料得知是有些软

  • Robocup 2D新手导读（入门总结）

    目前 xff0c 我们团队接触Robocup 2D比赛也有好几个月了 xff0c 不得不说 这个比赛也有一定难度 尤其是对于第一次接触这个比赛的我们来 说 下面是我的一些总结 xff0c 希望能够帮助你们 1 Robocup 2D的比赛基本

  • Robocup 2D比赛代码导读（agent底层代码指导）

    相信刚开始接触Robocup 2D比赛的萌新 xff0c 都清楚agent的代码看起来是很痛苦的 xff0c 而且不知道从何看起 我个人观点看代码先看bhv和role这部分的代码 bhv行为动作的类的代码涉及到球员的进攻 跑位 防守 铲球等

  • python爬虫接单-资料总结

    相信看到我这篇文章的朋友们 xff0c 都是奔着赚钱来的 当初我也是冲着爬虫接单 可以赚钱 xff0c 一下就报了个将近3000元的爬虫班 python爬虫确实可以接单赚钱 后来学会了之后就迫不及待地加入了接单行列 也确实赚了一些 希望这篇

  • 爬虫Scrapy框架的介绍与使用

    Scrapy Scrapy介绍 爬取网站数据 xff0c 提取结构数据而编写的爬虫应用框架 Scrapy工作流程图 Scrapy命令 span class token number 1 span 创建Scrapy项目 scrapy star

  • 太难为我了，为了这份P7岗offer，我承受了7轮面试

    前言 今年的大环境非常差 xff0c 互联网企业裁员的现象比往年更严重了 xff0c 可今年刚好是我的第一个 五年计划 截止的时间点 xff0c 说什么也不能够耽搁了 xff0c 所以早早准备的跳槽也在疫情好转之后开始进行了 但是 xff0

  • 云计算与大数据概论第五周

    对于 大数据 xff08 Big data xff09 研究机构Gartner给出了这样的定义 大数据 是需要新处理模式才能具有更强的决策力 洞察发现力和流程优化能力来适应海量 高增长率和多样化的信息资产 麦肯锡全球研究所给出的定义是 xf

  • 【hadoop_读写流程】

    14 hadoop 补充 xff1a 解决 linux 网卡丢失问题 xff1a service NetworkManager stop chkconfig NetworkManager off 重启网络 systemctl restart

  • 半监督学习

    半监督学习 xff1a 在训练阶段结合了大量未标记的数据和少量标签数据 与使用所有标签数据的模型相比 xff0c 使用训练集的训练模型在训练时更为准确 xff0c 而且训练成本更低 如何综合利用已标签例子和未标签例子 xff0c 是半监督学

  • [控制原理基础]浅谈PID算法

    一 PID使用背景 当今的自动控制技术都是基于反馈的概念 即一个In Loop闭环的理论 xff0c 反馈理论的要素包括三个部分 xff1a 测量 比较和执行 测量关心的变量 xff0c 与期望值相比较 xff0c 用这个误差纠正调节控制系

  • 解决Github下载慢的问题！

    从GitHub上下载文件对于国内的我们简直太痛苦了 xff0c 下载了半天稳定在了8kb s 于是我开始寻找解决的方法 xff0c 最开是的方法是去查github com网站的IP xff0c 然后将IP添加到hosts文件里 xff0c

  • [Pix：地面站Mission Planner常见故障和处理方法]

    连接飞控的MP软件以后 xff0c 观察飞行数据界面 xff08 简称HUD xff09 以下为常见错误 xff1a RC not calibrated 未校正遥控器 xff0c 需要校正遥控器Compass not calibrated

  • Pixhawk指示灯的含义

    飞控的指示灯含义 xff1a 红灯与蓝灯交替闪烁 xff1a 系统初始化蓝灯闪烁 xff1a 飞控锁定状态 xff0c GPS正在搜星状态 黄灯连闪两次 xff1a 系统拒绝解锁 xff0c 提示错误 绿灯闪烁 xff1a 飞控锁定状态 x

  • 转载：Pixhawk无人机扩展教程--树莓派安装Dronekit及读取飞控数据

    寻找了一段时间 xff0c 发现Dronekit Python可以实现脱离遥控器来控制无人机 xff0c 找到了一些较好的教程 xff0c 但是中间也踩了一些坑 先转载一下教程 xff0c 遇到的哪些坑以后再写 这里

  • 转载：Pixhawk无人机扩展教程--使用Dronekit编写一个控制程序

    跳转 xff1a 详细教程

  • SQL执行错误#1251.从数据库的响应：

    问题 xff1a 使用 MySQL Font 连接数据库时 xff0c 显示如下错误 xff1a 解决方法 xff1a 使用管理员身份运行命令提示符 xff08 win 43 R 然后输入 cmd xff0c 进入到MySQL的bin目录中

  • 使用 Whisper AI 领先游戏：最佳和免费的语音到文本 AI

    Whisper AI 是一种语音识别和转录软件 xff0c 它使用人工智能 AI 将口头语言转换为书面文本 它旨在通过消除手动转录语音内容的需要来帮助个人和企业节省时间并提高工作效率 在下文中 xff0c 您将学习如何使用 Whisper

随机推荐

热门标签