Browse Source

failure detector: change imbalanced propeller metric

Use standard deviations instead of variances to get a linear metric
master
bresch 3 years ago committed by Daniel Agar
parent
commit
953c90d3a6
  1. 9
      src/modules/commander/failure_detector/FailureDetector.cpp

9
src/modules/commander/failure_detector/FailureDetector.cpp

@ -195,11 +195,12 @@ void FailureDetector::updateImbalancedPropStatus() @@ -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();

Loading…
Cancel
Save