AMP_Control: move pitch and roll autotune_start into base `AP_FW_Co…

…ntroller`
IamPete1 · Nov 8, 2024 · 209cd68 · 209cd68
1 parent b0686f8
commit 209cd68
Show file tree

Hide file tree

Showing 6 changed files with 32 additions and 49 deletions.
diff --git 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 <AP_AHRS/AP_AHRS.h>
 #include <AP_Scheduler/AP_Scheduler.h>
+#include <GCS_MAVLink/GCS.h>
 
-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
@@ -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
@@ -20,7 +20,6 @@
 #include "AP_PitchController.h"
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Scheduler/AP_Scheduler.h>
-#include <GCS_MAVLink/GCS.h>
 
 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
@@ -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
@@ -21,7 +21,6 @@
 #include "AP_RollController.h"
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Scheduler/AP_Scheduler.h>
-#include <GCS_MAVLink/GCS.h>
 
 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
@@ -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();