fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 03:04:07 +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
commit b29174d86b
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/ekf.cpp
View File

@ -413,8 +413,8 @@ bool Ekf::initialiseFilter(void)
// initialise the state covariance matrix
initialiseCovariance();
// initialise the terrain estimator
initHagl();
// try to initialise the terrain estimator
_terrain_initialised = initHagl();
// reset the essential fusion timeout counters
_time_last_hgt_fuse = _time_last_imu;

4
EKF/terrain_estimator.cpp
View File

@ -47,9 +47,9 @@ bool Ekf::initHagl()
// get most recent range measurement from buffer
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
_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
_terrain_var = sq(_params.range_noise);
// success