From b2459c67d5e63eed58aa5887eaf9605bcd7a68d6 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 10 Aug 2017 09:11:22 +1000
Subject: [PATCH] AP_Arming: move Copter RC checking code into library

---
 libraries/AP_Arming/AP_Arming.cpp | 54 +++++++++++++++++++++++++++++++
 libraries/AP_Arming/AP_Arming.h   |  3 ++
 2 files changed, 57 insertions(+)

diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
index 0c9c3e3996..5730129e4c 100644
--- a/libraries/AP_Arming/AP_Arming.cpp
+++ b/libraries/AP_Arming/AP_Arming.cpp
@@ -565,3 +565,57 @@ AP_Arming::ArmingRequired AP_Arming::arming_required()
 {
     return (AP_Arming::ArmingRequired)require.get();
 }
+
+// Copter and sub share the same RC input limits
+bool AP_Arming::rc_checks_copter_sub(const bool display_failure, const RC_Channel *channels[4]) const
+{
+    // set rc-checks to success if RC checks are disabled
+    if ((checks_to_perform != ARMING_CHECK_ALL) && !(checks_to_perform & ARMING_CHECK_RC)) {
+        return true;
+    }
+
+    bool ret = true;
+
+    const char *channel_names[] = { "Roll", "Pitch", "Throttle", "Yaw" };
+
+    for (uint8_t i=0; i<ARRAY_SIZE(channel_names);i++) {
+        const RC_Channel *channel = channels[i];
+        const char *channel_name = channel_names[i];
+        // check if radio has been calibrated
+        if (!channel->min_max_configured()) {
+            if (display_failure) {
+                gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: RC %s not configured", channel_name);
+            }
+            ret = false;
+        }
+        if (channel->get_radio_min() > 1300) {
+            if (display_failure) {
+                gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio min too high", channel_name);
+            }
+            ret = false;
+        }
+        if (channel->get_radio_max() < 1700) {
+            if (display_failure) {
+                gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio max too low", channel_name);
+            }
+            ret = false;
+        }
+        if (i == 2) {
+            // skip checking trim for throttle as older code did not check it
+            continue;
+        }
+        if (channel->get_radio_trim() < channel->get_radio_min()) {
+            if (display_failure) {
+                gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio trim below min", channel_name);
+            }
+            ret = false;
+        }
+        if (channel->get_radio_trim() > channel->get_radio_max()) {
+            if (display_failure) {
+                gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio trim above max", channel_name);
+            }
+            ret = false;
+        }
+    }
+    return ret;
+}
diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h
index 5fa1a46e08..7ebded343f 100644
--- a/libraries/AP_Arming/AP_Arming.h
+++ b/libraries/AP_Arming/AP_Arming.h
@@ -5,6 +5,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Param/AP_Param.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
+#include <RC_Channel/RC_Channel.h>
 
 class AP_Arming {
 public:
@@ -104,4 +105,6 @@ protected:
 
     virtual enum HomeState home_status() const = 0;
 
+    bool rc_checks_copter_sub(bool display_failure, const RC_Channel *channels[4]) const;
+
 };