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) {