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.
PX4 · Jul 19, 2024 · 5cda4f4 · 5cda4f4
1 parent ca9948a
commit 5cda4f4
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 4 deletions.
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
@@ -2110,6 +2110,19 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f;
 	// Pass along the button states
 	manual_control_setpoint.buttons = mavlink_manual_control.buttons;
+
+	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;

diff --git a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp
@@ -68,10 +68,10 @@ class MavlinkStreamManualControl : public MavlinkStream
 			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 @@ class MavlinkStreamManualControl : public MavlinkStream
 				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;