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

ekf2-terr: allow range fusion before takeoff

This commit is contained in:
bresch 2023-11-16 14:19:26 +01:00 committed by Daniel Agar
parent 0f3378e194
commit 6320ca64a9
1 changed files with 7 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/ekf2/EKF/terrain_estimator.cpp
View File

@ -45,14 +45,6 @@
void Ekf::initHagl()
{
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
stopHaglFlowFusion();
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_RANGE_FINDER)
stopHaglRngFusion();
#endif // CONFIG_EKF2_RANGE_FINDER
// assume a ground clearance
_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
@ -74,6 +66,12 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
if (!_control_status.flags.in_air) {
_last_on_ground_posD = _state.pos(2);
_control_status.flags.rng_fault = false;
} else if (!_control_status_prev.flags.in_air) {
// Let the estimator run freely before arming for bench testing purposes, but reset on takeoff
// because when using optical flow measurements, it is safer to start with a small distance to ground
// as an overestimated distance leads to an overestimated velocity, causing a dangerous behavior.
initHagl();
}
predictHagl(imu_delayed);
@ -125,9 +123,8 @@ void Ekf::controlHaglRngFusion()
if (_range_sensor.isDataHealthy()) {
const bool continuing_conditions_passing = _control_status.flags.in_air
const bool continuing_conditions_passing = _rng_consistency_check.isKinematicallyConsistent();
//&& !_control_status.flags.rng_hgt // TODO: should not be fused when using range height
&& _rng_consistency_check.isKinematicallyConsistent();
const bool starting_conditions_passing = continuing_conditions_passing
&& _range_sensor.isRegularlySendingData()