Copter: factorize arm or land check

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) {

ArduCopter/mode.h

@@ -112,6 +112,7 @@ protected:
     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();

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;
     }

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;

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;
     }

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;
     }

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;
     }

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

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;

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;
     }

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;
     }