MPC: LAND: create Stop motion primitive to also be used elsewhere

msg/VehicleConstraints.msg

@@ -5,5 +5,6 @@ uint64 timestamp # time since system start (microseconds)
 
 float32 speed_up # in meters/sec
 float32 speed_down # in meters/sec
+float32 speed_xy # in meters/sec
 
 bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight)

src/lib/motion_planning/VelocitySmoothing.hpp

@@ -43,7 +43,6 @@ struct Trajectory {
 /**
  * @class VelocitySmoothing
  *
- * TODO: document the algorithm
  *    |T1| T2 |T3|
  *     ___
  *   __| |____   __ Jerk

src/modules/flight_mode_manager/CMakeLists.txt

@@ -42,6 +42,7 @@ list(APPEND flight_tasks_all
 	Auto
 	Descend
 	Failsafe
+	Land
 	ManualAcceleration
 	ManualAccelerationSlow
 	ManualAltitude

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -153,6 +153,7 @@ void FlightModeManager::start_flight_task()
 	bool matching_task_running = true;
 	bool task_failure = false;
 	const bool nav_state_descend = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
+	const bool nav_state_land = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND);
 
 	// Follow me
 	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
@@ -185,9 +186,20 @@ void FlightModeManager::start_flight_task()
 		}
 	}
 
+	// Landing
+	if (nav_state_land) {
+		found_some_task = true;
+		FlightTaskError error = switchTask(FlightTaskIndex::Land);
+
+		if (error != FlightTaskError::NoError) {
+			matching_task_running = false;
+			task_failure = true;
+		}
+	}
+
 	// Navigator interface for autonomous modes
 	if (_vehicle_control_mode_sub.get().flag_control_auto_enabled
-	    && !nav_state_descend) {
+	    && !nav_state_descend && !nav_state_land) {
 		found_some_task = true;
 
 		if (switchTask(FlightTaskIndex::Auto) != FlightTaskError::NoError) {

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -231,6 +231,8 @@ void FlightTaskAuto::rcHelpModifyYaw(float &yaw_sp)
 
 void FlightTaskAuto::_prepareLandSetpoints()
 {
+	// IMPORTANT: This Landing logic is only used for Precision Landing, for the other Landing see FlightTaskLand
+
 	_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
 
 	// Slow down automatic descend close to ground
@@ -810,6 +812,12 @@ void FlightTaskAuto::_updateTrajConstraints()
 	_position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get());
 	_position_smoothing.setMaxJerk(_param_mpc_jerk_auto.get()); // TODO : Should be computed using heading
 
+	// Stretch the constraints of the velocity controller to leave some room for an additional
+	// correction required by the altitude/vertical position controller
+	_constraints.speed_down = 1.2f * _param_mpc_z_v_auto_dn.get();
+	_constraints.speed_up = 1.2f * _param_mpc_xy_vel_max.get();
+	_constraints.speed_xy = 1.2f * _param_mpc_xy_vel_max.get();
+
 	if (_is_emergency_braking_active) {
 		// When initializing with large velocity, allow 1g of
 		// acceleration in 1s on all axes for fast braking
@@ -821,6 +829,7 @@ void FlightTaskAuto::_updateTrajConstraints()
 		// cutting out the feedforward
 		_constraints.speed_down = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _constraints.speed_down);
 		_constraints.speed_up = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _constraints.speed_up);
+		_constraints.speed_xy = math::max(_position_smoothing.getCurrentVelocityXY().norm(), _constraints.speed_xy);
 
 	} else if (_unsmoothed_velocity_setpoint(2) < 0.f) { // up
 		float z_accel_constraint = _param_mpc_acc_up_max.get();
@@ -847,11 +856,6 @@ void FlightTaskAuto::_updateTrajConstraints()
 		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
 		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get());
 	}
-
-	// Stretch the constraints of the velocity controller to leave some room for an additional
-	// correction required by the altitude/vertical position controller
-	_constraints.speed_down = math::max(_constraints.speed_down, 1.2f * _param_mpc_z_v_auto_dn.get());;
-	_constraints.speed_up = math::max(_constraints.speed_up, 1.2f * _param_mpc_z_v_auto_up.get());;
 }
 
 bool FlightTaskAuto::_highEnoughForLandingGear()

src/modules/flight_mode_manager/tasks/Land/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(FlightTaskLand
+	FlightTaskLand.cpp
+)
+
+target_link_libraries(FlightTaskLand PUBLIC FlightTask FlightTaskUtility)
+target_include_directories(FlightTaskLand PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp

@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskLand.cpp
+ */
+
+#include "FlightTaskLand.hpp"
+
+bool
+FlightTaskLand::activate(const trajectory_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTask::activate(last_setpoint);
+	Vector3f vel_prev{last_setpoint.velocity};
+	Vector3f pos_prev{last_setpoint.position};
+	Vector3f accel_prev{last_setpoint.acceleration};
+
+	for (int i = 0; i < 3; i++) {
+		// If the position setpoint is unknown, set to the current position
+		if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); }
+
+		// If the velocity setpoint is unknown, set to the current velocity
+		if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); }
+
+		// If no acceleration estimate available, set to zero if the setpoint is NAN
+		if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
+
+
+	}
+
+	_yaw_setpoint  = _land_heading = _yaw; // set the yaw setpoint to the current yaw
+
+	_acceleration_setpoint = accel_prev;
+	_velocity_setpoint = vel_prev;
+	_position_setpoint = pos_prev;
+
+	_stop.initialize(_acceleration_setpoint, _velocity_setpoint, _position, _deltatime);
+
+	_initial_land_position = _land_position = _stop.getStopPosition();
+
+	return ret;
+}
+
+void
+FlightTaskLand::reActivate()
+{
+	FlightTask::reActivate();
+}
+
+bool
+FlightTaskLand::update()
+{
+	bool ret = FlightTask::update();
+
+	if (!_stop.isActive() && !_landing) {
+		_landing = true;
+	}
+
+	if (_landing) {
+		_PerformLanding();
+
+	} else {
+		_PerformBraking();
+	}
+
+	return ret;
+}
+
+void
+FlightTaskLand::_PerformLanding()
+{
+	// Perform 3 phase landing
+	_velocity_setpoint.setNaN();
+
+	// Calculate the vertical speed based on the distance to the ground
+	float vertical_speed = math::interpolate(_dist_to_ground,
+			       _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+			       _param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get());
+
+	bool range_dist_available = PX4_ISFINITE(_dist_to_bottom);
+
+	// If we are below the altitude of the third landing phase , use the crawl speed
+	if (range_dist_available && _dist_to_bottom <= _param_mpc_land_alt3.get()) {
+		vertical_speed = _param_mpc_land_crawl_speed.get();
+	}
+
+	// User input assisted landing
+	if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
+		// Stick full up -1 -> stop, stick full down 1 -> double the speed
+		vertical_speed *= (1 - _sticks.getThrottleZeroCenteredExpo());
+
+		if (fabsf(_sticks.getYawExpo()) > FLT_EPSILON) {
+			_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, _sticks.getYawExpo(), _yaw, _deltatime);
+		}
+
+		Vector2f sticks_xy = _sticks.getPitchRollExpo();
+		Vector2f sticks_ne = sticks_xy;
+		Sticks::rotateIntoHeadingFrameXY(sticks_ne, _yaw, _land_heading);
+
+		const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(),
+						 _param_mpc_land_radius.get(), sticks_ne);
+		float max_speed;
+
+		if (PX4_ISFINITE(distance_to_circle)) {
+			max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(),
+					_stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f);
+
+			if (max_speed < 0.5f) {
+				sticks_xy.setZero();
+			}
+
+		} else {
+			max_speed = 0.f;
+			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);
+		_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
+
+	} else {
+		// Make sure we have a valid land position even in the case we loose RC while amending it
+		if (!PX4_ISFINITE(_land_position(0))) {
+			_land_position.xy() = Vector2f(_position);
+		}
+	}
+
+	_position_setpoint = {_land_position(0), _land_position(1), NAN}; // The last element of the land position has to stay NAN
+	_yaw_setpoint = _land_heading;
+	_velocity_setpoint(2) = vertical_speed;
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+
+}
+
+void
+FlightTaskLand::_PerformBraking()
+{
+	_stop.getConstraints(_constraints);
+	_stop.update(_acceleration_setpoint, _velocity_setpoint, _position, _deltatime);
+
+	_initial_land_position = _land_position = _stop.getStopPosition();
+	_jerk_setpoint = _stop.getJerkSetpoint();
+	_acceleration_setpoint = _stop.getAccelerationSetpoint();
+	_velocity_setpoint = _stop.getVelocitySetpoint();
+	_position_setpoint = _stop.getPositionSetpoint();
+	_yaw_setpoint = _land_heading;
+}
+
+void
+FlightTaskLand::updateParams()
+{
+	FlightTask::updateParams();
+
+	// make sure that alt1 is above alt2
+	_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
+}

