ardupilot/libraries/APM_Control/AP_PitchController.cpp

379 lines
15 KiB
C++
Raw Normal View History

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
2018-12-20 19:38:42 -04:00
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
2018-12-20 19:38:42 -04:00
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
APM_Control: ROLL and PITCH controllers These changes reduce height variation in turns and improve robustness. the specific changes are: 1) Linked roll and pitch integrator protection to the final output value so that if final output is on upper limit, the integrator is prevented from increasing and vice-versa. This improves wind-up protection. 2) Modified rate feedback in roll and pitch controllers to use body rates rather than Euler or earth rates. 3) Changed the roll to pitch compensation to use measured roll angle and estimated airspeed to calculate the component of turn rate (assuming a level coordinated turn) around the pitch axis. This a mathematically correct calculation and will work over a range of bank angles and aircraft with minimal (if any) tuning required. 4) The integrator in the roll and pitch loop is clamped when the estimated speed is below the minimum FBW speed 5) The noise filter in the pitch and roll loop has been changed to use a FOH discretisation. This gives improved noise rejection and less phase loss when compared to the previous filter that used a ZOH or equivalent discretisation. This has been flown on the rascal in the SITL and on a X-8 with limited flight testing. Initial results have been encouraging with reduced height variation in turns. Compare to standard PIDS, the revised pitch and roll controllers allow the use of rate feedback (effectively the same as the old D term) without beating the servos to death. The bank angle compensation in the pitch loop works effectively over a much larger range of bank angles and requires minimal tuning compared to the old calculation. YAW CONTROLLER Currently testing the a 3-loop acceleration autopilot topology for the yaw loop with feed forward yaw rate for turn compensation. This 3-loop topology is commonly used in tactical skid to to turn missiles and is easy to tune. The following block diagram shows the general signal flow Note that the acceleration measurement has to pass through an integrator before it gets to the actuator. This is a important feature as it eliminates problems of high frequency noise and potential coupling with structural modes associated with direct feedback of measured acceleration to actuator. The high pass filter has been inserted to compensate for airspeed and bank angle measurement errors which will cause steady state errors in the calculation of the turn yaw rate. The yaw controller flies SITL well, but hasn't been flight tested yet. It can be configured either as a simple yaw damper, or the acceleration and integral term can be turned on to allow feedback control of lateral acceleration/sideslip. TO DO: Need to reduce number of tuning parameters and provide consistent naming Need to provide guidance on tuning these loops with definitions for all the gain terms. Need to check signs and units into and out of lateral loops. DESIGN DECISIONS PENDING: 1) Can we remove the noise filters? Provided the mpu6k noise filter is running they are of limited benefit given the 25Hz Nyquist frequency 2) If we do remove them and rely on the mpu6k noise filter, what is the apprporiate default cutoff frequency for plane use. 20Hz is probably OK for most setups, but some noisy/high vibration setups would require as low as 10Hz 3) The inverted flight logic looks like a crash waiting to happen. It's problematic to test and even if implemented correctly would still crash a plane with poor inverted flight capability. We should either implement it properly and fully tested or delete it.
2013-04-23 08:02:18 -03:00
// Initial Code by Jon Challinger
// Modified by Paul Riseborough
#include <AP_HAL/AP_HAL.h>
#include "AP_PitchController.h"
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_PitchController::var_info[] = {
2013-05-31 02:31:25 -03:00
// @Param: TCONST
// @DisplayName: Pitch Time Constant
2018-12-20 19:38:42 -04:00
// @Description: Time constant in seconds from demanded to achieved pitch angle. Most models respond well to 0.5. May be reduced for faster responses, but setting lower than a model can achieve will not help.
// @Range: 0.4 1.0
// @Units: s
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("TCONST", 0, AP_PitchController, gains.tau, 0.5f),
2013-05-31 02:31:25 -03:00
// @Param: P
// @DisplayName: Proportional Gain
2018-12-20 19:38:42 -04:00
// @Description: Proportional gain from pitch angle demands to elevator. Higher values allow more servo response but can cause oscillations. Automatically set and adjusted by AUTOTUNE mode.
// @Range: 0.1 3.0
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("P", 1, AP_PitchController, gains.P, 1.0f),
2013-05-31 02:31:25 -03:00
// @Param: D
// @DisplayName: Damping Gain
2018-12-20 19:38:42 -04:00
// @Description: Damping gain from pitch acceleration to elevator. Higher values reduce pitching in turbulence, but can cause oscillations. Automatically set and adjusted by AUTOTUNE mode.
// @Range: 0 0.2
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("D", 2, AP_PitchController, gains.D, 0.04f),
2013-05-31 02:31:25 -03:00
// @Param: I
// @DisplayName: Integrator Gain
2018-12-20 19:38:42 -04:00
// @Description: Integrator gain from long-term pitch angle offsets to elevator. Higher values "trim" out offsets faster but can cause oscillations. Automatically set and adjusted by AUTOTUNE mode.
// @Range: 0 0.5
// @Increment: 0.05
// @User: Standard
AP_GROUPINFO("I", 3, AP_PitchController, gains.I, 0.3f),
2013-05-31 02:31:25 -03:00
// @Param: RMAX_UP
// @DisplayName: Pitch up max rate
2018-12-20 19:38:42 -04:00
// @Description: Maximum pitch up rate that the pitch controller demands (degrees/sec) in ACRO mode.
// @Range: 0 100
// @Units: deg/s
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("RMAX_UP", 4, AP_PitchController, gains.rmax, 0.0f),
2013-05-31 02:31:25 -03:00
// @Param: RMAX_DN
// @DisplayName: Pitch down max rate
// @Description: This sets the maximum nose down pitch rate that the controller will demand (degrees/sec). Setting it to zero disables the limit.
// @Range: 0 100
// @Units: deg/s
// @Increment: 1
// @User: Advanced
2013-05-31 02:31:25 -03:00
AP_GROUPINFO("RMAX_DN", 5, AP_PitchController, _max_rate_neg, 0.0f),
2013-05-31 02:31:25 -03:00
// @Param: RLL
2013-05-05 08:37:21 -03:00
// @DisplayName: Roll compensation
2018-12-20 19:38:42 -04:00
// @Description: Gain added to pitch to keep aircraft from descending or ascending in turns. Increase in increments of 0.05 to reduce altitude loss. Decrease for altitude gain.
2013-05-05 08:37:21 -03:00
// @Range: 0.7 1.5
// @Increment: 0.05
// @User: Standard
2013-05-31 02:31:25 -03:00
AP_GROUPINFO("RLL", 6, AP_PitchController, _roll_ff, 1.0f),
// @Param: IMAX
// @DisplayName: Integrator limit
2018-12-20 19:38:42 -04:00
// @Description: Limit of pitch integrator gain in centi-degrees of servo travel. Servos are assumed to have +/- 4500 centi-degrees of travel, so a value of 3000 allows trim of up to 2/3 of servo travel range.
// @Range: 0 4500
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("IMAX", 7, AP_PitchController, gains.imax, 3000),
// @Param: FF
// @DisplayName: Feed forward Gain
2018-12-20 19:38:42 -04:00
// @Description: Gain from demanded rate to elevator output.
// @Range: 0.1 4.0
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("FF", 8, AP_PitchController, gains.FF, 0.0f),
// @Param: SRMAX
// @DisplayName: Servo slew rate limit
// @Description: Sets an upper limit on the servo slew rate produced by the D-gain (pitch rate feedback). If the amplitude of the control action produced by the pitch rate feedback exceeds this value, then the D-gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive D-gain. The limit should be set to no more than 25% of the servo's specified slew rate to allow for inertia and aerodynamic load effects. Note: The D-gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Units: deg/s
// @Range: 0 500
// @Increment: 10.0
// @User: Advanced
AP_GROUPINFO("SRMAX", 9, AP_PitchController, _slew_rate_max, 150.0f),
// @Param: SRTAU
// @DisplayName: Servo slew rate decay time constant
// @Description: This sets the time constant used to recover the D gain after it has been reduced due to excessive servo slew rate.
// @Units: s
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("SRTAU", 10, AP_PitchController, _slew_rate_tau, 1.0f),
AP_GROUPEND
};
/*
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 rate in degrees/second
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_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed)
{
uint32_t tnow = AP_HAL::millis();
uint32_t dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) {
dt = 0;
}
_last_t = tnow;
float delta_time = (float)dt * 0.001f;
// Get body rate vector (radians/sec)
float omega_y = _ahrs.get_gyro().y;
// Calculate the pitch rate error (deg/sec) and scale
float achieved_rate = ToDeg(omega_y);
_pid_info.error = desired_rate - achieved_rate;
float rate_error = _pid_info.error * scaler;
_pid_info.target = desired_rate;
_pid_info.actual = achieved_rate;
// Multiply pitch rate error by _ki_rate and integrate
// Scaler is applied before integrator so that integrator state relates directly to elevator deflection
// This means elevator trim offset doesn't change as the value of scaler changes with airspeed
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
if (!disable_integrator && gains.I > 0) {
float k_I = gains.I;
if (is_zero(gains.FF)) {
/*
if the user hasn't set a direct FF then assume they are
not doing sophisticated tuning. Set a minimum I value of
0.15 to ensure that the time constant for trimming in
pitch is not too long. We have had a lot of user issues
with very small I value leading to very slow pitch
trimming, which causes a lot of problems for TECS. A
value of 0.15 is still quite small, but a lot better
than what many users are running.
*/
k_I = MAX(k_I, 0.15f);
}
float ki_rate = k_I * gains.tau;
//only integrate if gain and time step are positive and airspeed above min value.
if (dt > 0 && aspeed > 0.5f*float(aparm.airspeed_min)) {
float integrator_delta = rate_error * ki_rate * delta_time * scaler;
if (_last_out < -45) {
// prevent the integrator from increasing if surface defln demand is above the upper limit
integrator_delta = MAX(integrator_delta , 0);
} else if (_last_out > 45) {
// prevent the integrator from decreasing if surface defln demand is below the lower limit
integrator_delta = MIN(integrator_delta , 0);
}
_pid_info.I += integrator_delta;
}
} else {
_pid_info.I = 0;
}
// Scale the integration limit
float intLimScaled = gains.imax * 0.01f;
// Constrain the integrator state
_pid_info.I = constrain_float(_pid_info.I, -intLimScaled, intLimScaled);
// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
// No conversion is required for K_D
float eas2tas = _ahrs.get_EAS2TAS();
float kp_ff = MAX((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / eas2tas;
float k_ff = gains.FF / eas2tas;
// Calculate the demanded control surface deflection
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
// path, but want a 1/speed^2 scaler applied to the rate error path.
// This is because acceleration scales with speed^2, but rate scales with speed.
_pid_info.P = desired_rate * kp_ff * scaler;
_pid_info.FF = desired_rate * k_ff * scaler;
_pid_info.D = rate_error * gains.D * scaler;
if (dt > 0 && _slew_rate_max > 0) {
// Calculate the slew rate amplitude produced by the unmodified D term
// calculate a low pass filtered slew rate
2020-10-11 21:25:33 -03:00
float Dterm_slew_rate = _slew_rate_filter.apply((fabsf(_pid_info.D - _last_pid_info_D)/ delta_time), delta_time);
// rectify and apply a decaying envelope filter
float alpha = 1.0f - constrain_float(delta_time/_slew_rate_tau, 0.0f , 1.0f);
_slew_rate_amplitude = fmaxf(fabsf(Dterm_slew_rate), alpha * _slew_rate_amplitude);
_slew_rate_amplitude = fminf(_slew_rate_amplitude, 10.0f*_slew_rate_max);
// Calculate and apply the D gain adjustment
_pid_info.Dmod = _D_gain_modifier = _slew_rate_max / fmaxf(_slew_rate_amplitude, _slew_rate_max);
_pid_info.D *= _D_gain_modifier;
}
_last_pid_info_D = _pid_info.D;
_last_out = _pid_info.D + _pid_info.FF + _pid_info.P;
if (autotune.running && aspeed > aparm.airspeed_min) {
// let autotune have a go at the values
// Note that we don't pass the integrator component so we get
// a better idea of how much the base PD controller
// contributed
autotune.update(desired_rate, achieved_rate, _last_out);
// set down rate to rate up when auto-tuning
_max_rate_neg.set_and_save_ifchanged(gains.rmax);
}
_last_out += _pid_info.I;
/*
when we are past the users defined roll limit for the
aircraft our priority should be to bring the aircraft back
within the roll limit. Using elevator for pitch control at
large roll angles is ineffective, and can be counter
productive as it induces earth-frame yaw which can reduce
the ability to roll. We linearly reduce elevator input when
beyond the configured roll limit, reducing to zero at 90
degrees
*/
float roll_wrapped = labs(_ahrs.roll_sensor);
if (roll_wrapped > 9000) {
roll_wrapped = 18000 - roll_wrapped;
}
if (roll_wrapped > aparm.roll_limit_cd + 500 && aparm.roll_limit_cd < 8500 &&
labs(_ahrs.pitch_sensor) < 7000) {
float roll_prop = (roll_wrapped - (aparm.roll_limit_cd+500)) / (float)(9000 - aparm.roll_limit_cd);
_last_out *= (1 - roll_prop);
}
// Convert to centi-degrees and constrain
return constrain_float(_last_out * 100, -4500, 4500);
}
/*
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 rate in degrees/second
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_rate_out(float desired_rate, float scaler)
{
float aspeed;
if (!_ahrs.airspeed_estimate(aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
}
return _get_rate_out(desired_rate, scaler, false, aspeed);
}
/*
get the rate offset in degrees/second needed for pitch in body frame
to maintain height in a coordinated turn.
2018-12-20 19:38:42 -04:00
Also returns the inverted flag and the estimated airspeed in m/s for
use by the rest of the pitch controller
*/
float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inverted) const
{
float rate_offset;
float bank_angle = _ahrs.roll;
// limit bank angle between +- 80 deg if right way up
if (fabsf(bank_angle) < radians(90)) {
bank_angle = constrain_float(bank_angle,-radians(80),radians(80));
inverted = false;
} else {
inverted = true;
if (bank_angle > 0.0f) {
bank_angle = constrain_float(bank_angle,radians(100),radians(180));
} else {
bank_angle = constrain_float(bank_angle,-radians(180),-radians(100));
}
}
if (!_ahrs.airspeed_estimate(aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
}
if (abs(_ahrs.pitch_sensor) > 7000) {
// don't do turn coordination handling when at very high pitch angles
rate_offset = 0;
} else {
rate_offset = cosf(_ahrs.pitch)*fabsf(ToDeg((GRAVITY_MSS / MAX((aspeed * _ahrs.get_EAS2TAS()) , MAX(aparm.airspeed_min, 1))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
}
if (inverted) {
rate_offset = -rate_offset;
}
return rate_offset;
}
// 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);
APM_Control: ROLL and PITCH controllers These changes reduce height variation in turns and improve robustness. the specific changes are: 1) Linked roll and pitch integrator protection to the final output value so that if final output is on upper limit, the integrator is prevented from increasing and vice-versa. This improves wind-up protection. 2) Modified rate feedback in roll and pitch controllers to use body rates rather than Euler or earth rates. 3) Changed the roll to pitch compensation to use measured roll angle and estimated airspeed to calculate the component of turn rate (assuming a level coordinated turn) around the pitch axis. This a mathematically correct calculation and will work over a range of bank angles and aircraft with minimal (if any) tuning required. 4) The integrator in the roll and pitch loop is clamped when the estimated speed is below the minimum FBW speed 5) The noise filter in the pitch and roll loop has been changed to use a FOH discretisation. This gives improved noise rejection and less phase loss when compared to the previous filter that used a ZOH or equivalent discretisation. This has been flown on the rascal in the SITL and on a X-8 with limited flight testing. Initial results have been encouraging with reduced height variation in turns. Compare to standard PIDS, the revised pitch and roll controllers allow the use of rate feedback (effectively the same as the old D term) without beating the servos to death. The bank angle compensation in the pitch loop works effectively over a much larger range of bank angles and requires minimal tuning compared to the old calculation. YAW CONTROLLER Currently testing the a 3-loop acceleration autopilot topology for the yaw loop with feed forward yaw rate for turn compensation. This 3-loop topology is commonly used in tactical skid to to turn missiles and is easy to tune. The following block diagram shows the general signal flow Note that the acceleration measurement has to pass through an integrator before it gets to the actuator. This is a important feature as it eliminates problems of high frequency noise and potential coupling with structural modes associated with direct feedback of measured acceleration to actuator. The high pass filter has been inserted to compensate for airspeed and bank angle measurement errors which will cause steady state errors in the calculation of the turn yaw rate. The yaw controller flies SITL well, but hasn't been flight tested yet. It can be configured either as a simple yaw damper, or the acceleration and integral term can be turned on to allow feedback control of lateral acceleration/sideslip. TO DO: Need to reduce number of tuning parameters and provide consistent naming Need to provide guidance on tuning these loops with definitions for all the gain terms. Need to check signs and units into and out of lateral loops. DESIGN DECISIONS PENDING: 1) Can we remove the noise filters? Provided the mpu6k noise filter is running they are of limited benefit given the 25Hz Nyquist frequency 2) If we do remove them and rely on the mpu6k noise filter, what is the apprporiate default cutoff frequency for plane use. 20Hz is probably OK for most setups, but some noisy/high vibration setups would require as low as 10Hz 3) The inverted flight logic looks like a crash waiting to happen. It's problematic to test and even if implemented correctly would still crash a plane with poor inverted flight capability. We should either implement it properly and fully tested or delete it.
2013-04-23 08:02:18 -03:00
// 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);
}
void AP_PitchController::reset_I()
{
_pid_info.I = 0;
}