added RC Landing nudging

.vscode/settings.json

@@ -129,5 +129,15 @@
         "yaml.schemas": {
                 "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
         },
-        "ros.distro": "humble"
+        "ros.distro": "humble",
+        "python.autoComplete.extraPaths": [
+                "/home/claudio_chies/ros2_ws/install/px4_msgs/local/lib/python3.10/dist-packages",
+                "/opt/ros/humble/lib/python3.10/site-packages",
+                "/opt/ros/humble/local/lib/python3.10/dist-packages"
+        ],
+        "python.analysis.extraPaths": [
+                "/home/claudio_chies/ros2_ws/install/px4_msgs/local/lib/python3.10/dist-packages",
+                "/opt/ros/humble/lib/python3.10/site-packages",
+                "/opt/ros/humble/local/lib/python3.10/dist-packages"
+        ]
 }

msg/VehicleStatus.msg

@@ -49,7 +49,7 @@ uint8 NAVIGATION_STATE_OFFBOARD = 14
 uint8 NAVIGATION_STATE_STAB = 15                # Stabilized mode
 uint8 NAVIGATION_STATE_FREE1 = 16
 uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17        # Takeoff
-uint8 NAVIGATION_STATE_AUTO_LAND = 18           # Land
+uint8 NAVIGATION_STATE_LAND = 18           	# Land
 uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19  # Auto Follow
 uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20       # Precision land with landing target
 uint8 NAVIGATION_STATE_ORBIT = 21               # Orbit in a circle

src/drivers/osd/atxxxx/atxxxx.cpp

@@ -402,7 +402,7 @@ OSDatxxxx::get_flight_mode(uint8_t nav_state)
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
+	case vehicle_status_s::NAVIGATION_STATE_LAND:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
 		flight_mode = "AUTO";

src/drivers/rc/crsf_rc/CrsfRc.cpp

@@ -291,13 +291,13 @@ void CrsfRc::Run()
 					case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 					case vehicle_status_s::NAVIGATION_STATE_DESCEND:
 					case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
-					case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
+					case vehicle_status_s::NAVIGATION_STATE_LAND:
 					case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
 					case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
 						flight_mode = "Auto";
 						break;
 
-					/*case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
+					/*case vehicle_status_s::NAVIGATION_STATE_LANDENGFAIL:
 						flight_mode = "Failure";
 						break;*/
 

src/drivers/rc_input/crsf_telemetry.cpp

@@ -156,7 +156,7 @@ bool CRSFTelemetry::send_flight_mode()
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
+	case vehicle_status_s::NAVIGATION_STATE_LAND:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
 		flight_mode = "Auto";

src/lib/modes/standard_modes.hpp

@@ -64,7 +64,7 @@ static inline StandardMode getStandardModeFromNavState(uint8_t nav_state)
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: return StandardMode::MISSION;
 
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: return StandardMode::LAND;
+	case vehicle_status_s::NAVIGATION_STATE_LAND: return StandardMode::LAND;
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: return StandardMode::TAKEOFF;
 		// Note: all other standard modes do not directly map, or are vehicle-type specific
@@ -83,7 +83,7 @@ static inline uint8_t getNavStateFromStandardMode(StandardMode mode)
 
 	case StandardMode::MISSION: return vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
 
-	case StandardMode::LAND: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+	case StandardMode::LAND: return vehicle_status_s::NAVIGATION_STATE_LAND;
 
 	case StandardMode::TAKEOFF: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
 

src/lib/modes/ui.hpp

@@ -57,7 +57,7 @@ static inline uint32_t getValidNavStates()
 	       (1u << vehicle_status_s::NAVIGATION_STATE_OFFBOARD) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_STAB) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) |
-	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_LAND) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_ORBIT) |

src/lib/motion_planning/VelocitySmoothing.hpp

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

src/modules/commander/Commander.cpp

@@ -831,7 +831,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							break;
 
 						case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
-							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 							break;
 
 						case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET:
@@ -1062,14 +1062,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd))) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_LAND, getSourceFromCommand(cmd))) {
 				mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t");
 				events::send(events::ID("commander_landing_current_pos"), events::Log::Info,
 					     "Landing at current position");
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
-				printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND);
+				printRejectMode(vehicle_status_s::NAVIGATION_STATE_LAND);
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 		}

src/modules/commander/HealthAndArmingChecks/checks/windCheck.cpp

@@ -70,7 +70,7 @@ void WindChecks::checkAndReport(const Context &context, Report &reporter)
 			   && wind_limit_exceeded
 			   && warning_timeout_passed
 			   && context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
