make terrain estimate invalid after range sensor timeout

src/lib/terrain_estimation/terrain_estimator.cpp

@@ -38,6 +38,8 @@
 
 #include "terrain_estimator.h"
 
+#define DISTANCE_TIMEOUT 100000		// time in usec after which laser is considered dead
+
 TerrainEstimator::TerrainEstimator() :
 	_distance_last(0.0f),
 	_terrain_valid(false),
@@ -100,9 +102,14 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
 	       B * R * B.transposed() + Q) * dt;
 }
 
-void TerrainEstimator::measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance,
+void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance,
 				const struct vehicle_attitude_s *attitude)
 {
+	// terrain estimate is invalid if we have range sensor timeout
+	if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) {
+		_terrain_valid = false;
+	}
+
 	if (distance->timestamp > _time_last_distance) {
 
 		float d = distance->current_distance;

src/lib/terrain_estimation/terrain_estimator.h

@@ -65,7 +65,7 @@ public:
 
 	void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor,
 		const struct distance_sensor_s *distance);
-	void measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance,
+	void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance,
 				const struct vehicle_attitude_s *attitude);
 
 private:

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -702,7 +702,7 @@ void AttitudePositionEstimatorEKF::task_main()
 
 					// Run separate terrain estimator
 					_terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance);
-					_terrain_estimator->measurement_update(&_gps, &_distance, &_att);
+					_terrain_estimator->measurement_update(hrt_absolute_time(), &_gps, &_distance, &_att);
 
 					// Publish attitude estimations
 					publishAttitude();