四轴飞行器偏航角不同飞控的C语言实现方法

        偏航角的控制与俯仰角以及翻滚角不同。俯仰角以及翻滚角都是要随着遥控器改变，特别是遥控器归中的时候，就俯仰角以及翻滚角也要归0。但是偏航角最大的区别就是遥控器归中后不需要恢复。

蓝鸟飞行器：

对于Z轴偏航角的控制：

是对接受来的遥控器数据进行分段处理，当大于某一值的时候，对期望的偏航角度值进行累加。由于是分段控制，所以遥控器摇杆归位以后，期望的偏航角度值并不归0。所以不会出现摇杆归位后，飞机又转回去的现象。

特点：

偏航角数据分段处理不同担心在打油门的时候误触偏航角

Z轴有角度环--角速度环PID控制，所以在未起飞的时候对飞机的偏航角进行改变后，飞机会将认为偏航角的改变是外界的误差干扰，起飞时，为了消除误差会回到初始化时(一般是开机时)的偏航角。

同样的，在飞行的时候，如果强行改变飞行器的偏航角，由于Z轴有角度环PID控制，飞机也会很快的调整到原来的偏航角

简要代码节选：

//遥控器
const float roll_pitch_ratio = 0.04f;//将控制量限制在-20~20之间
​
pidPitch.desired    = -(Remote.pitch-1500)*roll_pitch_ratio 
pidRoll.desired     = -(Remote.roll-1500)*roll_pitch_ratio; 
​
if(Remote.yaw>1820)//当且仅当舵量里中心值（1500）偏移较多时，才对偏航角进行加和   
    {
        pidYaw.desired += 0.75f;    
    }
        else if(Remote.yaw <1180)
    {
        pidYaw.desired -= 0.75f;    
    }
​
​
//PID控制
pidRateX.measured = MPU6050.gyroX * Gyro_G; //陀螺仪测量的角速度
pidRateY.measured = MPU6050.gyroY * Gyro_G;
pidRateZ.measured = MPU6050.gyroZ * Gyro_G;         
​
pidPitch.measured = Angle.pitch; //姿态结算的实际姿态角
pidRoll.measured  = Angle.roll;
pidYaw.measured   = Angle.yaw;
​
pidUpdate(&pidRoll,dt);     
pidRateX.desired = pidRoll.out; //将角度环的输出值作为角速度环的输入值
pidUpdate(&pidRateX,dt);  
​
pidUpdate(&pidPitch,dt);    
pidRateY.desired = pidPitch.out;  
pidUpdate(&pidRateY,dt);   
​
CascadePID(&pidRateZ,&pidYaw,dt);   
​

//PID算法
void pidUpdate(PidObject* pid,const float dt)
{
    float error;
    float deriv;
​
    error = pid->desired - pid->measured; //比例
    pid->integ += error * dt; //积分 
    deriv = (error - pid->prevError)/dt; //微分 
    pid->out = pid->kp * error + pid->ki * pid->integ + pid->kd * deriv;
    pid->prevError = error;  //更新误差（用于微分计算）
}

小马哥飞行器：

对于Z轴偏航角的控制：

没有对接受来的遥控器数据进行分段处理，和翻滚角以及俯仰角的处理方法相同。但是对偏航角只进行单级的角速度PID控制，而没有角度PID控制。将期望的偏航角度值，直接输入。这样一来也可以实现偏航角不随遥控器摇杆归中的效果。

特点：

没有了偏航角-角度环的控制，对偏航角PID控制仅使用角速度环，在未起飞的时候对飞机的偏航角进行改变后，起飞时飞机会按照已经改变的方向起飞；

飞机在空中飞行的时候，偏航角不像俯仰角与翻滚角，收到外界的干扰相对而言还是少一些的，所以去掉偏航角的角度环问题不大

简要代码节选：

//遥控器
Target_Angle.rol = (float)((rc_in->ROLL-1500)/12.0f);
Target_Angle.pit = (float)((rc_in->PITCH-1500)/12.0f);
Target_Angle.yaw = (float)((1500-rc_in->YAW)/12.0f); 
​
//角度环
    PID_Postion_Cal(&PID_ROL_Angle,Target_Angle.rol,Measure_Angle.rol);//ROLL角度环PID （输入角度 输出角速度）
    PID_Postion_Cal(&PID_PIT_Angle,Target_Angle.pit,Measure_Angle.pit);//PITH角度环PID （输入角度 输出角速度）
//  PID_Postion_Cal(&PID_YAW_Angle,Target_Angle.yaw,Measure_Angle.yaw);//YAW角度环PID  （输入角度 输出角速度）
    
//角速度环
PID_Postion_Cal(&PID_ROL_Rate,PID_ROL_Angle.OutPut,(gyr_in->Y*RadtoDeg)); //ROLL角速度环PID （输入角度环的输出，输出电机控制量）
PID_Postion_Cal(&PID_PIT_Rate,PID_PIT_Angle.OutPut,-(gyr_in->X*RadtoDeg)); //PITH角速度环PID （输入角度环的输出，输出电机控制量）
PID_Postion_Cal(&PID_YAW_Rate,Target_Angle.yaw*PID_YAW_Angle.P,gyr_in->Z*RadtoDeg); //YAW角速度环PID （输入角度，输出电机控制量）`
​
//PID算法
void PID_Postion_Cal(PID_TYPE*PID,float target,float measure)
{
PID->Error  = target - measure;              //误差
PID->Differ = PID->Error - PID->PreError;    //微分量
​
PID->Pout = PID->P * PID->Error;                        //比例控制
PID->Iout = PID->Ilimit_flag * PID->I * PID->Integral;  //积分控制（Ilimit_flag可以为1或者0）
PID->Dout = PID->D * PID->Differ;                       //微分控制
​
PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制
​
if(Airplane_Enable == 1&&RC_Control.THROTTLE >= 180)    //飞机解锁之后再加入积分,防止积分过调
{
    if(measure > (PID->Ilimit)||measure < -PID->Ilimit)   //积分分离
    {PID->Ilimit_flag = 0;}
    else
    {
        PID->Ilimit_flag = 1;                               //加入积分(只有测量值在-PID->Ilimit~PID->Ilimit 范围内时才加入积分)
        PID->Integral += PID->Error;                        //对误差进行积分
        if(PID->Integral > PID->Irang)                      //积分限幅
            PID->Integral = PID->Irang;
        if(PID->Integral < -PID->Irang)                     //积分限幅
            PID->Integral = -PID->Irang;                    
    }
}else
{PID->Integral = 0;}
PID->PreError = PID->Error ;                            //前一个误差值
    }