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

Merge pull request #193 from PX4/pr-haglInit

Browse Source 
Pr hagl init
This commit is contained in:
Paul Riseborough
2016-10-04 08:21:15 +11:00
committed by GitHub
parent 5702c2d51b c6e1d97176
commit b29174d86b
2 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+2 -2
View File
@@ -413,8 +413,8 @@ bool Ekf::initialiseFilter(void)
// initialise the state covariance matrix // initialise the state covariance matrix
initialiseCovariance(); initialiseCovariance();
// initialise the terrain estimator // try to initialise the terrain estimator
initHagl(); _terrain_initialised = initHagl();
// reset the essential fusion timeout counters // reset the essential fusion timeout counters
_time_last_hgt_fuse = _time_last_imu; _time_last_hgt_fuse = _time_last_imu;
EKF/terrain_estimator.cpp
+2 -2
View File
@@ -47,9 +47,9 @@ bool Ekf::initHagl()
// get most recent range measurement from buffer // get most recent range measurement from buffer
rangeSample latest_measurement = _range_buffer.get_newest(); rangeSample latest_measurement = _range_buffer.get_newest();
if ((_time_last_imu - latest_measurement.time_us) < 2e5) { if ((_time_last_imu - latest_measurement.time_us) < 2e5 && _R_to_earth(2,2) > 0.7071f) {
// if we have a fresh measurement, use it to initialise the terrain estimator // if we have a fresh measurement, use it to initialise the terrain estimator
_terrain_vpos = _state.pos(2) + latest_measurement.rng; _terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_to_earth(2, 2);
// initialise state variance to variance of measurement // initialise state variance to variance of measurement
_terrain_var = sq(_params.range_noise); _terrain_var = sq(_params.range_noise);
// success // success