dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

failure detector: negative guard sqrt in std dev calc

This commit is contained in:
Thomas Stastny 2022-11-24 16:13:27 +01:00 committed by Daniel Agar
parent eb80e0410c
commit 4e8381e8cf
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/commander/failure_detector/FailureDetector.cpp
View File

@ -329,9 +329,9 @@ void FailureDetector::updateImbalancedPropStatus()
_imbalanced_prop_lpf.setParameters(dt, _imbalanced_prop_lpf_time_constant);
const float std_x = sqrtf(imu_status.var_accel[0]);
const float std_y = sqrtf(imu_status.var_accel[1]);
const float std_z = sqrtf(imu_status.var_accel[2]);
const float std_x = sqrtf(math::max(imu_status.var_accel[0], 0.f));
const float std_y = sqrtf(math::max(imu_status.var_accel[1], 0.f));
const float std_z = sqrtf(math::max(imu_status.var_accel[2], 0.f));
// Note: the metric is done using standard deviations instead of variances to be linear
const float metric = (std_x + std_y) / 2.f - std_z;