mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-28 18:53:57 -04:00
APM_Control: add AP_FW_Controller as common base class to roll and pitch controllers
This commit is contained in:
Iampete1
Andrew Tridgell
parent
4db73d3883
commit
f8dd0b2d76
135
libraries/APM_Control/AP_FW_Controller.cpp
Normal file
135
libraries/APM_Control/AP_FW_Controller.cpp
Normal file
@ -0,0 +1,135 @@
|
||||
/*
|
||||
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_FW_Controller.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Scheduler/AP_Scheduler.h>
|
||||
|
||||
AP_FW_Controller::AP_FW_Controller(const AP_FixedWing &parms, const AC_PID::Defaults &defaults)
|
||||
: aparm(parms),
|
||||
rate_pid(defaults)
|
||||
{
|
||||
rate_pid.set_slew_limit_scale(45);
|
||||
}
|
||||
|
||||
/*
|
||||
AC_PID based rate controller
|
||||
*/
|
||||
float AP_FW_Controller::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed, bool ground_mode)
|
||||
{
|
||||
const float dt = AP::scheduler().get_loop_period_s();
|
||||
|
||||
const float eas2tas = AP::ahrs().get_EAS2TAS();
|
||||
bool limit_I = fabsf(_last_out) >= 45;
|
||||
const float rate = get_measured_rate();
|
||||
const float old_I = rate_pid.get_i();
|
||||
|
||||
const bool underspeed = is_underspeed(aspeed);
|
||||
if (underspeed) {
|
||||
limit_I = true;
|
||||
}
|
||||
|
||||
// the P and I elements are scaled by sq(scaler). To use an
|
||||
// unmodified AC_PID object we scale the inputs and calculate FF separately
|
||||
//
|
||||
// note that we run AC_PID in radians so that the normal scaling
|
||||
// range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
|
||||
rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate * scaler * scaler, dt, limit_I);
|
||||
|
||||
if (underspeed) {
|
||||
// when underspeed we lock the integrator
|
||||
rate_pid.set_integrator(old_I);
|
||||
}
|
||||
|
||||
// FF should be scaled by scaler/eas2tas, but since we have scaled
|
||||
// the AC_PID target above by scaler*scaler we need to instead
|
||||
// divide by scaler*eas2tas to get the right scaling
|
||||
const float ff = degrees(ff_scale * rate_pid.get_ff() / (scaler * eas2tas));
|
||||
ff_scale = 1.0;
|
||||
|
||||
if (disable_integrator) {
|
||||
rate_pid.reset_I();
|
||||
}
|
||||
|
||||
// convert AC_PID info object to same scale as old controller
|
||||
_pid_info = rate_pid.get_pid_info();
|
||||
auto &pinfo = _pid_info;
|
||||
|
||||
const float deg_scale = degrees(1);
|
||||
pinfo.FF = ff;
|
||||
pinfo.P *= deg_scale;
|
||||
pinfo.I *= deg_scale;
|
||||
pinfo.D *= deg_scale;
|
||||
pinfo.DFF *= deg_scale;
|
||||
|
||||
// fix the logged target and actual values to not have the scalers applied
|
||||
pinfo.target = desired_rate;
|
||||
pinfo.actual = degrees(rate);
|
||||
|
||||
// sum components
|
||||
float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D + pinfo.DFF;
|
||||
if (ground_mode) {
|
||||
// when on ground suppress D and half P term to prevent oscillations
|
||||
out -= pinfo.D + 0.5*pinfo.P;
|
||||
}
|
||||
|
||||
// remember the last output to trigger the I limit
|
||||
_last_out = out;
|
||||
|
||||
if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {
|
||||
// let autotune have a go at the values
|
||||
autotune->update(pinfo, scaler, angle_err_deg);
|
||||
}
|
||||
|
||||
// output is scaled to notional centidegrees of deflection
|
||||
return constrain_float(out * 100, -4500, 4500);
|
||||
}
|
||||
|
||||
/*
|
||||
Function returns an equivalent control surface deflection in centi-degrees in the range from -4500 to 4500
|
||||
*/
|
||||
float AP_FW_Controller::get_rate_out(float desired_rate, float scaler)
|
||||
{
|
||||
return _get_rate_out(desired_rate, scaler, false, get_airspeed(), false);
|
||||
}
|
||||
|
||||
void AP_FW_Controller::reset_I()
|
||||
{
|
||||
rate_pid.reset_I();
|
||||
}
|
||||
|
||||
/*
|
||||
reduce the integrator, used when we have a low scale factor in a quadplane hover
|
||||
*/
|
||||
void AP_FW_Controller::decay_I()
|
||||
{
|
||||
// this reduces integrator by 95% over 2s
|
||||
_pid_info.I *= 0.995f;
|
||||
rate_pid.set_integrator(rate_pid.get_i() * 0.995);
|
||||
}
|
||||
|
||||
/*
|
||||
restore autotune gains
|
||||
*/
|
||||
void AP_FW_Controller::autotune_restore(void)
|
||||
{
|
||||
if (autotune != nullptr) {
|
||||
autotune->stop();
|
||||
}
|
||||
}
|
||||
65
libraries/APM_Control/AP_FW_Controller.h
Normal file
65
libraries/APM_Control/AP_FW_Controller.h
Normal file
@ -0,0 +1,65 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include "AP_AutoTune.h"
|
||||
#include <AC_PID/AC_PID.h>
|
||||
|
||||
class AP_FW_Controller
|
||||
{
|
||||
public:
|
||||
AP_FW_Controller(const AP_FixedWing &parms, const AC_PID::Defaults &defaults);
|
||||
|
||||
/* Do not allow copies */
|
||||
CLASS_NO_COPY(AP_FW_Controller);
|
||||
|
||||
float get_rate_out(float desired_rate, float scaler);
|
||||
virtual float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode) = 0;
|
||||
|
||||
// setup a one loop FF scale multiplier. This replaces any previous scale applied
|
||||
// so should only be used when only one source of scaling is needed
|
||||
void set_ff_scale(float _ff_scale) { ff_scale = _ff_scale; }
|
||||
|
||||
void reset_I();
|
||||
|
||||
/*
|
||||
reduce the integrator, used when we have a low scale factor in a quadplane hover
|
||||
*/
|
||||
void decay_I();
|
||||
|
||||
virtual void autotune_start(void) = 0;
|
||||
void autotune_restore(void);
|
||||
|
||||
const AP_PIDInfo& get_pid_info(void) const
|
||||
{
|
||||
return _pid_info;
|
||||
}
|
||||
|
||||
// set the PID notch sample rates
|
||||
void set_notch_sample_rate(float sample_rate) { rate_pid.set_notch_sample_rate(sample_rate); }
|
||||
|
||||
AP_Float &kP(void) { return rate_pid.kP(); }
|
||||
AP_Float &kI(void) { return rate_pid.kI(); }
|
||||
AP_Float &kD(void) { return rate_pid.kD(); }
|
||||
AP_Float &kFF(void) { return rate_pid.ff(); }
|
||||
AP_Float &tau(void) { return gains.tau; }
|
||||
|
||||
protected:
|
||||
const AP_FixedWing &aparm;
|
||||
AP_AutoTune::ATGains gains;
|
||||
AP_AutoTune *autotune;
|
||||
bool failed_autotune_alloc;
|
||||
float _last_out;
|
||||
AC_PID rate_pid;
|
||||
float angle_err_deg;
|
||||
float ff_scale = 1.0;
|
||||
|
||||
AP_PIDInfo _pid_info;
|
||||
|
||||
float _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed, bool ground_mode);
|
||||
|
||||
virtual bool is_underspeed(const float aspeed) const = 0;
|
||||
|
||||
virtual float get_airspeed() const = 0;
|
||||
|
||||
virtual float get_measured_rate() const = 0;
|
||||
};
|
||||
@ -162,105 +162,41 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
|
||||
};
|
||||
|
||||
AP_PitchController::AP_PitchController(const AP_FixedWing &parms)
|
||||
: aparm(parms)
|
||||
: AP_FW_Controller(parms,
|
||||
AC_PID::Defaults{
|
||||
.p = 0.04,
|
||||
.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_Param::setup_object_defaults(this, var_info);
|
||||
rate_pid.set_slew_limit_scale(45);
|
||||
}
|
||||
|
||||
/*
|
||||
AC_PID based rate controller
|
||||
*/
|
||||
float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed, bool ground_mode)
|
||||
float AP_PitchController::get_measured_rate() const
|
||||
{
|
||||
const float dt = AP::scheduler().get_loop_period_s();
|
||||
|
||||
const AP_AHRS &_ahrs = AP::ahrs();
|
||||
|
||||
const float eas2tas = _ahrs.get_EAS2TAS();
|
||||
bool limit_I = fabsf(_last_out) >= 45;
|
||||
float rate_y = _ahrs.get_gyro().y;
|
||||
float old_I = rate_pid.get_i();
|
||||
|
||||
bool underspeed = aspeed <= 0.5*float(aparm.airspeed_min);
|
||||
if (underspeed) {
|
||||
limit_I = true;
|
||||
}
|
||||
|
||||
// the P and I elements are scaled by sq(scaler). To use an
|
||||
// unmodified AC_PID object we scale the inputs and calculate FF separately
|
||||
//
|
||||
// note that we run AC_PID in radians so that the normal scaling
|
||||
// range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
|
||||
rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_y * scaler * scaler, dt, limit_I);
|
||||
|
||||
if (underspeed) {
|
||||
// when underspeed we lock the integrator
|
||||
rate_pid.set_integrator(old_I);
|
||||
}
|
||||
|
||||
// FF should be scaled by scaler/eas2tas, but since we have scaled
|
||||
// the AC_PID target above by scaler*scaler we need to instead
|
||||
// divide by scaler*eas2tas to get the right scaling
|
||||
const float ff = degrees(ff_scale * rate_pid.get_ff() / (scaler * eas2tas));
|
||||
ff_scale = 1.0;
|
||||
|
||||
if (disable_integrator) {
|
||||
rate_pid.reset_I();
|
||||
}
|
||||
|
||||
// convert AC_PID info object to same scale as old controller
|
||||
_pid_info = rate_pid.get_pid_info();
|
||||
auto &pinfo = _pid_info;
|
||||
|
||||
const float deg_scale = degrees(1);
|
||||
pinfo.FF = ff;
|
||||
pinfo.P *= deg_scale;
|
||||
pinfo.I *= deg_scale;
|
||||
pinfo.D *= deg_scale;
|
||||
pinfo.DFF *= deg_scale;
|
||||
|
||||
// fix the logged target and actual values to not have the scalers applied
|
||||
pinfo.target = desired_rate;
|
||||
pinfo.actual = degrees(rate_y);
|
||||
|
||||
// sum components
|
||||
float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D + pinfo.DFF;
|
||||
if (ground_mode) {
|
||||
// when on ground suppress D and half P term to prevent oscillations
|
||||
out -= pinfo.D + 0.5*pinfo.P;
|
||||
}
|
||||
|
||||
// remember the last output to trigger the I limit
|
||||
_last_out = out;
|
||||
|
||||
if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {
|
||||
// let autotune have a go at the values
|
||||
autotune->update(pinfo, scaler, angle_err_deg);
|
||||
}
|
||||
|
||||
// output is scaled to notional centidegrees of deflection
|
||||
return constrain_float(out * 100, -4500, 4500);
|
||||
return AP::ahrs().get_gyro().y;
|
||||
}
|
||||
|
||||
/*
|
||||
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)
|
||||
*/
|
||||
float AP_PitchController::get_rate_out(float desired_rate, float scaler)
|
||||
float AP_PitchController::get_airspeed() const
|
||||
{
|
||||
float aspeed;
|
||||
if (!AP::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, false);
|
||||
return aspeed;
|
||||
}
|
||||
|
||||
bool AP_PitchController::is_underspeed(const float aspeed) const
|
||||
{
|
||||
return aspeed <= 0.5*float(aparm.airspeed_min);
|
||||
}
|
||||
|
||||
/*
|
||||
@ -270,7 +206,7 @@ float AP_PitchController::get_rate_out(float desired_rate, float scaler)
|
||||
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 AP_PitchController::_get_coordination_rate_offset(const float &aspeed, bool &inverted) const
|
||||
{
|
||||
float rate_offset;
|
||||
float bank_angle = AP::ahrs().get_roll();
|
||||
@ -288,10 +224,6 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv
|
||||
}
|
||||
}
|
||||
const AP_AHRS &_ahrs = AP::ahrs();
|
||||
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;
|
||||
@ -318,7 +250,6 @@ float AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool di
|
||||
// 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;
|
||||
|
||||
@ -326,6 +257,8 @@ float AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool di
|
||||
gains.tau.set(0.05f);
|
||||
}
|
||||
|
||||
const float aspeed = get_airspeed();
|
||||
|
||||
rate_offset = _get_coordination_rate_offset(aspeed, inverted);
|
||||
|
||||
// Calculate the desired pitch rate (deg/sec) from the angle error
|
||||
@ -370,11 +303,6 @@ float AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool di
|
||||
return _get_rate_out(desired_rate, scaler, disable_integrator, aspeed, ground_mode);
|
||||
}
|
||||
|
||||
void AP_PitchController::reset_I()
|
||||
{
|
||||
rate_pid.reset_I();
|
||||
}
|
||||
|
||||
/*
|
||||
convert from old to new PIDs
|
||||
this is a temporary conversion function during development
|
||||
@ -424,13 +352,3 @@ void AP_PitchController::autotune_start(void)
|
||||
autotune->start();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
restore autotune gains
|
||||
*/
|
||||
void AP_PitchController::autotune_restore(void)
|
||||
{
|
||||
if (autotune != nullptr) {
|
||||
autotune->stop();
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,11 +1,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include "AP_AutoTune.h"
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AC_PID/AC_PID.h>
|
||||
#include "AP_FW_Controller.h"
|
||||
|
||||
class AP_PitchController
|
||||
class AP_PitchController : public AP_FW_Controller
|
||||
{
|
||||
public:
|
||||
AP_PitchController(const AP_FixedWing &parms);
|
||||
@ -13,60 +10,20 @@ public:
|
||||
/* Do not allow copies */
|
||||
CLASS_NO_COPY(AP_PitchController);
|
||||
|
||||
float get_rate_out(float desired_rate, float scaler);
|
||||
float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode);
|
||||
|
||||
// setup a one loop FF scale multiplier. This replaces any previous scale applied
|
||||
// so should only be used when only one source of scaling is needed
|
||||
void set_ff_scale(float _ff_scale) { ff_scale = _ff_scale; }
|
||||
|
||||
void reset_I();
|
||||
|
||||
/*
|
||||
reduce the integrator, used when we have a low scale factor in a quadplane hover
|
||||
*/
|
||||
void decay_I()
|
||||
{
|
||||
// this reduces integrator by 95% over 2s
|
||||
_pid_info.I *= 0.995f;
|
||||
rate_pid.set_integrator(rate_pid.get_i() * 0.995);
|
||||
}
|
||||
|
||||
void autotune_start(void);
|
||||
void autotune_restore(void);
|
||||
|
||||
const AP_PIDInfo& get_pid_info(void) const
|
||||
{
|
||||
return _pid_info;
|
||||
}
|
||||
|
||||
// set the PID notch sample rates
|
||||
void set_notch_sample_rate(float sample_rate) { rate_pid.set_notch_sample_rate(sample_rate); }
|
||||
float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode) override;
|
||||
void autotune_start(void) override;
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
AP_Float &kP(void) { return rate_pid.kP(); }
|
||||
AP_Float &kI(void) { return rate_pid.kI(); }
|
||||
AP_Float &kD(void) { return rate_pid.kD(); }
|
||||
AP_Float &kFF(void) { return rate_pid.ff(); }
|
||||
AP_Float &tau(void) { return gains.tau; }
|
||||
|
||||
void convert_pid();
|
||||
|
||||
private:
|
||||
const AP_FixedWing &aparm;
|
||||
AP_AutoTune::ATGains gains;
|
||||
AP_AutoTune *autotune;
|
||||
bool failed_autotune_alloc;
|
||||
AP_Int16 _max_rate_neg;
|
||||
AP_Float _roll_ff;
|
||||
float _last_out;
|
||||
AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 3, 0, 12, 150, 1};
|
||||
float angle_err_deg;
|
||||
float ff_scale = 1.0;
|
||||
|
||||
AP_PIDInfo _pid_info;
|
||||
float _get_coordination_rate_offset(const float &aspeed, bool &inverted) const;
|
||||
|
||||
float get_airspeed() const override;
|
||||
bool is_underspeed(const float aspeed) const override;
|
||||
float get_measured_rate() const override;
|
||||
|
||||
float _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed, bool ground_mode);
|
||||
float _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
|
||||
};
|
||||
|
||||
@ -146,101 +146,41 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
|
||||
|
||||
// constructor
|
||||
AP_RollController::AP_RollController(const AP_FixedWing &parms)
|
||||
: aparm(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_Param::setup_object_defaults(this, var_info);
|
||||
rate_pid.set_slew_limit_scale(45);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
AC_PID based rate controller
|
||||
*/
|
||||
float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, bool ground_mode)
|
||||
float AP_RollController::get_measured_rate() const
|
||||
{
|
||||
const AP_AHRS &_ahrs = AP::ahrs();
|
||||
return AP::ahrs().get_gyro().x;
|
||||
}
|
||||
|
||||
const float dt = AP::scheduler().get_loop_period_s();
|
||||
const float eas2tas = _ahrs.get_EAS2TAS();
|
||||
bool limit_I = fabsf(_last_out) >= 45;
|
||||
float rate_x = _ahrs.get_gyro().x;
|
||||
float AP_RollController::get_airspeed() const
|
||||
{
|
||||
float aspeed;
|
||||
float old_I = rate_pid.get_i();
|
||||
|
||||
if (!_ahrs.airspeed_estimate(aspeed)) {
|
||||
aspeed = 0;
|
||||
if (!AP::ahrs().airspeed_estimate(aspeed)) {
|
||||
// If no airspeed available use 0
|
||||
aspeed = 0.0;
|
||||
}
|
||||
bool underspeed = aspeed <= float(aparm.airspeed_min);
|
||||
if (underspeed) {
|
||||
limit_I = true;
|
||||
}
|
||||
|
||||
// the P and I elements are scaled by sq(scaler). To use an
|
||||
// unmodified AC_PID object we scale the inputs and calculate FF separately
|
||||
//
|
||||
// note that we run AC_PID in radians so that the normal scaling
|
||||
// range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
|
||||
rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_x * scaler * scaler, dt, limit_I);
|
||||
|
||||
if (underspeed) {
|
||||
// when underspeed we lock the integrator
|
||||
rate_pid.set_integrator(old_I);
|
||||
}
|
||||
|
||||
// FF should be scaled by scaler/eas2tas, but since we have scaled
|
||||
// the AC_PID target above by scaler*scaler we need to instead
|
||||
// divide by scaler*eas2tas to get the right scaling
|
||||
const float ff = degrees(ff_scale * rate_pid.get_ff() / (scaler * eas2tas));
|
||||
ff_scale = 1.0;
|
||||
|
||||
if (disable_integrator) {
|
||||
rate_pid.reset_I();
|
||||
}
|
||||
|
||||
// convert AC_PID info object to same scale as old controller
|
||||
_pid_info = rate_pid.get_pid_info();
|
||||
auto &pinfo = _pid_info;
|
||||
|
||||
const float deg_scale = degrees(1);
|
||||
pinfo.FF = ff;
|
||||
pinfo.P *= deg_scale;
|
||||
pinfo.I *= deg_scale;
|
||||
pinfo.D *= deg_scale;
|
||||
pinfo.DFF *= deg_scale;
|
||||
|
||||
// fix the logged target and actual values to not have the scalers applied
|
||||
pinfo.target = desired_rate;
|
||||
pinfo.actual = degrees(rate_x);
|
||||
|
||||
// sum components
|
||||
float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D + pinfo.DFF;
|
||||
if (ground_mode) {
|
||||
// when on ground suppress D term to prevent oscillations
|
||||
out -= pinfo.D + 0.5*pinfo.P;
|
||||
}
|
||||
|
||||
// remember the last output to trigger the I limit
|
||||
_last_out = out;
|
||||
|
||||
if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {
|
||||
// let autotune have a go at the values
|
||||
autotune->update(pinfo, scaler, angle_err_deg);
|
||||
}
|
||||
|
||||
// output is scaled to notional centidegrees of deflection
|
||||
return constrain_float(out * 100, -4500, 4500);
|
||||
return aspeed;
|
||||
}
|
||||
|
||||
/*
|
||||
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
|
||||
*/
|
||||
float AP_RollController::get_rate_out(float desired_rate, float scaler)
|
||||
bool AP_RollController::is_underspeed(const float aspeed) const
|
||||
{
|
||||
return _get_rate_out(desired_rate, scaler, false, false);
|
||||
return aspeed <= float(aparm.airspeed_min);
|
||||
}
|
||||
|
||||
/*
|
||||
@ -269,12 +209,7 @@ float AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool dis
|
||||
desired_rate = gains.rmax_pos;
|
||||
}
|
||||
|
||||
return _get_rate_out(desired_rate, scaler, disable_integrator, ground_mode);
|
||||
}
|
||||
|
||||
void AP_RollController::reset_I()
|
||||
{
|
||||
rate_pid.reset_I();
|
||||
return _get_rate_out(desired_rate, scaler, disable_integrator, get_airspeed(), ground_mode);
|
||||
}
|
||||
|
||||
/*
|
||||
@ -325,13 +260,3 @@ void AP_RollController::autotune_start(void)
|
||||
autotune->start();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
restore autotune gains
|
||||
*/
|
||||
void AP_RollController::autotune_restore(void)
|
||||
{
|
||||
if (autotune != nullptr) {
|
||||
autotune->stop();
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,11 +1,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include "AP_AutoTune.h"
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AC_PID/AC_PID.h>
|
||||
#include "AP_FW_Controller.h"
|
||||
|
||||
class AP_RollController
|
||||
class AP_RollController : public AP_FW_Controller
|
||||
{
|
||||
public:
|
||||
AP_RollController(const AP_FixedWing &parms);
|
||||
@ -13,59 +10,16 @@ public:
|
||||
/* Do not allow copies */
|
||||
CLASS_NO_COPY(AP_RollController);
|
||||
|
||||
float get_rate_out(float desired_rate, float scaler);
|
||||
float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode);
|
||||
float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode) override;
|
||||
|
||||
// setup a one loop FF scale multiplier. This replaces any previous scale applied
|
||||
// so should only be used when only one source of scaling is needed
|
||||
void set_ff_scale(float _ff_scale) { ff_scale = _ff_scale; }
|
||||
|
||||
void reset_I();
|
||||
|
||||
/*
|
||||
reduce the integrator, used when we have a low scale factor in a quadplane hover
|
||||
*/
|
||||
void decay_I()
|
||||
{
|
||||
// this reduces integrator by 95% over 2s
|
||||
_pid_info.I *= 0.995f;
|
||||
rate_pid.set_integrator(rate_pid.get_i() * 0.995);
|
||||
}
|
||||
|
||||
void autotune_start(void);
|
||||
void autotune_restore(void);
|
||||
|
||||
const AP_PIDInfo& get_pid_info(void) const
|
||||
{
|
||||
return _pid_info;
|
||||
}
|
||||
|
||||
// set the PID notch sample rates
|
||||
void set_notch_sample_rate(float sample_rate) { rate_pid.set_notch_sample_rate(sample_rate); }
|
||||
void autotune_start(void) override;
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
|
||||
// tuning accessors
|
||||
AP_Float &kP(void) { return rate_pid.kP(); }
|
||||
AP_Float &kI(void) { return rate_pid.kI(); }
|
||||
AP_Float &kD(void) { return rate_pid.kD(); }
|
||||
AP_Float &kFF(void) { return rate_pid.ff(); }
|
||||
AP_Float &tau(void) { return gains.tau; }
|
||||
|
||||
void convert_pid();
|
||||
|
||||
private:
|
||||
const AP_FixedWing &aparm;
|
||||
AP_AutoTune::ATGains gains;
|
||||
AP_AutoTune *autotune;
|
||||
bool failed_autotune_alloc;
|
||||
float _last_out;
|
||||
AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 3, 0, 12, 150, 1};
|
||||
float angle_err_deg;
|
||||
float ff_scale = 1.0;
|
||||
|
||||
AP_PIDInfo _pid_info;
|
||||
|
||||
float _get_rate_out(float desired_rate, float scaler, bool disable_integrator, bool ground_mode);
|
||||
float get_airspeed() const override;
|
||||
bool is_underspeed(const float aspeed) const override;
|
||||
float get_measured_rate() const override;
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user