From bb14ec1a2cdb7de5c6ef10ca233f0fd18854704a Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 30 May 2019 12:28:33 +1000
Subject: [PATCH] AP_Arming: make proximity sensor checks common

---
 libraries/AP_Arming/AP_Arming.cpp | 26 +++++++++++++++++++++++++-
 libraries/AP_Arming/AP_Arming.h   |  4 +++-
 2 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
index b65534569e..89bf10ea43 100644
--- a/libraries/AP_Arming/AP_Arming.cpp
+++ b/libraries/AP_Arming/AP_Arming.cpp
@@ -21,6 +21,7 @@
 #include <GCS_MAVLink/GCS.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_Mission/AP_Mission.h>
+#include <AP_Proximity/AP_Proximity.h>
 #include <AP_Rally/AP_Rally.h>
 #include <SRV_Channel/SRV_Channel.h>
 #include <AC_Fence/AC_Fence.h>
@@ -669,6 +670,28 @@ bool AP_Arming::system_checks(bool report)
     return true;
 }
 
+
+// check nothing is too close to vehicle
+bool AP_Arming::proximity_checks(bool report) const
+{
+    const AP_Proximity *proximity = AP::proximity();
+    // return true immediately if no sensor present
+    if (proximity == nullptr) {
+        return true;
+    }
+    if (proximity->get_status() == AP_Proximity::Proximity_NotConnected) {
+        return true;
+    }
+
+    // return false if proximity sensor unhealthy
+    if (proximity->get_status() < AP_Proximity::Proximity_Good) {
+        check_failed(ARMING_CHECK_NONE, report, "check proximity sensor");
+        return false;
+    }
+
+    return true;
+}
+
 bool AP_Arming::can_checks(bool report)
 {
 #if HAL_WITH_UAVCAN
@@ -752,7 +775,8 @@ bool AP_Arming::pre_arm_checks(bool report)
         &  servo_checks(report)
         &  board_voltage_checks(report)
         &  system_checks(report)
-        &  can_checks(report);
+        &  can_checks(report)
+        &  proximity_checks(report);
 }
 
 bool AP_Arming::arm_checks(AP_Arming::Method method)
diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h
index 8a28361809..6a6b182dc1 100644
--- a/libraries/AP_Arming/AP_Arming.h
+++ b/libraries/AP_Arming/AP_Arming.h
@@ -122,7 +122,9 @@ protected:
     virtual bool system_checks(bool report);
 
     bool can_checks(bool report);
-    
+
+    virtual bool proximity_checks(bool report) const;
+
     bool servo_checks(bool report) const;
     bool rc_checks_copter_sub(bool display_failure, const RC_Channel *channels[4]) const;