Add Altitude Cruise mode

-add new NAVIGATION_STATE_ALTITUDE_VOYAGER
-this mode does require manual control to enter
-but you can disable the manual control loss failsafe to continue
flying in case of manual control loss
-for MC: in throttle and yaw are controlled like in Altitude mode,
the tilt is controlled via integrated rate input (similar to Acro,
but with tilt limit)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

docs/en/SUMMARY.md

@@ -7,6 +7,7 @@
       - [Position Mode (MC)](flight_modes_mc/position.md)
       - [Position Slow Mode (MC)](flight_modes_mc/position_slow.md)
       - [Altitude Mode (MC)](flight_modes_mc/altitude.md)
+      - [Altitude Cruise Mode (MC)](flight_modes_mc/altitude_cruise.md)
       - [Stabilized Mode (MC)](flight_modes_mc/manual_stabilized.md)
       - [Acro Mode (MC)](flight_modes_mc/acro.md)
       - [Orbit Mode (MC)](flight_modes_mc/orbit.md)

docs/en/flight_modes_fw/index.md

@@ -17,6 +17,8 @@ Manual-Easy:
   Airspeed is actively controlled if an airspeed sensor is installed.
 - [Altitude](../flight_modes_fw/altitude.md) — Easiest and safest _non-GPS_ manual mode.
   The only difference compared to _Position mode_ is that the pilot always directly controls the roll angle of the plane and there is no automatic course holding.
