feat(commander): external modes setpoint types & reliability

msg/CMakeLists.txt

@@ -233,8 +233,8 @@ set(msg_files
 	UlogStreamAck.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
-	VehicleAngularAccelerationSetpoint.msg
 	VehicleConstraints.msg
+	VehicleControlMode.msg
 	VehicleImu.msg
 	VehicleImuStatus.msg
 	VehicleLocalPositionSetpoint.msg
@@ -268,14 +268,15 @@ 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
 	versioned/VehicleAttitude.msg
 	versioned/VehicleAttitudeSetpoint.msg
 	versioned/VehicleCommandAck.msg
 	versioned/VehicleCommand.msg
-	versioned/VehicleControlMode.msg
 	versioned/VehicleGlobalPosition.msg
 	versioned/VehicleLandDetected.msg
 	versioned/VehicleLocalPosition.msg

msg/versioned/VehicleControlMode.msg → 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
@@ -12,13 +12,6 @@ bool flag_control_position_enabled		# true if position is controlled
 bool flag_control_velocity_enabled		# true if horizontal velocity (implies direction) is controlled
 bool flag_control_altitude_enabled		# true if altitude is controlled
 bool flag_control_climb_rate_enabled		# true if climb rate is controlled
-bool flag_control_acceleration_enabled		# true if acceleration is controlled
 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

msg/versioned/ConfigOverrides.msg

@@ -18,4 +18,4 @@ uint8 source_id                  # ID depending on source_type
 
 uint8 ORB_QUEUE_LENGTH = 4
 
-# TOPICS config_overrides config_overrides_request
+# TOPICS config_overrides config_overrides_request config_overrides_confirm

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
 

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

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

src/modules/commander/Commander.cpp

@@ -2590,7 +2590,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) {
@@ -2759,18 +2759,17 @@ 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 =
 		(_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
 		&& (_vehicle_control_mode.flag_control_altitude_enabled
 		    || _vehicle_control_mode.flag_control_climb_rate_enabled
 		    || _vehicle_control_mode.flag_control_position_enabled
-		    || _vehicle_control_mode.flag_control_velocity_enabled
-		    || _vehicle_control_mode.flag_control_acceleration_enabled);
+		    || _vehicle_control_mode.flag_control_velocity_enabled);
 	_vehicle_control_mode.timestamp = hrt_absolute_time();
 	_vehicle_control_mode_pub.publish(_vehicle_control_mode);
 }

src/modules/commander/ModeManagement.cpp

@@ -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;
@@ -671,6 +683,10 @@ void ModeManagement::checkConfigOverrides()
 
 			break;
 		}
+
+		// Publish confirmation
+		override_request.timestamp = hrt_absolute_time();
+		_config_overrides_confirm_pub.publish(override_request);
 	}
 }