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

ekf2: estimator aid source status (range height)

Browse Source
This commit is contained in:
Daniel Agar
2022-05-25 09:25:12 -04:00
parent 1e25aee6fa
commit 58ea6235fe
9 changed files with 63 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+8 -6
View File
@@ -125,8 +125,8 @@ void Ekf::controlFusionModes()
if (_range_buffer) {
// Get range data from buffer and check validity
bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(is_rng_data_ready);
_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(_rng_data_ready);
// update range sensor angle parameters in case they have changed
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
@@ -959,13 +959,15 @@ void Ekf::controlHeightFusion()
}
}
if (_control_status.flags.rng_hgt) {
if (_rng_data_ready) {
updateRngHgt(_aid_src_rng_hgt);
if (_range_sensor.isDataHealthy()) {
fuseRngHgt();
if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
fuseRngHgt(_aid_src_rng_hgt);
}
}
} else if (_control_status.flags.ev_hgt) {
if (_control_status.flags.ev_hgt) {
if (_control_status.flags.ev_hgt && _ev_data_ready) {
fuseEvHgt();