+- Altitude Cruise mode — It behaves exactly like _Altitude mode_, with the only difference being that the manual control failsafe can be disabled. This is done by setting the corresponding flag in [COM_RCL_EXCEPT](../advanced_config/parameter_reference.md#COM_RCL_EXCEPT). In that case the current altitude, airspeed and heading (by leveling out the roll angle) are kept until the manual control link is regained or the mode is exited.
+It is highly recommended to only disable the manual control loss failsafe for this mode if there is a stable data link connection to the vehicle at all times, or to enable the data link loss failsafe through [NAV_DLL_ACT](../advanced_config/parameter_reference.md#NAV_DLL_ACT).
 - [Stabilized mode](../flight_modes_fw/stabilized.md) — The pilot directly commands the roll and pitch angle and the vehicle keeps the setpoint until the sticks are moved again.
   Thrust is directly set by the pilot.
   Turn coordination is still handled by the controller.

docs/en/flight_modes_mc/altitude.md

@@ -21,7 +21,7 @@ The diagram below shows the mode behaviour visually (for a [mode 2 transmitter](
 RC/manual mode like [Stabilized mode](../flight_modes_mc/manual_stabilized.md) but with _altitude stabilization_ (centred sticks level vehicle and hold it to fixed altitude).
 The horizontal position of the vehicle can move due to wind (or pre-existing momentum).
 
-- Centered sticks (inside deadband):
+- Centered sticks:
   - RPY sticks levels vehicle.
   - Throttle (~50%) holds current altitude steady against wind.
 - Outside center:

docs/en/flight_modes_mc/altitude_cruise.md

@@ -0,0 +1,45 @@
+# Altitude Cruise Mode (Multicopter)
+
+<img src="../../assets/site/difficulty_easy.png" title="Easy to fly" width="30px" />&nbsp;<img src="../../assets/site/remote_control.svg" title="Manual/Remote control required" width="30px" />&nbsp;<img src="../../assets/site/altitude_icon.svg" title="Altitude required (e.g. Baro, Rangefinder)" width="30px" />
+
+_Altitude Cruise mode_ is a _relatively_ easy-to-fly manual control mode in which roll and pitch sticks control vehicle movement in the left-right and forward-back directions (relative to the "front" of the vehicle), yaw stick controls rate of rotation over the horizontal plane, and throttle controls speed of ascent-descent.
+
+When the sticks are released/centered the vehicle will keep the current tilt and heading angle and maintain the current _altitude_.
+If moving in the horizontal plane the vehicle will accelerate until the wind resistance equals the acceleration caused by the set tilt angle.
+The vehicle will then continue to move with a constant velocity (unlike for Altitude mode, in which the vehicle will eventually slow down and stop).
+If the wind blows the aircraft will drift in the direction of the wind even if flying perfectly level.
+
+:::tip
+_Altitude Cruise mode_ is intended for long distance flights where the same tilt angle is kept for a long period of time. It is just like [Altitude](../flight_modes_mc/altitude.md) mode but does not go back to level tilt when the sticks are released.
+:::
+
+The diagram below shows the mode behaviour visually (for a [mode 2 transmitter](../getting_started/rc_transmitter_receiver.md#transmitter_modes)).
+
+![Altitude Control MC - Mode2 RC Controller](../../assets/flight_modes/altitude_mc.png)
+
+## Technical Summary
+
+A manual mode that is similar to [Altitude mode](../flight_modes_mc/altitude.md) but with different interpretation of roll and pitch sticks.
+
+- Centered sticks:
+  - Roll/Pitch sticks: the current tilt is kept.
+  - Yaw: the current heading is kept.
+  - Throttle (~50%) holds current altitude.
+- Outside center:
+  - Roll/Pitch sticks control the rate of change of the tilt angle, resulting in corresponding left-right and forward-back movement. A maximum stick deflection results in a tilting rate setpoint to go from level to max tilt within 0.5 seconds.
+  - Yaw stick deflection rotates the tilt angle either left or right, causing the vehicle to change course. It is _not_ causing a direct rotation around the body yaw axis like in [Acro mode](../flight_modes_mc/acro.md).
+  - Throttle stick controls up/down speed with a predetermined maximum rate (and movement speed in other axes).
+- Takeoff:
+  - When landed, the vehicle will take off if the throttle stick is raised above 62.5% percent (of the full range from bottom).
+- Manual control input is required (such as RC control, joystick) to enter this mode. Other than in all other manual modes, it's though possible to disable the manual control loss failsafe by setting the corresponding flag in [COM_RCL_EXCEPT](../advanced_config/parameter_reference.md#COM_RCL_EXCEPT). In that case the current altitude, tilt and heading are kept until the manual control link is regained or the mode is exited.
+  It is highly recommended to only disable the manual control loss failsafe for this mode if there is a stable data link connection to the vehicle at all times, and to enable the data link loss failsafe through [NAV_DLL_ACT](../advanced_config/parameter_reference.md#NAV_DLL_ACT).
+
+## Parameters
+
+Most of the relevant parameters are already covered in the corresponding section in the [Altitude mode](../flight_modes_mc/altitude.md). Here a list of parameters of particular importance for Altitude Cruise.
+
+| Parameter                                                                                                   | Description                                                                                                                            |
+| ----------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------- |
+| <a id="COM_RCL_EXCEPT"></a>[COM_RCL_EXCEPT](../advanced_config/parameter_reference.md#COM_RCL_EXCEPT)       | The manual control failsafe can be disabled for Altitude Cruise by setting the corresponding bit in this parameter.                    |
+| <a id="NAV_DLL_ACT"></a>[NAV_DLL_ACT](../advanced_config/parameter_reference.md#NAV_DLL_ACT)                | Data link lost failsafe action. Recommended to set if the manual control failsafe is disabled to avoid fly-aways.                      |
+| <a id="MPC_MAN_TILT_MAX"></a>[MPC_MAN_TILT_MAX](../advanced_config/parameter_reference.md#MPC_MAN_TILT_MAX) | The maximum tilt angle the vehicle will go to. At max stick deflection, it will take 0.5 seconds from level flight to this tilt angle. |

docs/en/flight_modes_mc/index.md

@@ -21,10 +21,12 @@ Manual-Easy:
 - [Stabilized mode](../flight_modes_mc/manual_stabilized.md) — Releasing the sticks levels and maintains the vehicle horizontal posture (but not altitude or position).
   The vehicle will continue to move with momentum, and both altitude and horizontal position may be affected by wind.
   This mode is also used if "Manual mode" is selected in a ground station.
+- [Altitude Cruise mode](../flight_modes_mc/altitude_cruise.md) — Very similar to _Altitude mode_, with the difference that when the roll and pitch sticks are released the vehicle does not level out but keeps the tilt until further inputs are given.
+  Additionally it is possible to disable the manual control failsafe for this mode, having the vehicle continue on it's set path even if there are no new control inputs.
 
 Manual-Acrobatic
 
-- [Acro](../flight_modes_mc/acro.md) — Manual mode for performing acrobatic maneuvers, such as rolls and loops.
+- [Acro](../flight_modes_mc/acro.md) — Manual mode for performing acrobatic manoeuvrers, such as rolls and loops.
   Releasing the sticks stops the vehicle rotating in the roll, pitch, yaw axes, but does not otherwise stabilise the vehicle.
 
 Autonomous:

docs/en/flight_modes_mc/manual_stabilized.md

@@ -31,7 +31,7 @@ Throttle is rescaled (see [below](#params)) and passed directly to control alloc
 The autopilot controls the attitude, meaning it regulates the roll and pitch angles to zero when the RC sticks are centered inside the controller deadzone (consequently leveling-out the attitude).
 The autopilot does not compensate for drift due to wind (or other sources).
 
-- Centered sticks (inside deadband):
+- Centered sticks:
   - Roll/Pitch sticks level vehicle.
 - Outside center:
   - Roll/Pitch sticks control tilt angle in those orientations, resulting in corresponding left-right and forward-back movement.

docs/en/releases/main.md

@@ -44,7 +44,9 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
 
 ### Control
 
-- TBD
+- Added new flight mode(s): [Altitude Cruise (MC)](../flight_modes_mc/altitude_cruise.md), Altitude Cruise (FW).
+  For fixed-wing the mode behaves the same as Altitude mode but you can disable the manual control loss failsafe. ([PX4-Autopilot#25435: Add new flight mode: Altitude Cruise
+  ](https://github.com/PX4/PX4-Autopilot/pull/25435)).
 
 ### Estimation
 
@@ -72,7 +74,8 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
 
 ### Multi-Rotor
 
-- TBD
+- Removed parameters `MPC_{XY/Z/YAW}_MAN_EXPO` and use default value instead, as they were not deemed necessary anymore. ([PX4-Autopilot#25435: Add new flight mode: Altitude Cruise](https://github.com/PX4/PX4-Autopilot/pull/25435)).
+- Renamed `MPC_HOLD_DZ` to `MAN_DEADZONE` to have it globally available in modes that allow for a dead zone. ([PX4-Autopilot#25435: Add new flight mode: Altitude Cruise](https://github.com/PX4/PX4-Autopilot/pull/25435)).
 
 ### VTOL
 

msg/versioned/VehicleStatus.msg

@@ -41,7 +41,7 @@ uint8 NAVIGATION_STATE_AUTO_LOITER = 4          # Auto loiter mode
 uint8 NAVIGATION_STATE_AUTO_RTL = 5             # Auto return to launch mode
 uint8 NAVIGATION_STATE_POSITION_SLOW = 6
 uint8 NAVIGATION_STATE_FREE5 = 7
-uint8 NAVIGATION_STATE_FREE4 = 8
+uint8 NAVIGATION_STATE_ALTITUDE_CRUISE = 8      # Altitude with Cruise mode
 uint8 NAVIGATION_STATE_FREE3 = 9
 uint8 NAVIGATION_STATE_ACRO = 10                # Acro mode
 uint8 NAVIGATION_STATE_FREE2 = 11

src/drivers/osd/atxxxx/atxxxx.cpp

@@ -387,6 +387,10 @@ OSDatxxxx::get_flight_mode(uint8_t nav_state)
 		flight_mode = "ALTITUDE";
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE:
+		flight_mode = "CRUISE";
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
 		flight_mode = "POSITION";
 		break;

src/drivers/rc/crsf_rc/CrsfRc.cpp

@@ -281,6 +281,10 @@ void CrsfRc::Run()
 						flight_mode = "Altitude";
 						break;
 
+					case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE:
+						flight_mode = "Altitude Cruise";
+						break;
+
 					case vehicle_status_s::NAVIGATION_STATE_POSCTL:
 						flight_mode = "Position";
 						break;

src/drivers/rc_input/crsf_telemetry.cpp

@@ -141,6 +141,10 @@ bool CRSFTelemetry::send_flight_mode()
 		flight_mode = "Altitude";
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE:
+		flight_mode = "Altitude Cruise";
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
 		flight_mode = "Position";
 		break;

src/lib/events/enums.json

@@ -604,6 +604,10 @@
                             "name": "external8",
                             "description": "External 8"
                         },
+                        "24": {
+                            "name": "altitude_cruise",
+                            "description": "Altitude Cruise"
+                        },
                         "255": {
                             "name": "unknown",
                             "description": "[Unknown]"

src/lib/modes/ui.hpp

@@ -47,6 +47,7 @@ static inline uint32_t getValidNavStates()
 {
 	return (1u << vehicle_status_s::NAVIGATION_STATE_MANUAL) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_ALTCTL) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_POSCTL) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) |
@@ -75,7 +76,7 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
 	"Return",
 	"Position Slow",
 	"7: unallocated",
-	"8: unallocated",
+	"Altitude Cruise",
 	"9: unallocated",
 	"Acro",
 	"11: UNUSED",
@@ -108,6 +109,8 @@ static inline bool isAdvanced(uint8_t nav_state)
 	switch (nav_state) {
 	case vehicle_status_s::NAVIGATION_STATE_ALTCTL: return false;
 
+	case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE: return false;
+
 	case vehicle_status_s::NAVIGATION_STATE_POSCTL: return false;
 
 	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1: return false;

src/modules/commander/Commander.cpp

@@ -380,6 +380,9 @@ int Commander::custom_command(int argc, char *argv[])
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_POSCTL,
 						     PX4_CUSTOM_SUB_MODE_POSCTL_SLOW);
 
+			} else if (!strcmp(argv[1], "altitude_cruise")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ALTITUDE_CRUISE);
+
 			} else if (!strcmp(argv[1], "auto:mission")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_AUTO_MISSION);
@@ -794,6 +797,9 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
 					desired_nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
 
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTITUDE_CRUISE) {
+					desired_nav_state = vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE;
+
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
 					switch (custom_sub_mode) {
 					default:
@@ -3026,7 +3032,7 @@ The commander module contains the state machine for mode switching and failsafe
 	PRINT_MODULE_USAGE_COMMAND("land");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode");
-	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|position:slow|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1",
+	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|altitude_cruise|position:slow|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1",
 			"Flight mode", false);
 	PRINT_MODULE_USAGE_COMMAND("pair");
 	PRINT_MODULE_USAGE_COMMAND("termination");

src/modules/commander/ModeUtil/control_mode.cpp

@@ -63,6 +63,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 		break;
 
 	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
+	case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE:
 		vehicle_control_mode.flag_control_manual_enabled = true;
 		vehicle_control_mode.flag_control_altitude_enabled = true;
 		vehicle_control_mode.flag_control_climb_rate_enabled = true;

src/modules/commander/ModeUtil/conversions.hpp

@@ -50,6 +50,8 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state)
 
 	case vehicle_status_s::NAVIGATION_STATE_ALTCTL: return navigation_mode_t::altctl;
 
+	case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE: return navigation_mode_t::altitude_cruise;
+
 	case vehicle_status_s::NAVIGATION_STATE_POSCTL: return navigation_mode_t::posctl;
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: return navigation_mode_t::auto_mission;

src/modules/commander/ModeUtil/mode_requirements.cpp

@@ -69,6 +69,13 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_local_alt);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_manual_control);
 
+	// NAVIGATION_STATE_ALTITUDE_CRUISE
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE, flags.mode_req_angular_velocity);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE, flags.mode_req_attitude);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE, flags.mode_req_local_alt);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE,
+		       flags.mode_req_manual_control); // COM_RCL_EXCEPT can override this
+
 	// NAVIGATION_STATE_POSCTL
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_attitude);

src/modules/commander/commander_params.c

@@ -613,11 +613,12 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
  * External modes requiring stick input will still failsafe.
  *
  * @min 0
- * @max 15
+ * @max 31
  * @bit 0 Mission
  * @bit 1 Hold
  * @bit 2 Offboard
  * @bit 3 External Mode
+ * @bit 4 Altitude Cruise
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);

src/modules/commander/failsafe/failsafe.cpp

@@ -471,6 +471,9 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 	const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
 					      state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
 					     && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
+	const bool rc_loss_ignored_altitude_cruise = (state.user_intended_mode ==
+			vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE
+			&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::AltitudeCruise));
 
 	const bool rc_loss_ignored_external_mode =
 		(state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 ||
@@ -485,7 +488,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 
 	const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard ||
 				     rc_loss_ignored_takeoff || rc_loss_ignored_external_mode || ignore_any_link_loss_vtol_takeoff_fixedwing
-				     || _manual_control_lost_at_arming;
+				     || _manual_control_lost_at_arming || rc_loss_ignored_altitude_cruise;
 
 	if (_param_com_rc_in_mode.get() != int32_t(RcInMode::StickInputDisabled) && !rc_loss_ignored) {
 		CHECK_FAILSAFE(status_flags, manual_control_signal_lost,

src/modules/commander/failsafe/failsafe.h

@@ -57,7 +57,8 @@ private:
 		Mission = (1 << 0),
 		Hold = (1 << 1),
 		Offboard = (1 << 2),
-		ExternalMode = (1 << 3)
+		ExternalMode = (1 << 3),
+		AltitudeCruise = (1 << 4)
 	};
 
 	enum class DatalinkLossExceptionBits : int32_t {

src/modules/commander/module.yaml

@@ -38,6 +38,7 @@ parameters:
                 8: Stabilized
                 12: Follow Me
                 13: Precision Land
+                16: Altitude Cruise
                 100: External Mode 1
                 101: External Mode 2
                 102: External Mode 3

src/modules/commander/px4_custom_mode.h

@@ -51,7 +51,8 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_STABILIZED,
 	PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
 	PX4_CUSTOM_MAIN_MODE_SIMPLE, /* unused, but reserved for future use */
-	PX4_CUSTOM_MAIN_MODE_TERMINATION
+	PX4_CUSTOM_MAIN_MODE_TERMINATION,
+	PX4_CUSTOM_MAIN_MODE_ALTITUDE_CRUISE
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
@@ -112,6 +113,10 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTITUDE_CRUISE;
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
 		break;

src/modules/flight_mode_manager/CMakeLists.txt

@@ -48,6 +48,7 @@ list(APPEND flight_tasks_all
 	ManualAltitudeSmoothVel
 	ManualPosition
 	Transition
+	AltitudeCruise
 )
 
 if(NOT px4_constrained_flash_build)

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -249,6 +249,17 @@ void FlightModeManager::start_flight_task()
 		matching_task_running = matching_task_running && !task_failure;
 	}
 
+	// Altitude cruise
+	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE) {
+		found_some_task = true;
+		FlightTaskError error = FlightTaskError::NoError;
+
+		error = switchTask(FlightTaskIndex::AltitudeCruise);
+
+		task_failure = (error != FlightTaskError::NoError);
+		matching_task_running = matching_task_running && !task_failure;
+	}
+
 	// Emergency descend
 	if (nav_state_descend || task_failure) {
 		found_some_task = true;

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

@@ -0,0 +1,6 @@
+px4_add_library(FlightTaskAltitudeCruise
+        FlightTaskAltitudeCruise.cpp
+)
+
+target_link_libraries(FlightTaskAltitudeCruise PUBLIC FlightTaskManualAltitudeSmoothVel)
+target_include_directories(FlightTaskAltitudeCruise PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/AltitudeCruise/FlightTaskAltitudeCruise.cpp

@@ -0,0 +1,19 @@
+#include "FlightTaskAltitudeCruise.hpp"
+
+FlightTaskAltitudeCruise::FlightTaskAltitudeCruise()
+{
+	_sticks_data_required = false; // disable stick requirement to not report flight task failure when they're lost
+}
+
+void FlightTaskAltitudeCruise::reActivate()
+{
+	FlightTaskManualAltitudeSmoothVel::reActivate();
+	_stick_tilt_xy.reset();
+}
+
+void FlightTaskAltitudeCruise::_updateXYSetpoint()
+{
+	_acceleration_setpoint.xy() =
+		_stick_tilt_xy.generateAccelerationSetpointsForAltitudeCruise(
+			_sticks.getPitchRoll(), _deltatime, _yaw, _yaw_setpoint);
+}

src/modules/flight_mode_manager/tasks/AltitudeCruise/FlightTaskAltitudeCruise.hpp

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "FlightTaskManualAltitudeSmoothVel.hpp"
+
+class FlightTaskAltitudeCruise : public FlightTaskManualAltitudeSmoothVel
+{
+public:
+	FlightTaskAltitudeCruise();
+	virtual ~FlightTaskAltitudeCruise() = default;
+
+	void reActivate() override;
+
+protected:
+	void _updateXYSetpoint() override;
+};

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -274,13 +274,25 @@ void FlightTaskManualAltitude::_ekfResetHandlerHagl(float delta_hagl)
 
 void FlightTaskManualAltitude::_updateSetpoints()
 {
-	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime, _unaided_yaw);
-	_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
-				      _yaw_setpoint);
+	_updateYawSetpoint();
+	_updateXYSetpoint();
 	_updateAltitudeLock();
 	_respectGroundSlowdown();
 }
 
+void FlightTaskManualAltitude::_updateYawSetpoint()
+{
+	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint,
+				       _sticks.getYawExpo(), _yaw, _deltatime,
+				       _unaided_yaw);
+}
+
+void FlightTaskManualAltitude::_updateXYSetpoint()
+{
+	_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(
+					      _sticks.getPitchRoll(), _deltatime, _yaw, _yaw_setpoint);
+}
+
 bool FlightTaskManualAltitude::_checkTakeoff()
 {
 	// stick is deflected above 65% throttle (throttle stick is in the range [-1,1])

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -60,6 +60,8 @@ protected:
 	void _ekfResetHandlerHagl(float delta_hagl) override;
 
 	virtual void _updateSetpoints(); /**< updates all setpoints */
+	virtual void _updateYawSetpoint();
+	virtual void _updateXYSetpoint();
 	virtual void _scaleSticks(); /**< scales sticks to velocity in z */
 	bool _checkTakeoff() override;
 	void _updateConstraintsFromEstimator();

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

@@ -44,6 +44,11 @@ StickTiltXY::StickTiltXY(ModuleParams *parent) :
 	updateParams();
 }
 
+void StickTiltXY::reset()
+{
+	_altitude_cruise_acceleration.setZero();
+}
+
 void StickTiltXY::updateParams()
 {
 	ModuleParams::updateParams();
@@ -62,3 +67,21 @@ Vector2f StickTiltXY::generateAccelerationSetpoints(Vector2f stick_xy, const flo
 	Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_setpoint);
 	return stick_xy * _maximum_acceleration;
 }
+
+Vector2f StickTiltXY::generateAccelerationSetpointsForAltitudeCruise(Vector2f stick_xy, const float dt, const float yaw,
+		const float yaw_setpoint)
+{
+	Sticks::limitStickUnitLengthXY(stick_xy);
+	const Vector2f increment = stick_xy;
+	// at full stick deflection it takes 1s from -tilt_max to tilt_max
+	_altitude_cruise_acceleration += increment * _maximum_acceleration * 2.f * dt;
+
+	if (_altitude_cruise_acceleration.longerThan(_maximum_acceleration)) {
+		_altitude_cruise_acceleration =
+			_altitude_cruise_acceleration.unit_or_zero() * _maximum_acceleration;
+	}
+
+	Vector2f global_altitude_cruise_acceleration = _altitude_cruise_acceleration;
+	Sticks::rotateIntoHeadingFrameXY(global_altitude_cruise_acceleration, yaw, yaw_setpoint);
+	return global_altitude_cruise_acceleration;
+}

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

@@ -62,11 +62,17 @@ public:
 	 */
 	matrix::Vector2f generateAccelerationSetpoints(matrix::Vector2f stick_xy, const float dt, const float yaw,
 			const float yaw_setpoint);
+
+	matrix::Vector2f generateAccelerationSetpointsForAltitudeCruise(matrix::Vector2f stick_xy, const float dt,
+			const float yaw, const float yaw_setpoint);
+
+	void reset();
 private:
 	void updateParams() override;
 
 	float _maximum_acceleration{0.f};
 	AlphaFilter<matrix::Vector2f> _man_input_filter;
+	matrix::Vector2f _altitude_cruise_acceleration{};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, ///< maximum tilt allowed for manual flight

src/modules/fw_mode_manager/CMakeLists.txt

@@ -41,7 +41,7 @@ set(POSCONTROL_DEPENDENCIES
 	SlewRate
 	tecs
 	motion_planning
-    	performance_model
+	performance_model
 	Sticks
 )
 

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -1953,17 +1953,12 @@ FixedWingModeManager::Run()
 
 	perf_begin(_loop_perf);
 
-	if (_vehicle_status_sub.updated()) {
-
-		if (_vehicle_status_sub.update(&_vehicle_status)) {
-			_nav_state = _vehicle_status.nav_state;
-		}
-	}
+	_vehicle_status_sub.update(&_vehicle_status);
 
 	/* only run controller if position changed and we are not running an external mode*/
 
-	const bool is_external_nav_state = (_nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1)
-					   && (_nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8);
+	const bool is_external_nav_state = (_vehicle_status.nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1)
+					   && (_vehicle_status.nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8);
 
 	if (is_external_nav_state) {
 		// this will cause the configuration handler to publish immediately the next time an internal flight

src/modules/fw_mode_manager/FixedWingModeManager.hpp

@@ -246,8 +246,6 @@ private:
 
 	// VEHICLE STATES
 
-	uint8_t _nav_state;
-
 	double _current_latitude{0};
 	double _current_longitude{0};
 	float _current_altitude{0.f};

src/modules/manual_control/ManualControl.cpp

@@ -590,6 +590,7 @@ int8_t ManualControl::navStateFromParam(int32_t param_value)
 		case 13: return vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
 		case 14: return vehicle_status_s::NAVIGATION_STATE_ORBIT;
 		case 15: return vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF;
+		case 16: return vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE;
 
 		case 100: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL1;
 		case 101: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL2;

src/modules/mc_pos_control/multicopter_stabilized_mode_params.c

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * Maximal tilt angle in Stabilized or Altitude mode
+ * Maximal tilt angle in Stabilized, Altitude and Altitude Cruise mode
  *
  * @unit deg
  * @min 1

src/modules/navigator/navigator_main.cpp

@@ -823,6 +823,7 @@ void Navigator::run()
 		case vehicle_status_s::NAVIGATION_STATE_MANUAL:
 		case vehicle_status_s::NAVIGATION_STATE_ACRO:
 		case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
+		case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE:
 		case vehicle_status_s::NAVIGATION_STATE_POSCTL:
 		case vehicle_status_s::NAVIGATION_STATE_DESCEND:
 		case vehicle_status_s::NAVIGATION_STATE_TERMINATION: