From 209cd68c3fb002b9aff02f07fcb681ce5db0b4dd Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 8 Nov 2024 14:00:47 +0000 Subject: [PATCH] AMP_Control: move pitch and roll `autotune_start` into base `AP_FW_Controller` --- libraries/APM_Control/AP_FW_Controller.cpp | 26 ++++++++++++++++++-- libraries/APM_Control/AP_FW_Controller.h | 6 +++-- libraries/APM_Control/AP_PitchController.cpp | 23 ++--------------- libraries/APM_Control/AP_PitchController.h | 1 - libraries/APM_Control/AP_RollController.cpp | 23 ++--------------- libraries/APM_Control/AP_RollController.h | 2 -- 6 files changed, 32 insertions(+), 49 deletions(-) diff --git a/libraries/APM_Control/AP_FW_Controller.cpp b/libraries/APM_Control/AP_FW_Controller.cpp index 1aa6f64a51aee..22a51e75a787d 100644 --- a/libraries/APM_Control/AP_FW_Controller.cpp +++ b/libraries/APM_Control/AP_FW_Controller.cpp @@ -20,10 +20,12 @@ #include "AP_FW_Controller.h" #include #include +#include -AP_FW_Controller::AP_FW_Controller(const AP_FixedWing &parms, const AC_PID::Defaults &defaults) +AP_FW_Controller::AP_FW_Controller(const AP_FixedWing &parms, const AC_PID::Defaults &defaults, AP_AutoTune::ATType _autotune_type) : aparm(parms), - rate_pid(defaults) + rate_pid(defaults), + autotune_type(_autotune_type) { rate_pid.set_slew_limit_scale(45); } @@ -133,3 +135,23 @@ void AP_FW_Controller::autotune_restore(void) autotune->stop(); } } + +/* + start an autotune + */ +void AP_FW_Controller::autotune_start(void) +{ + if (autotune == nullptr) { + autotune = NEW_NOTHROW AP_AutoTune(gains, autotune_type, aparm, rate_pid); + if (autotune == nullptr) { + if (!failed_autotune_alloc) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed %s allocation", AP_AutoTune::axis_string(autotune_type)); + } + failed_autotune_alloc = true; + } + } + if (autotune != nullptr) { + autotune->start(); + } +} + diff --git a/libraries/APM_Control/AP_FW_Controller.h b/libraries/APM_Control/AP_FW_Controller.h index a552776215fd0..5cd81bdc41242 100644 --- a/libraries/APM_Control/AP_FW_Controller.h +++ b/libraries/APM_Control/AP_FW_Controller.h @@ -7,7 +7,7 @@ class AP_FW_Controller { public: - AP_FW_Controller(const AP_FixedWing &parms, const AC_PID::Defaults &defaults); + AP_FW_Controller(const AP_FixedWing &parms, const AC_PID::Defaults &defaults, AP_AutoTune::ATType _autotune_type); /* Do not allow copies */ CLASS_NO_COPY(AP_FW_Controller); @@ -26,7 +26,7 @@ class AP_FW_Controller */ void decay_I(); - virtual void autotune_start(void) = 0; + void autotune_start(void); void autotune_restore(void); const AP_PIDInfo& get_pid_info(void) const @@ -62,4 +62,6 @@ class AP_FW_Controller virtual float get_airspeed() const = 0; virtual float get_measured_rate() const = 0; + + const AP_AutoTune::ATType autotune_type; }; diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 071a3564e0595..64908641d08fc 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -20,7 +20,6 @@ #include "AP_PitchController.h" #include #include -#include extern const AP_HAL::HAL& hal; @@ -174,7 +173,8 @@ AP_PitchController::AP_PitchController(const AP_FixedWing &parms) .filt_D_hz = 12.0, .srmax = 150.0, .srtau = 1.0 - }) + }, + AP_AutoTune::ATType::AUTOTUNE_PITCH) { AP_Param::setup_object_defaults(this, var_info); } @@ -333,22 +333,3 @@ void AP_PitchController::convert_pid() rate_pid.kD().set_and_save_ifchanged(0); rate_pid.kIMAX().set_and_save_ifchanged(old_imax/4500.0); } - -/* - start an autotune - */ -void AP_PitchController::autotune_start(void) -{ - if (autotune == nullptr) { - autotune = NEW_NOTHROW AP_AutoTune(gains, AP_AutoTune::AUTOTUNE_PITCH, aparm, rate_pid); - if (autotune == nullptr) { - if (!failed_autotune_alloc) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed pitch allocation"); - } - failed_autotune_alloc = true; - } - } - if (autotune != nullptr) { - autotune->start(); - } -} diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 48736e7421d43..1cef2b5ea9adf 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -11,7 +11,6 @@ class AP_PitchController : public AP_FW_Controller CLASS_NO_COPY(AP_PitchController); 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[]; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 3766fb79f77fb..3cf2f2f978179 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -21,7 +21,6 @@ #include "AP_RollController.h" #include #include -#include extern const AP_HAL::HAL& hal; @@ -158,7 +157,8 @@ AP_RollController::AP_RollController(const AP_FixedWing &parms) .filt_D_hz = 12.0, .srmax = 150.0, .srtau = 1.0 - }) + }, + AP_AutoTune::ATType::AUTOTUNE_ROLL) { AP_Param::setup_object_defaults(this, var_info); } @@ -241,22 +241,3 @@ void AP_RollController::convert_pid() rate_pid.kD().set_and_save_ifchanged(0); rate_pid.kIMAX().set_and_save_ifchanged(old_imax/4500.0); } - -/* - start an autotune - */ -void AP_RollController::autotune_start(void) -{ - if (autotune == nullptr) { - autotune = NEW_NOTHROW AP_AutoTune(gains, AP_AutoTune::AUTOTUNE_ROLL, aparm, rate_pid); - if (autotune == nullptr) { - if (!failed_autotune_alloc) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed roll allocation"); - } - failed_autotune_alloc = true; - } - } - if (autotune != nullptr) { - autotune->start(); - } -} diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index aef4a933d6a8a..d625d3b533eb9 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -12,8 +12,6 @@ class AP_RollController : public AP_FW_Controller 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[]; void convert_pid();