From 3202a2e0a61b18f5a6470103eb9f31fc895ed22d Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Wed, 10 Apr 2019 00:32:15 -0700
Subject: [PATCH] AP_Rangefinder: Add arming checks

---
 libraries/AP_RangeFinder/RangeFinder.cpp | 12 ++++++++++++
 libraries/AP_RangeFinder/RangeFinder.h   |  3 +++
 2 files changed, 15 insertions(+)

diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index 7512c207d8..c10e3fabab 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -693,6 +693,18 @@ void RangeFinder::Log_RFND()
     }
 }
 
+bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const
+{
+    for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
+        if ((params[i].type != RangeFinder_TYPE_NONE) && (drivers[i] == nullptr)) {
+          hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %d was not detected", i + 1);
+          return false;
+        }
+    }
+
+    return true;
+}
+
 RangeFinder *RangeFinder::_singleton;
 
 namespace AP {
diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h
index afc6a98887..914f40ca73 100644
--- a/libraries/AP_RangeFinder/RangeFinder.h
+++ b/libraries/AP_RangeFinder/RangeFinder.h
@@ -111,6 +111,9 @@ public:
         return num_instances;
     }
 
+    // prearm checks
+    bool prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const;
+
     // detect and initialise any available rangefinders
     void init(enum Rotation orientation_default);