diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp index 3623fb7158..74b0dadcbd 100644 --- a/libraries/AP_RPM/AP_RPM.cpp +++ b/libraries/AP_RPM/AP_RPM.cpp @@ -46,6 +46,12 @@ const AP_Param::GroupInfo AP_RPM::var_info[] = { // @Increment: 1 AP_GROUPINFO("_MIN", 3, AP_RPM, _minimum[0], 0), + // @Param: _MIN_QUAL + // @DisplayName: Minimum Quality + // @Description: Minimum data quality to be used + // @Increment: 0.1 + AP_GROUPINFO("_MIN_QUAL", 4, AP_RPM, _quality_min[0], 0.5), + #if RPM_MAX_INSTANCES > 1 // @Param: 2_TYPE // @DisplayName: Second RPM type @@ -133,6 +139,12 @@ bool AP_RPM::healthy(uint8_t instance) const if (AP_HAL::millis() - state[instance].last_reading_ms > 1000) { return false; } + + // check that data quality is above minimum required + if (state[instance].signal_quality < _quality_min[0]) { + return false; + } + return true; } diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h index ac76742d81..9cf86dde23 100644 --- a/libraries/AP_RPM/AP_RPM.h +++ b/libraries/AP_RPM/AP_RPM.h @@ -53,6 +53,7 @@ public: AP_Float _scaling[RPM_MAX_INSTANCES]; AP_Float _maximum[RPM_MAX_INSTANCES]; AP_Float _minimum[RPM_MAX_INSTANCES]; + AP_Float _quality_min[RPM_MAX_INSTANCES]; static const struct AP_Param::GroupInfo var_info[];