Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: roll-pitch fix to allow I to reduce

Browse Source 
Contributed by Leonard Hall
This commit is contained in:
Randy Mackay 2013-07-10 15:12:04 +09:00
parent d11e5497a4
commit 352a25e53f
1 changed files with 13 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/Attitude.pde
View File

@ -443,11 +443,12 @@ get_rate_roll(int32_t target_rate)
rate_error = target_rate - current_rate; rate_error = target_rate - current_rate;
p = g.pid_rate_roll.get_p(rate_error); p = g.pid_rate_roll.get_p(rate_error);
// freeze I term if we've breached roll-pitch limits // get i term
if( motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) ) { i = g.pid_rate_roll.get_integrator();
i = g.pid_rate_roll.get_integrator();
}else{ // update i term as long as we haven't breached the limits or the I term will certainly reduce
i = g.pid_rate_roll.get_i(rate_error, G_Dt); if (!motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
i = g.pid_rate_roll.get_i(rate_error, G_Dt);
} }
d = g.pid_rate_roll.get_d(rate_error, G_Dt); d = g.pid_rate_roll.get_d(rate_error, G_Dt);
@ -485,12 +486,15 @@ get_rate_pitch(int32_t target_rate)
// call pid controller // call pid controller
rate_error = target_rate - current_rate; rate_error = target_rate - current_rate;
p = g.pid_rate_pitch.get_p(rate_error); p = g.pid_rate_pitch.get_p(rate_error);
// freeze I term if we've breached roll-pitch limits
if( motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) ) { // get i term
i = g.pid_rate_pitch.get_integrator(); i = g.pid_rate_pitch.get_integrator();
}else{
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
i = g.pid_rate_pitch.get_i(rate_error, G_Dt); i = g.pid_rate_pitch.get_i(rate_error, G_Dt);
} }
d = g.pid_rate_pitch.get_d(rate_error, G_Dt); d = g.pid_rate_pitch.get_d(rate_error, G_Dt);
output = p + i + d; output = p + i + d;