From 5eb8c0d702cae7cf1f7e0012ae548509eaa930a8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 17 Jun 2022 14:10:26 +0200 Subject: [PATCH] mavlink_shell: set target system and component id These got added in https://github.com/mavlink/mavlink/pull/1725 and need to be set for correct routing. --- src/modules/mavlink/mavlink_main.cpp | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 1 + src/modules/mavlink/mavlink_shell.h | 8 ++++++++ 3 files changed, 11 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 43c87f612e52..776f1dfe9ef4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2427,6 +2427,8 @@ Mavlink::task_main(int argc, char *argv[]) msg.timeout = 0; msg.device = SERIAL_CONTROL_DEV_SHELL; msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data)); + msg.target_system = _mavlink_shell->targetSysid(); + msg.target_component = _mavlink_shell->targetCompid(); mavlink_msg_serial_control_send_struct(get_channel(), &msg); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5983c0017003..8fa244e79968 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1778,6 +1778,7 @@ MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg) if (shell) { // we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message if (serial_control_mavlink.count > 0) { + shell->setTargetID(msg->sysid, msg->compid); shell->write(serial_control_mavlink.data, serial_control_mavlink.count); } diff --git a/src/modules/mavlink/mavlink_shell.h b/src/modules/mavlink/mavlink_shell.h index 649b429b4547..abb581f751dd 100644 --- a/src/modules/mavlink/mavlink_shell.h +++ b/src/modules/mavlink/mavlink_shell.h @@ -42,6 +42,7 @@ #include #include #include +#include #pragma once @@ -77,8 +78,15 @@ class MavlinkShell */ size_t available(); + void setTargetID(uint8_t sysid, uint8_t compid) { _target_sysid.store(sysid); _target_compid.store(compid); } + + uint8_t targetSysid() const { return _target_sysid.load(); } + uint8_t targetCompid() const { return _target_compid.load(); } private: + px4::atomic _target_sysid{}; + px4::atomic _target_compid{}; + int _to_shell_fd = -1; /** fd to write to the shell */ int _from_shell_fd = -1; /** fd to read from the shell */ int _shell_fds[2] = { -1, -1}; /** stdin & out used by the shell */