From 7e5808a97bf7e42de348b5e579599b043e25432d Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 20 Jul 2022 10:36:39 +1000
Subject: [PATCH] AP_RangeFinder: add prearm for analogue/pwm pin conflicts

---
 libraries/AP_RangeFinder/AP_RangeFinder.cpp   | 26 +++++++++++++++++++
 .../AP_RangeFinder/AP_RangeFinder_Backend.h   |  4 +++
 2 files changed, 30 insertions(+)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp
index 5a62e5dd7e..7169c40231 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp
@@ -838,6 +838,32 @@ bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_le
             return false;
         }
 
+        // backend-specific checks.  This might end up drivers[i]->arming_checks(...).
+        switch (drivers[i]->allocated_type()) {
+        case Type::ANALOG:
+        case Type::PX4_PWM:
+        case Type::PWM: {
+            // ensure pin is configured
+            if (params[i].pin == -1) {
+                hal.util->snprintf(failure_msg, failure_msg_len, "RNGFND%u_PIN not set", unsigned(i + 1));
+                return false;
+            }
+            // ensure that the pin we're configured to use is available
+            if (!hal.gpio->valid_pin(params[i].pin)) {
+                uint8_t servo_ch;
+                if (hal.gpio->pin_to_servo_channel(params[i].pin, servo_ch)) {
+                    hal.util->snprintf(failure_msg, failure_msg_len, "RNGFND%u_PIN=%d, set SERVO%u_FUNCTION=-1", unsigned(i + 1), int(params[i].pin.get()), unsigned(servo_ch+1));
+                } else {
+                    hal.util->snprintf(failure_msg, failure_msg_len, "RNGFND%u_PIN=%d invalid", unsigned(i + 1), int(params[i].pin.get()));
+                }
+                return false;
+            }
+            break;
+        }
+        default:
+            break;
+        }
+
         switch (drivers[i]->status()) {
         case Status::NoData:
             hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %X: No Data", i + 1);
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h
index d1e6901cb3..e5e2a02d22 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h
@@ -69,6 +69,10 @@ public:
     // quality is not available
     virtual bool get_signal_quality_pct(uint8_t &quality_pct) const { return false; }
 
+    // return the actual type of the rangefinder, as opposed to the
+    // parameter value which may be changed at runtime.
+    RangeFinder::Type allocated_type() const { return _backend_type; }
+
 protected:
 
     // update status based on distance measurement