diff --git a/docs/en/SUMMARY.md b/docs/en/SUMMARY.md index feb8bf2ecc..a5d0c37f6b 100644 --- a/docs/en/SUMMARY.md +++ b/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) diff --git a/docs/en/flight_modes_fw/index.md b/docs/en/flight_modes_fw/index.md index 8bd9e4b062..052bfe7dd3 100644 --- a/docs/en/flight_modes_fw/index.md +++ b/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. diff --git a/docs/en/flight_modes_mc/altitude.md b/docs/en/flight_modes_mc/altitude.md index 13d54b9148..fda7dde9bb 100644 --- a/docs/en/flight_modes_mc/altitude.md +++ b/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: diff --git a/docs/en/flight_modes_mc/altitude_cruise.md b/docs/en/flight_modes_mc/altitude_cruise.md new file mode 100644 index 0000000000..87728cc311 --- /dev/null +++ b/docs/en/flight_modes_mc/altitude_cruise.md @@ -0,0 +1,45 @@ +# Altitude Cruise Mode (Multicopter) + +   + +_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 | +| ----------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------- | +| [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. | +| [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. | +| [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. | diff --git a/docs/en/flight_modes_mc/index.md b/docs/en/flight_modes_mc/index.md index c0fe5405f2..bbab81b23d 100644 --- a/docs/en/flight_modes_mc/index.md +++ b/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: diff --git a/docs/en/flight_modes_mc/manual_stabilized.md b/docs/en/flight_modes_mc/manual_stabilized.md index 9d3f29112d..36a9ae345f 100644 --- a/docs/en/flight_modes_mc/manual_stabilized.md +++ b/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. diff --git a/docs/en/releases/main.md b/docs/en/releases/main.md index 84fb901659..5bbc07873a 100644 --- a/docs/en/releases/main.md +++ b/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 diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index f433d564e9..0dffdc2c92 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/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 diff --git a/src/drivers/osd/atxxxx/atxxxx.cpp b/src/drivers/osd/atxxxx/atxxxx.cpp index a5acabd391..6f85fcd383 100644 --- a/src/drivers/osd/atxxxx/atxxxx.cpp +++ b/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; diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 9db8911636..9e311da18e 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/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; diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 60286e0f40..fb557f8497 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/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; diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 3b73cff62c..c3cbc2578a 100644 --- a/src/lib/events/enums.json +++ b/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]" diff --git a/src/lib/modes/ui.hpp b/src/lib/modes/ui.hpp index 9e4156ae00..e81a3609ad 100644 --- a/src/lib/modes/ui.hpp +++ b/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; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0e2c50ec4f..169cc34587 100644 --- a/src/modules/commander/Commander.cpp +++ b/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"); diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index fbebc7b93d..59858a51e8 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/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; diff --git a/src/modules/commander/ModeUtil/conversions.hpp b/src/modules/commander/ModeUtil/conversions.hpp index 045db4570f..e6bb4e2102 100644 --- a/src/modules/commander/ModeUtil/conversions.hpp +++ b/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; diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 9e64c529f2..850a0cf6e9 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/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); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 1e194ac5df..45da253f11 100644 --- a/src/modules/commander/commander_params.c +++ b/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); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index cdc770190c..d86e672d7e 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/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, diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index f9382e0428..e3849bf8b1 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/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 { diff --git a/src/modules/commander/module.yaml b/src/modules/commander/module.yaml index d2113d9359..67f97e3a7e 100644 --- a/src/modules/commander/module.yaml +++ b/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 diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e4936982ff..fc901d1492 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/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; diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 84ccba6ac0..efd7d9a261 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/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) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index f17e00aa96..d69ca84c25 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/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; diff --git a/src/modules/flight_mode_manager/tasks/AltitudeCruise/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AltitudeCruise/CMakeLists.txt new file mode 100644 index 0000000000..238b5bbd0f --- /dev/null +++ b/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}) diff --git a/src/modules/flight_mode_manager/tasks/AltitudeCruise/FlightTaskAltitudeCruise.cpp b/src/modules/flight_mode_manager/tasks/AltitudeCruise/FlightTaskAltitudeCruise.cpp new file mode 100644 index 0000000000..3e42cdf2c4 --- /dev/null +++ b/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); +} diff --git a/src/modules/flight_mode_manager/tasks/AltitudeCruise/FlightTaskAltitudeCruise.hpp b/src/modules/flight_mode_manager/tasks/AltitudeCruise/FlightTaskAltitudeCruise.hpp new file mode 100644 index 0000000000..1915b20304 --- /dev/null +++ b/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; +}; diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 1d049b344a..778cea55f2 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/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]) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 6c04d7388a..92648daf7c 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/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(); diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp index 32ad13bdf8..6148bb985d 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp +++ b/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; +} diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp index f5caea8550..992c975224 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp +++ b/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 _man_input_filter; + matrix::Vector2f _altitude_cruise_acceleration{}; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_man_tilt_max, ///< maximum tilt allowed for manual flight diff --git a/src/modules/fw_mode_manager/CMakeLists.txt b/src/modules/fw_mode_manager/CMakeLists.txt index dec814aebc..dc32389580 100644 --- a/src/modules/fw_mode_manager/CMakeLists.txt +++ b/src/modules/fw_mode_manager/CMakeLists.txt @@ -41,7 +41,7 @@ set(POSCONTROL_DEPENDENCIES SlewRate tecs motion_planning - performance_model + performance_model Sticks ) diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index b5e6d758c1..9d43e1f637 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/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 diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.hpp b/src/modules/fw_mode_manager/FixedWingModeManager.hpp index 71fe564de7..fcc6270a22 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.hpp +++ b/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}; diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 082b2ccaa3..517639100c 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/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; diff --git a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c index 6229b18bc5..c804a034bb 100644 --- a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c +++ b/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 diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7ee9bcaabe..69ca9896d4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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: