diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 6e1c24bce946..54de4e12c28c 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -15,6 +15,7 @@ #include "translation_config_overrides_v1.h" #include "translation_event_v1.h" #include "translation_home_position_v1.h" +#include "translation_manual_control_setpoint_v1.h" #include "translation_register_ext_component_reply_v1.h" #include "translation_register_ext_component_request_v1.h" #include "translation_register_ext_component_request_v2.h" diff --git a/msg/translation_node/translations/translation_manual_control_setpoint_v1.h b/msg/translation_node/translations/translation_manual_control_setpoint_v1.h new file mode 100644 index 000000000000..6ac7c0cccbf4 --- /dev/null +++ b/msg/translation_node/translations/translation_manual_control_setpoint_v1.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * Copyright (c) 2026 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate ManualControlSetpoint v0 <--> v1 +#include +#include + +class ManualControlSetpointV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::ManualControlSetpointV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::ManualControlSetpoint; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/out/manual_control_setpoint"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + msg_newer.timestamp = msg_older.timestamp; + msg_newer.timestamp_sample = msg_older.timestamp_sample; + msg_newer.valid = msg_older.valid; + msg_newer.data_source = msg_older.data_source; + msg_newer.roll = msg_older.roll; + msg_newer.pitch = msg_older.pitch; + msg_newer.yaw = msg_older.yaw; + msg_newer.throttle = msg_older.throttle; + msg_newer.flaps = msg_older.flaps; + msg_newer.aux1 = msg_older.aux1; + msg_newer.aux2 = msg_older.aux2; + msg_newer.aux3 = msg_older.aux3; + msg_newer.aux4 = msg_older.aux4; + msg_newer.aux5 = msg_older.aux5; + msg_newer.aux6 = msg_older.aux6; + msg_newer.sticks_moving = msg_older.sticks_moving; + msg_newer.buttons = msg_older.buttons; + msg_newer.sender_system_id = 0; // Default value for v1 + msg_newer.sender_component_id = 0; // Default value for v1 + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + msg_older.timestamp = msg_newer.timestamp; + msg_older.timestamp_sample = msg_newer.timestamp_sample; + msg_older.valid = msg_newer.valid; + msg_older.data_source = msg_newer.data_source; + msg_older.roll = msg_newer.roll; + msg_older.pitch = msg_newer.pitch; + msg_older.yaw = msg_newer.yaw; + msg_older.throttle = msg_newer.throttle; + msg_older.flaps = msg_newer.flaps; + msg_older.aux1 = msg_newer.aux1; + msg_older.aux2 = msg_newer.aux2; + msg_older.aux3 = msg_newer.aux3; + msg_older.aux4 = msg_newer.aux4; + msg_older.aux5 = msg_newer.aux5; + msg_older.aux6 = msg_newer.aux6; + msg_older.sticks_moving = msg_newer.sticks_moving; + msg_older.buttons = msg_newer.buttons; + // sender_system_id / sender_component_id dropped (not present in v0) + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(ManualControlSetpointV1Translation); diff --git a/msg/versioned/ManualControlSetpoint.msg b/msg/versioned/ManualControlSetpoint.msg index 0d30cf757557..cda3ea0f0d6e 100644 --- a/msg/versioned/ManualControlSetpoint.msg +++ b/msg/versioned/ManualControlSetpoint.msg @@ -1,4 +1,4 @@ -uint32 MESSAGE_VERSION = 0 +uint32 MESSAGE_VERSION = 1 uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) @@ -41,6 +41,11 @@ bool sticks_moving # manual control override request in an auto or offboard mode uint16 buttons # From uint16 buttons field of Mavlink manual_control message +# System/component ID of the system/component sending manual control messages providing this input. +# Only valid if data_source is SOURCE_MAVLINK_0 or higher. +uint8 sender_system_id +uint8 sender_component_id + # TOPICS manual_control_setpoint manual_control_input # DEPRECATED: float32 x # DEPRECATED: float32 y diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index f9cb1f9e4829..958d282cacc0 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit f9cb1f9e482977446c3dd953a16368d4834c6aa5 +Subproject commit 958d282cacc02cdb6d536d9a0d14761060ed6573 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 98f3d155ec62..0efe135c31e3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1917,6 +1917,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("HYGROMETER_SENSOR", 1.0f); configure_stream_local("MAG_CAL_REPORT", 1.0f); configure_stream_local("MANUAL_CONTROL", 5.0f); +#if defined(MAVLINK_MSG_ID_MANUAL_CONTROL_STATUS) + configure_stream_local("MANUAL_CONTROL_STATUS", 1.0f); +#endif // MAVLINK_MSG_ID_MANUAL_CONTROL_STATUS configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8eb1f42eea43..a70bca6f9dd6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -97,6 +97,9 @@ #include "streams/LOCAL_POSITION_NED.hpp" #include "streams/MAG_CAL_REPORT.hpp" #include "streams/MANUAL_CONTROL.hpp" +#if defined(MAVLINK_MSG_ID_MANUAL_CONTROL_STATUS) +#include "streams/MANUAL_CONTROL_STATUS.hpp" +#endif // MAVLINK_MSG_ID_MANUAL_CONTROL_STATUS #include "streams/MOUNT_ORIENTATION.hpp" #include "streams/NAV_CONTROLLER_OUTPUT.hpp" #include "streams/OBSTACLE_DISTANCE.hpp" @@ -395,6 +398,9 @@ static const StreamListItem streams_list[] = { #if defined(MANUAL_CONTROL_HPP) create_stream_list_item(), #endif // MANUAL_CONTROL_HPP +#if defined(MANUAL_CONTROL_STATUS_HPP) + create_stream_list_item(), +#endif // MANUAL_CONTROL_STATUS_HPP #if defined(OPTICAL_FLOW_RAD_HPP) create_stream_list_item(), #endif // OPTICAL_FLOW_RAD_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7720719e864f..a69702053a02 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2351,6 +2351,8 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) && math::isInRange((int)mavlink_manual_control.aux6, -1000, 1000)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6 / 1000.0f; } manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink.get_instance_id(); + manual_control_setpoint.sender_system_id = msg->sysid; + manual_control_setpoint.sender_component_id = msg->compid; manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time(); manual_control_setpoint.valid = true; _manual_control_input_pub.publish(manual_control_setpoint); diff --git a/src/modules/mavlink/streams/MANUAL_CONTROL_STATUS.hpp b/src/modules/mavlink/streams/MANUAL_CONTROL_STATUS.hpp new file mode 100644 index 000000000000..b256604f88eb --- /dev/null +++ b/src/modules/mavlink/streams/MANUAL_CONTROL_STATUS.hpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef MANUAL_CONTROL_STATUS_HPP +#define MANUAL_CONTROL_STATUS_HPP + +#include + +class MavlinkStreamManualControlStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamManualControlStatus(mavlink); } + + static constexpr const char *get_name_static() { return "MANUAL_CONTROL_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_MANUAL_CONTROL_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _manual_control_setpoint_sub.advertised() ? + (MAVLINK_MSG_ID_MANUAL_CONTROL_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamManualControlStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + + bool send() override + { + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + mavlink_manual_control_status_t msg{}; + + msg.source = manual_control_setpoint.data_source; + msg.sender_system_id = manual_control_setpoint.sender_system_id; + msg.sender_component_id = manual_control_setpoint.sender_component_id; + msg.valid = manual_control_setpoint.valid ? 1 : 0; + + mavlink_msg_manual_control_status_send_struct(_mavlink->get_channel(), &msg); + return true; + } + + return false; + } +}; + +#endif // MANUAL_CONTROL_STATUS_HPP