zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

master
dakejahl 7 years ago committed by Lorenz Meier
parent
17d40478bb
commit
80146273ba
1 changed files with 7 additions and 9 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 16
      EKF/control.cpp

16
EKF/control.cpp
Unescape View File

@ -110,17 +110,15 @@ void Ekf::controlFusionModes()
// Get range data from buffer and check validity // 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); _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) { if (_range_data_ready && !_rng_hgt_faulty) {
// correct the range data for position offset relative to the IMU // 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_body = _params.rng_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_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; _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. // 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. // This means we stop looking for new data until the old data has been fused.
if (!_flow_data_ready) { if (!_flow_data_ready) {

Write Preview
Loading…
Cancel
Save