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