Plane: move set_servos_idle functionality to ModeAuto

ArduPlane/Plane.h

@@ -494,11 +494,6 @@ private:
         // have we checked for an auto-land?
         bool checked_for_autoland;
 
-        // Altitude threshold to complete a takeoff command in autonomous modes.  Centimeters
-        // are we in idle mode? used for balloon launch to stop servo
-        // movement until altitude is reached
-        bool idle_mode;
-
         // are we in VTOL mode in AUTO?
         bool vtol_mode;
 
@@ -930,7 +925,6 @@ private:
     void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
     void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
     void do_loiter_time(const AP_Mission::Mission_Command& cmd);
-    void do_altitude_wait(const AP_Mission::Mission_Command& cmd);
     void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
     void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
     void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);

ArduPlane/commands_logic.cpp

@@ -23,9 +23,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         // except in a takeoff
         auto_state.takeoff_complete = true;
 
-        // start non-idle
-        auto_state.idle_mode = false;
-        
         nav_controller->set_data_is_stale();
 
         // reset loiter start time. New command is a new loiter
@@ -93,7 +90,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_NAV_ALTITUDE_WAIT:
-        do_altitude_wait(cmd);
         break;
 
 #if HAL_QUADPLANE_ENABLED
@@ -519,12 +515,6 @@ void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
     reset_offset_altitude();
 }
 
-void Plane::do_altitude_wait(const AP_Mission::Mission_Command& cmd)
-{
-    // set all servos to trim until we reach altitude or descent speed
-    auto_state.idle_mode = true;
-}
-
 void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
 {
     //set target alt  

ArduPlane/mode.h

@@ -225,6 +225,8 @@ public:
     void do_nav_delay(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
 
+    void run() override;
+
 protected:
 
     bool _enter() override;

ArduPlane/mode_auto.cpp

@@ -164,3 +164,19 @@ bool ModeAuto::is_landing() const
 {
     return (plane.flight_stage == AP_FixedWing::FlightStage::LAND);
 }
+
+void ModeAuto::run()
+{
+    if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT) {
+        // Wiggle servos
+        plane.set_servos_idle();
+
+        // Relax attitude control
+        reset_controllers();
+
+    } else {
+        // Normal flight, run base class
+        Mode::run();
+
+    }
+}

ArduPlane/servos.cpp

@@ -373,7 +373,6 @@ void Plane::set_servos_idle(void)
     SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, servo_value);
     SRV_Channels::set_output_to_trim(SRV_Channel::k_throttle);
 
-    SRV_Channels::output_ch_all();
 }
 
 
@@ -815,13 +814,6 @@ void Plane::set_servos(void)
     quadplane.update();
 #endif
 
-    if (control_mode == &mode_auto && auto_state.idle_mode) {
-        // special handling for balloon launch
-        set_servos_idle();
-        servos_output();
-        return;
-    }
-
     if (control_mode != &mode_manual) {
         set_servos_controlled();
         set_takeoff_expected();