Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

HTE: do not update the estimator during fast up/down motions

Browse Source 
Drag and prop wash effects produce significant forces at high speed
that can bias the estimator when applied for an extended period of time
This commit is contained in:
bresch
2020-12-16 11:34:31 +01:00
committed by Julian Kent
parent 39251daf28
commit 690c1158ad
1 changed files with 4 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
+4 -1
View File
@@ -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);