// Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
// A positive demand is up
// Inputs are:
// 1) demanded pitch angle in centi-degrees
// 2) control gain scaler = scaling_speed / aspeed
// 3) boolean which is true when stabilise mode is active
// 4) minimum FBW airspeed (metres/sec)
// 5) maximum FBW airspeed (metres/sec)
//
int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator)
{
// Calculate offset to pitch rate demand required to maintain pitch angle whilst banking
// Calculate ideal turn rate from bank angle and airspeed assuming a level coordinated turn
// Pitch rate offset is the component of turn rate about the pitch axis
float aspeed;
float rate_offset;
bool inverted;
if (gains.tau < 0.1f) {
gains.tau.set(0.1f);
}
rate_offset = _get_coordination_rate_offset(aspeed, inverted);
// Calculate the desired pitch rate (deg/sec) from the angle error
float desired_rate = angle_err * 0.01f / gains.tau;
// limit the maximum pitch rate demand. Don't apply when inverted
// as the rates will be tuned when upright, and it is common that
// much higher rates are needed inverted
if (!inverted) {
if (_max_rate_neg && desired_rate < -_max_rate_neg) {
desired_rate = -_max_rate_neg;
} else if (gains.rmax && desired_rate > gains.rmax) {
desired_rate = gains.rmax;
}
}
if (inverted) {
desired_rate = -desired_rate;
}
// Apply the turn correction offset
desired_rate = desired_rate + rate_offset;
return _get_rate_out(desired_rate, scaler, disable_integrator, aspeed);
}
/*
get the rate offset in degrees/second needed for pitch in body frame
to maintain height in a coordinated turn.
*/
float AP_PitchController::get_coordination_rate_offset(void) const
{
float aspeed;
bool inverted;
return _get_coordination_rate_offset(aspeed, inverted);
}
