fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 20:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

failure detector: change imbalanced propeller metric

Browse Source 
Use standard deviations instead of variances to get a linear metric
This commit is contained in:
bresch
2022-01-18 12:24:35 -05:00
committed by Daniel Agar
parent 408cf011b2
commit 953c90d3a6
1 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/failure_detector/FailureDetector.cpp
+5 -4
View File
@@ -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();