mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-02 10:23:25 +08:00
Peter Barker
da9ecf550a
AP_RollController::convert_pid() and AP_PitchController::convert_pid() converted the old RLL2SRV_/PTCH2SRV_ gains into the AC_PID form. Added Apr-2021 and described in the code as "a temporary conversion function during development"; present in the 4.3.0 release, so anybody running 4.3.0 or later has already had it applied.
248 lines
8.8 KiB
C++
248 lines
8.8 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"
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <AP_Scheduler/AP_Scheduler.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
const AP_Param::GroupInfo AP_RollController::var_info[] = {
|
|
// @Param: 2SRV_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("2SRV_TCONST", 0, AP_RollController, gains.tau, 0.5f),
|
|
|
|
// index 1 to 3 reserved for old PID values
|
|
|
|
// @Param: 2SRV_RMAX
|
|
// @DisplayName: Maximum Roll Rate
|
|
// @Description: This sets the maximum roll rate that the attitude controller will demand (degrees/sec) in angle stabilized modes. Setting it to zero disables this limit.
|
|
// @Range: 0 180
|
|
// @Units: deg/s
|
|
// @Increment: 1
|
|
// @User: Advanced
|
|
AP_GROUPINFO("2SRV_RMAX", 4, AP_RollController, gains.rmax_pos, 0),
|
|
|
|
// index 5, 6 reserved for old IMAX, FF
|
|
|
|
// @Param: _RATE_P
|
|
// @DisplayName: Roll axis rate controller P gain
|
|
// @Description: Roll axis rate controller P gain. Corrects in proportion to the difference between the desired roll rate vs actual roll rate
|
|
// @Range: 0.08 0.35
|
|
// @Increment: 0.005
|
|
// @User: Standard
|
|
|
|
// @Param: _RATE_I
|
|
// @DisplayName: Roll axis rate controller I gain
|
|
// @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
|
|
// @Range: 0.01 0.6
|
|
// @Increment: 0.01
|
|
// @User: Standard
|
|
|
|
// @Param: _RATE_IMAX
|
|
// @DisplayName: Roll axis rate controller I gain maximum
|
|
// @Description: Roll axis rate controller I gain maximum. Constrains the maximum that the I term will output
|
|
// @Range: 0 1
|
|
// @Increment: 0.01
|
|
// @User: Standard
|
|
|
|
// @Param: _RATE_D
|
|
// @DisplayName: Roll axis rate controller D gain
|
|
// @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
|
|
// @Range: 0.001 0.03
|
|
// @Increment: 0.001
|
|
// @User: Standard
|
|
|
|
// @Param: _RATE_FF
|
|
// @DisplayName: Roll axis rate controller feed forward
|
|
// @Description: Roll axis rate controller feed forward
|
|
// @Range: 0 3.0
|
|
// @Increment: 0.001
|
|
// @User: Standard
|
|
|
|
// @Param: _RATE_FLTT
|
|
// @DisplayName: Roll axis rate controller target frequency in Hz
|
|
// @Description: Roll axis rate controller target frequency in Hz
|
|
// @Range: 2 50
|
|
// @Increment: 1
|
|
// @Units: Hz
|
|
// @User: Standard
|
|
|
|
// @Param: _RATE_FLTE
|
|
// @DisplayName: Roll axis rate controller error frequency in Hz
|
|
// @Description: Roll axis rate controller error frequency in Hz
|
|
// @Range: 2 50
|
|
// @Increment: 1
|
|
// @Units: Hz
|
|
// @User: Standard
|
|
|
|
// @Param: _RATE_FLTD
|
|
// @DisplayName: Roll axis rate controller derivative frequency in Hz
|
|
// @Description: Roll axis rate controller derivative frequency in Hz
|
|
// @Range: 0 50
|
|
// @Increment: 1
|
|
// @Units: Hz
|
|
// @User: Standard
|
|
|
|
// @Param: _RATE_SMAX
|
|
// @DisplayName: Roll slew rate limit
|
|
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
|
|
// @Range: 0 200
|
|
// @Increment: 0.5
|
|
// @User: Advanced
|
|
|
|
// @Param: _RATE_PDMX
|
|
// @DisplayName: Roll axis rate controller PD sum maximum
|
|
// @Description: Roll axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
|
|
// @Range: 0 1
|
|
// @Increment: 0.01
|
|
|
|
// @Param: _RATE_D_FF
|
|
// @DisplayName: Roll Derivative FeedForward Gain
|
|
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
|
|
// @Range: 0 0.03
|
|
// @Increment: 0.001
|
|
// @User: Advanced
|
|
|
|
// @Param: _RATE_NTF
|
|
// @DisplayName: Roll Target notch filter index
|
|
// @Description: Roll Target notch filter index, zero disables
|
|
// @Range: 0 8
|
|
// @User: Advanced
|
|
|
|
// @Param: _RATE_NEF
|
|
// @DisplayName: Roll Error notch filter index
|
|
// @Description: Roll Error notch filter index, zero disables
|
|
// @Range: 0 8
|
|
// @User: Advanced
|
|
|
|
AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_RollController, AC_PID),
|
|
|
|
// @Param: 2SRV_ACCEL
|
|
// @DisplayName: Roll max acceleration
|
|
// @Description: Roll acceleration limit. Setting to zero disables input shaping.
|
|
// @Range: 0 2500
|
|
// @Units: deg/s/s
|
|
// @Increment: 1
|
|
// @User: Advanced
|
|
AP_GROUPINFO("2SRV_ACCEL", 10, AP_RollController, accel_limit, 500),
|
|
|
|
// @Param: _ANGLE_P
|
|
// @DisplayName: Roll angle P gain
|
|
// @Description: Roll angle P gain. If zero a gain of (1 / RLL2SRV_TCONST) will be used.
|
|
// @Range: 0.000 12.000
|
|
// @Increment: 0.01
|
|
// @User: Advanced
|
|
AP_GROUPINFO("_ANGLE_P", 11, AP_RollController, angle_p, 0.0),
|
|
|
|
AP_GROUPEND
|
|
};
|
|
|
|
// constructor
|
|
AP_RollController::AP_RollController(const AP_FixedWing &parms)
|
|
: AP_FW_Controller(parms,
|
|
AC_PID::Defaults{
|
|
.p = 0.08,
|
|
.i = 0.15,
|
|
.d = 0.0,
|
|
.ff = 0.345,
|
|
.imax = 0.666,
|
|
.filt_T_hz = 3.0,
|
|
.filt_E_hz = 0.0,
|
|
.filt_D_hz = 12.0,
|
|
.srmax = 150.0,
|
|
.srtau = 1.0
|
|
},
|
|
AP_AutoTune::ATType::AUTOTUNE_ROLL)
|
|
{
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
}
|
|
|
|
// Return the measured roll angle in degrees
|
|
float AP_RollController::get_measured_angle_deg() const
|
|
{
|
|
return AP::ahrs().get_roll_deg();
|
|
}
|
|
|
|
// Return the measured roll rate in radians per second
|
|
float AP_RollController::get_measured_rate_rads() const
|
|
{
|
|
return AP::ahrs().get_gyro().x;
|
|
}
|
|
|
|
// Return true if the airspeed should be considered as under speed
|
|
bool AP_RollController::is_underspeed() const
|
|
{
|
|
return get_airspeed() <= float(aparm.airspeed_min);
|
|
}
|
|
|
|
// Return positive rate limit in deg per second, zero if disabled
|
|
float AP_RollController::get_positive_rate_limit_degs() const
|
|
{
|
|
return MAX(gains.rmax_pos.get(), 0.0);
|
|
}
|
|
|
|
// Return negative rate limit in deg per second (as a positive number) zero if disabled
|
|
float AP_RollController::get_negative_rate_limit_degs() const
|
|
{
|
|
return get_positive_rate_limit_degs();
|
|
}
|
|
|
|
// Return true if rate limits should be applied
|
|
bool AP_RollController::should_apply_rate_limits() const
|
|
{
|
|
return !in_recovery;
|
|
}
|
|
|
|
/*
|
|
Function returns an equivalent aileron deflection in centi-degrees in the range from -4500 to 4500
|
|
A positive demand is up
|
|
*/
|
|
float AP_RollController::run_axis_rate_control(float desired_rate_degs, float scaler, bool disable_integrator, bool ground_mode)
|
|
{
|
|
/*
|
|
prevent indecision in the roll controller when target roll is
|
|
close to 180 degrees from the current roll
|
|
*/
|
|
const float indecision_threshold_deg = 160;
|
|
const float last_desired_rate = _pid_info.target;
|
|
const float abs_angle_err_deg = fabsf(angle_err_deg);
|
|
if (abs_angle_err_deg > indecision_threshold_deg &&
|
|
angle_err_deg <= 180) {
|
|
if (desired_rate_degs * last_desired_rate < 0) {
|
|
desired_rate_degs = -desired_rate_degs;
|
|
// increase the desired rate in proportion to the extra
|
|
// angle we are requesting
|
|
const float new_angle_err_deg = abs_angle_err_deg + (180 - abs_angle_err_deg)*2;
|
|
desired_rate_degs *= new_angle_err_deg / abs_angle_err_deg;
|
|
}
|
|
}
|
|
|
|
// in_recovery flag is only valid for single loop, clear it
|
|
in_recovery = false;
|
|
|
|
return run_rate_control(desired_rate_degs, scaler, disable_integrator, ground_mode);
|
|
}
|