Skip to content

Commit fc872d8

Browse files
amarburgamarburg
committed
Sub: When disarmed and in manual mode, set throttle out to neutral value.
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.
1 parent 4f4457d commit fc872d8

File tree

10 files changed

+19
-15
lines changed

10 files changed

+19
-15
lines changed

‎ArduSub/config.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -189,6 +189,10 @@
189189
# define AUTO_DISARMING_DELAY 0
190190
#endif
191191

192+
#ifndef NEUTRAL_THROTTLE
193+
# define NEUTRAL_THROTTLE (0.5f) // Throttle output for "no vertical thrust"
194+
#endif
195+
192196
//////////////////////////////////////////////////////////////////////////////
193197
// Logging control
194198
//

‎ArduSub/mode_acro.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@ void ModeAcro::run()
1919
// if not armed set throttle to zero and exit immediately
2020
if (!motors.armed()) {
2121
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
22-
attitude_control->set_throttle_out(0,true,g.throttle_filt);
22+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
2323
attitude_control->relax_attitude_controllers();
2424
return;
2525
}

‎ArduSub/mode_althold.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ void ModeAlthold::run_pre()
4040
if (!motors.armed()) {
4141
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
4242
// Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
43-
attitude_control->set_throttle_out(0.5,true,g.throttle_filt);
43+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
4444
attitude_control->relax_attitude_controllers();
4545
position_control->D_relax_controller(motors.get_throttle_hover());
4646
sub.last_pilot_heading_rad = ahrs.get_yaw_rad();

‎ArduSub/mode_auto.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -111,7 +111,7 @@ void ModeAuto::auto_wp_run()
111111
// call attitude controller
112112
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
113113
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
114-
attitude_control->set_throttle_out(0,true,g.throttle_filt);
114+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
115115
attitude_control->relax_attitude_controllers();
116116
sub.wp_nav.wp_and_spline_init_m(); // Reset xy target
117117
return;
@@ -302,7 +302,7 @@ void ModeAuto::auto_loiter_run()
302302
if (!motors.armed()) {
303303
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
304304
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
305-
attitude_control->set_throttle_out(0,true,g.throttle_filt);
305+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
306306
attitude_control->relax_attitude_controllers();
307307

308308
sub.wp_nav.wp_and_spline_init_m(); // Reset xy target
@@ -479,7 +479,7 @@ void ModeAuto::auto_terrain_recover_run()
479479
// if not armed set throttle to zero and exit immediately
480480
if (!motors.armed()) {
481481
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
482-
attitude_control->set_throttle_out(0,true,g.throttle_filt);
482+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
483483
attitude_control->relax_attitude_controllers();
484484

485485
sub.loiter_nav.init_target(); // Reset xy target

‎ArduSub/mode_circle.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ void ModeCircle::run()
4646
// To-Do: add some initialisation of position controllers
4747
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
4848
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
49-
attitude_control->set_throttle_out(0,true,g.throttle_filt);
49+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
5050
attitude_control->relax_attitude_controllers();
5151
sub.circle_nav.init();
5252
return;

‎ArduSub/mode_guided.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -464,7 +464,7 @@ void ModeGuided::guided_pos_control_run()
464464
if (!motors.armed()) {
465465
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
466466
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
467-
attitude_control->set_throttle_out(0,true,g.throttle_filt);
467+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
468468
attitude_control->relax_attitude_controllers();
469469
sub.wp_nav.wp_and_spline_init_m();
470470
return;
@@ -529,7 +529,7 @@ void ModeGuided::guided_vel_control_run()
529529
if (!motors.armed()) {
530530
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
531531
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
532-
attitude_control->set_throttle_out(0,true,g.throttle_filt);
532+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
533533
attitude_control->relax_attitude_controllers();
534534
// initialise velocity controller
535535
position_control->D_init_controller();
@@ -602,7 +602,7 @@ void ModeGuided::guided_posvel_control_run()
602602
if (!motors.armed()) {
603603
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
604604
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
605-
attitude_control->set_throttle_out(0,true,g.throttle_filt);
605+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
606606
attitude_control->relax_attitude_controllers();
607607
// initialise velocity controller
608608
position_control->D_init_controller();
@@ -682,7 +682,7 @@ void ModeGuided::guided_angle_control_run()
682682
if (!motors.armed()) {
683683
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
684684
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
685-
attitude_control->set_throttle_out(0.0f,true,g.throttle_filt);
685+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
686686
attitude_control->relax_attitude_controllers();
687687
// initialise velocity controller
688688
position_control->D_init_controller();

‎ArduSub/mode_manual.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@ void ModeManual::run()
1919
// if not armed set throttle to zero and exit immediately
2020
if (!sub.motors.armed()) {
2121
sub.motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
22-
attitude_control->set_throttle_out(0,true,g.throttle_filt);
22+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
2323
attitude_control->relax_attitude_controllers();
2424
return;
2525
}

‎ArduSub/mode_poshold.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ bool ModePoshold::init(bool ignore_checks)
2626
position_control->D_init_controller();
2727

2828
// Stop all thrusters
29-
attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt);
29+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE ,true, g.throttle_filt);
3030
attitude_control->relax_attitude_controllers();
3131
position_control->D_relax_controller(0.5f);
3232

@@ -44,7 +44,7 @@ void ModePoshold::run()
4444
if (!motors.armed()) {
4545
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
4646
// Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
47-
attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt);
47+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE ,true, g.throttle_filt);
4848
attitude_control->relax_attitude_controllers();
4949
position_control->NE_init_controller_stopping_point();
5050
position_control->D_relax_controller(0.5f);

‎ArduSub/mode_stabilize.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ void ModeStabilize::run()
1818
// if not armed set throttle to zero and exit immediately
1919
if (!motors.armed()) {
2020
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
21-
attitude_control->set_throttle_out(0,true,g.throttle_filt);
21+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
2222
attitude_control->relax_attitude_controllers();
2323
sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
2424
return;

‎ArduSub/mode_surface.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ void ModeSurface::run()
2525
if (!motors.armed()) {
2626
motors.output_min();
2727
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
28-
attitude_control->set_throttle_out(0,true,g.throttle_filt);
28+
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
2929
attitude_control->relax_attitude_controllers();
3030
position_control->D_init_controller();
3131
return;

0 commit comments

Comments
 (0)