这部分是关于匿名串级PID的，我觉得有需要的同学可以直接移植，不需要自己写了，确实有点麻烦，基本上代码里面都注释的很清楚了，且由于本人水平有限，所以也不是都很懂，只能做到这里了。
#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"
/*
在这里需要纠正一个概念，基于C语言实现的PID输出就是反馈，而基于控制系统实现的PID最终输出是作用于电机上的
即输出跟反馈并无直接联系
匿名”Ano_Pid.c“文件中PID函数有几个重要的入口参数，
期望值，反馈值，最后在结构体指针里面还有输出值。在这里，遥控器打杆值作为期望，
姿态解算测得的欧拉角作为反馈，经过PID最终输出一个不断接近我们期望的值，
通过PWM驱动电机，同时姿态改变，不断进行上述过程直至最后稳定。
*/
//1->内环即角速度环
//2->外环即角度环
//经过查看这些变量的定义发现，其是来源于”Ano_Pid.h“中的结构体，再滑到下面看到有好多个”Ano_Pid.c“
//中PID函数的引用，不难想到定义这些变量是为了给下面PID函数调用提供初始化，事实上确实是这样
//但是”Ano_Pid.c“中的PID函数除了这两个比较重要的参数外，还有PID的期望值和反馈值也很重要
//到"Ano_AttCtrl.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];
/************************************************************************
*************************************************************************
*************************************************************************
*************************************************************************
*************************************************************************
*************************************************************************/
//PIT ROW YAW 各有一套PID吗？
//初始化为下面调用”Ano_Pid.c“中的PID函数做准备
/*角度环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;
}
/*
姿态角速率部分控制参数
arg_1_kp：调整角速度响应速度，不震荡的前提下，尽量越高越好。
震荡时，可以降低arg_1_kp，增大arg_1_kd。
若增大arg_1_kd已经不能抑制震荡，需要将kp和kd同时减小。
*/
#define CTRL_1_KI_START 0.0f
/************************************************************************
*************************************************************************
*************************************************************************
*************************************************************************
*************************************************************************
*************************************************************************/
/*角速度环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.005f ;
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.005f ;
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.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_ex = arg_1[ROL].kd_ex *DIFF_GAIN;
// arg_1[PIT].kd_ex = arg_1[PIT].kd_ex *DIFF_GAIN;
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_ex = arg_1[ROL].kd_ex *DIFF_GAIN;
// arg_1[PIT].kd_ex = arg_1[PIT].kd_ex *DIFF_GAIN;
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拿出来？不懂？
//设置角速度环的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];
if(ABS(exp_rol_tmp + att_2l_ct.exp_rol_adj) < 5)
{
att_2l_ct.exp_rol_adj += 0.1f *exp_rol_tmp *dT_s;
att_2l_ct.exp_rol_adj = LIMIT(att_2l_ct.exp_rol_adj,-1,1);
}
if(ABS(exp_pit_tmp + att_2l_ct.exp_pit_adj) < 5)
{
att_2l_ct.exp_pit_adj += 0.1f *exp_pit_tmp *dT_s;
att_2l_ct.exp_pit_adj = LIMIT(att_2l_ct.exp_pit_adj,-1,1);
}
//外环控制
//正负参考ANO坐标参考方向
//得出期望欧拉角，但是没有yaw
//按理来说应该就是摇杆量转换为的欧拉角
att_2l_ct.exp_rol =cvPid_y.pit_out+exp_rol_tmp + att_2l_ct.exp_rol_adj + POS_V_DAMPING *imu_data.h_acc[Y];
att_2l_ct.exp_pit =cvPid_x.rol_out+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);
//下面一堆都是关于Yaw的处理，虽然看起来比较麻烦，但是我们可以采取和上面一样的方法分析即假设Yaw角
//就是直接来源于遥控的打杆量，省去中间过程，使用ADT来进行理解，不管他怎么处理，最终输出必然是期待的Yaw
/****************************************************************************************************/
if(flag.speed_mode == 3)
{
max_yaw_speed = MAX_SPEED_YAW;
}
else if(flag.speed_mode == 2 )
{
max_yaw_speed = 280;
}
else
{
max_yaw_speed = 200;
}
/*摇杆量转换为YAW期望角速度*/
att_1l_ct.set_yaw_speed = (s32)(0.0023f *my_deadzone(CH_N[CH_YAW],0,65) *max_yaw_speed);
/*最大YAW角速度限幅*/
set_yaw_av_tmp = LIMIT(att_1l_ct.set_yaw_speed ,-max_yaw_speed,max_yaw_speed);
/*没有起飞，复位*/
if(flag.taking_off==0)//if(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;
}
/*设置期望YAW角度*/
att_2l_ct.exp_yaw += set_yaw_av_tmp *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;
/*限制误差增大*/
if(att_2l_ct.yaw_err>90)
{
if(set_yaw_av_tmp>0)
{
set_yaw_av_tmp = 0;
}
}
else if(att_2l_ct.yaw_err<-90)
{
if(set_yaw_av_tmp<0)
{
set_yaw_av_tmp = 0;
}
}
/****************************************************************************************************/
//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？
//这个PID实现的是什么过程？输入期望，则输出就会慢慢向着我期望的方向变化，最终稳定
//那么这个输入是什么？遥控器各个通道的打杆值，对应于 Pitch Row Yaw Thr的输出，
//意思是我遥控器打到多少，代表我想要多少，然后PID会输出这样一个值，驱动电机
//因此一个通道对应于一套PID
//以遥控器的四通道为例 1-4分别是副翼，升降，油门，方向，
//而滚转角->副翼 俯仰角->升降 偏航角->方向，油门没有对应的欧拉角，直接控制PWM输出
//不难回答上述问题
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，积分限幅
) ;
}
_att_1l_ct_st att_1l_ct;
static float ct_val[4];
//经过PID，基本能够达到我们的期望值，至于能否快速且误差极小的达到就是另外一回事了
//涉及到PID调参，先实现原理，再进行调参
//如果你看懂了上面关于外环的分析，那么下面的内环也是同样的道理
//最终目的就是调用”Ano_Pid.c“中的PID函数，输入最重要的两个参数期望值和反馈值
//最终达到我们想要的效果，其他的步骤都是为了达到这个目标所做的必要的准备
//比如需要将期望摇杆量转化为欧拉角，需要对期望值进行限幅处理等等
/************************************************************************
*************************************************************************
*************************************************************************
*************************************************************************
*************************************************************************
*************************************************************************/
/*角速度环控制*/
void Att_1level_Ctrl(float dT_s)
{
改变控制参数任务（最小控制周期内）
ctrl_parameter_change_task();
/*期望角速度赋值*/
//内环的输入即为外环的输出，不考虑单位
for(u8 i = 0;i<3;i++)
{
att_1l_ct.exp_angular_velocity[i] = val_2[i].out;// val_2[i].out;//
}
/*期望角速度限幅*/
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_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];
mc.ct_val_pit = X_PROPORTION_X_Y *FINAL_P *ct_val[PIT];
mc.ct_val_yaw = FINAL_P *ct_val[YAW];
/*输出量限幅*/
//最终得到的值即将用于电机控制
//串级PID到此结束
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;
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
