fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-06 17:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: conditional range aid change height ref

Browse Source
This commit is contained in:
Daniel Agar
2025-09-25 18:36:13 -04:00
parent 2ce8258468
commit 2b8fa62936
3 changed files with 17 additions and 6 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
+8
View File
@@ -163,6 +163,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_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);
} }
@@ -173,6 +174,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
+4 -1
View File
@@ -112,8 +112,11 @@ 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;
} }
} }
src/modules/ekf2/EKF2.cpp
+5 -5
View File
@@ -1059,12 +1059,12 @@ void EKF2::PublishBaroBias(const hrt_abstime &timestamp)
if (_ekf.aid_src_baro_hgt().timestamp_sample != 0) { if (_ekf.aid_src_baro_hgt().timestamp_sample != 0) {
const BiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus(); const BiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus();
if (fabsf(status.bias - _last_baro_bias_published) > 1e-6f) { //if (fabsf(status.bias - _last_baro_bias_published) > 1e-6f) {
_estimator_baro_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.aid_src_baro_hgt().timestamp_sample, timestamp, _estimator_baro_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.aid_src_baro_hgt().timestamp_sample, timestamp,
_device_id_baro)); _device_id_baro));
_last_baro_bias_published = status.bias; _last_baro_bias_published = status.bias;
} //}
} }
} }
#endif // CONFIG_EKF2_BAROMETER #endif // CONFIG_EKF2_BAROMETER