From 114628afe4c7b178f7cc157a8980e8e656cb26b4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 29 Dec 2017 13:36:18 +1100 Subject: [PATCH] Copter: use zero_throttle_and_relax_ac in stab, guided, acro and auto --- ArduCopter/mode_acro.cpp | 3 +-- ArduCopter/mode_auto.cpp | 10 +--------- ArduCopter/mode_guided.cpp | 10 +--------- ArduCopter/mode_stabilize.cpp | 3 +-- 4 files changed, 4 insertions(+), 22 deletions(-) diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index 8a12a260d9..acf47e747f 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -26,8 +26,7 @@ void Copter::ModeAcro::run() // if not armed set throttle to zero and exit immediately if (!motors->armed() || ap.throttle_zero || !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(); return; } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index c2d89f38b3..3af5a772ed 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -803,15 +803,7 @@ bool Copter::ModeAuto::payload_place_run_should_run() void Copter::ModeAuto::payload_place_run() { if (!payload_place_run_should_run()) { -#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw - // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); - attitude_control->set_throttle_out(0,false,g.throttle_filt); -#else - motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // multicopters do not stabilize roll/pitch/yaw when disarmed - attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt); -#endif + zero_throttle_and_relax_ac(); // set target to current position wp_nav->init_loiter_target(); return; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 68174f6dd7..8e6ebf3398 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -363,15 +363,7 @@ void Copter::ModeGuided::takeoff_run() if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { // initialise wpnav targets wp_nav->shift_wp_origin_to_current_pos(); -#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw - // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); - attitude_control->set_throttle_out(0,false,g.throttle_filt); -#else // multicopters do not stabilize roll/pitch/yaw when disarmed - motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // reset attitude control targets - attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt); -#endif + zero_throttle_and_relax_ac(); // clear i term when we're taking off set_throttle_takeoff(); return; diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index 8e7d72c1a1..ed91b4b606 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -28,8 +28,7 @@ void Copter::ModeStabilize::run() // if not armed set throttle to zero and exit immediately if (!motors->armed() || ap.throttle_zero || !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(); return; }