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

TradHeli: Rate PID Improvement. Limit Integrator wind-up when PID output is saturated. Also a little bit of general clean-up for readability.

This commit is contained in:
Robert Lefebvre 2013-07-02 17:07:32 -04:00 committed by Randy Mackay
parent a8970ee509
commit cea0812b82
1 changed files with 82 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
ArduCopter/Attitude.pde
View File

@ -428,17 +428,20 @@ get_heli_rate_roll(int32_t target_rate)
int32_t current_rate; // this iteration's rate
int32_t rate_error; // simply target_rate - current_rate
int32_t output; // output from pid controller
static bool pid_saturated; // tracker from last loop if the PID was saturated
// get current rate
current_rate = (omega.x * DEGX100);
current_rate = (omega.x * DEGX100); // get current rate
// filter input
current_rate = rate_roll_filter.apply(current_rate);
// call pid controller
rate_error = target_rate - current_rate;
p = g.pid_rate_roll.get_p(rate_error);
if (pid_saturated){
i = g.pid_rate_roll.get_integrator(); // Locked Integrator due to PID saturation on previous cycle
} else {
if (motors.flybar_mode == 1) { // Mechanical Flybars get regular integral for rate auto trim
if (target_rate > -50 && target_rate < 50){ // Frozen at high rates
i = g.pid_rate_roll.get_i(rate_error, G_Dt);
@ -448,6 +451,7 @@ get_heli_rate_roll(int32_t target_rate)
} else {
i = g.pid_rate_roll.get_leaky_i(rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE); // Flybarless Helis get huge I-terms. I-term controls much of the rate
}
}
d = g.pid_rate_roll.get_d(rate_error, G_Dt);
@ -455,8 +459,12 @@ get_heli_rate_roll(int32_t target_rate)
output = p + i + d + ff;
// constrain output
output = constrain_int32(output, -4500, 4500);
if (labs(output) > 4500){
output = constrain_int32(output, -4500, 4500); // constrain output
pid_saturated = true; // freeze integrator next cycle
} else {
pid_saturated = false; // unfreeze integrator
}
#if LOGGING_ENABLED == ENABLED
// log output if PID logging is on and we are tuning the rate P, I or D gains
@ -469,8 +477,7 @@ get_heli_rate_roll(int32_t target_rate)
}
#endif
// output control
return output;
return output; // output control
}
static int16_t
@ -480,17 +487,20 @@ get_heli_rate_pitch(int32_t target_rate)
int32_t current_rate; // this iteration's rate
int32_t rate_error; // simply target_rate - current_rate
int32_t output; // output from pid controller
static bool pid_saturated; // tracker from last loop if the PID was saturated
// get current rate
current_rate = (omega.y * DEGX100);
current_rate = (omega.y * DEGX100); // get current rate
// filter input
current_rate = rate_pitch_filter.apply(current_rate);
// call pid controller
rate_error = target_rate - current_rate;
p = g.pid_rate_pitch.get_p(rate_error); // Helicopters get huge feed-forward
p = g.pid_rate_pitch.get_p(rate_error);
if (pid_saturated){
i = g.pid_rate_pitch.get_integrator(); // Locked Integrator due to PID saturation on previous cycle
} else {
if (motors.flybar_mode == 1) { // Mechanical Flybars get regular integral for rate auto trim
if (target_rate > -50 && target_rate < 50){ // Frozen at high rates
i = g.pid_rate_pitch.get_i(rate_error, G_Dt);
@ -500,6 +510,7 @@ get_heli_rate_pitch(int32_t target_rate)
} else {
i = g.pid_rate_pitch.get_leaky_i(rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE); // Flybarless Helis get huge I-terms. I-term controls much of the rate
}
}
d = g.pid_rate_pitch.get_d(rate_error, G_Dt);
@ -507,8 +518,12 @@ get_heli_rate_pitch(int32_t target_rate)
output = p + i + d + ff;
// constrain output
output = constrain_int32(output, -4500, 4500);
if (labs(output) > 4500){
output = constrain_int32(output, -4500, 4500); // constrain output
pid_saturated = true; // freeze integrator next cycle
} else {
pid_saturated = false; // unfreeze integrator
}
#if LOGGING_ENABLED == ENABLED
// log output if PID logging is on and we are tuning the rate P, I or D gains
@ -519,8 +534,7 @@ get_heli_rate_pitch(int32_t target_rate)
}
#endif
// output control
return output;
return output; // output control
}
static int16_t
@ -530,12 +544,9 @@ get_heli_rate_yaw(int32_t target_rate)
int32_t current_rate; // this iteration's rate
int32_t rate_error;
int32_t output;
static bool pid_saturated; // tracker from last loop if the PID was saturated
// get current rate
current_rate = (omega.z * DEGX100);
// filter input
// current_rate = rate_yaw_filter.apply(current_rate);
current_rate = (omega.z * DEGX100); // get current rate
// rate control
rate_error = target_rate - current_rate;
@ -543,14 +554,24 @@ get_heli_rate_yaw(int32_t target_rate)
// separately calculate p, i, d values for logging
p = g.pid_rate_yaw.get_p(rate_error);
if (pid_saturated){
i = g.pid_rate_yaw.get_integrator(); // Locked Integrator due to PID saturation on previous cycle
} else {
i = g.pid_rate_yaw.get_i(rate_error, G_Dt);
}
d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
ff = g.heli_yaw_ff*target_rate;
output = p + i + d + ff;
output = constrain_float(output, -4500, 4500);
if (labs(output) > 4500){
output = constrain_int32(output, -4500, 4500); // constrain output
pid_saturated = true; // freeze integrator next cycle
} else {
pid_saturated = false; // unfreeze integrator
}
#if LOGGING_ENABLED == ENABLED
// log output if PID loggins is on and we are tuning the yaw
@ -564,8 +585,7 @@ get_heli_rate_yaw(int32_t target_rate)
#endif
// output control
return output;
return output; // output control
}
#endif // HELI_FRAME