fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 07:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Enable logging of mavlink tunnel messages

Browse Source 
Co-authored-by: Vit Hanousek <vithanousek@seznam.cz>
This commit is contained in:
Roman Dvořák
2022-01-26 17:10:43 +01:00
committed by GitHub
parent ae3070bbf1
commit c8349811d1
7 changed files with 63 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+24
View File
@@ -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)
{
src/modules/mavlink/mavlink_receiver.h
+3
View File
@@ -82,6 +82,7 @@
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/log_message.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mavlink_tunnel.h>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/onboard_computer_status.h>
@@ -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_s> _irlock_report_pub{ORB_ID(irlock_report)};
uORB::Publication<landing_target_pose_s> _landing_target_pose_pub{ORB_ID(landing_target_pose)};
uORB::Publication<log_message_s> _log_message_pub{ORB_ID(log_message)};
uORB::Publication<mavlink_tunnel_s> _mavlink_tunnel_pub{ORB_ID(mavlink_tunnel)};
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance)};
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
uORB::Publication<onboard_computer_status_s> _onboard_computer_status_pub{ORB_ID(onboard_computer_status)};