src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp

@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskLand.hpp
+ *
+ * Flight task for landing
+ *
+ * @author Claudio Chies <claudio@chies.com>
+ */
+#pragma once
+
+#include "FlightTask.hpp"
+#include <lib/motion_planning/PositionSmoothing.hpp>
+#include <lib/geo/geo.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include "Sticks.hpp"
+#include "StickAccelerationXY.hpp"
+#include "StickYaw.hpp"
+#include "Stop.hpp"
+
+using matrix::Vector3f;
+using matrix::Vector2f;
+
+class FlightTaskLand : public FlightTask
+{
+public:
+	FlightTaskLand() = default;
+	virtual ~FlightTaskLand() = default;
+
+	bool update() override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	void reActivate() override;
+
+protected:
+	void updateParams() override;
+
+	Sticks _sticks{this};
+	StickAccelerationXY _stick_acceleration_xy{this};
+	StickYaw _stick_yaw{this};
+	Stop _stop{this};
+
+	uORB::SubscriptionData<home_position_s>		_sub_home_position{ORB_ID(home_position)};
+	uORB::SubscriptionData<vehicle_status_s>	_sub_vehicle_status{ORB_ID(vehicle_status)};
+	uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+
+					(ParamFloat<px4::params::MPC_LAND_ALT1>) 	_param_mpc_land_alt1,
+					(ParamFloat<px4::params::MPC_LAND_ALT2>) 	_param_mpc_land_alt2,
+					(ParamFloat<px4::params::MPC_LAND_ALT3>) 	_param_mpc_land_alt3,
+					(ParamFloat<px4::params::MPC_LAND_CRWL>) 	_param_mpc_land_crawl_speed,
+					(ParamFloat<px4::params::MPC_LAND_RADIUS>) 	_param_mpc_land_radius,
+					(ParamInt<px4::params::MPC_LAND_RC_HELP>) 	_param_mpc_land_rc_help,
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) 	_param_mpc_land_speed
+				       );
+private:
+	void _PerformLanding();
+	void _PerformBraking();
+
+	bool _landing{false};
+
+	Vector3f _initial_land_position;
+	Vector3f _land_position;
+	float _land_heading{0.0f};
+};

