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);