From 9b2ef4f6f10cc690c707a4dfeb12c3bc1613a620 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 7 Dec 2020 10:47:39 +1100
Subject: [PATCH] Plane: move rudder-arming arm checks into Plane's AP_Arming

---
 ArduPlane/AP_Arming.cpp | 40 ++++++++++++++++++++++++++++++++++++++++
 ArduPlane/radio.cpp     | 27 +++------------------------
 2 files changed, 43 insertions(+), 24 deletions(-)

diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp
index e677e79386..62b0af8779 100644
--- a/ArduPlane/AP_Arming.cpp
+++ b/ArduPlane/AP_Arming.cpp
@@ -136,6 +136,33 @@ 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
+
+            // if we emit a message here then someone doing surface
+            // checks may be bothered by the message being emitted.
+            // 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 false;
+        }
+
+        // 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 != &plane.mode_cruise && plane.control_mode != &plane.mode_fbwb)) {
+            check_failed(true, "Mode not rudder-armable");
+            return false;
+        }
+    }
+
     //are arming checks disabled?
     if (checks_to_perform == 0) {
         return true;
@@ -205,6 +232,19 @@ 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 (plane.is_flying()) {
+            // obviously this could happen in-flight so we can't warn about it
+            return false;
+        }
+        // option must be enabled:
+        if (get_rudder_arming_type() != AP_Arming::RudderArming::ARMDISARM) {
+            gcs().send_text(MAV_SEVERITY_INFO, "Rudder disarm: disabled");
+            return false;
+        }
+    }
+
     if (!AP_Arming::disarm(method)) {
         return false;
     }
diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp
index 425bb55fef..bfc066811d 100644
--- a/ArduPlane/radio.cpp
+++ b/ArduPlane/radio.cpp
@@ -117,27 +117,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) {
@@ -158,8 +137,8 @@ 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
+	} else {
+		// full left rudder starts disarming counter
 		if (channel_rudder->get_control_in() < -4000) {
 			uint32_t now = millis();
 
@@ -177,7 +156,7 @@ void Plane::rudder_arm_disarm_check()
 			// not at full left rudder
 			rudder_arm_timer = 0;
 		}
-	}
+    }
 }
 
 void Plane::read_radio()