src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt

@@ -36,6 +36,7 @@ px4_add_library(FlightTaskUtility
 	StickAccelerationXY.cpp
 	StickTiltXY.cpp
 	StickYaw.cpp
+	Stop.cpp
 )
 
 target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib)

src/modules/flight_mode_manager/tasks/Utility/Stop.cpp

@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+#include "Stop.hpp"
+
+Stop::Stop(ModuleParams *parent) :
+	ModuleParams(parent)
+{
+	// update params of the position smoothing
+	_position_smoothing.setCruiseSpeed(_param_mpc_xy_vel_max.get());
+	_position_smoothing.setHorizontalTrajectoryGain(_param_mpc_xy_traj_p.get());
+	_position_smoothing.setMaxAllowedHorizontalError(_param_mpc_xy_err_max.get());
+	_position_smoothing.setTargetAcceptanceRadius(_param_nav_mc_alt_rad.get());
+	_position_smoothing.setVerticalAcceptanceRadius(_param_nav_mc_alt_rad.get());
+
+	// Update the constraints of the trajectories
+	_position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get());
+	_position_smoothing.setMaxVelocityZ(math::max(_param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()));
+	_position_smoothing.setMaxAccelerationXY(_param_mpc_acc_hor.get());
+	_position_smoothing.setMaxAccelerationZ(math::max(_param_mpc_acc_down_max.get(), _param_mpc_acc_up_max.get()));
+	_position_smoothing.setMaxJerkXY(_param_mpc_jerk_auto.get());
+	_position_smoothing.setMaxJerkZ(_param_mpc_jerk_max.get());
+}
+
+void Stop::getConstraints(vehicle_constraints_s &constraints)
+{
+	constraints.speed_down = 1.2f * _param_mpc_z_vel_max_dn.get();
+	constraints.speed_up = 1.2f * _param_mpc_z_vel_max_up.get();
+	constraints.speed_xy = 1.2f * _param_mpc_xy_vel_max.get();
+	_position_smoothing.setMaxJerkXY(_param_mpc_jerk_auto.get());
+	_position_smoothing.setMaxJerkZ(_param_mpc_jerk_max.get());
+
+	if (_exceeded_max_vel) {
+		constraints.speed_down = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), constraints.speed_down);
+		constraints.speed_up = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), constraints.speed_up);
+		constraints.speed_xy = math::max(_position_smoothing.getCurrentVelocityXY().norm(), constraints.speed_xy);
+
+		_position_smoothing.setMaxAccelerationXY(CONSTANTS_ONE_G);
+		_position_smoothing.setMaxAccelerationZ(2 * CONSTANTS_ONE_G);
+		_position_smoothing.setMaxJerk(CONSTANTS_ONE_G);
+		_position_smoothing.setMaxJerkZ(10.f * CONSTANTS_ONE_G); // Jerk in Z direction is only limited by motor inertia.
+	}
+}
+
+void
+Stop::initialize(const Vector3f &acceleration, const Vector3f &velocity, const Vector3f &position,
+		 const float &deltatime)
+{
+	if (velocity.isAllNan() || position.isAllNan() || acceleration.isAllNan()) {
+		PX4_ERR("Initialize stop with valid values");
+	}
+
+	_isActive = true;
+	_position_smoothing.reset(acceleration, velocity, position);
+	update(acceleration, velocity, position, deltatime);
+}
+
+void
+Stop::update(const Vector3f &acceleration, const Vector3f &velocity, const Vector3f &position, const float &deltatime)
+{
+	if (checkMaxVelocityLimit(velocity) && !_exceeded_max_vel) {
+		_exceeded_max_vel = true;
+
+	} else if (_position_smoothing.getCurrentVelocityZ() < 0.01f
+		   && _position_smoothing.getCurrentVelocityZ() > -0.01f
+		   && !_position_smoothing.getCurrentVelocityXY().longerThan(0.01f)) {
+		_isActive = false;
+		_stop_position = position;
+	}
+
+	PositionSmoothing::PositionSmoothingSetpoints out_setpoints;
+	// Generate the setpoints
+	_position_smoothing.generateSetpoints(
+		position,
+		_stop_position,
+		Vector3f{0.f, 0.f, 0.f},
+		deltatime,
+		true,
+		out_setpoints
+	);
+	_jerk_setpoint = out_setpoints.jerk;
+	_acceleration_setpoint = out_setpoints.acceleration;
+	_velocity_setpoint = out_setpoints.velocity;
+	_position_setpoint = out_setpoints.position;
+	_unsmoothed_velocity = out_setpoints.unsmoothed_velocity;
+}
+
+bool
+Stop::checkMaxVelocityLimit(const Vector3f &velocity, const float &factor)
+{
+	const bool exceeded_vel_z = fabsf(velocity(2)) > math::max(_param_mpc_z_vel_max_dn.get(),
+				    _param_mpc_z_vel_max_up.get());
+	const bool exceeded_vel_xy = velocity.xy().norm() > _param_mpc_xy_vel_max.get();
+
+	return (exceeded_vel_xy || exceeded_vel_z);
+}

