From c8349811d105bd7c030e60c1200354ecbbdd736e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Roman=20Dvo=C5=99=C3=A1k?= Date: Wed, 26 Jan 2022 17:10:43 +0100 Subject: [PATCH] Enable logging of mavlink tunnel messages Co-authored-by: Vit Hanousek --- msg/CMakeLists.txt | 1 + msg/mavlink_tunnel.msg | 20 ++++++++++++++++++++ src/modules/logger/logged_topics.cpp | 9 +++++++++ src/modules/logger/logged_topics.h | 4 +++- src/modules/logger/params.c | 4 +++- src/modules/mavlink/mavlink_receiver.cpp | 24 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 3 +++ 7 files changed, 63 insertions(+), 2 deletions(-) create mode 100644 msg/mavlink_tunnel.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index ff74b99ef7..07939461d4 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -108,6 +108,7 @@ set(msg_files manual_control_setpoint.msg manual_control_switches.msg mavlink_log.msg + mavlink_tunnel.msg mission.msg mission_result.msg mount_orientation.msg diff --git a/msg/mavlink_tunnel.msg b/msg/mavlink_tunnel.msg new file mode 100644 index 0000000000..16934a9522 --- /dev/null +++ b/msg/mavlink_tunnel.msg @@ -0,0 +1,20 @@ +# MAV_TUNNEL_PAYLOAD_TYPE enum + +uint8 MAV_TUNNEL_PAYLOAD_TYPE_UNKNOWN = 0 # Encoding of payload unknown +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED0 = 200 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED1 = 201 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED2 = 202 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED3 = 203 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED4 = 204 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED5 = 205 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED6 = 206 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED7 = 207 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED8 = 208 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED9 = 209 # Registered for STorM32 gimbal controller + +uint64 timestamp # Time since system start (microseconds) +uint16 payload_type # A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. +uint8 target_system # System ID (can be 0 for broadcast, but this is discouraged) +uint8 target_component # Component ID (can be 0 for broadcast, but this is discouraged) +uint8 payload_length # Length of the data transported in payload +uint8[128] payload # Data itself diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 35c97b7372..3a799025d7 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -299,6 +299,11 @@ void LoggedTopics::add_system_identification_topics() add_topic("vehicle_torque_setpoint"); } +void LoggedTopics::add_mavlink_tunnel() +{ + add_topic("mavlink_tunnel"); +} + int LoggedTopics::add_topics_from_file(const char *fname) { int ntopics = 0; @@ -496,4 +501,8 @@ void LoggedTopics::initialize_configured_topics(SDLogProfileMask profile) if (profile & SDLogProfileMask::RAW_IMU_ACCEL_FIFO) { add_raw_imu_accel_fifo(); } + + if (profile & SDLogProfileMask::MAVLINK_TUNNEL) { + add_mavlink_tunnel(); + } } diff --git a/src/modules/logger/logged_topics.h b/src/modules/logger/logged_topics.h index b69e06b213..baa389e396 100644 --- a/src/modules/logger/logged_topics.h +++ b/src/modules/logger/logged_topics.h @@ -53,7 +53,8 @@ enum class SDLogProfileMask : int32_t { SENSOR_COMPARISON = 1 << 6, VISION_AND_AVOIDANCE = 1 << 7, RAW_IMU_GYRO_FIFO = 1 << 8, - RAW_IMU_ACCEL_FIFO = 1 << 9 + RAW_IMU_ACCEL_FIFO = 1 << 9, + MAVLINK_TUNNEL = 1 << 10 }; enum class MissionLogType : int32_t { @@ -168,6 +169,7 @@ private: void add_vision_and_avoidance_topics(); void add_raw_imu_gyro_fifo(); void add_raw_imu_accel_fifo(); + void add_mavlink_tunnel(); /** * add a logged topic (called by add_topic() above). diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c index 81ce9e426d..2fe6b07c19 100644 --- a/src/modules/logger/params.c +++ b/src/modules/logger/params.c @@ -125,9 +125,10 @@ PARAM_DEFINE_INT32(SDLOG_MISSION, 0); * 7 : Topics for computer vision and collision avoidance * 8 : Raw FIFO high-rate IMU (Gyro) * 9 : Raw FIFO high-rate IMU (Accel) + * 10: Logging of mavlink tunnel message (useful for payload communication debugging) * * @min 0 - * @max 1023 + * @max 2047 * @bit 0 Default set (general log analysis) * @bit 1 Estimator replay (EKF2) * @bit 2 Thermal calibration @@ -138,6 +139,7 @@ PARAM_DEFINE_INT32(SDLOG_MISSION, 0); * @bit 7 Computer Vision and Avoidance * @bit 8 Raw FIFO high-rate IMU (Gyro) * @bit 9 Raw FIFO high-rate IMU (Accel) + * @bit 10 Mavlink tunnel message logging * @reboot_required true * @group SD Logging */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f29a8bac13..24351ea38e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -245,6 +245,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_obstacle_distance(msg); break; + case MAVLINK_MSG_ID_TUNNEL: + handle_message_tunnel(msg); + break; + case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER: handle_message_trajectory_representation_bezier(msg); break; @@ -1895,6 +1899,26 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg) _obstacle_distance_pub.publish(obstacle_distance); } +void +MavlinkReceiver::handle_message_tunnel(mavlink_message_t *msg) +{ + mavlink_tunnel_t mavlink_tunnel; + mavlink_msg_tunnel_decode(msg, &mavlink_tunnel); + + mavlink_tunnel_s tunnel{}; + + tunnel.timestamp = hrt_absolute_time(); + tunnel.payload_type = mavlink_tunnel.payload_type; + tunnel.target_system = mavlink_tunnel.target_system; + tunnel.target_component = mavlink_tunnel.target_component; + tunnel.payload_length = mavlink_tunnel.payload_length; + memcpy(tunnel.payload, mavlink_tunnel.payload, sizeof(tunnel.payload)); + static_assert(sizeof(tunnel.payload) == sizeof(mavlink_tunnel.payload), "mavlink_tunnel.payload size mismatch"); + + _mavlink_tunnel_pub.publish(tunnel); + +} + void MavlinkReceiver::handle_message_trajectory_representation_bezier(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 2a929ea594..a2d45e0087 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -82,6 +82,7 @@ #include #include #include +#include #include #include #include @@ -190,6 +191,7 @@ private: void handle_message_set_position_target_global_int(mavlink_message_t *msg); void handle_message_set_position_target_local_ned(mavlink_message_t *msg); void handle_message_statustext(mavlink_message_t *msg); + void handle_message_tunnel(mavlink_message_t *msg); void handle_message_trajectory_representation_bezier(mavlink_message_t *msg); void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); void handle_message_utm_global_position(mavlink_message_t *msg); @@ -297,6 +299,7 @@ private: uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; uORB::Publication _log_message_pub{ORB_ID(log_message)}; + uORB::Publication _mavlink_tunnel_pub{ORB_ID(mavlink_tunnel)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)};