initial control allocation support

- control allocation module with multirotor, VTOL standard, and tiltrotor support
 - angular_velocity_controller
 - See https://github.com/PX4/PX4-Autopilot/pull/13351 for details

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Roman Bapst <bapstroman@gmail.com>

msg/CMakeLists.txt

@@ -48,6 +48,7 @@ set(msg_files
 	collision_constraints.msg
 	collision_report.msg
 	commander_state.msg
+	control_allocator_status.msg
 	cpuload.msg
 	differential_pressure.msg
 	distance_sensor.msg
@@ -140,8 +141,10 @@ set(msg_files
 	ulog_stream.msg
 	ulog_stream_ack.msg
 	vehicle_acceleration.msg
+	vehicle_actuator_setpoint.msg
 	vehicle_air_data.msg
 	vehicle_angular_acceleration.msg
+	vehicle_angular_acceleration_setpoint.msg
 	vehicle_angular_velocity.msg
 	vehicle_attitude.msg
 	vehicle_attitude_setpoint.msg
@@ -162,6 +165,8 @@ set(msg_files
 	vehicle_roi.msg
 	vehicle_status.msg
 	vehicle_status_flags.msg
+	vehicle_thrust_setpoint.msg
+	vehicle_torque_setpoint.msg
 	vehicle_trajectory_bezier.msg
 	vehicle_trajectory_waypoint.msg
 	vtol_vehicle_status.msg

msg/actuator_controls.msg

@@ -1,6 +1,6 @@
 uint64 timestamp			# time since system start (microseconds)
 uint8 NUM_ACTUATOR_CONTROLS = 8
-uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 6
 uint8 INDEX_ROLL = 0
 uint8 INDEX_PITCH = 1
 uint8 INDEX_YAW = 2
@@ -16,10 +16,13 @@ uint8 GROUP_INDEX_ATTITUDE = 0
 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
 uint8 GROUP_INDEX_GIMBAL = 2
 uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3
+uint8 GROUP_INDEX_ALLOCATED_PART1 = 4
+uint8 GROUP_INDEX_ALLOCATED_PART2 = 5
 uint8 GROUP_INDEX_PAYLOAD = 6
 
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
 float32[8] control
 
 # TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
+# TOPICS actuator_controls_4 actuator_controls_5
 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc

msg/control_allocator_status.msg

@@ -0,0 +1,21 @@
+uint64 timestamp                        # time since system start (microseconds)
+
+uint8 torque_setpoint_achieved          # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
+float32[3] allocated_torque             # Torque allocated to actuators. Equal to `vehicle_torque_setpoint_s::xyz` if the setpoint was achieved.
+float32[3] unallocated_torque           # Unallocated torque. Equal to 0 if the setpoint was achieved.
+                                        # Computed as: unallocated_torque = torque_setpoint - allocated_torque
+
+uint8 thrust_setpoint_achieved          # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
+float32[3] allocated_thrust             # Thrust allocated to actuators. Equal to `vehicle_thrust_setpoint_s::xyz` if the setpoint was achieved.
+float32[3] unallocated_thrust           # Unallocated thrust. Equal to 0 if the setpoint was achieved.
+                                        # Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust
+
+int8 ACTUATOR_SATURATION_OK        =  0 # The actuator is not saturated
+int8 ACTUATOR_SATURATION_UPPER_DYN =  1 # The actuator is saturated (with a value <= the desired value) because it cannot increase its value faster
+int8 ACTUATOR_SATURATION_UPPER     =  2 # The actuator is saturated (with a value <= the desired value) because it has reached its maximum value
+int8 ACTUATOR_SATURATION_LOWER_DYN = -1 # The actuator is saturated (with a value >= the desired value) because it cannot decrease its value faster
+int8 ACTUATOR_SATURATION_LOWER     = -2 # The actuator is saturated (with a value >= the desired value) because it has reached its minimum value
+
+int8[16] actuator_saturation            # Indicates actuator saturation status.
+                                        # Note 1: actuator saturation does not necessarily imply that the thrust setpoint or the torque setpoint were not achieved.
+                                        # Note 2: an actuator with limited dynamics can be indicated as upper-saturated even if it as not reached its maximum value.

msg/tools/uorb_rtps_message_ids.yaml

@@ -298,6 +298,16 @@ rtps:
     id: 141
   - msg: rtl_flight_time
     id: 142
+  - msg: vehicle_angular_acceleration_setpoint
+    id: 143
+  - msg: vehicle_torque_setpoint
+    id: 144
+  - msg: vehicle_thrust_setpoint
+    id: 145
+  - msg: vehicle_actuator_setpoint
+    id: 146
+  - msg: control_allocator_status
+    id: 147
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 170

msg/vehicle_actuator_setpoint.msg

@@ -0,0 +1,6 @@
+uint64 timestamp                   # time since system start (microseconds)
+uint64 timestamp_sample            # the timestamp the data this control response is based on was sampled
+
+uint8 NUM_ACTUATOR_SETPOINT = 16
+
+float32[16] actuator

msg/vehicle_angular_acceleration_setpoint.msg

@@ -0,0 +1,5 @@
+
+uint64 timestamp         # time since system start (microseconds)
+uint64 timestamp_sample  # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz           # angular acceleration about X, Y, Z body axis in rad/s^2

msg/vehicle_thrust_setpoint.msg

@@ -0,0 +1,5 @@
+
+uint64 timestamp        # time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz          # thrust setpoint along X, Y, Z body axis (in N)

msg/vehicle_torque_setpoint.msg

@@ -0,0 +1,5 @@
+
+uint64 timestamp        # time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz          # torque setpoint about X, Y, Z body axis (in N.m)