FLightTaskAuto: limit nudging speed based on distance sensor

PX4 · Feb 7, 2024 · 97cb933 · 97cb933
1 parent 584d8ab
commit 97cb933
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
@@ -281,6 +281,12 @@ void FlightTaskAuto::_prepareLandSetpoints()
 			sticks_xy.setZero();
 		}
 
+		// If ground distance estimate valid (distance sensor) during nudging then limit horizontal speed
+		if (PX4_ISFINITE(_dist_to_bottom)) {
+			// Below 50cm no horizontal speed, above allow per meter altitude 0.5m/s speed
+			max_speed = math::max(0.f, math::min(max_speed, (_dist_to_bottom - .5f) * .5f));
+		}
+
 		_stick_acceleration_xy.setVelocityConstraint(max_speed);
 		_stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position,
 				_velocity_setpoint_feedback.xy(), _deltatime);