-			   && context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
+			   && context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_LAND) {
 
 			events::send<float>(events::ID("check_above_wind_limits_warning"),
 			{events::Log::Warning, events::LogInternal::Warning},
@@ -81,7 +81,7 @@ void WindChecks::checkAndReport(const Context &context, Report &reporter)
 			   && wind.longerThan(_param_com_wind_warn.get())
 			   && warning_timeout_passed
 			   && context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
-			   && context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
+			   && context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_LAND) {
 
 			events::send<float>(events::ID("check_high_wind_warning"),
 			{events::Log::Warning, events::LogInternal::Info},

src/modules/commander/ModeUtil/control_mode.cpp

@@ -84,7 +84,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 		break;
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
+	case vehicle_status_s::NAVIGATION_STATE_LAND:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:

src/modules/commander/ModeUtil/conversions.hpp

@@ -68,7 +68,7 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state)
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: return navigation_mode_t::auto_takeoff;
 
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: return navigation_mode_t::auto_land;
+	case vehicle_status_s::NAVIGATION_STATE_LAND: return navigation_mode_t::auto_land;
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: return navigation_mode_t::auto_follow_target;
 

src/modules/commander/ModeUtil/mode_requirements.cpp

@@ -137,12 +137,12 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_local_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_local_alt);
 
-	// NAVIGATION_STATE_AUTO_LAND
-	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_angular_velocity);
-	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_attitude);
-	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_local_alt);
-	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_local_position);
-	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_prevent_arming);
+	// NAVIGATION_STATE_LAND
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_LAND, flags.mode_req_angular_velocity);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_LAND, flags.mode_req_attitude);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_LAND, flags.mode_req_local_alt);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_LAND, flags.mode_req_local_position);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_LAND, flags.mode_req_prevent_arming);
 
 	// NAVIGATION_STATE_AUTO_FOLLOW_TARGET
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_angular_velocity);

src/modules/commander/failsafe/failsafe.cpp

@@ -304,7 +304,7 @@ FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t
 
 	case offboard_loss_failsafe_mode::Land_mode:
 		action = Action::Land;
-		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_LAND;
 		break;
 
 	case offboard_loss_failsafe_mode::Hold_mode:
@@ -435,7 +435,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 	}
 
 	// GCS connection loss
