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

144
ArduCopter/Attitude.pde
View File

@ -424,30 +424,34 @@ void init_rate_controllers()
static int16_t
get_heli_rate_roll(int32_t target_rate)
{
int32_t p,i,d,ff; // used to capture pid values for logging
int32_t current_rate; // this iteration's rate
int32_t rate_error; // simply target_rate - current_rate
int32_t output; // output from pid controller
int32_t p,i,d,ff; // used to capture pid values for logging
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);
rate_error = target_rate - current_rate;
p = g.pid_rate_roll.get_p(rate_error);
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);
} else {
i = g.pid_rate_roll.get_integrator();
}
} 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
}
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);
} else {
i = g.pid_rate_roll.get_integrator();
}
} 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,37 +477,40 @@ get_heli_rate_roll(int32_t target_rate)
}
#endif
// output control
return output;
return output; // output control
}
static int16_t
get_heli_rate_pitch(int32_t target_rate)
{
int32_t p,i,d,ff; // used to capture pid values for logging
int32_t current_rate; // this iteration's rate
int32_t rate_error; // simply target_rate - current_rate
int32_t output; // output from pid controller
// get current rate
current_rate = (omega.y * DEGX100);
int32_t p,i,d,ff; // used to capture pid values for logging
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
current_rate = (omega.y * DEGX100); // get current rate
// filter input
current_rate = rate_pitch_filter.apply(current_rate);
rate_error = target_rate - current_rate;
p = g.pid_rate_pitch.get_p(rate_error);
// call pid controller
rate_error = target_rate - current_rate;
p = g.pid_rate_pitch.get_p(rate_error); // Helicopters get huge feed-forward
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);
} else {
i = g.pid_rate_pitch.get_integrator();
}
} 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
}
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);
} else {
i = g.pid_rate_pitch.get_integrator();
}
} 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,9 +518,13 @@ 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
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_ROLL_PITCH_KP || g.radio_tuning == CH6_RATE_ROLL_PITCH_KI || g.radio_tuning == CH6_RATE_ROLL_PITCH_KD) ) {
@ -519,38 +534,44 @@ get_heli_rate_pitch(int32_t target_rate)
}
#endif
// output control
return output;
return output; // output control
}
static int16_t
get_heli_rate_yaw(int32_t target_rate)
{
int32_t p,i,d,ff; // used to capture pid values for logging
int32_t current_rate; // this iteration's rate
int32_t rate_error;
int32_t output;
int32_t p,i,d,ff; // used to capture pid values for logging
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;
rate_error = target_rate - current_rate;
// separately calculate p, i, d values for logging
p = g.pid_rate_yaw.get_p(rate_error);
i = g.pid_rate_yaw.get_i(rate_error, G_Dt);
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);
output = p + i + d + ff;
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