?固定翼无人机pid控制arduion stm32/
// ? ?功能：通过ky9250模块取得欧拉角后控制无人机舵机
// ? ?版本：v1.0
// ? ?编写：fanxd
// ? ?编写于：20210828
/
// 当前角度及期望角度
float ? current_attitude[2]； ?//roll,pitch
float ? current_heading, ? ? ? //yaw
desired_attitude[2] = {0.f}, desired_heading = {0.f}, desired_thro = 1000.f;
?// PID参数
float a_p_gain = 4.08f, a_i_gain = 0.f, a_d_gain = 238.f,
h_p_gain = 2.f, h_i_gain = 0.f, h_d_gain = 235.f;
?// PID过程变量
float last_attitude[2] = {0.f}, ? ? ? ? ?last_heading = 0.f,
attitude_int_value[2] = {0.f}, ? ? heading_int_value = 0.f,
error;
// PID调整量
float attitude_trim[2], heading_trim;
?// 控制量
uint16_t thro[4];
?// 状态变量
?uint8_t offline = 0, sbus_rtn, n;
float ATTI_INT_LIMIT_US_F=0; ?//roll,pitch 的限制，暂时为零度
float MAX_ATTI_TRIM_US_F=3; ? ? ?//无人机roll,pitch滚动、俯仰调整过载限制，暂时为3.?
float MAX_HEAD_TRIM_US_F=3; ? //无人机yaw转向调整过载限制，暂时为3.
float MAX_THRO_US =500; ? ? ? ? //舵机调整控制的速度（快慢）暂时为500.
?while(1)
?{
?//通过ky9250得到无人机实时姿态,roll=滚动角，pitch=俯仰角，yaw=航向角
if ?(qcpd（）=flase) ? //强磁判断，当受到强磁干扰时（磁通压缩导弹等时），不取新值进入纯惯导。qcpd()为强磁判断函数
{
current_attitude[0] =roll; ? ? ? ? ? //通过ky9250模块得到无人机roll,pitch,yaw姿态角
current_attitude[1] =pitch;
current_heading ? ?=yaw ?;
}
?// 读测站的无线下发指令信号
?? ? ? ? ? ? ? ? ? ?sbus_rtn = sbus_frame_analy();
?? ? ? ? ? ? ? ? ? ?if(!sbus_rtn && !(G_sbus_flag & 0xC))
?? ? ? ? ? ? ? ? ? ?{
?? ? ? ? ? ? ? ? ? ? ? ? ? ?offline = 0;
?? ? ? ? ? ? ? ? ? ? ? ? ? ?desired_thro = (float)(1684 - G_sbus_channel[2]) / 1320.f * (float)(MAX_THRO_US - 1000) + 1000.f; ? //无人机新的转向的速度控制信号
?? ? ? ? ? ? ? ? ? ? ? ? ? ?desired_attitude[0] = - (float)(G_sbus_channel[0] - 1024) / 1024.f * MAX_ATTITUDE_DEG_F; ? ? ? ? ? ? ?//无人机新的roll调整控制信号
?? ? ? ? ? ? ? ? ? ? ? ? ? ?desired_attitude[1] = (float)(G_sbus_channel[1] - 1024) / 1024.f * MAX_ATTITUDE_DEG_F; ? ? ? ? ? ? ? //无人机新的pitch调整控制信号
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? desired_heading ? ?= (float)(G_sbus_channel[3] - 1024) / 1024.f * MAX_ATTITUDE_DEG_F; ? ? ? ? ? ? ? //无人机新的yaw调整控制信号
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? // 1、可改为你自己定义的控制量
?? ? ? ? ? ? ? ? ? ? ? ? ? // ?2、当测站没有下发无线电指令时无人机做惯导。
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?// ?3、或没有GPS/北斗修正信号时无人机做惯导。
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?// ?4、GPS/北斗在整个飞行过程中只打开3次做惯导的校正。 ?
?? ? ? ? ? ? ? ? ? ?} ? ??
?
/ PID控制
?? ? ? ? ? ? ? ? ? ?if(!offline)
?? ? ? ? ? ? ? ? ? ?{
?? ? ? ? ? ? ? ? ? ? ? ? ? ?// 姿态控制
?? ? ? ? ? ? ? ? ? ? ? ? ? ?for(n = 0; n < 2; n++)
?? ? ? ? ? ? ? ? ? ? ? ? ? ?{
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?error = desired_attitude[n] - current_attitude[n];
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?attitude_int_value[n] += error * a_i_gain;
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?if(attitude_int_value[n] > ATTI_INT_LIMIT_US_F)
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?attitude_int_value[n] = ATTI_INT_LIMIT_US_F;
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?else if(attitude_int_value[n] < - ATTI_INT_LIMIT_US_F)
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?attitude_int_value[n] = - ATTI_INT_LIMIT_US_F;
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?attitude_trim[n] = (error * a_p_gain) + attitude_int_value[n] - ((current_attitude[n] - last_attitude[n]) * a_d_gain);
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?last_attitude[n] = current_attitude[n];
?? ? ? ? ? ? ? ? ? ? ? ? ? ?}
?? ? ? ? ? ? ? ? ? ? ? ? ? ?// 航向控制
?? ? ? ? ? ? ? ? ? ? ? ? ? ?error = relative_error(desired_heading, current_heading);
?? ? ? ? ? ? ? ? ? ? ? ? ? ?heading_int_value += error * h_i_gain;
?? ? ? ? ? ? ? ? ? ? ? ? ? ?if(heading_int_value > HEAD_INT_LIMI_US_F)
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?heading_int_value = HEAD_INT_LIMI_US_F;
?? ? ? ? ? ? ? ? ? ? ? ? ? ?else if(heading_int_value < - HEAD_INT_LIMI_US_F)
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?heading_int_value = - HEAD_INT_LIMI_US_F;
?? ? ? ? ? ? ? ? ? ? ? ? ? ?heading_trim = (error * h_p_gain) + heading_int_value - (relative_error(current_heading, last_heading) * h_d_gain);
?? ? ? ? ? ? ? ? ? ? ? ? ? ?last_heading = current_heading;
?? ? ? ? ? ? ? ? ? ? ? ? ??
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?// to be continue...
?? ? ? ? ? ? ? ? ? ? ? ? ? ?// 限制最大控制量
?? ? ? ? ? ? ? ? ? ? ? ? ? ?for(n = 0; n < 2; n++)
?? ? ? ? ? ? ? ? ? ? ? ? ? ?{
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?if(attitude_trim[n] > MAX_ATTI_TRIM_US_F)
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?attitude_trim[n] = MAX_ATTI_TRIM_US_F;
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?else if(attitude_trim[n] < - MAX_ATTI_TRIM_US_F)
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?attitude_trim[n] = - MAX_ATTI_TRIM_US_F;
?? ? ? ? ? ? ? ? ? ? ? ? ? ?}
?? ? ? ? ? ? ? ? ? ? ? ? ? ?if(heading_trim > MAX_HEAD_TRIM_US_F)
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?heading_trim = MAX_HEAD_TRIM_US_F;
?? ? ? ? ? ? ? ? ? ? ? ? ? ?else if(heading_trim < - MAX_HEAD_TRIM_US_F)
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?heading_trim = - MAX_HEAD_TRIM_US_F;
?? ? ? ? ? ? ? ? ? ? ? ? ??
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?// 限制舵机调整的快慢
?? ? ? ? ? ? ? ? ? ? ? ? ? ?for(n = 0; n < 4; n++)
?? ? ? ? ? ? ? ? ? ? ? ? ? ?{
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?if(thro[n] > MAX_THRO_US)
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?thro[n] = MAX_THRO_US;
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?else if(thro[n] < 1000)
?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?thro[n] = 1000;
?? ? ? ? ? ? ? ? ? ? ? ? ? ?}
?? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //控制两个水平舵机
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? TIM_SetCompare1(TIM3, attitude_trim[1]);
?? ? ? ? ? ? ? ? ? ? ? ? ? ?TIM_SetCompare2(TIM3, attitude_trim[1]);
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //控制两个垂直舵机
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? TIM_SetCompare3(TIM3, heading_trim);
?? ? ? ? ? ? ? ? ? ? ? ? ? ?TIM_SetCompare4(TIM3, heading_trim);
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??
?? ? ? ? ? ? ? ? ? ? ? ??
?? ? ? ? ? ? ? ? ? ?}
? ? ? ? ??
}
float relative_error(float current_heading,float last_heading)?
{
? ? ? ? ? ?float fhz;
? ? ? ? ? ?fhz=current_heading- last_heading;
? ? ? ? ? ?if (fhz<0)
? ? ? ? ? ?{
? ? ? ? ? ? ? ? ?fhz=360+fhz;
? ? ? ? ? ?}
? ? ? ? ? ?return fhz;
}