Plane: move rudder-arming arm checks into Plane's AP_Arming

Author: peterbarker

ArduPlane/AP_Arming.cpp

@@ -136,6 +136,30 @@ bool AP_Arming_Plane::ins_checks(bool display_failure)
 
 bool AP_Arming_Plane::arm_checks(AP_Arming::Method method)
 {
+    if (method == AP_Arming::Method::RUDDER) {
+        const AP_Arming::RudderArming arming_rudder = get_rudder_arming_type();
+
+        if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) {
+            //parameter disallows rudder arming/disabling
+            check_failed(true, "Rudder arming disabled");
+            return false;
+        }
+
+        // if throttle is not down, then pilot cannot rudder arm/disarm
+        if (plane.get_throttle_input() != 0){
+            check_failed(true, "Non-zero throttle");
+            return;
+        }
+
+        // if not in a manual throttle mode and not in CRUISE or FBWB
+        // modes then disallow rudder arming/disarming
+        if (plane.auto_throttle_mode &&
+            (plane.control_mode != &mode_cruise && plane.control_mode != &mode_fbwb)) {
+            check_failed(true, "Mode not rudder-armable");
+            return;
+        }
+    }
+
     //are arming checks disabled?
     if (checks_to_perform == 0) {
         return true;
@@ -205,6 +229,17 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
  */
 bool AP_Arming_Plane::disarm(const AP_Arming::Method method)
 {
+    if (method == AP_Arming::Method::RUDDER) {
+        // don't allow rudder-disarming in flight:
+        if (is_flying()) {
+            return false;
+        }
+        // option must be enabled:
+        if (get_rudder_arming_type() != AP_Arming::RudderArming::ARMDISARM) {
+            return false;
+        }
+    }
+
     if (!AP_Arming::disarm(method)) {
         return false;
     }

ArduPlane/radio.cpp

@@ -119,27 +119,6 @@ void Plane::init_rc_out_aux()
 */
 void Plane::rudder_arm_disarm_check()
 {
-    AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type();
-
-    if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) {
-        //parameter disallows rudder arming/disabling
-        return;
-    }
-
-    // if throttle is not down, then pilot cannot rudder arm/disarm
-    if (get_throttle_input() != 0){
-        rudder_arm_timer = 0;
-        return;
-    }
-
-    // if not in a manual throttle mode and not in CRUISE or FBWB
-    // modes then disallow rudder arming/disarming
-    if (auto_throttle_mode &&
-        (control_mode != &mode_cruise && control_mode != &mode_fbwb)) {
-        rudder_arm_timer = 0;
-        return;      
-    }
-
 	if (!arming.is_armed()) {
 		// when not armed, full right rudder starts arming counter
 		if (channel_rudder->get_control_in() > 4000) {
@@ -160,26 +139,29 @@ void Plane::rudder_arm_disarm_check()
 			// not at full right rudder
 			rudder_arm_timer = 0;
 		}
-	} else if ((arming_rudder == AP_Arming::RudderArming::ARMDISARM) && !is_flying()) {
-		// when armed and not flying, full left rudder starts disarming counter
-		if (channel_rudder->get_control_in() < -4000) {
-			uint32_t now = millis();
-
-			if (rudder_arm_timer == 0 ||
-				now - rudder_arm_timer < 3000) {
-				if (rudder_arm_timer == 0) {
-                    rudder_arm_timer = now;
-                }
-			} else {
-				//time to disarm!
-				arming.disarm(AP_Arming::Method::RUDDER);
-				rudder_arm_timer = 0;
-			}
-		} else {
-			// not at full left rudder
-			rudder_arm_timer = 0;
-		}
+        return;
 	}
+
+    // full left rudder starts disarming counter
+    if (channel_rudder->get_control_in() >= -4000) {
+        // not at full left rudder
+        rudder_arm_timer = 0;
+        return;
+    }
+    const uint32_t now = millis();
+
+    if (rudder_arm_timer == 0) {
+        // start counter:
+        rudder_arm_timer = now;
+    }
+    if (now - rudder_arm_timer < 3000) {
+        // not yet time to disarm
+        return;
+    }
+
+    //time to disarm!
+    arming.disarm(AP_Arming::Method::RUDDER);
+    rudder_arm_timer = 0;
 }
 
 void Plane::read_radio()