Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-10-06 09:02:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(manual_control): Publish source of manual control setpoint via mavlink (#27857)

Browse Source 
* feat(mavlink): stream manual input status which specifies the active source for manual control setpoint generation on the autopilot
---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Roman Bapst
2026-09-09 12:29:33 +03:00
committed by GitHub
co-authored by Hamish Willee Matthias Grob
parent eaf0cef68d
commit 43a57e75eb
8 changed files with 240 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/px4_msgs_old/msg/ManualControlSetpointV0.msg
+48
View File
@@ -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
msg/translation_node/translations/all_translations.h
+1
View File
@@ -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"
msg/translation_node/translations/translation_manual_control_setpoint_v1.h
+65
View File
@@ -0,0 +1,65 @@
/****************************************************************************
* Copyright (c) 2026 PX4 Development Team.
* SPDX-License-Identifier: BSD-3-Clause
****************************************************************************/
#pragma once
// Translate ManualControlSetpoint v0 <--> v1
#include <px4_msgs_old/msg/manual_control_setpoint_v0.hpp>
#include <px4_msgs/msg/manual_control_setpoint.hpp>
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);
msg/versioned/ManualControlSetpoint.msg
+15 -1
View File
@@ -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.
src/modules/mavlink/mavlink_main.cpp
+18
View File
@@ -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},
src/modules/mavlink/mavlink_messages.cpp
+6
View File
@@ -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<MavlinkStreamManualControl>(),
#endif // MANUAL_CONTROL_HPP
#if defined(MANUAL_INPUT_STATUS_HPP)
create_stream_list_item<MavlinkStreamManualInputStatus>(),
#endif // MANUAL_INPUT_STATUS_HPP
#if defined(OPTICAL_FLOW_RAD_HPP)
create_stream_list_item<MavlinkStreamOpticalFlowRad>(),
#endif // OPTICAL_FLOW_RAD_HPP
src/modules/mavlink/mavlink_receiver.cpp
+2
View File
@@ -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);
src/modules/mavlink/streams/MANUAL_INPUT_STATUS.hpp
+85
View File
@@ -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 <uORB/topics/manual_control_setpoint.h>
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