From 690c1158addf03cfd29be10945dca644707309e7 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 16 Dec 2020 11:34:31 +0100 Subject: [PATCH] HTE: do not update the estimator during fast up/down motions Drag and prop wash effects produce significant forces at high speed that can bias the estimator when applied for an extended period of time --- .../MulticopterHoverThrustEstimator.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 9e9da09fb9..2b0bb94381 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -164,7 +164,10 @@ void MulticopterHoverThrustEstimator::Run() if (PX4_ISFINITE(local_pos_sp.thrust[2])) { // Inform the hover thrust estimator about the measured vertical // acceleration (positive acceleration is up) and the current thrust (positive thrust is up) - _hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]); + // Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects + if (fabsf(local_pos.vz) < 2.f) { + _hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]); + } const bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f) && (_hover_thrust_ekf.getInnovationTestRatio() < 1.f);