From 3132219457f743f5b05e1a1c15729a140e615ce3 Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Wed, 21 Jan 2026 18:57:19 +0000 Subject: [PATCH] Sub: Set throttle to neutral when outside ALT_HOLD. If the sub is disarmed in many modes, the output "throttle" (vertical control) is set to 0.0, corresponding to full downward thrust. While this does not affect manual control, it can affect subsequent mode changes (e.g. to ALT_HOLD). Set to a constant NEUTRAL_THROTTLE, set to 0.5, instead. --- ArduSub/config.h | 4 ++++ ArduSub/mode_acro.cpp | 2 +- ArduSub/mode_althold.cpp | 2 +- ArduSub/mode_auto.cpp | 6 +++--- ArduSub/mode_circle.cpp | 2 +- ArduSub/mode_guided.cpp | 8 ++++---- ArduSub/mode_manual.cpp | 2 +- ArduSub/mode_poshold.cpp | 4 ++-- ArduSub/mode_stabilize.cpp | 2 +- ArduSub/mode_surface.cpp | 2 +- 10 files changed, 19 insertions(+), 15 deletions(-) diff --git a/ArduSub/config.h b/ArduSub/config.h index db5c2f954f32f1..d2ee20d65120d6 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -183,6 +183,10 @@ # define AUTO_DISARMING_DELAY 0 #endif +#ifndef NEUTRAL_THROTTLE +# define NEUTRAL_THROTTLE (0.5f) // Throttle output for "no vertical thrust" +#endif + ////////////////////////////////////////////////////////////////////////////// // Logging control // diff --git a/ArduSub/mode_acro.cpp b/ArduSub/mode_acro.cpp index 29b1e61891667c..8df7ddb44c4872 100644 --- a/ArduSub/mode_acro.cpp +++ b/ArduSub/mode_acro.cpp @@ -23,7 +23,7 @@ void ModeAcro::run() // if not armed set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); return; } diff --git a/ArduSub/mode_althold.cpp b/ArduSub/mode_althold.cpp index cf5b2a952f508c..f1742d4b845406 100644 --- a/ArduSub/mode_althold.cpp +++ b/ArduSub/mode_althold.cpp @@ -38,7 +38,7 @@ void ModeAlthold::run_pre() if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) - attitude_control->set_throttle_out(0.5,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); position_control->relax_z_controller(motors.get_throttle_hover()); sub.last_pilot_heading = ahrs.yaw_sensor; diff --git a/ArduSub/mode_auto.cpp b/ArduSub/mode_auto.cpp index 57e086118982e4..8792db4bf86de2 100644 --- a/ArduSub/mode_auto.cpp +++ b/ArduSub/mode_auto.cpp @@ -111,7 +111,7 @@ void ModeAuto::auto_wp_run() // call attitude controller // Sub vehicles do not stabilize roll/pitch/yaw when disarmed motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); sub.wp_nav.wp_and_spline_init(); // Reset xy target return; @@ -302,7 +302,7 @@ void ModeAuto::auto_loiter_run() if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); sub.wp_nav.wp_and_spline_init(); // Reset xy target @@ -475,7 +475,7 @@ void ModeAuto::auto_terrain_recover_run() // if not armed set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); sub.loiter_nav.init_target(); // Reset xy target diff --git a/ArduSub/mode_circle.cpp b/ArduSub/mode_circle.cpp index 28be79dbad54cd..3af9b2a67abcb2 100644 --- a/ArduSub/mode_circle.cpp +++ b/ArduSub/mode_circle.cpp @@ -41,7 +41,7 @@ void ModeCircle::run() // To-Do: add some initialisation of position controllers motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); sub.circle_nav.init(); return; diff --git a/ArduSub/mode_guided.cpp b/ArduSub/mode_guided.cpp index ff1bb07df5681a..199a392162f459 100644 --- a/ArduSub/mode_guided.cpp +++ b/ArduSub/mode_guided.cpp @@ -457,7 +457,7 @@ void ModeGuided::guided_pos_control_run() if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); sub.wp_nav.wp_and_spline_init(); return; @@ -522,7 +522,7 @@ void ModeGuided::guided_vel_control_run() if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); // initialise velocity controller position_control->init_z_controller(); @@ -595,7 +595,7 @@ void ModeGuided::guided_posvel_control_run() if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); // initialise velocity controller position_control->init_z_controller(); @@ -675,7 +675,7 @@ void ModeGuided::guided_angle_control_run() if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control->set_throttle_out(0.0f,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); // initialise velocity controller position_control->init_z_controller(); diff --git a/ArduSub/mode_manual.cpp b/ArduSub/mode_manual.cpp index 9d6e29892ea91a..5b9da67eeea0e5 100644 --- a/ArduSub/mode_manual.cpp +++ b/ArduSub/mode_manual.cpp @@ -19,7 +19,7 @@ void ModeManual::run() // if not armed set throttle to zero and exit immediately if (!sub.motors.armed()) { sub.motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); return; } diff --git a/ArduSub/mode_poshold.cpp b/ArduSub/mode_poshold.cpp index a5cb3d9bfd050d..d9037abb7105e2 100644 --- a/ArduSub/mode_poshold.cpp +++ b/ArduSub/mode_poshold.cpp @@ -25,7 +25,7 @@ bool ModePoshold::init(bool ignore_checks) position_control->init_z_controller(); // Stop all thrusters - attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE ,true, g.throttle_filt); attitude_control->relax_attitude_controllers(); position_control->relax_z_controller(0.5f); @@ -43,7 +43,7 @@ void ModePoshold::run() if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) - attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE ,true, g.throttle_filt); attitude_control->relax_attitude_controllers(); position_control->init_xy_controller_stopping_point(); position_control->relax_z_controller(0.5f); diff --git a/ArduSub/mode_stabilize.cpp b/ArduSub/mode_stabilize.cpp index 52620b8d5d0b6a..fd11d4d9011f72 100644 --- a/ArduSub/mode_stabilize.cpp +++ b/ArduSub/mode_stabilize.cpp @@ -18,7 +18,7 @@ void ModeStabilize::run() // if not armed set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); sub.last_pilot_heading = ahrs.yaw_sensor; return; diff --git a/ArduSub/mode_surface.cpp b/ArduSub/mode_surface.cpp index 6bab07a067b860..cb56b4783b06f1 100644 --- a/ArduSub/mode_surface.cpp +++ b/ArduSub/mode_surface.cpp @@ -26,7 +26,7 @@ void ModeSurface::run() if (!motors.armed()) { motors.output_min(); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); position_control->init_z_controller(); return;