From 953c90d3a6ee9a85f93464136f4f74526c96347d Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 16 Dec 2021 14:29:52 +0100 Subject: [PATCH] failure detector: change imbalanced propeller metric Use standard deviations instead of variances to get a linear metric --- .../commander/failure_detector/FailureDetector.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 017da04b94..e57d4a940b 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -195,11 +195,12 @@ void FailureDetector::updateImbalancedPropStatus() _imbalanced_prop_lpf.setParameters(dt, _imbalanced_prop_lpf_time_constant); - const float var_x = imu_status.var_accel[0]; - const float var_y = imu_status.var_accel[1]; - const float var_z = imu_status.var_accel[2]; + 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 metric = var_x + var_y - var_z; + // 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; const float metric_lpf = _imbalanced_prop_lpf.update(metric); const bool is_imbalanced = metric_lpf > _param_fd_imb_prop_thr.get();