-
Notifications
You must be signed in to change notification settings - Fork 17.6k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
APM_Control: add AP_FW_Controller as common base class to roll and pi…
…tch controlers
- Loading branch information
Showing
6 changed files
with
266 additions
and
312 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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(); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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; | ||
| }; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.