From 822ee75e208728772afbe42b73e467a2cdc5410c Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 16 Jun 2022 17:13:08 +0900
Subject: [PATCH] AP_Mount: add healthy and pre_arm_checks

Co-authored-by: olliw42 <waldmanns@gmx.de>
---
 libraries/AP_Mount/AP_Mount.cpp       | 28 +++++++++++++++++++++++++++
 libraries/AP_Mount/AP_Mount.h         |  4 ++++
 libraries/AP_Mount/AP_Mount_Backend.h |  3 +++
 3 files changed, 35 insertions(+)

diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
index 222dfcc230..12bbc38ec9 100644
--- a/libraries/AP_Mount/AP_Mount.cpp
+++ b/libraries/AP_Mount/AP_Mount.cpp
@@ -734,6 +734,34 @@ void AP_Mount::send_mount_status(mavlink_channel_t chan)
     }
 }
 
+// run pre-arm check.  returns false on failure and fills in failure_msg
+// any failure_msg returned will not include a prefix
+bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
+{
+    // check type parameters
+    for (uint8_t i=0; i<AP_MOUNT_MAX_INSTANCES; i++) {
+        if ((state[i]._type != Mount_Type_None) && (_backends[i] == nullptr)) {
+            strncpy(failure_msg, "check TYPE", failure_msg_len);
+            return false;
+        }
+    }
+
+    // return true if no mount configured
+    if (_num_instances == 0) {
+        return true;
+    }
+
+    // check healthy
+    for (uint8_t i=0; i<AP_MOUNT_MAX_INSTANCES; i++) {
+        if ((_backends[i] != nullptr) && !_backends[i]->healthy()) {
+            strncpy(failure_msg, "not healthy", failure_msg_len);
+            return false;
+        }
+    }
+
+    return true;
+}
+
 // point at system ID sysid
 void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid)
 {
diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
index 42d9a39a8e..ebb42c910d 100644
--- a/libraries/AP_Mount/AP_Mount.h
+++ b/libraries/AP_Mount/AP_Mount.h
@@ -143,6 +143,10 @@ public:
     // send a MOUNT_STATUS message to GCS:
     void send_mount_status(mavlink_channel_t chan);
 
+    // run pre-arm check.  returns false on failure and fills in failure_msg
+    // any failure_msg returned will not include a prefix
+    bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len);
+
     // parameter var table
     static const struct AP_Param::GroupInfo        var_info[];
 
diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h
index db279e4dd2..d46a3bcea8 100644
--- a/libraries/AP_Mount/AP_Mount_Backend.h
+++ b/libraries/AP_Mount/AP_Mount_Backend.h
@@ -43,6 +43,9 @@ public:
     // used for gimbals that need to read INS data at full rate
     virtual void update_fast() {}
 
+    // return true if healthy
+    virtual bool healthy() const { return true; }
+
     // has_pan_control - returns true if this mount can control it's pan (required for multicopters)
     virtual bool has_pan_control() const = 0;