UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-24 15:27:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(commander): add setpoint types

Browse Source 
Switches from using VehicleControlMode to a specific setpoint type message
with reply.
Reasons:
- in case a VehicleControlMode was dropped (e.g. when the mode switched
  setpoint type), no confirmation was returned, and resulting in wrong
  controller flags.
- cleaner interface separation: external modes do not need to configure
  (or know) which controller to run for a certain setpoint
- allows for external modes to check for compatibility: e.g. a mode using
  fixed-wing setpoint types can now be rejected on a multicopter.
- allows for further extensions, like a setpoint timeout

This makes it a bit more effort to add a new setpoint type. Specifically,
setpoint_types.cpp needs to be extended when adding a new setpoint message.

PX4 internal modes also make use of the setpoint types. The information
flow is:
nav_state -> setpoint type -> vehicle control mode flags

It also adds a timeout to the setpoint config, but is not implemented
yet.

This changes the interface for external modes and thus the compatibility
version is increased.
This commit is contained in:
Beat Küng
2026-06-16 13:15:23 +02:00
committed by Ramon Roche
parent 0280f41867
commit 343eab2f05
17 changed files with 508 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
msg/CMakeLists.txt
View File

@@ -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

7
msg/versioned/VehicleControlMode.msg → msg/VehicleControlMode.msg
View File

@@ -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

2
msg/versioned/RegisterExtComponentRequest.msg
View File

@@ -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

33
msg/versioned/SetpointConfig.msg Normal file
View File

@@ -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).

22
msg/versioned/SetpointConfigReply.msg Normal file
View File

@@ -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

6
src/modules/commander/Commander.cpp
View File

@@ -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 =

96
src/modules/commander/ModeManagement.cpp
View File

@@ -34,6 +34,7 @@
#ifndef CONSTRAINED_FLASH
#include "ModeManagement.hpp"
#include "ModeUtil/setpoint_types.hpp"
#include <px4_platform_common/events.h>
@@ -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<mode_util::SetpointType>(setpoint_config.type);
reply.result = static_cast<uint8_t>(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;

38
src/modules/commander/ModeManagement.hpp
View File

@@ -39,6 +39,8 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/register_ext_component_request.h>
#include <uORB/topics/register_ext_component_reply.h>
#include <uORB/topics/setpoint_config.h>
#include <uORB/topics/setpoint_config_reply.h>
#include <uORB/topics/unregister_ext_component.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -47,6 +49,7 @@
#include <lib/modes/ui.hpp>
#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_s> _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_s> _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 {}

1
src/modules/commander/ModeUtil/CMakeLists.txt
View File

@@ -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)

101
src/modules/commander/ModeUtil/control_mode.cpp
View File

@@ -32,6 +32,7 @@
****************************************************************************/
#include "control_mode.hpp"
#include "setpoint_types.hpp"
#include <uORB/topics/vehicle_status.h>
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;
}

3
src/modules/commander/ModeUtil/control_mode.hpp
View File

@@ -37,12 +37,13 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <stdint.h>
#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

238
src/modules/commander/ModeUtil/setpoint_types.cpp Normal file
View File

@@ -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 <uORB/topics/vehicle_status.h>
#include <px4_platform_common/log.h>
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

82
src/modules/commander/ModeUtil/setpoint_types.hpp Normal file
View File

@@ -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 <uORB/topics/setpoint_config.h>
#include <uORB/topics/setpoint_config_reply.h>
#include <uORB/topics/vehicle_control_mode.h>
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

2
src/modules/mc_nn_control/mc_nn_control.cpp
View File

@@ -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);

2
src/modules/mc_raptor/mc_raptor.cpp
View File

@@ -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();

10
src/modules/uxrce_dds_client/dds_topics.yaml
View File

@@ -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

13
src/modules/zenoh/dds_topics.yaml
View File

@@ -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