APM_Control: support yaw rate controller autotune

libraries/APM_Control/AP_AutoTune.cpp

@@ -147,6 +147,19 @@ void AP_AutoTune::stop(void)
     }
 }
 
+const char *AP_AutoTune::axis_string(void) const
+{
+    switch (type) {
+    case AUTOTUNE_ROLL:
+        return "Roll";
+    case AUTOTUNE_PITCH:
+        return "Pitch";
+    case AUTOTUNE_YAW:
+        return "Yaw";
+    }
+    return "";
+}
+
 /*
   one update cycle of the autotuner
  */
@@ -189,13 +202,21 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e
     max_SRate_P = MAX(max_SRate_P, slew_limiter_P.get_slew_rate());
     max_SRate_D = MAX(max_SRate_D, slew_limiter_D.get_slew_rate());
 
-    float att_limit_deg;
+    float att_limit_deg = 0;
-    if (type == AUTOTUNE_ROLL) {
+    switch (type) {
+    case AUTOTUNE_ROLL:
         att_limit_deg = aparm.roll_limit_cd * 0.01;
-    } else {
+        break;
+    case AUTOTUNE_PITCH:
         att_limit_deg = MIN(abs(aparm.pitch_limit_max_cd),abs(aparm.pitch_limit_min_cd))*0.01;
+        break;
+    case AUTOTUNE_YAW:
+        // arbitrary value for yaw angle
+        att_limit_deg = 20;
+        break;
     }
 
+
     // thresholds for when we consider an event to start and end
     const float rate_threshold1 = 0.4 * MIN(att_limit_deg / current.tau.get(), current.rmax_pos);
     const float rate_threshold2 = 0.25 * rate_threshold1;
@@ -337,7 +358,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e
             D_limit = D;
             D_set_ms = now;
             action = Action::LOWER_D;
-            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", type==AUTOTUNE_ROLL?"Roll":"Pitch", D_limit);
+            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", axis_string(), D_limit);
         }
     } else if (min_Dmod < 1.0) {
         // oscillation, with D_limit set
@@ -349,7 +370,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e
                 D_limit = D;
                 D_set_ms = now;
                 action = Action::LOWER_D;
-                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", type==AUTOTUNE_ROLL?"Roll":"Pitch", D_limit);
+                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", axis_string(), D_limit);
                 done_count = 0;
             } else if (now - P_set_ms > 2500) {
                 if (is_positive(P_limit)) {
@@ -365,7 +386,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e
                 P_set_ms = now;
                 action = Action::LOWER_P;
                 done_count = 0;
-                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sP: %.4f", type==AUTOTUNE_ROLL?"Roll":"Pitch", P_limit);
+                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sP: %.4f", axis_string(), P_limit);
             }
         }
     } else if (ff_count < 4) {
@@ -384,7 +405,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e
         // have done 3 cycles without reducing P
         if (done_count < 3) {
             if (++done_count == 3) {
-                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s: Finished", type==AUTOTUNE_ROLL?"Roll":"Pitch");
+                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s: Finished", axis_string());
                 save_gains();
             }
         }

libraries/APM_Control/AP_AutoTune.h

@@ -19,7 +19,8 @@ public:
 
     enum ATType {
         AUTOTUNE_ROLL  = 0,
-        AUTOTUNE_PITCH = 1
+        AUTOTUNE_PITCH = 1,
+        AUTOTUNE_YAW = 2,
     };
 
     struct PACKED log_ATRP {
@@ -105,6 +106,7 @@ private:
     void save_float_if_changed(AP_Float &v, float value);
     void save_int16_if_changed(AP_Int16 &v, int16_t value);
     void state_change(ATState newstate);
+    const char *axis_string(void) const;
 
     // get gains with PID components
     ATGains get_gains(void);

libraries/APM_Control/AP_YawController.cpp

@@ -148,6 +148,16 @@ const AP_Param::GroupInfo AP_YawController::var_info[] = {
     AP_GROUPEND
 };
 
+AP_YawController::AP_YawController(const AP_Vehicle::FixedWing &parms)
+    : aparm(parms)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+    _pid_info.target = 0;
+    _pid_info.FF = 0;
+    _pid_info.P = 0;
+    rate_pid.set_slew_limit_scale(45);
+}
+
 int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
 {
     uint32_t tnow = AP_HAL::millis();
@@ -315,12 +325,12 @@ float AP_YawController::get_rate_out(float desired_rate, float scaler, bool disa
     // remember the last output to trigger the I limit
     _last_out = out;
 
-#if 0
     if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {
+        // fake up an angular error based on a notional time constant of 0.5s
+        const float angle_err_deg = desired_rate * gains.tau;
         // let autotune have a go at the values
         autotune->update(pinfo, scaler, angle_err_deg);
     }
-#endif
 
     // output is scaled to notional centidegrees of deflection
     return constrain_float(out * 100, -4500, 4500);
@@ -330,3 +340,36 @@ void AP_YawController::reset_I()
 {
     _integrator = 0;
 }
+
+/*
+  start an autotune
+ */
+void AP_YawController::autotune_start(void)
+{
+    if (autotune == nullptr && rate_control_enabled()) {
+        // the autotuner needs a time constant. Use an assumed tconst of 0.5
+        gains.tau = 0.5;
+        gains.rmax_pos.set(90);
+
+        autotune = new AP_AutoTune(gains, AP_AutoTune::AUTOTUNE_YAW, aparm, rate_pid);
+        if (autotune == nullptr) {
+            if (!failed_autotune_alloc) {
+                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed yaw allocation");
+            }
+            failed_autotune_alloc = true;
+        }
+    }
+    if (autotune != nullptr) {
+        autotune->start();
+    }
+}
+
+/*
+  restore autotune gains
+ */
+void AP_YawController::autotune_restore(void)
+{
+    if (autotune != nullptr) {
+        autotune->stop();
+    }
+}

libraries/APM_Control/AP_YawController.h

@@ -4,18 +4,12 @@
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <AP_Logger/AP_Logger.h>
 #include <AC_PID/AC_PID.h>
+#include "AP_AutoTune.h"
 
 class AP_YawController
 {
 public:
-    AP_YawController(const AP_Vehicle::FixedWing &parms)
+    AP_YawController(const AP_Vehicle::FixedWing &parms);
-        : aparm(parms)
-    {
-        AP_Param::setup_object_defaults(this, var_info);
-        _pid_info.target = 0;
-        _pid_info.FF = 0;
-        _pid_info.P = 0;
-    }
 
     /* Do not allow copies */
     AP_YawController(const AP_YawController &other) = delete;
@@ -47,6 +41,10 @@ public:
         return _pid_info;
     }
 
+    // start/stop auto tuner
+    void autotune_start(void);
+    void autotune_restore(void);
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -67,5 +65,9 @@ private:
 
     float _integrator;
 
+    AP_AutoTune::ATGains gains;
+    AP_AutoTune *autotune;
+    bool failed_autotune_alloc;
+    
     AP_Logger::PID_Info _pid_info;
 };