diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp index 0ea92585c0..6cc2434fbf 100644 --- a/libraries/AP_RPM/AP_RPM.cpp +++ b/libraries/AP_RPM/AP_RPM.cpp @@ -129,3 +129,18 @@ bool AP_RPM::healthy(uint8_t instance) const } return true; } + +/* + check if an instance is activated + */ +bool AP_RPM::enabled(uint8_t instance) const +{ + if (instance >= num_instances) { + return false; + } + // if no sensor type is selected, the sensor is not activated. + if (_type[instance] == RPM_TYPE_NONE) { + return false; + } + return true; +} diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h index 1f821837ff..ff6e988413 100644 --- a/libraries/AP_RPM/AP_RPM.h +++ b/libraries/AP_RPM/AP_RPM.h @@ -77,6 +77,8 @@ public: bool healthy(uint8_t instance) const; + bool enabled(uint8_t instance) const; + private: RPM_State state[RPM_MAX_INSTANCES]; AP_RPM_Backend *drivers[RPM_MAX_INSTANCES];