//函数名: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]); //一键操作,翻滚返航等,测试功能,不要用 }