From 5cda4f4e186d94a736e5262910578b34641a719f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 20 Sep 2023 15:14:36 +0200 Subject: [PATCH] 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 | 13 +++++++ .../mavlink/streams/MANUAL_CONTROL.hpp | 38 +++++++++++++++++-- 2 files changed, 47 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 06fb97c8a65f..b2a6bfe5a4ce 100644 --- 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 index 8be68fdc3fad..3feabf124d68 100644 --- 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;