fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-14 01:47:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
- 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
This commit is contained in:
Daniel Agar
2025-09-25 10:23:14 -04:00
parent 469fe62166
commit b3a1bfb28c
1 changed files with 18 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+18 -11
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))
@@ -129,7 +128,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
} else {
if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
if (do_conditional_range_aid) {
if (do_conditional_range_aid && starting_conditions_passing) {
// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
_height_sensor_ref = HeightSensor::RANGE;
@@ -142,7 +141,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
resetAidSourceStatusZeroInnovation(aid_src);
}
} else if (do_range_aid) {
} else if (do_range_aid && starting_conditions_passing) {
// Range finder is the primary height source, the ground is now the datum used
// to compute the local vertical position
ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
@@ -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) && starting_conditions_passing) {
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);
}