Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:36:48 +08:00
Code Issues Actions 5 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
2 changed files with 12 additions and 2 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
+9 -1
View File
@@ -182,11 +182,12 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
} }
} else { } 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); ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
_control_status.flags.rng_hgt = true; _control_status.flags.rng_hgt = true;
if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { 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); resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(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 (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
if (continuing_conditions_passing) { 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() if (_range_sensor.isDataHealthy()
&& _control_status.flags.rng_kin_consistent && _control_status.flags.rng_kin_consistent
) { ) {
src/modules/ekf2/EKF/height_control.cpp
+3 -1
View File
@@ -112,8 +112,10 @@ void Ekf::checkHeightSensorRefFallback()
|| ((fallback_list[i] == HeightSensor::GNSS) && _control_status.flags.gps_hgt) || ((fallback_list[i] == HeightSensor::GNSS) && _control_status.flags.gps_hgt)
|| ((fallback_list[i] == HeightSensor::RANGE) && _control_status.flags.rng_hgt) || ((fallback_list[i] == HeightSensor::RANGE) && _control_status.flags.rng_hgt)
|| ((fallback_list[i] == HeightSensor::EV) && _control_status.flags.ev_hgt)) { || ((fallback_list[i] == HeightSensor::EV) && _control_status.flags.ev_hgt)) {
ECL_INFO("fallback to secondary height reference");
_height_sensor_ref = fallback_list[i]; _height_sensor_ref = fallback_list[i];
ECL_WARN("fallback to secondary height reference %d", (int)_height_sensor_ref);
break; break;
} }
} }