From efa7a8e95caacc799bbced2a4d77932a83502d80 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 3 Aug 2024 19:33:22 +0100 Subject: [PATCH] APM_Control: add AP_FW_Controller as common base class to roll and pitch controlers --- libraries/APM_Control/AP_FW_Controller.cpp | 135 +++++++++++++++++++ libraries/APM_Control/AP_FW_Controller.h | 65 +++++++++ libraries/APM_Control/AP_PitchController.cpp | 132 ++++-------------- libraries/APM_Control/AP_PitchController.h | 61 ++------- libraries/APM_Control/AP_RollController.cpp | 125 ++++------------- libraries/APM_Control/AP_RollController.h | 60 +-------- 6 files changed, 266 insertions(+), 312 deletions(-) create mode 100644 libraries/APM_Control/AP_FW_Controller.cpp create mode 100644 libraries/APM_Control/AP_FW_Controller.h diff --git a/libraries/APM_Control/AP_FW_Controller.cpp b/libraries/APM_Control/AP_FW_Controller.cpp new file mode 100644 index 00000000000000..1aa6f64a51aeea --- /dev/null +++ b/libraries/APM_Control/AP_FW_Controller.cpp @@ -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 . + */ +// Code by Jon Challinger +// Modified by Paul Riseborough +// + + +#include "AP_FW_Controller.h" +#include +#include + +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(); + } +} diff --git a/libraries/APM_Control/AP_FW_Controller.h b/libraries/APM_Control/AP_FW_Controller.h new file mode 100644 index 00000000000000..a552776215fd0b --- /dev/null +++ b/libraries/APM_Control/AP_FW_Controller.h @@ -0,0 +1,65 @@ +#pragma once + +#include +#include "AP_AutoTune.h" +#include + +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; +}; diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 0bd2e9417e590b..071a3564e05954 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -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(); - } -} diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index e2900d15ad11a4..48736e7421d435 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -1,11 +1,8 @@ #pragma once -#include -#include "AP_AutoTune.h" -#include -#include +#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 @@ class AP_PitchController /* 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; }; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 6f16fd2b0245db..3766fb79f77fb4 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -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; - } - 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); + if (!AP::ahrs().airspeed_estimate(aspeed)) { + // If no airspeed available use 0 + aspeed = 0.0; } - - // 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(); - } -} diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index 35a8a331cc8305..aef4a933d6a8ae 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -1,11 +1,8 @@ #pragma once -#include -#include "AP_AutoTune.h" -#include -#include +#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 @@ class AP_RollController /* 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; };