src/modules/flight_mode_manager/tasks/Utility/Stop.hpp

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file StickYaw.hpp
+ * @brief Generate setpoints to Stop the vehicle smoothly from any given initial velocity
+ * @author Claudio Chies <claudio@chies.com>
+ */
+#include <px4_platform_common/module_params.h>
+#include <lib/motion_planning/PositionSmoothing.hpp>
+#include <uORB/topics/vehicle_constraints.h>
+#include <matrix/matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <px4_platform_common/log.h>
+
+using matrix::Vector3f;
+using matrix::Vector2f;
+#pragma once
+/**
+ * This utility class generates setpoints which slow down the vehicle smoothly from any given initial velocity
+ * to use it, first call initialize(), and then update() at every iteration.
+ * it is important to use the getConstraints() to adjust the PositionController constraints if we exceed the maximum velocity in the
+ * current flighttask.
+ */
+class Stop : public ModuleParams
+{
+public:
+	Stop(ModuleParams *parent);
+	~Stop() = default;
+
+	void initialize(const Vector3f &acceleration, const Vector3f &velocity, const Vector3f &position,
+			const float &deltatime);
+	void update(const Vector3f &acceleration, const Vector3f &velocity, const Vector3f &position, const float &deltatime);
+
+	/** @brief the the current constraints based on the current state of the vehicle, and update the constraints in the FlightTask by passing the reference
+	 */
+	void getConstraints(vehicle_constraints_s &constraints);
+
+	bool checkMaxVelocityLimit(const Vector3f &velocity, const float &factor = 1.0f);
+
+	// Getters
+	Vector3f getPositionSetpoint() const 		{ return _position_setpoint; }
+	Vector3f getVelocitySetpoint() const 		{ return _velocity_setpoint; }
+	Vector3f getAccelerationSetpoint() const 	{ return _acceleration_setpoint; }
+	Vector3f getJerkSetpoint() const 		{ return _jerk_setpoint; }
+	Vector3f getUnsmoothedVelocity() const 		{ return _unsmoothed_velocity; }
+	Vector3f getStopPosition() const 		{ return _stop_position; }
+
+	bool isActive() const {return _isActive;};
+private:
+
+	PositionSmoothing _position_smoothing;
+
+	Vector3f _stop_position;
+	bool _exceeded_max_vel{false}; // true if we exceed the maximum velcoity of the auto flight task.
+	bool _isActive{false};		// true if the condition for braking is still valid
+
+	Vector3f _jerk_setpoint;
+	Vector3f _acceleration_setpoint;
+	Vector3f _velocity_setpoint;
+	Vector3f _position_setpoint;
+	Vector3f _unsmoothed_velocity;
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) 	_param_mpc_acc_down_max,
+		(ParamFloat<px4::params::MPC_ACC_HOR>) 		_param_mpc_acc_hor,
+		(ParamFloat<px4::params::MPC_ACC_UP_MAX>) 	_param_mpc_acc_up_max,
+		(ParamFloat<px4::params::MPC_JERK_AUTO>) 	_param_mpc_jerk_auto,
+		(ParamFloat<px4::params::MPC_JERK_MAX>) 	_param_mpc_jerk_max,
+		(ParamFloat<px4::params::MPC_XY_ERR_MAX>) 	_param_mpc_xy_err_max,
+		(ParamFloat<px4::params::MPC_XY_TRAJ_P>) 	_param_mpc_xy_traj_p,
+		(ParamFloat<px4::params::MPC_XY_VEL_MAX>) 	_param_mpc_xy_vel_max,
+		(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) 	_param_mpc_z_vel_max_dn,
+		(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) 	_param_mpc_z_vel_max_up,
+		(ParamFloat<px4::params::NAV_MC_ALT_RAD>) 	_param_nav_mc_alt_rad
+	);
+};

