diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index fe93be6f3a..cbbadab5ff 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -814,6 +814,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 ce9a83554a..f1fa450a21 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -68,6 +68,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