diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index fc23f3deafb..0c0ab388cbf 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -235,6 +235,7 @@ set(msg_files VehicleAcceleration.msg VehicleAirData.msg VehicleConstraints.msg + VehicleControlMode.msg VehicleImu.msg VehicleImuStatus.msg VehicleLocalPositionSetpoint.msg @@ -268,6 +269,8 @@ set(msg_files versioned/RaptorStatus.msg versioned/RegisterExtComponentReply.msg versioned/RegisterExtComponentRequest.msg + versioned/SetpointConfig.msg + versioned/SetpointConfigReply.msg versioned/TrajectorySetpoint.msg versioned/UnregisterExtComponent.msg versioned/VehicleAngularVelocity.msg @@ -275,7 +278,6 @@ set(msg_files versioned/VehicleAttitudeSetpoint.msg versioned/VehicleCommandAck.msg versioned/VehicleCommand.msg - versioned/VehicleControlMode.msg versioned/VehicleGlobalPosition.msg versioned/VehicleLandDetected.msg versioned/VehicleLocalPosition.msg diff --git a/msg/versioned/VehicleControlMode.msg b/msg/VehicleControlMode.msg similarity index 85% rename from msg/versioned/VehicleControlMode.msg rename to msg/VehicleControlMode.msg index 0a1fdedfbf1..95e88aab77e 100644 --- a/msg/versioned/VehicleControlMode.msg +++ b/msg/VehicleControlMode.msg @@ -1,4 +1,4 @@ -uint32 MESSAGE_VERSION = 0 +# Defines which controllers should run uint64 timestamp # time since system start (microseconds) bool flag_armed # synonym for actuator_armed.armed @@ -17,8 +17,3 @@ bool flag_control_attitude_enabled # true if attitude stabilization is mixed in bool flag_control_rates_enabled # true if rates are stabilized bool flag_control_allocation_enabled # true if control allocation is enabled bool flag_control_termination_enabled # true if flighttermination is enabled - -# TODO: use dedicated topic for external requests -uint8 source_id # Mode ID (nav_state) - -# TOPICS vehicle_control_mode config_control_setpoints diff --git a/msg/versioned/RegisterExtComponentRequest.msg b/msg/versioned/RegisterExtComponentRequest.msg index 1ed4295f71e..c073431df54 100644 --- a/msg/versioned/RegisterExtComponentRequest.msg +++ b/msg/versioned/RegisterExtComponentRequest.msg @@ -7,7 +7,7 @@ uint64 timestamp # time since system start (microseconds) uint64 request_id # ID, set this to a random value char[25] name # either the requested mode name, or component name -uint16 LATEST_PX4_ROS2_API_VERSION = 1 # API version compatibility. Increase this on a breaking semantic change. Changes to any message field are detected separately and do not require an API version change. +uint16 LATEST_PX4_ROS2_API_VERSION = 2 # API version compatibility. Increase this on a breaking semantic change. Changes to any message field are detected separately and do not require an API version change. uint16 px4_ros2_api_version # Set to LATEST_PX4_ROS2_API_VERSION diff --git a/msg/versioned/SetpointConfig.msg b/msg/versioned/SetpointConfig.msg new file mode 100644 index 00000000000..d4a1daf69ef --- /dev/null +++ b/msg/versioned/SetpointConfig.msg @@ -0,0 +1,33 @@ +# Setpoint configuration message +# +# Published by external modes and PX4 will respond with SetpointConfigReply. + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # [us] Time since system start + +uint16 TYPE_INVALID = 0 +uint16 TYPE_DIRECT_ACTUATORS = 1 # ActuatorMotors and ActuatorServos +uint16 TYPE_MULTICOPTER_GOTO = 2 # GotoSetpoint +uint16 TYPE_FIXEDWING_LATERAL_LONGITUDINAL = 3 # FixedWingLateralSetpoint and FixedWingLongitudinalSetpoint +uint16 TYPE_TRAJECTORY = 4 # TrajectorySetpoint +uint16 TYPE_RATES = 5 # VehicleRatesSetpoint +uint16 TYPE_ATTITUDE = 6 # VehicleAttitudeSetpoint +uint16 TYPE_ROVER_POSITION = 7 # RoverPositionSetpoint +uint16 TYPE_ROVER_SPEED_ATTITUDE = 8 # RoverSpeedSetpoint and RoverAttitudeSetpoint +uint16 TYPE_ROVER_SPEED_RATE = 9 # RoverSpeedSetpoint and RoverRateSetpoint +uint16 TYPE_ROVER_SPEED_STEERING = 10 # RoverSpeedSetpoint and RoverSteeringSetpoint +uint16 TYPE_ROVER_THROTTLE_ATTITUDE = 11 # RoverThrottleSetpoint and RoverAttitudeSetpoint +uint16 TYPE_ROVER_THROTTLE_RATE = 12 # RoverThrottleSetpoint and RoverRateSetpoint +uint16 TYPE_ROVER_THROTTLE_STEERING = 13 # RoverThrottleSetpoint and RoverSteeringSetpoint +uint16 TYPE_TRAJECTORY_6DOF = 14 # TrajectorySetpoint6dof +uint16 TYPE_THRUST_AND_TORQUE = 15 # VehicleThrustSetpoint and VehicleTorqueSetpoint +uint16 TYPE_POSITION_TRIPLET = 16 # PositionSetpointTriplet + +uint16 type # [@enum TYPE] setpoint type (corresponding to one or more setpoint messages) + +uint8 source_id # nav_state of the mode + +bool should_apply # if true: apply as current setpoint configuration (mode should be active). If false: setpoint configuration is not changed (can be used to check if a setpoint can be used with the current vehicle configuration). + +uint16 timeout_ms # Configure setpoint timeout. If no setpoint received for this time, PX4 triggers a failsafe. 0 disables the timeout (unresponsive modes still trigger a timeout through arming checks). diff --git a/msg/versioned/SetpointConfigReply.msg b/msg/versioned/SetpointConfigReply.msg new file mode 100644 index 00000000000..d5c8cb56246 --- /dev/null +++ b/msg/versioned/SetpointConfigReply.msg @@ -0,0 +1,22 @@ +# Reply to SetpointConfig + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # [us] Time since system start + +uint16 type # See SetpointConfig::TYPE_* + +uint8 source_id # nav_state of the mode that sent the SetpointConfig + +uint8 RESULT_SUCCESS = 0 +uint8 RESULT_FAILURE_OTHER = 1 +uint8 RESULT_UNSUPPORTED = 2 # Setpoint type is unsupported for the current vehicle +uint8 RESULT_UNKNOWN_SETPOINT_TYPE = 3 # The setpoint type is not known +uint8 result # [@enum RESULT] + +# Mode requirements for using the given setpoint type. A mode will use these and apply them to the arming check reply (PX4 does not do that itself). +# Certain setpoint types can be used in a reduced way, for example a TrajectorySetpoint without position control. In that case PX4 still sets all requirement flags, and the mode will then ignore mode_req_local_position. +bool mode_req_angular_velocity +bool mode_req_attitude +bool mode_req_local_alt +bool mode_req_local_position diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e3a63cc33ee..2fb8fb06224 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2593,7 +2593,7 @@ void Commander::checkAndInformReadyForTakeoff() void Commander::modeManagementUpdate() { ModeManagement::UpdateRequest mode_management_update{}; - _mode_management.update(isArmed(), _vehicle_status.nav_state_user_intention, + _mode_management.update(_vehicle_status.vehicle_type, isArmed(), _vehicle_status.nav_state_user_intention, mode_management_update); if (!isArmed() && mode_management_update.change_user_intended_nav_state) { @@ -2762,9 +2762,9 @@ void Commander::updateControlMode() { _vehicle_control_mode = {}; + const auto external_mode_setpoint_type = _mode_management.getSetpointType(_vehicle_status.nav_state); mode_util::getVehicleControlMode(_vehicle_status.nav_state, - _vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode); - _mode_management.updateControlMode(_vehicle_status.nav_state, _vehicle_control_mode); + _vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode, external_mode_setpoint_type); _vehicle_control_mode.flag_armed = isArmed(); _vehicle_control_mode.flag_multicopter_position_control_enabled = diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index 5700f6e9090..ba1aef789e1 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -34,6 +34,7 @@ #ifndef CONSTRAINED_FLASH #include "ModeManagement.hpp" +#include "ModeUtil/setpoint_types.hpp" #include @@ -367,7 +368,7 @@ void ModeManagement::checkUnregistrations(uint8_t user_intended_nav_state, Updat } } -void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, UpdateRequest &update_request) +void ModeManagement::update(uint8_t vehicle_type, bool armed, uint8_t user_intended_nav_state, UpdateRequest &update_request) { _external_checks.update(); @@ -411,7 +412,7 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, UpdateR checkUnregistrations(user_intended_nav_state, update_request); } - update_request.control_setpoint_update = checkConfigControlSetpointUpdates(); + update_request.control_setpoint_update = checkConfigControlSetpointUpdates(vehicle_type); checkConfigOverrides(); } @@ -533,43 +534,25 @@ uint8_t ModeManagement::getNavStateDisplay(uint8_t nav_state) const } } -bool ModeManagement::updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) +mode_util::SetpointType ModeManagement::getSetpointType(uint8_t nav_state) { - bool ret = false; + mode_util::SetpointType ret = Modes::Mode::kDefaultSetpointType; - const bool activation = (nav_state != _last_served_nav_state); + const bool mode_change = (nav_state != _last_served_nav_state); + + if (mode_change) { + if (_modes.valid(_last_served_nav_state)) { + // Reset the setpoint type for the deactivated mode + Modes::Mode &mode = _modes.mode(_last_served_nav_state); + mode.current_setpoint_type = Modes::Mode::kDefaultSetpointType; + } - if (activation) { _last_served_nav_state = nav_state; - _last_served_change_us = hrt_absolute_time(); } - if (nav_state >= Modes::FIRST_EXTERNAL_NAV_STATE && nav_state <= Modes::LAST_EXTERNAL_NAV_STATE) { - if (_modes.valid(nav_state)) { - const Modes::Mode &mode = _modes.mode(nav_state); - - // Refuse a cached config_control_setpoints entry that predates the current - // activation of this nav_state; publish safe defaults until a fresh one arrives. - const bool stale = (mode.config_control_setpoint.timestamp == 0) - || (mode.config_control_setpoint.timestamp + 10_ms < _last_served_change_us); - - if (stale) { - Modes::Mode::setControlModeDefaults(control_mode); - - if (activation) { - PX4_DEBUG("External mode %i: stale config_control_setpoints on activation, using safe defaults", - nav_state); - } - - } else { - control_mode = mode.config_control_setpoint; - } - - ret = true; - - } else { - Modes::Mode::setControlModeDefaults(control_mode); - } + if (_modes.valid(nav_state)) { + const Modes::Mode &mode = _modes.mode(nav_state); + ret = mode.current_setpoint_type; } return ret; @@ -623,25 +606,54 @@ void ModeManagement::updateActiveConfigOverrides(uint8_t nav_state, config_overr } } -bool ModeManagement::checkConfigControlSetpointUpdates() +bool ModeManagement::checkConfigControlSetpointUpdates(uint8_t vehicle_type) { bool had_update = false; - vehicle_control_mode_s config_control_setpoint; + setpoint_config_s setpoint_config; int max_updates = 5; - while (_config_control_setpoints_sub.update(&config_control_setpoint) && --max_updates >= 0) { - if (_modes.valid(config_control_setpoint.source_id)) { - Modes::Mode &mode = _modes.mode(config_control_setpoint.source_id); - mode.config_control_setpoint = config_control_setpoint; - mode.config_control_setpoint.timestamp = hrt_absolute_time(); - had_update = true; + while (_setpoint_config_sub.update(&setpoint_config) && --max_updates >= 0) { + setpoint_config_reply_s reply{}; + reply.source_id = setpoint_config.source_id; + reply.type = setpoint_config.type; + + if (_modes.valid(setpoint_config.source_id)) { + Modes::Mode &mode = _modes.mode(setpoint_config.source_id); + + const auto setpoint_type = static_cast(setpoint_config.type); + reply.result = static_cast(mode_util::isSetpointTypeValid(setpoint_type, vehicle_type)); + + if (reply.result == setpoint_config_reply_s::RESULT_SUCCESS) { + // Get control mode + vehicle_control_mode_s config_control_setpoint{}; + mode_util::getControlMode(setpoint_type, config_control_setpoint); + + // Set mode requirement flags + reply.mode_req_angular_velocity = config_control_setpoint.flag_control_rates_enabled; + reply.mode_req_attitude = config_control_setpoint.flag_control_attitude_enabled; + reply.mode_req_local_alt = config_control_setpoint.flag_control_altitude_enabled + || config_control_setpoint.flag_control_climb_rate_enabled; + reply.mode_req_local_position = config_control_setpoint.flag_control_position_enabled || + config_control_setpoint.flag_control_velocity_enabled; + + if (setpoint_config.should_apply) { + mode.current_setpoint_type = setpoint_type; + had_update = true; + } + } } else { if (!_invalid_mode_printed) { - PX4_ERR("Control sp config request for invalid mode: %i", config_control_setpoint.source_id); + PX4_ERR("Setpoint config request for invalid mode: %i", setpoint_config.source_id); _invalid_mode_printed = true; } + + reply.result = setpoint_config_reply_s::RESULT_FAILURE_OTHER; } + + // Return reply + reply.timestamp = hrt_absolute_time(); + _setpoint_config_reply_pub.publish(reply); } return had_update; diff --git a/src/modules/commander/ModeManagement.hpp b/src/modules/commander/ModeManagement.hpp index 43568842975..d1703592d1d 100644 --- a/src/modules/commander/ModeManagement.hpp +++ b/src/modules/commander/ModeManagement.hpp @@ -39,6 +39,8 @@ #include #include #include +#include +#include #include #include #include @@ -47,6 +49,7 @@ #include #include "UserModeIntention.hpp" #include "HealthAndArmingChecks/checks/externalChecks.hpp" +#include "ModeUtil/setpoint_types.hpp" class ModeExecutors { @@ -82,22 +85,9 @@ public: static constexpr int MAX_NUM = LAST_EXTERNAL_NAV_STATE - FIRST_EXTERNAL_NAV_STATE + 1; struct Mode { - Mode() - { - // Set defaults for control mode - setControlModeDefaults(config_control_setpoint); - } - static void setControlModeDefaults(vehicle_control_mode_s &config_control_setpoint_) - { - config_control_setpoint_.flag_control_position_enabled = true; - config_control_setpoint_.flag_control_velocity_enabled = true; - config_control_setpoint_.flag_control_altitude_enabled = true; - config_control_setpoint_.flag_control_climb_rate_enabled = true; - config_control_setpoint_.flag_control_acceleration_enabled = true; - config_control_setpoint_.flag_control_attitude_enabled = true; - config_control_setpoint_.flag_control_rates_enabled = true; - config_control_setpoint_.flag_control_allocation_enabled = true; - } + static constexpr auto kDefaultSetpointType = mode_util::SetpointType::Trajectory; + + Mode() = default; static constexpr uint8_t REPLACES_NAV_STATE_NONE = 0xff; @@ -109,7 +99,7 @@ public: int mode_executor_registration_id{-1}; bool request_offboard_setpoints{false}; config_overrides_s overrides{}; - vehicle_control_mode_s config_control_setpoint{}; + mode_util::SetpointType current_setpoint_type{kDefaultSetpointType}; }; void printStatus() const; @@ -141,7 +131,7 @@ public: bool control_setpoint_update{false}; }; - void update(bool armed, uint8_t user_intended_nav_state, UpdateRequest &update_request); + void update(uint8_t vehicle_type, bool armed, uint8_t user_intended_nav_state, UpdateRequest &update_request); void setFailsafeState(bool failsafe_action_active) { _failsafe_action_active = failsafe_action_active; @@ -166,7 +156,7 @@ public: uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true); - bool updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode); + mode_util::SetpointType getSetpointType(uint8_t nav_state); void printStatus() const; @@ -177,14 +167,15 @@ public: void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out); private: - bool checkConfigControlSetpointUpdates(); + bool checkConfigControlSetpointUpdates(uint8_t vehicle_type); void checkNewRegistrations(UpdateRequest &update_request); void checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request); void checkConfigOverrides(); void removeModeExecutor(int mode_executor_id); - uORB::Subscription _config_control_setpoints_sub{ORB_ID(config_control_setpoints)}; + uORB::Subscription _setpoint_config_sub{ORB_ID(setpoint_config)}; + uORB::Publication _setpoint_config_reply_pub{ORB_ID(setpoint_config_reply)}; uORB::Subscription _register_ext_component_request_sub{ORB_ID(register_ext_component_request)}; uORB::Subscription _unregister_ext_component_sub{ORB_ID(unregister_ext_component)}; uORB::Publication _register_ext_component_reply_pub{ORB_ID(register_ext_component_reply)}; @@ -202,7 +193,6 @@ private: bool _invalid_mode_printed{false}; uint8_t _last_served_nav_state{0xff}; - hrt_abstime _last_served_change_us{0}; }; #else /* CONSTRAINED_FLASH */ @@ -219,7 +209,7 @@ public: bool control_setpoint_update{false}; }; - void update(bool armed, uint8_t user_intended_nav_state, UpdateRequest &update_request) {} + void update(uint8_t vehicle_type, bool armed, uint8_t user_intended_nav_state, UpdateRequest &update_request) {} void setFailsafeState(bool failsafe_action_active) {} int modeExecutorInCharge() const { return ModeExecutors::AUTOPILOT_EXECUTOR_ID; } @@ -231,7 +221,7 @@ public: uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true) { return nav_state; } - bool updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) { return false; } + mode_util::SetpointType getSetpointType(uint8_t nav_state) { return mode_util::SetpointType::Trajectory; } void printStatus() const {} diff --git a/src/modules/commander/ModeUtil/CMakeLists.txt b/src/modules/commander/ModeUtil/CMakeLists.txt index 98708bba269..eb39df89670 100644 --- a/src/modules/commander/ModeUtil/CMakeLists.txt +++ b/src/modules/commander/ModeUtil/CMakeLists.txt @@ -34,5 +34,6 @@ add_library(mode_util control_mode.cpp mode_requirements.cpp + setpoint_types.cpp ) add_dependencies(mode_util uorb_headers prebuild_targets) diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index 4ce20ab018c..f5e5d8d0378 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "control_mode.hpp" +#include "setpoint_types.hpp" #include namespace mode_util @@ -44,44 +45,39 @@ static bool stabilization_required(uint8_t vehicle_type) void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, const offboard_control_mode_s &offboard_control_mode, - vehicle_control_mode_s &vehicle_control_mode) + vehicle_control_mode_s &vehicle_control_mode, SetpointType external_mode_setpoint_type) { switch (nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: vehicle_control_mode.flag_control_manual_enabled = true; - vehicle_control_mode.flag_control_attitude_enabled = stabilization_required(vehicle_type); - vehicle_control_mode.flag_control_rates_enabled = stabilization_required(vehicle_type); - vehicle_control_mode.flag_control_allocation_enabled = true; + + if (stabilization_required(vehicle_type)) { + getControlMode(SetpointType::Attitude, vehicle_control_mode); + + } else { + getControlMode(SetpointType::ThrustAndTorque, vehicle_control_mode); + } + break; case vehicle_status_s::NAVIGATION_STATE_STAB: vehicle_control_mode.flag_control_manual_enabled = true; - vehicle_control_mode.flag_control_attitude_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; - vehicle_control_mode.flag_control_allocation_enabled = true; + getControlMode(SetpointType::Attitude, vehicle_control_mode); 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; - vehicle_control_mode.flag_control_attitude_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; - vehicle_control_mode.flag_control_allocation_enabled = true; + getControlMode(SetpointType::Trajectory, vehicle_control_mode); + vehicle_control_mode.flag_control_velocity_enabled = false; + vehicle_control_mode.flag_control_position_enabled = false; break; case vehicle_status_s::NAVIGATION_STATE_POSCTL: case vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW: vehicle_control_mode.flag_control_manual_enabled = true; - vehicle_control_mode.flag_control_position_enabled = true; - vehicle_control_mode.flag_control_velocity_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; - vehicle_control_mode.flag_control_attitude_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; - vehicle_control_mode.flag_control_allocation_enabled = true; + getControlMode(SetpointType::Trajectory, vehicle_control_mode); break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: @@ -92,20 +88,12 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, case vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE: case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: - vehicle_control_mode.flag_control_auto_enabled = true; - vehicle_control_mode.flag_control_position_enabled = true; - vehicle_control_mode.flag_control_velocity_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; - vehicle_control_mode.flag_control_attitude_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; - vehicle_control_mode.flag_control_allocation_enabled = true; + getControlMode(SetpointType::PositionTriplet, vehicle_control_mode); break; case vehicle_status_s::NAVIGATION_STATE_ACRO: vehicle_control_mode.flag_control_manual_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; - vehicle_control_mode.flag_control_allocation_enabled = true; + getControlMode(SetpointType::Rates, vehicle_control_mode); break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: @@ -125,44 +113,31 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_offboard_enabled = true; if (offboard_control_mode.position) { - vehicle_control_mode.flag_control_position_enabled = true; - vehicle_control_mode.flag_control_velocity_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; - vehicle_control_mode.flag_control_acceleration_enabled = true; - vehicle_control_mode.flag_control_attitude_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; - vehicle_control_mode.flag_control_allocation_enabled = true; + getControlMode(SetpointType::Trajectory, vehicle_control_mode); } else if (offboard_control_mode.velocity) { - vehicle_control_mode.flag_control_velocity_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; - vehicle_control_mode.flag_control_acceleration_enabled = true; - vehicle_control_mode.flag_control_attitude_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; - vehicle_control_mode.flag_control_allocation_enabled = true; + getControlMode(SetpointType::Trajectory, vehicle_control_mode); + vehicle_control_mode.flag_control_position_enabled = false; } else if (offboard_control_mode.acceleration) { - vehicle_control_mode.flag_control_acceleration_enabled = true; - vehicle_control_mode.flag_control_attitude_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; - vehicle_control_mode.flag_control_allocation_enabled = true; + getControlMode(SetpointType::Trajectory, vehicle_control_mode); + vehicle_control_mode.flag_control_position_enabled = false; + vehicle_control_mode.flag_control_velocity_enabled = false; + vehicle_control_mode.flag_control_altitude_enabled = false; + vehicle_control_mode.flag_control_climb_rate_enabled = false; } else if (offboard_control_mode.attitude) { - vehicle_control_mode.flag_control_attitude_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; - vehicle_control_mode.flag_control_allocation_enabled = true; + getControlMode(SetpointType::Attitude, vehicle_control_mode); } else if (offboard_control_mode.body_rate) { - vehicle_control_mode.flag_control_rates_enabled = true; - vehicle_control_mode.flag_control_allocation_enabled = true; + getControlMode(SetpointType::Rates, vehicle_control_mode); } else if (offboard_control_mode.thrust_and_torque) { - vehicle_control_mode.flag_control_allocation_enabled = true; - } + getControlMode(SetpointType::ThrustAndTorque, vehicle_control_mode); - // direct_actuator: no flags set — companion bypasses PX4 controllers and allocator entirely + } else if (offboard_control_mode.direct_actuator) { + getControlMode(SetpointType::DirectActuators, vehicle_control_mode); + } break; @@ -172,17 +147,13 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, // the Flight Task from exiting itself when RC stick movement is detected. case vehicle_status_s::NAVIGATION_STATE_ORBIT: vehicle_control_mode.flag_control_manual_enabled = false; - vehicle_control_mode.flag_control_auto_enabled = false; - vehicle_control_mode.flag_control_position_enabled = true; - vehicle_control_mode.flag_control_velocity_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; - vehicle_control_mode.flag_control_attitude_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; - vehicle_control_mode.flag_control_allocation_enabled = true; + getControlMode(SetpointType::Trajectory, vehicle_control_mode); + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 ... vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: + getControlMode(external_mode_setpoint_type, vehicle_control_mode); break; - // vehicle_status_s::NAVIGATION_STATE_EXTERNALx: handled in ModeManagement default: break; } diff --git a/src/modules/commander/ModeUtil/control_mode.hpp b/src/modules/commander/ModeUtil/control_mode.hpp index 46e10fbb372..8c9819553a3 100644 --- a/src/modules/commander/ModeUtil/control_mode.hpp +++ b/src/modules/commander/ModeUtil/control_mode.hpp @@ -37,12 +37,13 @@ #include #include +#include "setpoint_types.hpp" namespace mode_util { void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, const offboard_control_mode_s &offboard_control_mode, - vehicle_control_mode_s &vehicle_control_mode); + vehicle_control_mode_s &vehicle_control_mode, SetpointType external_mode_setpoint_type); } // namespace mode_util diff --git a/src/modules/commander/ModeUtil/setpoint_types.cpp b/src/modules/commander/ModeUtil/setpoint_types.cpp new file mode 100644 index 00000000000..4063a92ead5 --- /dev/null +++ b/src/modules/commander/ModeUtil/setpoint_types.cpp @@ -0,0 +1,238 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +#ifndef MODULE_NAME +#define MODULE_NAME "ModeUtil" +#endif + +#include "setpoint_types.hpp" +#include +#include + +namespace mode_util +{ +void getControlMode(SetpointType setpoint_type, vehicle_control_mode_s &control_mode) +{ + switch (setpoint_type) { + case SetpointType::Invalid: + PX4_ERR("Invalid setpoint type"); + break; + + case SetpointType::DirectActuators: + // Nothing enabled + break; + + case SetpointType::Goto: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_acceleration_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + break; + + case SetpointType::FixedwingLateralLongitudinal: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_acceleration_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + break; + + case SetpointType::Trajectory: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_acceleration_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + break; + + case SetpointType::Rates: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_rates_enabled = true; + break; + + case SetpointType::Attitude: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + break; + + case SetpointType::RoverPosition: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_acceleration_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_position_enabled = true; + break; + + case SetpointType::RoverSpeedAttitude: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_acceleration_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case SetpointType::RoverSpeedRate: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_acceleration_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case SetpointType::RoverSpeedSteering: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_acceleration_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case SetpointType::RoverThrottleAttitude: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_acceleration_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case SetpointType::RoverThrottleRate: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_acceleration_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case SetpointType::RoverThrottleSteering: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_acceleration_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case SetpointType::Trajectory_6dof: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_acceleration_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + break; + + case SetpointType::ThrustAndTorque: + control_mode.flag_control_allocation_enabled = true; + break; + + case SetpointType::PositionTriplet: + control_mode.flag_control_allocation_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_acceleration_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_auto_enabled = true; + break; + } +} + +SetpointTypeResult isSetpointTypeValid(SetpointType setpoint_type, uint8_t vehicle_type) +{ + switch (setpoint_type) { + case SetpointType::Invalid: + return SetpointTypeResult::Unknown; + + case SetpointType::DirectActuators: + return SetpointTypeResult::Success; + + case SetpointType::Goto: + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + return SetpointTypeResult::Success; + } + + return SetpointTypeResult::Unsupported; + + case SetpointType::FixedwingLateralLongitudinal: + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return SetpointTypeResult::Success; + } + + return SetpointTypeResult::Unsupported; + + case SetpointType::Trajectory: + case SetpointType::Rates: + case SetpointType::Attitude: + return SetpointTypeResult::Success; + + case SetpointType::RoverPosition: + case SetpointType::RoverSpeedAttitude: + case SetpointType::RoverSpeedRate: + case SetpointType::RoverSpeedSteering: + case SetpointType::RoverThrottleAttitude: + case SetpointType::RoverThrottleRate: + case SetpointType::RoverThrottleSteering: + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { + return SetpointTypeResult::Success; + } + + return SetpointTypeResult::Unsupported; + + case SetpointType::Trajectory_6dof: + + // Used by spacecraft but that does not seem to set the vehicle_type + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_UNSPECIFIED) { + return SetpointTypeResult::Success; + } + + return SetpointTypeResult::Unsupported; + + case SetpointType::ThrustAndTorque: + return SetpointTypeResult::Success; + + case SetpointType::PositionTriplet: + return SetpointTypeResult::Success; + } + + return SetpointTypeResult::Unknown; +} +} // namespace mode_util diff --git a/src/modules/commander/ModeUtil/setpoint_types.hpp b/src/modules/commander/ModeUtil/setpoint_types.hpp new file mode 100644 index 00000000000..a48cb0dc56f --- /dev/null +++ b/src/modules/commander/ModeUtil/setpoint_types.hpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace mode_util +{ +enum class SetpointType : uint16_t { + Invalid = setpoint_config_s::TYPE_INVALID, + DirectActuators = setpoint_config_s::TYPE_DIRECT_ACTUATORS, + Goto = setpoint_config_s::TYPE_MULTICOPTER_GOTO, + FixedwingLateralLongitudinal = setpoint_config_s::TYPE_FIXEDWING_LATERAL_LONGITUDINAL, + Trajectory = setpoint_config_s::TYPE_TRAJECTORY, + Rates = setpoint_config_s::TYPE_RATES, + Attitude = setpoint_config_s::TYPE_ATTITUDE, + RoverPosition = setpoint_config_s::TYPE_ROVER_POSITION, + RoverSpeedAttitude = setpoint_config_s::TYPE_ROVER_SPEED_ATTITUDE, + RoverSpeedRate = setpoint_config_s::TYPE_ROVER_SPEED_RATE, + RoverSpeedSteering = setpoint_config_s::TYPE_ROVER_SPEED_STEERING, + RoverThrottleAttitude = setpoint_config_s::TYPE_ROVER_THROTTLE_ATTITUDE, + RoverThrottleRate = setpoint_config_s::TYPE_ROVER_THROTTLE_RATE, + RoverThrottleSteering = setpoint_config_s::TYPE_ROVER_THROTTLE_STEERING, + Trajectory_6dof = setpoint_config_s::TYPE_TRAJECTORY_6DOF, + ThrustAndTorque = setpoint_config_s::TYPE_THRUST_AND_TORQUE, + PositionTriplet = setpoint_config_s::TYPE_POSITION_TRIPLET, +}; + +/** + * Fill in the required control mode flags based on a setpoint type. + * Note that flags are not cleared, only set + * + * If a setpoint type has optional flags (e.g. position), it will be set here too. + */ +void getControlMode(SetpointType setpoint_type, vehicle_control_mode_s &control_mode); + +enum class SetpointTypeResult : uint8_t { + Success = setpoint_config_reply_s::RESULT_SUCCESS, + FailureOther = setpoint_config_reply_s::RESULT_FAILURE_OTHER, + Unknown = setpoint_config_reply_s::RESULT_UNKNOWN_SETPOINT_TYPE, + Unsupported = setpoint_config_reply_s::RESULT_UNSUPPORTED, +}; + +/** + * Check if a setpoint type is valid for a given vehicle type + */ +SetpointTypeResult isSetpointTypeValid(SetpointType setpoint_type, uint8_t vehicle_type); + +} // namespace mode_util diff --git a/src/modules/mc_nn_control/mc_nn_control.cpp b/src/modules/mc_nn_control/mc_nn_control.cpp index 99ad9e03936..267aa9bcac9 100644 --- a/src/modules/mc_nn_control/mc_nn_control.cpp +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -159,7 +159,7 @@ void MulticopterNeuralNetworkControl::RegisterNeuralFlightMode() register_ext_component_request.timestamp = hrt_absolute_time(); strncpy(register_ext_component_request.name, "Neural Control", sizeof(register_ext_component_request.name) - 1); register_ext_component_request.request_id = _mode_request_id; - register_ext_component_request.px4_ros2_api_version = 1; + register_ext_component_request.px4_ros2_api_version = register_ext_component_request_s::LATEST_PX4_ROS2_API_VERSION; register_ext_component_request.register_arming_check = true; register_ext_component_request.register_mode = true; _register_ext_component_request_pub.publish(register_ext_component_request); diff --git a/src/modules/mc_raptor/mc_raptor.cpp b/src/modules/mc_raptor/mc_raptor.cpp index cdf2b2af233..69344678065 100644 --- a/src/modules/mc_raptor/mc_raptor.cpp +++ b/src/modules/mc_raptor/mc_raptor.cpp @@ -255,7 +255,7 @@ bool Raptor::init() register_ext_component_request.timestamp = hrt_absolute_time(); strncpy(register_ext_component_request.name, "RAPTOR", sizeof(register_ext_component_request.name) - 1); register_ext_component_request.request_id = Raptor::EXT_COMPONENT_REQUEST_ID; - register_ext_component_request.px4_ros2_api_version = 1; + register_ext_component_request.px4_ros2_api_version = register_ext_component_request_s::LATEST_PX4_ROS2_API_VERSION; register_ext_component_request.register_arming_check = true; register_ext_component_request.register_mode = true; register_ext_component_request.enable_replace_internal_mode = _param_mc_raptor_offboard.get(); diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index e1d6bfb4c20..40ab2b71fbf 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -10,7 +10,9 @@ publications: - topic: /fmu/out/arming_check_request type: px4_msgs::msg::ArmingCheckRequest - rate_limit: 5. + + - topic: /fmu/out/setpoint_config_reply + type: px4_msgs::msg::SetpointConfigReply - topic: /fmu/out/mode_completed type: px4_msgs::msg::ModeCompleted @@ -129,6 +131,9 @@ subscriptions: - topic: /fmu/in/unregister_ext_component type: px4_msgs::msg::UnregisterExtComponent + - topic: /fmu/in/setpoint_config + type: px4_msgs::msg::SetpointConfig + - topic: /fmu/in/config_overrides_request type: px4_msgs::msg::ConfigOverrides @@ -141,9 +146,6 @@ subscriptions: - topic: /fmu/in/mode_completed type: px4_msgs::msg::ModeCompleted - - topic: /fmu/in/config_control_setpoints - type: px4_msgs::msg::VehicleControlMode - - topic: /fmu/in/distance_sensor type: px4_msgs::msg::DistanceSensor diff --git a/src/modules/zenoh/dds_topics.yaml b/src/modules/zenoh/dds_topics.yaml index 6a76d84588c..2e1889d9c4a 100644 --- a/src/modules/zenoh/dds_topics.yaml +++ b/src/modules/zenoh/dds_topics.yaml @@ -32,6 +32,13 @@ publications: rel: reliable express: true + - topic: /fmu/out/setpoint_config_reply + type: px4_msgs::msg::SetpointConfigReply + options: + cc: block + rel: reliable + express: true + - topic: /fmu/out/mode_completed type: px4_msgs::msg::ModeCompleted options: @@ -135,6 +142,9 @@ subscriptions: - topic: /fmu/in/unregister_ext_component type: px4_msgs::msg::UnregisterExtComponent + - topic: /fmu/in/setpoint_config + type: px4_msgs::msg::SetpointConfig + - topic: /fmu/in/config_overrides_request type: px4_msgs::msg::ConfigOverrides @@ -144,9 +154,6 @@ subscriptions: - topic: /fmu/in/mode_completed type: px4_msgs::msg::ModeCompleted - - topic: /fmu/in/config_control_setpoints - type: px4_msgs::msg::VehicleControlMode - - topic: /fmu/in/distance_sensor type: px4_msgs::msg::DistanceSensor