diff --git a/validation/data_validator.cpp b/validation/data_validator.cpp index 8c36cf6d2c..983ec2a439 100644 --- a/validation/data_validator.cpp +++ b/validation/data_validator.cpp @@ -55,6 +55,7 @@ DataValidator::DataValidator(DataValidator *prev_sibling) : _M2{0.0f}, _rms{0.0f}, _value{0.0f}, + _vibe{0.0f}, _value_equal_count(0), _sibling(prev_sibling) { @@ -112,8 +113,10 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, in } } + _vibe[i] = _vibe[i] * 0.99f + 0.01f * fabsf(val[i] - _lp[i]); + // XXX replace with better filter, make it auto-tune to update rate - _lp[i] = _lp[i] * 0.5f + val[i] * 0.5f; + _lp[i] = _lp[i] * 0.99f + 0.01f * val[i]; _value[i] = val[i]; } diff --git a/validation/data_validator.h b/validation/data_validator.h index 28914454c5..59e4472983 100644 --- a/validation/data_validator.h +++ b/validation/data_validator.h @@ -117,6 +117,12 @@ public: */ float* rms() { return _rms; } + /** + * Get the vibration offset + * @return the stored vibration offset + */ + float* vibration_offset() { return _vibe; } + /** * Print the validator value * @@ -154,6 +160,7 @@ private: float _M2[3]; /**< RMS component value */ float _rms[3]; /**< root mean square error */ float _value[3]; /**< last value */ + float _vibe[3]; /**< vibration level, in sensor unit */ float _value_equal_count; /**< equal values in a row */ DataValidator *_sibling; /**< sibling in the group */ const unsigned NORETURN_ERRCOUNT = 10000; /**< if the error count reaches this value, return sensor as invalid */ diff --git a/validation/data_validator_group.cpp b/validation/data_validator_group.cpp index 8bf060cf65..3f04180a77 100644 --- a/validation/data_validator_group.cpp +++ b/validation/data_validator_group.cpp @@ -197,6 +197,30 @@ DataValidatorGroup::get_vibration_factor(uint64_t timestamp) return vibe; } +float +DataValidatorGroup::get_vibration_offset(uint64_t timestamp, int axis) +{ + DataValidator *next = _first; + + float vibe = -1.0f; + + /* find the best vibration value of a non-timed out sensor */ + while (next != nullptr) { + + if (next->confidence(timestamp) > 0.5f) { + float* vibration_offset = next->vibration_offset(); + + if (vibe < 0.0f || vibration_offset[axis] < vibe) { + vibe = vibration_offset[axis]; + } + } + + next = next->sibling(); + } + + return vibe; +} + void DataValidatorGroup::print() { diff --git a/validation/data_validator_group.h b/validation/data_validator_group.h index 855c3ed4ae..dbc40f9ad9 100644 --- a/validation/data_validator_group.h +++ b/validation/data_validator_group.h @@ -74,6 +74,13 @@ public: */ float get_vibration_factor(uint64_t timestamp); + /** + * Get the vibration offset in the sensor unit + * + * @return float value representing the vibration offset + */ + float get_vibration_offset(uint64_t timestamp, int axis); + /** * Get the number of failover events *