From a998da1781ac17d13076306d65346b06d19cf0a0 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Fri, 3 Apr 2020 08:48:05 +0200
Subject: [PATCH] terrain_estimator: initialize with valid delayed measurement
---
EKF/terrain_estimator.cpp | 8 ++------
1 file changed, 2 insertions(+), 6 deletions(-)
diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp
index 74986f4d51..5f96da1875 100644
--- a/EKF/terrain_estimator.cpp
+++ b/EKF/terrain_estimator.cpp
@@ -46,8 +46,6 @@
bool Ekf::initHagl()
{
bool initialized = false;
- // get most recent range measurement from buffer
- const rangeSample &latest_measurement = _range_buffer.get_newest();
if (!_control_status.flags.in_air) {
// if on ground, do not trust the range sensor, but assume a ground clearance
@@ -57,11 +55,9 @@ bool Ekf::initHagl()
initialized = true;
} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
- && _range_sensor.isHealthy()
- && isRecent(latest_measurement.time_us, (uint64_t)2e5)) {
+ && _range_sensor.isDataHealthy()) {
// if we have a fresh measurement, use it to initialise the terrain estimator
- // TODO: the latest measurment did not go through the checks and could be invalid!
- _terrain_vpos = _state.pos(2) + latest_measurement.rng * _range_sensor.getCosTilt();
+ _terrain_vpos = _state.pos(2) + _range_sensor.getRange() * _range_sensor.getCosTilt();
// initialise state variance to variance of measurement
_terrain_var = sq(_params.range_noise);
// success