diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp index 47dd2feb60..ca124888a5 100644 --- a/libraries/AP_RPM/AP_RPM.cpp +++ b/libraries/AP_RPM/AP_RPM.cpp @@ -188,6 +188,18 @@ bool AP_RPM::enabled(uint8_t instance) const return true; } +/* + get RPM value, return true on success + */ +bool AP_RPM::get_rpm(uint8_t instance, float &rpm_value) const +{ + if (!healthy(instance)) { + return false; + } + rpm_value = state[instance].rate_rpm; + return true; +} + // singleton instance AP_RPM *AP_RPM::_singleton; diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h index b7d297a9a7..76150d75f4 100644 --- a/libraries/AP_RPM/AP_RPM.h +++ b/libraries/AP_RPM/AP_RPM.h @@ -75,12 +75,7 @@ public: /* return RPM for a sensor. Return -1 if not healthy */ - float get_rpm(uint8_t instance) const { - if (!healthy(instance)) { - return -1; - } - return state[instance].rate_rpm; - } + bool get_rpm(uint8_t instance, float &rpm_value) const; /* return signal quality for a sensor. diff --git a/libraries/AP_RPM/examples/RPM_generic/RPM_generic.cpp b/libraries/AP_RPM/examples/RPM_generic/RPM_generic.cpp index 283ad99687..7aacdcb689 100644 --- a/libraries/AP_RPM/examples/RPM_generic/RPM_generic.cpp +++ b/libraries/AP_RPM/examples/RPM_generic/RPM_generic.cpp @@ -56,9 +56,11 @@ void loop(void) sensor_state = '-'; } + float rpm = -1; + RPM.get_rpm(ii, rpm); hal.console->printf("%u - (%c) RPM: %8.2f Quality: %.2f ", ii, sensor_state, - (double)RPM.get_rpm(ii), + (double)rpm, (double)RPM.get_signal_quality(ii)); if (ii+1 < RPM.num_sensors()) {