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>

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: