Sub: modes keep controllers initialized when disarmed

ArduSub/control_auto.cpp

@@ -116,6 +116,7 @@ void Sub::auto_wp_run()
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
+        wp_nav.wp_and_spline_init();                                                // Reset xy target
         return;
     }
 
@@ -307,6 +308,7 @@ void Sub::auto_loiter_run()
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
 
+        wp_nav.wp_and_spline_init();                                                // Reset xy target
         return;
     }
 
@@ -591,6 +593,9 @@ void Sub::auto_terrain_recover_run()
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
+
+        loiter_nav.init_target();                                                   // Reset xy target
+        pos_control.relax_z_controller(motors.get_throttle_hover());                // Reset z axis controller
         return;
     }
 

ArduSub/control_circle.cpp

@@ -43,6 +43,7 @@ void Sub::circle_run()
         // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
+        circle_nav.init();
         return;
     }
 

ArduSub/control_guided.cpp

@@ -286,9 +286,7 @@ void Sub::guided_pos_control_run()
         // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
-        // initialise velocity controller
-        pos_control.init_z_controller();
-        pos_control.init_xy_controller();
+        wp_nav.wp_and_spline_init();
         return;
     }