From 457526ebe7c27190c49ba8f477283ecd28b8d8f0 Mon Sep 17 00:00:00 2001 From: Vasily Evseenko Date: Tue, 2 Aug 2016 10:30:40 +0400 Subject: [PATCH] Don't push bad values from lidar to EKF2 (#5196) Report terrain altitude --- src/modules/ekf2/ekf2_main.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index a613e18b61..67d12ef0a6 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -486,6 +486,9 @@ void Ekf2::task_main() if (range_finder_updated) { orb_copy(ORB_ID(distance_sensor), _range_finder_sub, &range_finder); + if (range_finder.min_distance >= range_finder.current_distance || range_finder.max_distance <= range_finder.current_distance) { + range_finder_updated = false; + } } orb_check(_ev_pos_sub, &vision_position_updated); @@ -816,9 +819,14 @@ void Ekf2::task_main() global_pos.eph = sqrt(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically - // TODO: implement terrain estimator - global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 - global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid + if (lpos.dist_bottom_valid) { + global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84 + global_pos.terrain_alt_valid = true; // Terrain altitude estimate is valid + } else { + global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 + global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid + } + // TODO use innovatun consistency check timouts to set this global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning