//函数名：CtrlMotor()
//输入：无
//输出: 4个电机的PWM输出
//描述：输出PWM，控制电机，本函数会被主循环中100Hz循环调用
void CtrlMotor(void)
{
float cosTilt = imu.accb[2] / ONE_G;
if(altCtrlMode==MANUAL)
{
DIF_ACC.Z = imu.accb[2] - ONE_G;
Thro = RC_DATA.THROTTLE;
cosTilt=imu.DCMgb[2][2];
Thro=Thro/cosTilt;
}else{
Thro=(-thrustZSp) * 1000;// /imu.DCMgb[2][2]; //倾角补偿后效果不错，有时过猛
if(Thro>1000)
Thro=1000;
}
//将输出值融合到四个电机
Motor[2] = (int16_t)(Thro - Pitch - Roll - Yaw ); //M3
Motor[0] = (int16_t)(Thro + Pitch + Roll - Yaw ); //M1
Motor[3] = (int16_t)(Thro - Pitch + Roll + Yaw ); //M4
Motor[1] = (int16_t)(Thro + Pitch - Roll + Yaw ); //M2
if((FLY_ENABLE!=0))
MotorPwmFlash(Motor[0],Motor[1],Motor[2],Motor[3]);
else
MotorPwmFlash(0,0,0,0);
}
//copter crash down
void FailSafeCrash(void)
{
if(fabs(imu.pitch)>80 || fabs(imu.roll)>80 )
{
// RC_DATA.THROTTLE=LAND_THRO;
MotorPwmFlash(0,0,0,0);
FLY_ENABLE=0;
}
}
//函数名：CtrlMotor()
//输入：无
//输出: 4个电机的PWM输出
//描述：输出PWM，控制电机，本函数会被主循环中100Hz循环调用
void CtrlMotor(void)
{
static float thrAngCorrect; //对倾斜做修正
float cosTilt = imu.accb[2] / ONE_G;
if(altCtrlMode==MANUAL)
{
DIF_ACC.Z = imu.accb[2] - ONE_G;
Thro = RC_DATA.THROTTLE;
// Thr = Thr/(cos) ; //对Z轴用一次负反馈控制
//way1
//thrAngCorrect = ANG_COR_COEF * (1-cosTilt) ;
//Thr += thrAngCorrect; //采用气压定高时，不用此修正。
//way2
cosTilt=imu.DCMgb[2][2];
Thro=Thro/cosTilt;
//way3
//thrAngCorrect=THR_HOLD_LEVEL * (1.0f/cosTilt - 1.0);
//Thro += thrAngCorrect;
}else{
Thro=(-thrustZSp) * 1000;// /imu.DCMgb[2][2]; //倾角补偿后效果不错，有时过猛
if(Thro>1000)
Thro=1000;
}
//将输出值融合到四个电机
Motor[2] = (int16_t)(Thro - Pitch - Roll - Yaw ); //M3
Motor[0] = (int16_t)(Thro + Pitch + Roll - Yaw ); //M1
Motor[3] = (int16_t)(Thro - Pitch + Roll + Yaw ); //M4
Motor[1] = (int16_t)(Thro + Pitch - Roll + Yaw ); //M2
if((FLY_ENABLE!=0))
MotorPwmFlash(Motor[0],Motor[1],Motor[2],Motor[3]);
else
MotorPwmFlash(0,0,0,0);
}
//函数名：PID_Calculate()
//输入：无
//输出: 无
//描述：飞机的自稳PID实现函数
//作者：马骏
//备注：没考上研，心情不好
void PID_Calculate(void)
{
static float Thr=0,Rool=0,Pitch=0,Yaw=0;
// static float g_init = DMP_DATA.dmp_accz; //当地重力加速度变量
long Motor[4]; //定义电机PWM数组，分别对应M1-M4
/*********************************************************
计算期望姿态与实际姿态的差值
*********************************************************/
EXP_ANGLE.X = (float)(RC_DATA.ROOL);
EXP_ANGLE.Y = (float)(RC_DATA.PITCH);
EXP_ANGLE.Z = (float)(RC_DATA.YAW);
DIF_ANGLE.X = EXP_ANGLE.X - Q_ANGLE.Roll;
DIF_ANGLE.X = DIF_ANGLE.X;
DIF_ANGLE.Y = EXP_ANGLE.Y - Q_ANGLE.Pitch;
DIF_ANGLE.Y = DIF_ANGLE.Y;
DIF_ACC.Z = DMP_DATA.dmp_accz - g; //Z 轴加速度实际与静止时的差值，g为当地重力加速度,初始化时采样
/*********************************************************
PID核心算法部分
*********************************************************/
//------------俯仰控制------------
//参数整定原则为先内后外，故在整定内环时将外环的PID均设为0
//外环控 制。输入为角度,输出为角速度。PID->Output作为内环的输入。
PID_Postion_Cal(&pitch_angle_PID,EXP_ANGLE.Y,Q_ANGLE.Pitch,0);
//内环控制，输入为角速度，输出为PWM增量
PID_Postion_Cal(&pitch_rate_PID,pitch_angle_PID.Output,DMP_DATA.GYROy,0);
//参数整定原则为先内后外，故在整定内环时将外环的PID均设为0
//外环控 制。输入为角度,输出为角速度。PID->Output作为内环的输入。
PID_Postion_Cal(&roll_angle_PID,EXP_ANGLE.X,Q_ANGLE.Roll,0);
//内环控制，输入为角速度，输出为PWM增量
PID_Postion_Cal(&roll_rate_PID,roll_angle_PID.Output,DMP_DATA.GYROx,0);
//参数整定原则为先内后外，故在整定内环时将外环的PID均设为0
//外环控 制。输入为角度,输出为角速度。PID->Output作为内环的输入。
PID_Postion_Cal(&yaw_angle_PID,EXP_ANGLE.Z,Q_ANGLE.Yaw,0);
//内环控制，输入为角速度，输出为PWM增量
PID_Postion_Cal(&yaw_rate_PID,-2*EXP_ANGLE.Z,DMP_DATA.GYROz,0);
//参数整定原则为先内后外，故在整定内环时将外环的PID均设为0
//基础油门动力
//Thr = 0.001*RC_DATA.THROTTLE*RC_DATA.THROTTLE; //RC_DATA.THROTTLE为0到1000,将摇杆油门曲线转换为下凹的抛物线
Thr = RC_DATA.THROTTLE;
Thr -=100*DIF_ACC.Z; //对Z轴用一次负反馈控制
Pitch = pitch_rate_PID.Output;
Rool = roll_rate_PID.Output;
Yaw = yaw_rate_PID.Output;
//将输出值融合到四个电机
Motor[2] = (int16_t)(Thr - Pitch -Rool- Yaw ); //M3
Motor[0] = (int16_t)(Thr + Pitch +Rool- Yaw ); //M1
Motor[3] = (int16_t)(Thr - Pitch +Rool+ Yaw ); //M4
Motor[1] = (int16_t)(Thr + Pitch -Rool+ Yaw ); //M2
if((FLY_ENABLE==0xA5))MotorPwmFlash(Motor[0],Motor[1],Motor[2],Motor[3]);
else MotorPwmFlash(0,0,0,0);//避免飞机落地重启时突然打转
if(NRF24L01_RXDATA[10]==0xA5) MotorPwmFlash(5,5,Motor[2],Motor[3]); //一键操作，翻滚返航等，测试功能，不要用
}
