From 891829d3a770bb98d7fd40ae1b0f7b39a188bbda Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 19:25:17 +0200
Subject: [PATCH] Land detector: Protect fixed wing logic from false-positives
 due to bad input data

---
 src/modules/land_detector/FixedwingLandDetector.cpp | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
index 6e8393617e..5f7ded9cb2 100644
--- a/src/modules/land_detector/FixedwingLandDetector.cpp
+++ b/src/modules/land_detector/FixedwingLandDetector.cpp
@@ -85,9 +85,16 @@ bool FixedwingLandDetector::update()
 	bool landDetected = false;
 
 	if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
-		_velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
+		float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
 					_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
-		_velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
+		if (isfinite(val)) {
+			_velocity_xy_filtered = val;
+		}
+		val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
+
+		if (isfinite(val)) {
+			_velocity_z_filtered = val;
+		}
 	}
 
 	if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {