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

move faultiness check above pop_first_older_than() and check independent of range aid to switch back to original sensor if available

Browse Source
This commit is contained in:
ChristophTobler
2017-06-29 17:07:58 +02:00
parent 0d9e535acb
commit e1274ad28a
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
@@ -79,6 +79,14 @@ void Ekf::controlFusionModes()
}
// check faultiness (before pop_first_older_than) to see if we can change back to original height sensor
baroSample baro_init = _baro_buffer.get_newest();
_baro_hgt_faulty = !((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
gpsSample gps_init = _gps_buffer.get_newest();
_gps_hgt_faulty = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
rangeSample rng_init = _range_buffer.get_newest();
_rng_hgt_faulty = !((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
// check for arrival of new sensor data at the fusion time horizon
_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
@@ -479,17 +487,6 @@ void Ekf::controlHeightSensorTimeouts()
// check if height has been inertial deadreckoning for too long
bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > 5e6);
// if in range aid mode, check faultiness that otherwise would never change back
if (_params.range_aid) {
// check if range finder data is available
rangeSample rng_init = _range_buffer.get_newest();
_rng_hgt_faulty = ((_time_last_imu - rng_init.time_us) > 2 * RNG_MAX_INTERVAL);
// check if GPS height is available
gpsSample gps_init = _gps_buffer.get_newest();
_gps_hgt_faulty = ((_time_last_imu - gps_init.time_us) > 2 * GPS_MAX_INTERVAL);
}
// reset the vertical position and velocity states
if ((P[9][9] > sq(_params.hgt_reset_lim)) && (hgt_fusion_timeout || continuous_bad_accel_hgt)) {
// boolean that indicates we will do a height reset