mirror of https://github.com/ArduPilot/ardupilot
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:
parent
a8970ee509
commit
cea0812b82
|
@ -428,17 +428,20 @@ get_heli_rate_roll(int32_t target_rate)
|
||||||
int32_t current_rate; // this iteration's rate
|
int32_t current_rate; // this iteration's rate
|
||||||
int32_t rate_error; // simply target_rate - current_rate
|
int32_t rate_error; // simply target_rate - current_rate
|
||||||
int32_t output; // output from pid controller
|
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); // get current rate
|
||||||
current_rate = (omega.x * DEGX100);
|
|
||||||
|
|
||||||
// filter input
|
// filter input
|
||||||
current_rate = rate_roll_filter.apply(current_rate);
|
current_rate = rate_roll_filter.apply(current_rate);
|
||||||
|
|
||||||
// call pid controller
|
|
||||||
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);
|
||||||
|
|
||||||
|
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 (motors.flybar_mode == 1) { // Mechanical Flybars get regular integral for rate auto trim
|
||||||
if (target_rate > -50 && target_rate < 50){ // Frozen at high rates
|
if (target_rate > -50 && target_rate < 50){ // Frozen at high rates
|
||||||
i = g.pid_rate_roll.get_i(rate_error, G_Dt);
|
i = g.pid_rate_roll.get_i(rate_error, G_Dt);
|
||||||
|
@ -448,6 +451,7 @@ get_heli_rate_roll(int32_t target_rate)
|
||||||
} else {
|
} 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
|
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);
|
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;
|
output = p + i + d + ff;
|
||||||
|
|
||||||
// constrain output
|
if (labs(output) > 4500){
|
||||||
output = constrain_int32(output, -4500, 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
|
#if LOGGING_ENABLED == ENABLED
|
||||||
// log output if PID logging is on and we are tuning the rate P, I or D gains
|
// 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
|
#endif
|
||||||
|
|
||||||
// output control
|
return output; // output control
|
||||||
return output;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static int16_t
|
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 current_rate; // this iteration's rate
|
||||||
int32_t rate_error; // simply target_rate - current_rate
|
int32_t rate_error; // simply target_rate - current_rate
|
||||||
int32_t output; // output from pid controller
|
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); // get current rate
|
||||||
current_rate = (omega.y * DEGX100);
|
|
||||||
|
|
||||||
// filter input
|
// filter input
|
||||||
current_rate = rate_pitch_filter.apply(current_rate);
|
current_rate = rate_pitch_filter.apply(current_rate);
|
||||||
|
|
||||||
// call pid controller
|
|
||||||
rate_error = target_rate - current_rate;
|
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 (motors.flybar_mode == 1) { // Mechanical Flybars get regular integral for rate auto trim
|
||||||
if (target_rate > -50 && target_rate < 50){ // Frozen at high rates
|
if (target_rate > -50 && target_rate < 50){ // Frozen at high rates
|
||||||
i = g.pid_rate_pitch.get_i(rate_error, G_Dt);
|
i = g.pid_rate_pitch.get_i(rate_error, G_Dt);
|
||||||
|
@ -500,6 +510,7 @@ get_heli_rate_pitch(int32_t target_rate)
|
||||||
} else {
|
} 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
|
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);
|
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;
|
output = p + i + d + ff;
|
||||||
|
|
||||||
// constrain output
|
if (labs(output) > 4500){
|
||||||
output = constrain_int32(output, -4500, 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
|
#if LOGGING_ENABLED == ENABLED
|
||||||
// log output if PID logging is on and we are tuning the rate P, I or D gains
|
// 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
|
#endif
|
||||||
|
|
||||||
// output control
|
return output; // output control
|
||||||
return output;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static int16_t
|
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 current_rate; // this iteration's rate
|
||||||
int32_t rate_error;
|
int32_t rate_error;
|
||||||
int32_t output;
|
int32_t output;
|
||||||
|
static bool pid_saturated; // tracker from last loop if the PID was saturated
|
||||||
|
|
||||||
// get current rate
|
current_rate = (omega.z * DEGX100); // get current rate
|
||||||
current_rate = (omega.z * DEGX100);
|
|
||||||
|
|
||||||
// filter input
|
|
||||||
// current_rate = rate_yaw_filter.apply(current_rate);
|
|
||||||
|
|
||||||
// rate control
|
// rate control
|
||||||
rate_error = target_rate - current_rate;
|
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
|
// separately calculate p, i, d values for logging
|
||||||
p = g.pid_rate_yaw.get_p(rate_error);
|
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);
|
i = g.pid_rate_yaw.get_i(rate_error, G_Dt);
|
||||||
|
}
|
||||||
|
|
||||||
d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
|
d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
|
||||||
|
|
||||||
ff = g.heli_yaw_ff*target_rate;
|
ff = g.heli_yaw_ff*target_rate;
|
||||||
|
|
||||||
output = p + i + d + ff;
|
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
|
#if LOGGING_ENABLED == ENABLED
|
||||||
// log output if PID loggins is on and we are tuning the yaw
|
// 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
|
#endif
|
||||||
|
|
||||||
// output control
|
return output; // output control
|
||||||
return output;
|
|
||||||
}
|
}
|
||||||
#endif // HELI_FRAME
|
#endif // HELI_FRAME
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue