fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 07:59:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: range height skip "unhealthy" samples, but respect timeout (#25634)

Browse Source 
* ekf2: range height skip "unhealthy" samples, but respect timeout

 - we should never directly use an "unhealthy" range finder sample for
   state corrections or resets, but we also shouldn't immediately abort
   active rng_hgt until the timeout has passed

* check starting_conditions_passing once

---------

Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
This commit is contained in:
Daniel Agar 2025-09-29 15:23:37 -04:00 committed by GitHub
parent 03e6f40995
commit 900a5ede01
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 17 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View File

@ -81,6 +81,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
// If we are supposed to be using range finder data but have bad range measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_control_status.flags.in_air
&& _control_status.flags.vehicle_at_rest
&& _range_sensor.isRegularlySendingData()
&& _range_sensor.isDataReady()) {
@ -105,14 +106,12 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
const bool continuing_conditions_passing = ((_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
|| (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
&& _control_status.flags.tilt_align
&& measurement_valid
&& _range_sensor.isDataHealthy()
&& _rng_consistency_check.isKinematicallyConsistent();
&& measurement_valid;
const bool starting_conditions_passing = continuing_conditions_passing
&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
&& _range_sensor.isRegularlySendingData();
&& _range_sensor.isRegularlySendingData()
&& _range_sensor.isDataHealthy();
const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
&& (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
@ -127,7 +126,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
stopRngHgtFusion();
}
} else {
} else if (starting_conditions_passing) {
if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
if (do_conditional_range_aid) {
// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
@ -159,7 +158,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
}
} else {
if (do_conditional_range_aid || do_range_aid) {
if ((do_conditional_range_aid || do_range_aid)) {
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
_control_status.flags.rng_hgt = true;
@ -174,11 +173,19 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
if (continuing_conditions_passing) {
fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
if (_range_sensor.isDataHealthy()
&& _control_status.flags.rng_kin_consistent
) {
fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
}
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
if (isHeightResetRequired()
&& _control_status.flags.rng_hgt
&& (_height_sensor_ref == HeightSensor::RANGE)
&& starting_conditions_passing
) {
// All height sources are failing
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
@ -200,7 +207,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
stopRngHgtFusion();
stopRngTerrFusion();
} else {
} else if (starting_conditions_passing) {
resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(aid_src);
}