Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: yaw limit fix to allow I to reduce 

We now allow the I term even if we've hit the yaw limits as long as
updating the I term will reduce it
This commit is contained in:
Randy Mackay 2013-07-05 13:49:45 -10:00 committed by rmackay9
parent 073e2c9f15
commit 40612ed551
1 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/Attitude.pde
View File

@ -522,12 +522,16 @@ get_rate_yaw(int32_t target_rate)
// separately calculate p, i, d values for logging
p = g.pid_rate_yaw.get_p(rate_error);
// freeze I term if we've breached yaw limits
if( motors.reached_limit(AP_MOTOR_YAW_LIMIT) ) {
i = g.pid_rate_yaw.get_integrator();
}else{
// get i term
i = g.pid_rate_yaw.get_integrator();
// 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_YAW_LIMIT) || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
i = g.pid_rate_yaw.get_i(rate_error, G_Dt);
}
// get d value
d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
output = p+i+d;