diff --git a/msg/px4_msgs_old/msg/ManualControlSetpointV0.msg b/msg/px4_msgs_old/msg/ManualControlSetpointV0.msg new file mode 100644 index 00000000000..0d30cf75755 --- /dev/null +++ b/msg/px4_msgs_old/msg/ManualControlSetpointV0.msg @@ -0,0 +1,48 @@ +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +bool valid + +uint8 SOURCE_UNKNOWN = 0 +uint8 SOURCE_RC = 1 # radio control (input_rc) +uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0 +uint8 SOURCE_MAVLINK_1 = 3 # mavlink instance 1 +uint8 SOURCE_MAVLINK_2 = 4 # mavlink instance 2 +uint8 SOURCE_MAVLINK_3 = 5 # mavlink instance 3 +uint8 SOURCE_MAVLINK_4 = 6 # mavlink instance 4 +uint8 SOURCE_MAVLINK_5 = 7 # mavlink instance 5 + +uint8 data_source + +# Any of the channels may not be available and be set to NaN +# to indicate that it does not contain valid data. + +# Stick positions [-1,1] +# on a common RC mode 1/2/3/4 remote/joystick the stick deflection: -1 is down/left, 1 is up/right +# Note: QGC sends throttle/z in range [0,1000] - [0,1]. The MAVLink input conversion [0,1] to [-1,1] is at the moment kept backwards compatible. +# Positive values are generally used for: +float32 roll # move right, positive roll rotation, right side down +float32 pitch # move forward, negative pitch rotation, nose down +float32 yaw # positive yaw rotation, clockwise when seen top down +float32 throttle # move up, positive thrust, -1 is minimum available 0% or -100% +1 is 100% thrust + +float32 flaps # position of flaps switch/knob/lever [-1, 1] + +float32 aux1 +float32 aux2 +float32 aux3 +float32 aux4 +float32 aux5 +float32 aux6 + +bool sticks_moving # manual control override request in an auto or offboard mode, only gets true if feature is enabled + +uint16 buttons # From uint16 buttons field of Mavlink manual_control message + +# TOPICS manual_control_setpoint manual_control_input +# DEPRECATED: float32 x +# DEPRECATED: float32 y +# DEPRECATED: float32 z +# DEPRECATED: float32 r diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 5d2529d8daf..76b31c54bdb 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -16,6 +16,7 @@ #include "translation_event_v1.h" #include "translation_home_position_v1.h" #include "translation_home_position_v2.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 00000000000..ab88c5ac461 --- /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.source_system_id = 0; // Default value for v1 + msg_newer.source_component_id = 0; // Default value for v1 + 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; + } + + 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; + // source_system_id / source_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 0d30cf75755..470fca8703f 100644 --- a/msg/versioned/ManualControlSetpoint.msg +++ b/msg/versioned/ManualControlSetpoint.msg @@ -1,4 +1,16 @@ -uint32 MESSAGE_VERSION = 0 +# Manual control input +# +# This message provides a representation of manual control input sources, +# such as RC controllers and MAVLink controller (Joystick). +# +# It defines the `manual_control_input` topic to represent each configured input, +# and the `manual_control_setpoint` topic to represent the input that is currently +# in charge of the vehicle (based on parameter COM_RC_IN_MODE). +# `manual_control_setpoint` is the topic that is published to the controllers. +# +# The message includes fields for the roll, pitch, yaw, throttle and flaps, along with auxiliary channels and button states. + +uint32 MESSAGE_VERSION = 1 uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) @@ -15,6 +27,8 @@ uint8 SOURCE_MAVLINK_4 = 6 # mavlink instance 4 uint8 SOURCE_MAVLINK_5 = 7 # mavlink instance 5 uint8 data_source +uint8 source_system_id # [@invalid 0] MAVLink system ID of the manual control source +uint8 source_component_id # [@invalid 0] MAVLink component ID of the manual control source # Any of the channels may not be available and be set to NaN # to indicate that it does not contain valid data. diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 838a43afb68..e60c8f83303 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1654,6 +1654,9 @@ constexpr StreamRateDefault streams_mode_normal[] = { {"GPS_GLOBAL_ORIGIN", 1.0f}, {"GPS_RAW_INT", 5.0f}, {"GPS_STATUS", 1.0f}, +#if defined(MAVLINK_MSG_ID_MANUAL_INPUT_STATUS) + {"MANUAL_INPUT_STATUS", 1.0f}, +#endif // MAVLINK_MSG_ID_MANUAL_INPUT_STATUS {"HOME_POSITION", 0.5f}, {"HYGROMETER_SENSOR", 0.1f}, {"LOCAL_POSITION_NED", 1.0f}, @@ -1734,6 +1737,9 @@ constexpr StreamRateDefault streams_mode_onboard[] = { {"GPS_GLOBAL_ORIGIN", 1.0f}, {"GPS_RAW_INT", unlimited_rate}, {"GPS_STATUS", 1.0f}, +#if defined(MAVLINK_MSG_ID_MANUAL_INPUT_STATUS) + {"MANUAL_INPUT_STATUS", 1.0f}, +#endif // MAVLINK_MSG_ID_MANUAL_INPUT_STATUS {"HOME_POSITION", 0.5f}, {"HYGROMETER_SENSOR", 1.0f}, {"NAV_CONTROLLER_OUTPUT", 10.0f}, @@ -1903,6 +1909,9 @@ constexpr StreamRateDefault streams_mode_config[] = { {"HYGROMETER_SENSOR", 1.0f}, {"MAG_CAL_REPORT", 1.0f}, {"MANUAL_CONTROL", 5.0f}, +#if defined(MAVLINK_MSG_ID_MANUAL_INPUT_STATUS) + {"MANUAL_INPUT_STATUS", 1.0f}, +#endif // MAVLINK_MSG_ID_MANUAL_INPUT_STATUS {"NAV_CONTROLLER_OUTPUT", 10.0f}, {"OPEN_DRONE_ID_LOCATION", 1.f}, {"OPEN_DRONE_ID_SYSTEM", 1.f}, @@ -1948,6 +1957,9 @@ constexpr StreamRateDefault streams_mode_minimal[] = { {"EXTENDED_SYS_STATE", 0.1f}, {"GLOBAL_POSITION_INT", 5.0f}, {"GPS_RAW_INT", 0.5f}, +#if defined(MAVLINK_MSG_ID_MANUAL_INPUT_STATUS) + {"MANUAL_INPUT_STATUS", 1.0f}, +#endif // MAVLINK_MSG_ID_MANUAL_INPUT_STATUS {"HOME_POSITION", 0.1f}, {"NAMED_VALUE_FLOAT", 1.0f}, {"RC_CHANNELS", 0.5f}, @@ -1998,6 +2010,9 @@ constexpr StreamRateDefault streams_mode_onboard_low_bandwidth[] = { #endif {"GPS2_RAW", unlimited_rate}, {"GPS_RAW_INT", unlimited_rate}, +#if defined(MAVLINK_MSG_ID_MANUAL_INPUT_STATUS) + {"MANUAL_INPUT_STATUS", 1.0f}, +#endif // MAVLINK_MSG_ID_MANUAL_INPUT_STATUS {"HOME_POSITION", 0.5f}, {"NAV_CONTROLLER_OUTPUT", 1.5f}, {"OPEN_DRONE_ID_LOCATION", 1.f}, @@ -2065,6 +2080,9 @@ constexpr StreamRateDefault streams_mode_low_bandwidth[] = { {"GPS_GLOBAL_ORIGIN", 0.1f}, {"GPS2_RAW", 1.0f}, {"GPS_RAW_INT", 1.0f}, +#if defined(MAVLINK_MSG_ID_MANUAL_INPUT_STATUS) + {"MANUAL_INPUT_STATUS", 1.0f}, +#endif // MAVLINK_MSG_ID_MANUAL_INPUT_STATUS {"HOME_POSITION", 0.5f}, {"NAV_CONTROLLER_OUTPUT", 0.1f}, {"OPTICAL_FLOW_RAD", 0.1f}, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 51354c1e91e..09eeb5cf106 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -96,6 +96,9 @@ #include "streams/LOCAL_POSITION_NED.hpp" #include "streams/MAG_CAL_REPORT.hpp" #include "streams/MANUAL_CONTROL.hpp" +#if defined(MAVLINK_MSG_ID_MANUAL_INPUT_STATUS) +#include "streams/MANUAL_INPUT_STATUS.hpp" +#endif // MAVLINK_MSG_ID_MANUAL_INPUT_STATUS #include "streams/MOUNT_ORIENTATION.hpp" #include "streams/NAV_CONTROLLER_OUTPUT.hpp" #include "streams/OBSTACLE_DISTANCE.hpp" @@ -395,6 +398,9 @@ static constexpr StreamListItem streams_list[] = { #if defined(MANUAL_CONTROL_HPP) create_stream_list_item(), #endif // MANUAL_CONTROL_HPP +#if defined(MANUAL_INPUT_STATUS_HPP) + create_stream_list_item(), +#endif // MANUAL_INPUT_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 7270ae658dd..a2aa988b237 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2402,6 +2402,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.source_system_id = msg->sysid; + manual_control_setpoint.source_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_INPUT_STATUS.hpp b/src/modules/mavlink/streams/MANUAL_INPUT_STATUS.hpp new file mode 100644 index 00000000000..181bb100361 --- /dev/null +++ b/src/modules/mavlink/streams/MANUAL_INPUT_STATUS.hpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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_INPUT_STATUS_HPP +#define MANUAL_INPUT_STATUS_HPP + +#include + +class MavlinkStreamManualInputStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamManualInputStatus(mavlink); } + + static constexpr const char *get_name_static() { return "MANUAL_INPUT_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_MANUAL_INPUT_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 MAVLINK_MSG_ID_MANUAL_INPUT_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamManualInputStatus(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{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + mavlink_manual_input_status_t msg{}; + + msg.source = MAV_MANUAL_INPUT_SOURCE_NONE; + + if (manual_control_setpoint.valid) { + if (manual_control_setpoint.data_source == manual_control_setpoint_s::SOURCE_RC) { + msg.source = MAV_MANUAL_INPUT_SOURCE_RC; + + } else if (manual_control_setpoint.data_source >= manual_control_setpoint_s::SOURCE_MAVLINK_0) { + msg.source = MAV_MANUAL_INPUT_SOURCE_MAVLINK; + msg.sender_system_id = manual_control_setpoint.source_system_id; + msg.sender_component_id = manual_control_setpoint.source_component_id; + } + } + + mavlink_msg_manual_input_status_send_struct(_mavlink->get_channel(), &msg); + return true; + } +}; + +#endif // MANUAL_INPUT_STATUS_HPP