fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 01:07:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add vibration magnitude estimation

Browse Source
This commit is contained in:
Lorenz Meier
2016-02-25 16:54:24 +01:00
parent 5483e7a477
commit cd5857e900
4 changed files with 42 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
validation/data_validator.cpp
+4 -1
View File
@@ -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];
}