fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf: range fusion: fix height ref (#25654)

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

* ekf2: conditional range aid change height ref

---------

Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Jacob Dahl 2025-09-30 11:14:26 -08:00 committed by GitHub
parent e3309b9f87
commit f3b2599d33
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -182,11 +182,12 @@ 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;
if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
ECL_INFO("starting %s height fusion, resetting terrain", HGT_SRC_NAME);
resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(aid_src);
}
@ -197,6 +198,13 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
if (continuing_conditions_passing) {
if (do_conditional_range_aid) {
_height_sensor_ref = HeightSensor::RANGE;
} else if (_height_sensor_ref == HeightSensor::RANGE) {
_height_sensor_ref = HeightSensor::UNKNOWN;
}
if (_range_sensor.isDataHealthy()
&& _control_status.flags.rng_kin_consistent
) {

4
src/modules/ekf2/EKF/height_control.cpp
View File

@ -112,8 +112,10 @@ void Ekf::checkHeightSensorRefFallback()
|| ((fallback_list[i] == HeightSensor::GNSS) && _control_status.flags.gps_hgt)
|| ((fallback_list[i] == HeightSensor::RANGE) && _control_status.flags.rng_hgt)
|| ((fallback_list[i] == HeightSensor::EV) && _control_status.flags.ev_hgt)) {
ECL_INFO("fallback to secondary height reference");
_height_sensor_ref = fallback_list[i];
ECL_WARN("fallback to secondary height reference %d", (int)_height_sensor_ref);
break;
}
}