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

Allow rangefinder fusion in vision height mode (Fix for #994) (#999)

Browse Source 
Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Eike
2021-04-30 20:17:39 +02:00
committed by GitHub
parent 4ac57d3bde
commit a7b8afe420
1 changed files with 8 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+8 -11
View File
@@ -788,8 +788,7 @@ void Ekf::controlHeightSensorTimeouts()
// check if height has been inertial deadreckoning for too long // check if height has been inertial deadreckoning for too long
// in vision hgt mode check for vision data // in vision hgt mode check for vision data
const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6) || const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6);
(_control_status.flags.ev_hgt && !isRecent(_time_last_ext_vision, 5 * EV_MAX_INTERVAL));
if (hgt_fusion_timeout || continuous_bad_accel_hgt) { if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
@@ -1079,15 +1078,13 @@ void Ekf::controlHeightFusion()
} }
} }
if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { if (_control_status.flags.ev_hgt && _ev_data_ready) {
// switch to baro if there was a reset to baro fuse_height = true;
fuse_height = true; } else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
} fuse_height = true;
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// determine if we should use the vertical position observation fuse_height = true;
if (_control_status.flags.ev_hgt) { }
fuse_height = true;
}
break; break;
} }