From c6b3f46e04436f70dd7aa8a3d8943ea6d08ee642 Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Mon, 18 Jan 2016 22:08:55 -0800 Subject: [PATCH] Sub: Enabled control modes to operate at zero throttle --- ArduSub/control_acro.cpp | 2 +- ArduSub/control_rov.cpp | 2 +- ArduSub/control_sport.cpp | 2 +- ArduSub/control_stabilize.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduSub/control_acro.cpp b/ArduSub/control_acro.cpp index 0c5c16b5d8..77fbd8fc23 100644 --- a/ArduSub/control_acro.cpp +++ b/ArduSub/control_acro.cpp @@ -27,7 +27,7 @@ void Sub::acro_run() int16_t pilot_throttle_scaled; // if motors not running reset angle targets - if(!motors.armed() || ap.throttle_zero) { + if(!motors.armed()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // slow start if landed if (ap.land_complete) { diff --git a/ArduSub/control_rov.cpp b/ArduSub/control_rov.cpp index fe197ebfde..417746b2b2 100644 --- a/ArduSub/control_rov.cpp +++ b/ArduSub/control_rov.cpp @@ -28,7 +28,7 @@ void Sub::rov_run() int16_t pilot_throttle_scaled; // if not armed or throttle at zero, set throttle to zero and exit immediately - if(!motors.armed() || ap.throttle_zero) { + if(!motors.armed()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // slow start if landed if (ap.land_complete) { diff --git a/ArduSub/control_sport.cpp b/ArduSub/control_sport.cpp index 4f2461ccfb..68dbfaea1d 100644 --- a/ArduSub/control_sport.cpp +++ b/ArduSub/control_sport.cpp @@ -33,7 +33,7 @@ void Sub::sport_run() pos_control.set_accel_z(g.pilot_accel_z); // if not armed or throttle at zero, set throttle to zero and exit immediately - if(!motors.armed() || ap.throttle_zero) { + if(!motors.armed()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); return; diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index 59cf807fba..f7b00aceba 100644 --- a/ArduSub/control_stabilize.cpp +++ b/ArduSub/control_stabilize.cpp @@ -28,7 +28,7 @@ void Sub::stabilize_run() int16_t pilot_throttle_scaled; // if not armed or throttle at zero, set throttle to zero and exit immediately - if(!motors.armed() || ap.throttle_zero) { + if(!motors.armed()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // slow start if landed if (ap.land_complete) {