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

EKF: fix indexing bug (#387)

Browse Source 
Should use index 9 for vertical position.
This commit is contained in:
Paul Riseborough
2018-01-29 08:09:55 +11:00
committed by GitHub
parent 1ddd94f237
commit a2d6a4dded
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+1 -1
View File
@@ -656,7 +656,7 @@ void Ekf::controlHeightSensorTimeouts()
const baroSample& baro_init = _baro_buffer.get_newest();
bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate);
bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[9][9]) * sq(_params.baro_innov_gate);
// if baro data is acceptable and GPS data is inaccurate, reset height to baro
bool reset_to_baro = baro_data_consistent && baro_data_fresh && !_baro_hgt_faulty && !gps_hgt_accurate;