fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 05:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

minor cleanup

Browse Source
This commit is contained in:
Marco Hauswirth
2025-05-19 14:45:35 +02:00
parent 10adbb3deb
commit a872c30ece
2 changed files with 12 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
+4 -1
View File
@@ -66,7 +66,10 @@ public:
void set_terrain_process_noise(float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; } void set_terrain_process_noise(float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; }
void reset() void reset()
{ {
_state = (_initialized && _state == KinematicState::CONSISTENT) ? KinematicState::UNKNOWN : _state; if (_initialized && _state == KinematicState::CONSISTENT) {
_state = KinematicState::UNKNOWN;
}
_initialized = false; _initialized = false;
} }
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+8 -16
View File
@@ -51,8 +51,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress()); rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(rng_data_ready); _range_sensor.setDataReadiness(rng_data_ready);
if (rng_data_ready) { if (_range_sensor.isDataReady()) {
// update range sensor angle parameters in case they have changed
_range_sensor.setPitchOffset(_params.rng_sens_pitch); _range_sensor.setPitchOffset(_params.rng_sens_pitch);
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s); _range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
@@ -82,20 +82,12 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(), _rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(),
dist_var, imu_sample.time_us); dist_var, imu_sample.time_us);
} else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) {
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setValidity(true);
} else { } else {
// If we are supposed to be using range finder data but have bad range measurements _rng_consistency_check.reset();
// and are on the ground, then synthesise a measurement at the expected on ground value
if (_range_sensor.isRegularlySendingData()
&& _range_sensor.isDataReady()) {
if (!_control_status.flags.in_air) {
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setValidity(true); // bypass the checks
} else {
_rng_consistency_check.reset();
}
}
} }
} }
@@ -109,7 +101,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
auto &aid_src = _aid_src_rng_hgt; auto &aid_src = _aid_src_rng_hgt;
if (rng_data_ready && _range_sensor.getSampleAddress()) { if (_range_sensor.isDataReady() && _range_sensor.getSampleAddress()) {
updateRangeHagl(aid_src); updateRangeHagl(aid_src);
const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance); const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);