Disregard horizontal velocity if local_position.v_xy_valid is false

PX4 · Dec 19, 2024 · 5e0a392 · 5e0a392
1 parent 93c25ef
commit 5e0a392
Showing 1 changed file with 11 additions and 3 deletions.
diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
@@ -71,9 +71,17 @@ bool FixedwingLandDetector::_get_landed_state()
 
 	} else if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) {
 
-		// Horizontal velocity complimentary filter.
-		float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx +
-				_vehicle_local_position.vy * _vehicle_local_position.vy);
+		float val;
+
+		if (!_vehicle_local_position.v_xy_valid) {
+			// set _velocity_xy_filtered to 0 if data is invalid
+			val = 0.0f;
+		} else {
+			// Horizontal velocity complimentary filter.
+			val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx +
+			_vehicle_local_position.vy * _vehicle_local_position.vy);
+		}
+
 
 		if (PX4_ISFINITE(val)) {
 			_velocity_xy_filtered = val;