From ab7a9c907371fe44e3a4cf3f4014585a957b9172 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 29 Dec 2017 13:37:22 +1100 Subject: [PATCH] Copter: use zero_throttle_and_relax_ac in poshold, drift and autotune --- ArduCopter/mode_autotune.cpp | 3 +-- ArduCopter/mode_drift.cpp | 3 +-- ArduCopter/mode_poshold.cpp | 3 +-- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 4077fc0fea..986a5beef4 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -328,8 +328,7 @@ void Copter::ModeAutoTune::run() // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately // this should not actually be possible because of the init() checks if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt); + zero_throttle_and_relax_ac(); pos_control->relax_alt_hold_controllers(0.0f); return; } diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index e69d8b5f72..cdf000736f 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -48,8 +48,7 @@ void Copter::ModeDrift::run() // if landed and throttle at zero, set throttle to zero and exit immediately if (!motors->armed() || !motors->get_interlock() || (ap.land_complete && ap.throttle_zero)) { - motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt); + zero_throttle_and_relax_ac(); return; } diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 036e355902..cff98118d5 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -140,9 +140,8 @@ void Copter::ModePosHold::run() // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); wp_nav->init_loiter_target(); - attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt); + zero_throttle_and_relax_ac(); pos_control->relax_alt_hold_controllers(0.0f); return; }