forked from Archive/PX4-Autopilot
failure detector: negative guard sqrt in std dev calc
This commit is contained in:
parent
eb80e0410c
commit
4e8381e8cf
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue