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

removed if statement that would prevent the update of the _rng_hgt_faulty flag (#480)

Browse Source
This commit is contained in:
dakejahl 2018-07-13 15:16:25 -06:00 committed by Lorenz Meier
parent 17d40478bb
commit 80146273ba
1 changed files with 7 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
EKF/control.cpp
View File

@ -110,17 +110,15 @@ void Ekf::controlFusionModes()
// Get range data from buffer and check validity
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed);
if (_range_data_ready) {
checkRangeDataValidity();
checkRangeDataValidity();
if (_range_data_ready && !_rng_hgt_faulty) {
// correct the range data for position offset relative to the IMU
Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
}
if (_range_data_ready && !_rng_hgt_faulty) {
// correct the range data for position offset relative to the IMU
Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
}
// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
// This means we stop looking for new data until the old data has been fused.
if (!_flow_data_ready) {