Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: prevent overflow on large PID gains

Browse Source 
this uses get_pid_4500() to prevent overflow of servo_out on large PID gains
This commit is contained in:
Andrew Tridgell 2013-04-01 22:10:15 +11:00
parent 6123ea2dac
commit e390726bdd
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduPlane/Attitude.pde
View File

@ -78,7 +78,7 @@ static void stabilize_roll(float speed_scaler)
#if APM_CONTROL == DISABLED
// Calculate dersired servo output for the roll
// ---------------------------------------------
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll_cd - ahrs.roll_sensor), speed_scaler);
g.channel_roll.servo_out = g.pidServoRoll.get_pid_4500((nav_roll_cd - ahrs.roll_sensor), speed_scaler);
#else // APM_CONTROL == ENABLED
// calculate roll and pitch control using new APM_Control library
g.channel_roll.servo_out = g.rollController.get_servo_out(nav_roll_cd, speed_scaler, control_mode == STABILIZE);
@ -101,7 +101,7 @@ static void stabilize_pitch(float speed_scaler)
// when flying upside down the elevator control is inverted
tempcalc = -tempcalc;
}
g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, speed_scaler);
g.channel_pitch.servo_out = g.pidServoPitch.get_pid_4500(tempcalc, speed_scaler);
#else // APM_CONTROL == ENABLED
g.channel_pitch.servo_out = g.pitchController.get_servo_out(nav_pitch_cd, speed_scaler, control_mode == STABILIZE);
#endif
@ -281,8 +281,9 @@ static void calc_nav_yaw(float speed_scaler, float ch4_inf)
{
if (hold_course != -1) {
// steering on or close to ground
g.channel_rudder.servo_out = g.pidWheelSteer.get_pid(bearing_error_cd, speed_scaler) +
g.channel_rudder.servo_out = g.pidWheelSteer.get_pid_4500(bearing_error_cd, speed_scaler) +
g.kff_rudder_mix * g.channel_roll.servo_out;
g.channel_rudder.servo_out = constrain_int16(g.channel_rudder.servo_out, -4500, 4500);
return;
}
@ -294,7 +295,7 @@ static void calc_nav_yaw(float speed_scaler, float ch4_inf)
Vector3f temp = ins.get_accel();
int32_t error = -temp.y*100.0;
g.channel_rudder.servo_out += g.pidServoRudder.get_pid(error, speed_scaler);
g.channel_rudder.servo_out += g.pidServoRudder.get_pid_4500(error, speed_scaler);
#else // APM_CONTROL == ENABLED
// use the new APM_Control library
g.channel_rudder.servo_out = g.yawController.get_servo_out(speed_scaler, ch4_inf < 0.25f) + g.channel_roll.servo_out * g.kff_rudder_mix;