From caf925eda58072bea2c1855878655415417b22a9 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 8 Apr 2019 10:15:57 +0200 Subject: [PATCH] Copter: factorize arm or land check --- ArduCopter/mode.cpp | 8 ++++++++ ArduCopter/mode.h | 1 + ArduCopter/mode_auto.cpp | 10 +++++----- ArduCopter/mode_brake.cpp | 2 +- ArduCopter/mode_circle.cpp | 2 +- ArduCopter/mode_follow.cpp | 2 +- ArduCopter/mode_guided.cpp | 6 +++--- ArduCopter/mode_land.cpp | 4 ++-- ArduCopter/mode_rtl.cpp | 8 ++++---- ArduCopter/mode_zigzag.cpp | 2 +- ArduCopter/takeoff.cpp | 2 +- 11 files changed, 28 insertions(+), 19 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 8f3c07216f940..564e42e752065 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -390,6 +390,14 @@ bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const return true; } +bool Copter::Mode::is_disarmed_or_landed() const +{ + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + return true; + } + return false; +} + void Copter::Mode::zero_throttle_and_relax_ac(bool spool_up) { if (spool_up) { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 0e6d8de6e42ff..403171c03e0f8 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -112,6 +112,7 @@ class Mode { void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const; // helper functions + bool is_disarmed_or_landed() const; void zero_throttle_and_relax_ac(bool spool_up = false); void zero_throttle_and_hold_attitude(); void make_safe_spool_down(); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 5b63bacc53ffe..505ae1c2940dd 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -740,7 +740,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) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); wp_nav->wp_and_spline_init(); return; @@ -770,7 +770,7 @@ void Copter::ModeAuto::wp_run() void Copter::ModeAuto::spline_run() { // if not armed set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); wp_nav->wp_and_spline_init(); return; @@ -811,7 +811,7 @@ void Copter::ModeAuto::land_run() { // if not armed set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); loiter_nav->init_target(); return; @@ -866,7 +866,7 @@ void Copter::ModeAuto::nav_guided_run() void Copter::ModeAuto::loiter_run() { // if not armed set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); wp_nav->wp_and_spline_init(); return; @@ -893,7 +893,7 @@ void Copter::ModeAuto::loiter_run() void Copter::ModeAuto::loiter_to_alt_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { + if (is_disarmed_or_landed() || !motors->get_interlock()) { zero_throttle_and_relax_ac(); return; } diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 0f972cda01787..7b5b90fe337d0 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -32,7 +32,7 @@ bool Copter::ModeBrake::init(bool ignore_checks) void Copter::ModeBrake::run() { // if not armed set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + if (is_disarmed_or_landed()) { 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 928fa92513165..f24bf258bd70b 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -48,7 +48,7 @@ void Copter::ModeCircle::run() } // if not armed set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); return; } diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index 8ed425eab00a2..b123c241af571 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -27,7 +27,7 @@ bool Copter::ModeFollow::init(const bool ignore_checks) void Copter::ModeFollow::run() { // if not armed set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); return; } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index c9eaa78705bdd..70a4e1c08be0f 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -426,7 +426,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) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); return; } @@ -468,7 +468,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) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); return; } @@ -521,7 +521,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) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); return; } diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 2148c24df18dc..fdb90c520c1c2 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -59,7 +59,7 @@ void Copter::ModeLand::gps_run() } // Land State Machine Determination - if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); loiter_nav->init_target(); } else { @@ -110,7 +110,7 @@ void Copter::ModeLand::nogps_run() } // Land State Machine Determination - if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); } else { // set motors to full range diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 9433e56fc4aeb..5e04d8614f9f2 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -128,7 +128,7 @@ void Copter::ModeRTL::return_start() 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) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); return; } @@ -185,7 +185,7 @@ void Copter::ModeRTL::loiterathome_start() void Copter::ModeRTL::loiterathome_run() { // if not armed set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); return; } @@ -257,7 +257,7 @@ void Copter::ModeRTL::descent_run() float target_yaw_rate = 0.0f; // if not armed set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); return; } @@ -362,7 +362,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) { + if (is_disarmed_or_landed()) { make_safe_spool_down(); loiter_nav->init_target(); return; diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index d53a40c3c4833..c7001475a9444 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -38,7 +38,7 @@ void Copter::ModeZigZag::run() pos_control->set_max_accel_z(g.pilot_accel_z); // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { + if (is_disarmed_or_landed() || !motors->get_interlock() ) { zero_throttle_and_relax_ac(copter.is_tradheli() && motors->get_interlock()); return; } diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 48356a0f098f8..0d71873f5ea95 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -138,7 +138,7 @@ void Copter::Mode::auto_takeoff_set_start_alt(void) // start with our current altitude auto_takeoff_no_nav_alt_cm = inertial_nav.get_altitude(); - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { + if (is_disarmed_or_landed() || !motors->get_interlock()) { // we are not flying, add the wp_navalt_min auto_takeoff_no_nav_alt_cm += g2.wp_navalt_min * 100; }