dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Enable logging of mavlink tunnel messages 

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
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 63 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -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

20
msg/mavlink_tunnel.msg Normal file
View File

@ -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

9
src/modules/logger/logged_topics.cpp
View File

@ -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();
}
}

4
src/modules/logger/logged_topics.h
View File

@ -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).

4
src/modules/logger/params.c
View File

@ -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
*/

24
src/modules/mavlink/mavlink_receiver.cpp
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)
{

3
src/modules/mavlink/mavlink_receiver.h
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)};