From 94738c3f86e48b15d60f0e479b3b59028981357f Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Thu, 14 Mar 2019 19:45:00 -0400 Subject: [PATCH] Copter: change make_safe_shut_down to make_safe_spool_down --- ArduCopter/mode.cpp | 4 ++-- ArduCopter/mode.h | 2 +- ArduCopter/mode_auto.cpp | 16 +++++++--------- ArduCopter/mode_brake.cpp | 2 +- ArduCopter/mode_circle.cpp | 2 +- ArduCopter/mode_follow.cpp | 2 +- ArduCopter/mode_guided.cpp | 10 +++++----- ArduCopter/mode_land.cpp | 4 ++-- ArduCopter/mode_rtl.cpp | 8 ++++---- 9 files changed, 24 insertions(+), 26 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 8d721c7513..5bcd152d91 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -407,10 +407,10 @@ void Copter::Mode::zero_throttle_and_hold_attitude() attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt); } -void Copter::Mode::make_safe_shut_down() +void Copter::Mode::make_safe_spool_down() { // command aircraft to initiate the shutdown process - motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); switch (motors->get_spool_mode()) { case AP_Motors::SHUT_DOWN: diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index c23338ca5a..186b151e25 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -117,7 +117,7 @@ protected: // helper functions void zero_throttle_and_relax_ac(bool spool_up = false); void zero_throttle_and_hold_attitude(); - void make_safe_shut_down(); + void make_safe_spool_down(); // functions to control landing // in modes that support landing diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 37f5e75ebb..de9427177b 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -741,7 +741,7 @@ void Copter::ModeAuto::wp_run() // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); wp_nav->wp_and_spline_init(); return; } @@ -771,7 +771,7 @@ void Copter::ModeAuto::spline_run() { // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); wp_nav->wp_and_spline_init(); return; } @@ -809,14 +809,10 @@ void Copter::ModeAuto::spline_run() // called by auto_run at 100hz or more void Copter::ModeAuto::land_run() { - // disarm when the landing detector says we've landed - if (ap.land_complete && motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { - copter.init_disarm_motors(); - } // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); loiter_nav->init_target(); return; } @@ -871,7 +867,7 @@ void Copter::ModeAuto::loiter_run() { // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); wp_nav->wp_and_spline_init(); return; } @@ -1731,7 +1727,9 @@ bool Copter::ModeAuto::verify_loiter_to_alt() // returns true with RTL has completed successfully bool Copter::ModeAuto::verify_RTL() { - return (copter.mode_rtl.state_complete() && (copter.mode_rtl.state() == RTL_FinalDescent || copter.mode_rtl.state() == RTL_Land)); + return (copter.mode_rtl.state_complete() && + (copter.mode_rtl.state() == RTL_FinalDescent || copter.mode_rtl.state() == RTL_Land) && + (motors->get_spool_mode() == AP_Motors::GROUND_IDLE)); } /********************************************************************************/ diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 4977a8a9a8..309d441511 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -33,7 +33,7 @@ void Copter::ModeBrake::run() { // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE); return; } diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 8b478eb186..ba15742cd1 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -49,7 +49,7 @@ void Copter::ModeCircle::run() // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); return; } diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index b27bbb5fb2..b433ab0289 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -28,7 +28,7 @@ void Copter::ModeFollow::run() { // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); return; } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 01a23dad2f..b70217e60a 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -379,7 +379,7 @@ void Copter::Mode::auto_takeoff_run() { // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed) { - make_safe_shut_down(); + make_safe_spool_down(); wp_nav->shift_wp_origin_to_current_pos(); return; } @@ -427,7 +427,7 @@ void Copter::ModeGuided::pos_control_run() // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); return; } @@ -469,7 +469,7 @@ void Copter::ModeGuided::vel_control_run() // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); return; } @@ -522,7 +522,7 @@ void Copter::ModeGuided::posvel_control_run() // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); return; } @@ -606,7 +606,7 @@ void Copter::ModeGuided::angle_control_run() // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || (ap.land_complete && (guided_angle_state.climb_rate_cms <= 0.0f))) { - make_safe_shut_down(); + make_safe_spool_down(); return; } diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 13aa04691c..c23328f5b9 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -60,7 +60,7 @@ void Copter::ModeLand::gps_run() // Land State Machine Determination if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); loiter_nav->init_target(); } else { // set motors to full range @@ -111,7 +111,7 @@ void Copter::ModeLand::nogps_run() // Land State Machine Determination if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); } else { // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 3487972b9b..08cfc8d820 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -129,7 +129,7 @@ void Copter::ModeRTL::climb_return_run() { // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); return; } @@ -186,7 +186,7 @@ void Copter::ModeRTL::loiterathome_run() { // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); return; } @@ -258,7 +258,7 @@ void Copter::ModeRTL::descent_run() // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); return; } @@ -363,7 +363,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land) // if not armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete) { - make_safe_shut_down(); + make_safe_spool_down(); loiter_nav->init_target(); return; }