src/modules/mc_pos_control/GotoControl/GotoControl.cpp

@@ -128,6 +128,7 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
 		.timestamp = goto_setpoint.timestamp,
 		.speed_up = NAN,
 		.speed_down = NAN,
+		.speed_xy = NAN,
 		.want_takeoff = false
 	};
 	_vehicle_constraints_pub.publish(vehicle_constraints);

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -530,7 +530,8 @@ void MulticopterPositionControl::Run()
 			const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f;
 			_control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get());
 
-			float max_speed_xy = _param_mpc_xy_vel_max.get();
+			float max_speed_xy = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_xy :
+					     _param_mpc_xy_vel_max.get();
 
 			if (PX4_ISFINITE(vehicle_local_position.vxy_max)) {
 				max_speed_xy = math::min(max_speed_xy, vehicle_local_position.vxy_max);

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -119,6 +119,7 @@ private:
 		.timestamp = 0,
 		.speed_up = NAN,
 		.speed_down = NAN,
+		.speed_xy = NAN,
 		.want_takeoff = false,
 	};
 

src/modules/navigator/land.cpp

@@ -57,12 +57,6 @@ Land::on_activation()
 
 	/* convert mission item to current setpoint */
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
-	    && _navigator->get_local_position()->xy_global) { // only execute if global position is valid
-		_navigator->preproject_stop_point(_mission_item.lat, _mission_item.lon);
-	}
-
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->next.valid = false;