dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

HTE: relax validity condition when already valid (#17783) 

The condition to get valid requires a low variance and test ratio. but
to stay valid, only the variance is required.

Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Mathieu Bresciani 2021-06-23 07:33:11 +02:00 committed by GitHub
parent 74cc1d901d
commit 9cedb169fc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
View File

@ -172,8 +172,13 @@ void MulticopterHoverThrustEstimator::Run()
_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);
bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f);
// The test ratio does not need to pass all the time to have a valid estimate
if (!_valid) {
valid = valid && (_hover_thrust_ekf.getInnovationTestRatio() < 1.f);
}
_valid_hysteresis.set_state_and_update(valid, local_pos.timestamp);
_valid = _valid_hysteresis.get_state();