-	const bool gcs_connection_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND ||
+	const bool gcs_connection_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_LAND ||
 			state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || ignore_link_failsafe;
 
 	if (_param_nav_dll_act.get() != int32_t(gcs_connection_loss_failsafe_mode::Disabled) && !gcs_connection_loss_ignored) {
@@ -613,7 +613,7 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_
 		if (user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSCTL
 		    && !modeCanRun(status_flags, user_intended_mode)) {
 			action = Action::Land;
-			user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+			user_intended_mode = vehicle_status_s::NAVIGATION_STATE_LAND;
 
 			// Land -> Descend
 			if (!modeCanRun(status_flags, user_intended_mode)) {

src/modules/commander/failsafe/framework.cpp

@@ -567,7 +567,7 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 
 	// fallthrough
 	case Action::Land:
-		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) {
+		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_LAND)) {
 			selected_action = Action::Land;
 			break;
 		}
@@ -600,9 +600,9 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 
 	// UX improvement (this is optional for safety): change failsafe to a warning in certain situations.
 	// If already landing, do not go into RTL
-	if (returned_state.updated_user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
+	if (returned_state.updated_user_intended_mode == vehicle_status_s::NAVIGATION_STATE_LAND) {
 		if ((selected_action == Action::RTL || returned_state.delayed_action == Action::RTL)
-		    && modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) {
+		    && modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_LAND)) {
 			selected_action = Action::Warn;
 			returned_state.delayed_action = Action::None;
 		}
@@ -656,7 +656,7 @@ uint8_t FailsafeBase::modeFromAction(const Action &action, uint8_t user_intended
 
 	case Action::RTL: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
-	case Action::Land: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+	case Action::Land: return vehicle_status_s::NAVIGATION_STATE_LAND;
 
 	case Action::Descend: return vehicle_status_s::NAVIGATION_STATE_DESCEND;
 

src/modules/commander/px4_custom_mode.h

@@ -162,7 +162,7 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
 		break;
 
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
+	case vehicle_status_s::NAVIGATION_STATE_LAND:
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 		break;

src/modules/events/set_leds.cpp

@@ -79,7 +79,7 @@ void StatusDisplay::set_leds()
 	_led_control.led_mask = BOARD_REAR_LED_MASK;
 
 	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
-	    || nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
+	    || nav_state == vehicle_status_s::NAVIGATION_STATE_LAND) {
 		_led_control.color = led_control_s::COLOR_PURPLE;
 
 	} else if (nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL) {

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_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/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,283 @@
+/****************************************************************************
+ *
+ *   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
+	_position_smoothing.reset(pos_prev, vel_prev, accel_prev);
+
+
+	_acceleration_setpoint = accel_prev;
+	_velocity_setpoint = vel_prev;
+	_position_setpoint = pos_prev;
+
+
+
+	// Initialize the Landing locations and parameters
+	// calculate where to land based on the current velocity and acceleration constraints
+	_CalculateBrakingLocation();
+	_initial_land_position = _land_position;
+
+	return ret;
+}
+
+void
+FlightTaskLand::reActivate()
+{
+	FlightTask::reActivate();
+	// On ground, reset acceleration and velocity to zero
+	_position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position);
+}
+
+bool
+FlightTaskLand::update()
+{
+	bool ret = FlightTask::update();
+
+	if (!_is_initialized) {
+		_position_smoothing.reset(_acceleration_setpoint, _velocity, _position);
+		_is_initialized = true;
+	}
+
+	if (_velocity.norm() < 1.f && !_landing) {
+		_landing = true;
+	}
+
+	if (_landing) {
+		_PerformLanding();
+
+	} else {
+		_SmoothBrakingPath();
+	}
+
+	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::_SmoothBrakingPath()
+{
+	PositionSmoothing::PositionSmoothingSetpoints out_setpoints;
+
+	_HandleHighVelocities();
+
+	_position_smoothing.generateSetpoints(
+		_position,
+		_land_position,
+		Vector3f{0.f, 0.f, 0.f},
+		_deltatime,
+		false,
+		out_setpoints
+	);
+
+	_jerk_setpoint = out_setpoints.jerk;
+	_acceleration_setpoint = out_setpoints.acceleration;
+	_velocity_setpoint = out_setpoints.velocity;
+	_position_setpoint = out_setpoints.position;
+	_yaw_setpoint = _land_heading;
+}
+
+void
+FlightTaskLand::_CalculateBrakingLocation()
+{
+	// Calculate the 3D point where we until where we can slow down smoothly and then land based on the current velocities and system constraints on jerk and acceleration.
+	_UpdateTrajConstraints();
+	float delay_scale = 0.4f; // delay scale factor
+	const float velocity_hor_abs = sqrtf(_velocity(0) * _velocity(0) + _velocity(1) * _velocity(1));
+	const float braking_dist_xy = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs,
+				      _param_mpc_jerk_auto.get(), _param_mpc_acc_hor.get(), delay_scale * _param_mpc_jerk_auto.get());
+	float braking_dist_z = 0.0f;
+
+	if (_velocity(2) < -0.1f) {
+		braking_dist_z = math::trajectory::computeBrakingDistanceFromVelocity(_velocity(2),
+				 _param_mpc_jerk_auto.get(), 9.81f +  _param_mpc_acc_down_max.get(), delay_scale * _param_mpc_jerk_auto.get());
+
+	} else if (_velocity(2) > 0.1f) {
+		braking_dist_z = math::trajectory::computeBrakingDistanceFromVelocity(_velocity(2),
+				 _param_mpc_jerk_auto.get(), _param_mpc_acc_up_max.get(), delay_scale * _param_mpc_jerk_auto.get());
+	}
+
+	const Vector3f braking_dir = _velocity.unit_or_zero();
+	const Vector3f braking_dist = {braking_dist_xy, braking_dist_xy, braking_dist_z};
+	_land_position = _position + braking_dir.emult(braking_dist);
+}
+
+
+void
+FlightTaskLand::_HandleHighVelocities()
+{
+	// This logic here is to fix the problem that the trajectory generator will generate a smoot trajectory from the current velocity to zero velocity,
+	// but if the velocity is too high, the Position Controller will be able to comand higher accelerations than set by the parameter which means the vehicle will break faster than expected predicted by the trajectory generator.
+	// But if we then do a reset the deceleration will be smooth again.
+	const bool _exceeded_vel_z = fabsf(_velocity(2)) > math::max(_param_mpc_z_v_auto_dn.get(),
+				     _param_mpc_z_vel_max_up.get());
+	const bool _exceeded_vel_xy = _velocity.xy().norm() > _param_mpc_xy_vel_max.get();
+
+	if ((_exceeded_vel_xy || _exceeded_vel_z) && !_exceeded_max_vel) {
+		_exceeded_max_vel = true;
+
+	} else if ((!_exceeded_vel_xy  && !_exceeded_vel_z)  && _exceeded_max_vel) {
+		// This Reset will be called when the velocity is again in the normal range and will be called only once.
+		_exceeded_max_vel = false;
+		_position_smoothing.reset(_acceleration_setpoint, _velocity, _position);
+		_CalculateBrakingLocation();
+		_initial_land_position = _land_position;
+	}
+}
+
+void
+FlightTaskLand::_UpdateTrajConstraints()
+{
+	// 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.setMaxAccelerationXY(_param_mpc_acc_hor.get());
+	_position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get());
+	_position_smoothing.setMaxJerk(_param_mpc_jerk_auto.get());
+
+	// set the constraints for the vertical direction
+	// if moving up, acceleration constraint is always in deceleration direction, eg opposite to the velocity
+	if (_velocity(2) < 0.0f && !_landing) {
+		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
+		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_up.get());
+
+	} else if (!_landing) {
+		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_up_max.get());
+		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get());
+
+	} else {
+		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
+		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get());
+	}
+}

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

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   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"
+
+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:
+	PositionSmoothing _position_smoothing;
+	Sticks _sticks{this};
+	StickAccelerationXY _stick_acceleration_xy{this};
+	StickYaw _stick_yaw{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_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_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,
+					(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_V_AUTO_DN>) 	_param_mpc_z_v_auto_dn,
+					(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) 	_param_mpc_z_v_auto_up,
+					(ParamFloat<px4::params::NAV_MC_ALT_RAD>) 	_param_nav_mc_alt_rad
+				       );
+private:
+	void _CalculateBrakingLocation();
+	void _HandleHighVelocities();
+	void _PerformLanding();
+	void _SmoothBrakingPath();
+	void _UpdateTrajConstraints();
+
+	bool _landing{false};
+	bool _is_initialized{false};
+	bool _exceeded_max_vel{false};
+
+	Vector3f _initial_land_position;
+	Vector3f _land_position;
+	float _land_heading{0.0f};
+
+
+};

src/modules/land_detector/AirshipLandDetector.cpp

@@ -51,7 +51,7 @@ bool AirshipLandDetector::_get_ground_contact_state()
 bool AirshipLandDetector::_get_landed_state()
 {
 
-	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
+	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_LAND) {
 		return true; // If Landing has been requested then say we have landed.
 
 	} else {

src/modules/land_detector/RoverLandDetector.cpp

@@ -51,7 +51,7 @@ bool RoverLandDetector::_get_ground_contact_state()
 
 bool RoverLandDetector::_get_landed_state()
 {
-	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
+	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_LAND) {
 		return true; // If Landing has been requested then say we have landed.
 
 	} else {

src/modules/manual_control/ManualControl.cpp

@@ -566,7 +566,7 @@ int8_t ManualControl::navStateFromParam(int32_t param_value)
 		case 8: return vehicle_status_s::NAVIGATION_STATE_STAB;
 		case 9: return vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW;
 		case 10: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
-		case 11: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+		case 11: return vehicle_status_s::NAVIGATION_STATE_LAND;
 		case 12: return vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
 		case 13: return vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
 		case 14: return vehicle_status_s::NAVIGATION_STATE_ORBIT;

src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp

@@ -140,7 +140,7 @@ private:
 
 			bool vehicle_in_auto_mode = (vehicle_status.timestamp > 0)
 						    && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET
-							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
+							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_LAND
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
 							|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER

src/modules/navigator/land.cpp

@@ -42,7 +42,7 @@
 #include "navigator.h"
 
 Land::Land(Navigator *navigator) :
-	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)
+	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_LAND)
 {
 }
 
@@ -54,15 +54,21 @@ Land::on_activation()
 	_navigator->get_mission_result()->finished = false;
 	_navigator->set_mission_result_updated();
 	reset_mission_item_reached();
-
 	/* convert mission item to current setpoint */
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
 	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-		_navigator->calculate_breaking_stop(_mission_item.lat, _mission_item.lon);
+		// set the lat and lon to NAN, as the target setpoint for landing is handeled in FlightTaskAuto, not in the navigator
+		pos_sp_triplet->current.lat = NAN;
+		pos_sp_triplet->current.lon = NAN;
+		pos_sp_triplet->previous.lat = NAN;
+		pos_sp_triplet->previous.lon = NAN;
+		pos_sp_triplet->next.lat = NAN;
+		pos_sp_triplet->next.lon = NAN;
 	}
 
-	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
 	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->next.valid = false;
 

src/modules/navigator/navigator_main.cpp

@@ -801,7 +801,7 @@ void Navigator::run()
 			break;
 #endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
 
-		case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
+		case vehicle_status_s::NAVIGATION_STATE_LAND:
 			_pos_sp_triplet_published_invalid_once = false;
 			navigation_mode_new = &_land;
 			break;

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -171,7 +171,7 @@ void VtolAttitudeControl::vehicle_cmd_poll()
 			// deny transition from MC to FW in Takeoff, Land, RTL and Orbit
 			if (transition_command_param1 == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW &&
 			    (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
-			     || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
+			     || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_LAND
 			     || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
 			     ||  _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)) {