Support MAVLink extension MANUAL_CONTROL.aux

Note that in uORB we don't currently know if the aux fields are
specifically valid or not so we can also not set the corresponding
bits in the field.

src/modules/mavlink/mavlink_receiver.cpp

@@ -2063,6 +2063,19 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	// For backwards compatibility at the moment interpret throttle in range [0,1000]
 	manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f;
 	manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f;
+
+	if (mavlink_manual_control.enabled_extensions & (1u << 2)) { manual_control_setpoint.aux1 = mavlink_manual_control.aux1; }
+
+	if (mavlink_manual_control.enabled_extensions & (1u << 3)) { manual_control_setpoint.aux2 = mavlink_manual_control.aux2; }
+
+	if (mavlink_manual_control.enabled_extensions & (1u << 4)) { manual_control_setpoint.aux3 = mavlink_manual_control.aux3; }
+
+	if (mavlink_manual_control.enabled_extensions & (1u << 5)) { manual_control_setpoint.aux4 = mavlink_manual_control.aux4; }
+
+	if (mavlink_manual_control.enabled_extensions & (1u << 6)) { manual_control_setpoint.aux5 = mavlink_manual_control.aux5; }
+
+	if (mavlink_manual_control.enabled_extensions & (1u << 7)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6; }
+
 	manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
 	manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time();
 	manual_control_setpoint.valid = true;

src/modules/mavlink/streams/MANUAL_CONTROL.hpp

@@ -68,10 +68,10 @@ private:
 			mavlink_manual_control_t msg{};
 
 			msg.target = mavlink_system.sysid;
-			msg.x = manual_control_setpoint.pitch * 1000;
-			msg.y = manual_control_setpoint.roll * 1000;
-			msg.z = manual_control_setpoint.throttle * 1000;
-			msg.r = manual_control_setpoint.yaw * 1000;
+			msg.x = manual_control_setpoint.pitch * 1000.f;
+			msg.y = manual_control_setpoint.roll * 1000.f;
+			msg.z = manual_control_setpoint.throttle * 1000.f;
+			msg.r = manual_control_setpoint.yaw * 1000.f;
 
 			manual_control_switches_s manual_control_switches{};
 
@@ -84,6 +84,36 @@ private:
 				msg.buttons |= (manual_control_switches.kill_switch << (shift * 6));
 			}
 
+			if (PX4_ISFINITE(manual_control_setpoint.aux1)) {
+				msg.enabled_extensions |= (1u << 2);
+				msg.aux1 = manual_control_setpoint.aux1 * 1000.f;
+			}
+
+			if (PX4_ISFINITE(manual_control_setpoint.aux2)) {
+				msg.enabled_extensions |= (1u << 3);
+				msg.aux2 = manual_control_setpoint.aux2 * 1000.f;
+			}
+
+			if (PX4_ISFINITE(manual_control_setpoint.aux3)) {
+				msg.enabled_extensions |= (1u << 4);
+				msg.aux3 = manual_control_setpoint.aux3 * 1000.f;
+			}
+
+			if (PX4_ISFINITE(manual_control_setpoint.aux4)) {
+				msg.enabled_extensions |= (1u << 5);
+				msg.aux4 = manual_control_setpoint.aux4 * 1000.f;
+			}
+
+			if (PX4_ISFINITE(manual_control_setpoint.aux5)) {
+				msg.enabled_extensions |= (1u << 6);
+				msg.aux5 = manual_control_setpoint.aux5 * 1000.f;
+			}
+
+			if (PX4_ISFINITE(manual_control_setpoint.aux6)) {
+				msg.enabled_extensions |= (1u << 7);
+				msg.aux6 = manual_control_setpoint.aux6 * 1000.f;
+			}
+
 			mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg);
 
 			return true;