ardupilot/libraries/APM_Control/AP_RollController.cpp

219 lines
7.9 KiB
C++

/*
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.
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.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
// Code by Jon Challinger
// Modified by Paul Riseborough
//
#include <AP_HAL/AP_HAL.h>
#include "AP_RollController.h"
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_RollController::var_info[] = {
// @Param: TCONST
// @DisplayName: Roll Time Constant
// @Description: Time constant in seconds from demanded to achieved roll 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_RollController, gains.tau, 0.5f),
// @Param: P
// @DisplayName: Proportional Gain
// @Description: Proportional gain from roll angle demands to ailerons. Higher values allow more servo response but can cause oscillations. Automatically set and adjusted by AUTOTUNE mode.
// @Range: 0.1 4.0
// @Increment: 0.1
// @User: User
AP_GROUPINFO("P", 1, AP_RollController, gains.P, 1.0f),
// @Param: D
// @DisplayName: Damping Gain
// @Description: Damping gain from roll acceleration to ailerons. Higher values reduce rolling in turbulence, but can cause oscillations. Automatically set and adjusted by AUTOTUNE mode.
// @Range: 0 0.2
// @Increment: 0.01
// @User: User
AP_GROUPINFO("D", 2, AP_RollController, gains.D, 0.08f),
// @Param: I
// @DisplayName: Integrator Gain
// @Description: Integrator gain from long-term roll angle offsets to ailerons. Higher values "trim" out offsets faster but can cause oscillations. Automatically set and adjusted by AUTOTUNE mode.
// @Range: 0 1.0
// @Increment: 0.05
// @User: User
AP_GROUPINFO("I", 3, AP_RollController, gains.I, 0.3f),
// @Param: RMAX
// @DisplayName: Maximum Roll Rate
// @Description: Maximum roll rate that the roll controller demands (degrees/sec) in ACRO mode.
// @Range: 0 180
// @Units: deg/s
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("RMAX", 4, AP_RollController, gains.rmax, 0),
// @Param: IMAX
// @DisplayName: Integrator limit
// @Description: Limit of roll 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", 5, AP_RollController, gains.imax, 3000),
// @Param: FF
// @DisplayName: Feed forward Gain
// @Description: Gain from demanded rate to aileron output.
// @Range: 0.1 4.0
// @Increment: 0.1
// @User: User
AP_GROUPINFO("FF", 6, AP_RollController, gains.FF, 0.0f),
AP_GROUPEND
};
/*
internal rate controller, called by attitude and rate controller
public functions
*/
int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator)
{
uint32_t tnow = AP_HAL::millis();
uint32_t dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) {
dt = 0;
}
_last_t = tnow;
// 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 ki_rate = gains.I * gains.tau;
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;
float delta_time = (float)dt * 0.001f;
// Get body rate vector (radians/sec)
float omega_x = _ahrs.get_gyro().x;
// Calculate the roll rate error (deg/sec) and apply gain scaler
float achieved_rate = ToDeg(omega_x);
float rate_error = (desired_rate - achieved_rate) * scaler;
// Get an airspeed estimate - default to zero if none available
float aspeed;
if (!_ahrs.airspeed_estimate(&aspeed)) {
aspeed = 0.0f;
}
// Multiply roll rate error by _ki_rate, apply scaler and integrate
// Scaler is applied before integrator so that integrator state relates directly to aileron deflection
// This means aileron 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 && ki_rate > 0) {
//only integrate if gain and time step are positive and airspeed above min value.
if (dt > 0 && aspeed > float(aparm.airspeed_min)) {
float integrator_delta = rate_error * ki_rate * delta_time * scaler;
// prevent the integrator from increasing if surface defln demand is above the upper limit
if (_last_out < -45) {
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 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.D = rate_error * gains.D * scaler;
_pid_info.P = desired_rate * kp_ff * scaler;
_pid_info.FF = desired_rate * k_ff * scaler;
_pid_info.target = desired_rate;
_pid_info.actual = achieved_rate;
_last_out = _pid_info.FF + _pid_info.P + _pid_info.D;
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);
}
_last_out += _pid_info.I;
// 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) desired roll rate in degrees/sec
2) control gain scaler = scaling_speed / aspeed
*/
int32_t AP_RollController::get_rate_out(float desired_rate, float scaler)
{
return _get_rate_out(desired_rate, scaler, false);
}
/*
Function returns an equivalent aileron deflection in centi-degrees in the range from -4500 to 4500
A positive demand is up
Inputs are:
1) demanded bank 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)
*/
int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator)
{
if (gains.tau < 0.1f) {
gains.tau.set(0.1f);
}
// Calculate the desired roll rate (deg/sec) from the angle error
float desired_rate = angle_err * 0.01f / gains.tau;
// Limit the demanded roll rate
if (gains.rmax && desired_rate < -gains.rmax) {
desired_rate = - gains.rmax;
} else if (gains.rmax && desired_rate > gains.rmax) {
desired_rate = gains.rmax;
}
return _get_rate_out(desired_rate, scaler, disable_integrator);
}
void AP_RollController::reset_I()
{
_pid_info.I = 0;
}