Plane:expand yaw autotune to other modes

ArduPlane/Attitude.cpp

@@ -717,7 +717,7 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler)
             plane.guided_state.last_forced_rpy_ms.z > 0 &&
             millis() - plane.guided_state.last_forced_rpy_ms.z < 3000) {
         commanded_rudder = plane.guided_state.forced_rpy_cd.z;
-    } else if (control_mode == &mode_autotune && g.acro_yaw_rate > 0 && yawController.rate_control_enabled()) {
+    } else if (autotuning && g.acro_yaw_rate > 0 && yawController.rate_control_enabled()) {
         // user is doing an AUTOTUNE with yaw rate control
         const float rudd_expo = rudder_in_expo(true);
         const float yaw_rate = (rudd_expo/SERVO_MAX) * g.acro_yaw_rate;

ArduPlane/Plane.h

@@ -968,8 +968,10 @@ private:
     void autotune_restore(void);
     void autotune_enable(bool enable);
     bool fly_inverted(void);
+    bool mode_allows_autotuning(void);
     uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); }
     Mode *mode_from_mode_num(const enum Mode::Number num);
+    bool autotuning;
 
     // events.cpp
     void failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason);

ArduPlane/RC_Channel.cpp

@@ -405,7 +405,13 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
         break;
 
     case AUX_FUNC::FW_AUTOTUNE:
-        plane.autotune_enable(ch_flag == AuxSwitchPos::HIGH);
+        if (ch_flag == AuxSwitchPos::HIGH && plane.control_mode->mode_allows_autotuning()) {
+           plane.autotune_enable(true);
+        } else if (ch_flag == AuxSwitchPos::HIGH) {
+           GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Autotuning not allowed in this mode!");
+        } else {
+           plane.autotune_enable(false); 
+        }
         break;
 
     default:

ArduPlane/control_modes.cpp

@@ -174,6 +174,7 @@ void Plane::autotune_start(void)
         if (tune_yaw) { 
             yawController.autotune_start();
         }
+        autotuning = true;
         gcs().send_text(MAV_SEVERITY_INFO, "Autotuning %s%s%s", tune_roll?"roll ":"", tune_pitch?"pitch ":"", tune_yaw?"yaw":"");
     } else {
         gcs().send_text(MAV_SEVERITY_INFO, "No axis selected for tuning!");
@@ -188,7 +189,10 @@ void Plane::autotune_restore(void)
     rollController.autotune_restore();
     pitchController.autotune_restore();
     yawController.autotune_restore();
-    gcs().send_text(MAV_SEVERITY_INFO, "Stopped autotune");
+    if (autotuning) {
+        autotuning = false;
+        gcs().send_text(MAV_SEVERITY_INFO, "Stopped autotune");
+    }
 }
 
 /*

ArduPlane/mode.cpp

@@ -15,6 +15,11 @@ void Mode::exit()
 {
     // call sub-classes exit
     _exit();
+    // stop autotuning if not AUTOTUNE mode
+    if (plane.control_mode != &plane.mode_autotune){
+        plane.autotune_restore();
+    }
+
 }
 
 bool Mode::enter()

ArduPlane/mode.h

@@ -113,6 +113,10 @@ public:
     // true if the mode sets the vehicle destination, which controls
     // whether control input is ignored with STICK_MIXING=0
     virtual bool does_auto_throttle() const { return false; }
+    
+    // true if the mode supports autotuning (via switch for modes other
+    // that AUTOTUNE itself
+    virtual bool mode_allows_autotuning() const { return false; }
 
     // method for mode specific target altitude profiles
     virtual void update_target_altitude();
@@ -175,6 +179,8 @@ public:
     bool does_auto_navigation() const override;
 
     bool does_auto_throttle() const override;
+    
+    bool mode_allows_autotuning() const override { return true; }
 
 protected:
 
@@ -193,11 +199,12 @@ public:
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
+    
+    bool mode_allows_autotuning() const override { return true; }
 
 protected:
 
     bool _enter() override;
-    void _exit() override;
 };
 
 class ModeGuided : public Mode
@@ -281,6 +288,8 @@ public:
     bool allows_terrain_disable() const override { return true; }
 
     void update_target_altitude() override;
+    
+    bool mode_allows_autotuning() const override { return true; }
 
 protected:
 
@@ -406,6 +415,8 @@ public:
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
+    
+    bool mode_allows_autotuning() const override { return true; }
 
 };
 
@@ -425,6 +436,8 @@ public:
     void update() override;
 
     bool does_auto_throttle() const override { return true; }
+    
+    bool mode_allows_autotuning() const override { return true; }
 
     void update_target_altitude() override {};
 

ArduPlane/mode_autotune.cpp

@@ -8,11 +8,6 @@ bool ModeAutoTune::_enter()
     return true;
 }
 
-void ModeAutoTune::_exit()
-{
-    // restore last gains
-    plane.autotune_restore();
-}
 
 void ModeAutoTune::update()
 {

ArduPlane/system.cpp

@@ -299,11 +299,6 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
         return false;
     }
 
-    if (previous_mode == &mode_autotune) {
-        // restore last gains
-        autotune_restore();
-    }
-
     // exit previous mode
     old_mode.exit();