diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 56ca11929d..25519ef74a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #ifdef CONFIG_NET #include @@ -249,6 +250,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_onboard_computer_status(msg); break; + case MAVLINK_MSG_ID_STATUSTEXT: + handle_message_statustext(msg); + break; + default: break; } @@ -2571,6 +2576,49 @@ MavlinkReceiver::handle_message_onboard_computer_status(mavlink_message_t *msg) _onboard_computer_status_pub.publish(onboard_computer_status_topic); } +void MavlinkReceiver::handle_message_statustext(mavlink_message_t *msg) +{ + if (msg->sysid == mavlink_system.sysid) { + // log message from the same system + + mavlink_statustext_t statustext; + mavlink_msg_statustext_decode(msg, &statustext); + + log_message_s log_message{}; + + switch (statustext.severity) { + case MAV_SEVERITY_EMERGENCY: + case MAV_SEVERITY_ALERT: + case MAV_SEVERITY_CRITICAL: + log_message.severity = 0; + break; + + case MAV_SEVERITY_ERROR: + log_message.severity = 3; + break; + + case MAV_SEVERITY_WARNING: + log_message.severity = 4; + break; + + case MAV_SEVERITY_NOTICE: + case MAV_SEVERITY_INFO: + log_message.severity = 6; + break; + + default: + return; + } + + log_message.timestamp = hrt_absolute_time(); + + snprintf(log_message.text, sizeof(log_message.text), + "[mavlink: component %d] %." STRINGIFY(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN) "s", msg->compid, statustext.text); + + _log_message_pub.publish(log_message); + } +} + /** * Receive data from UART/UDP */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 3062f2e63a..e770eb5049 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -68,6 +68,7 @@ #include #include #include +#include #include #include #include @@ -163,6 +164,7 @@ private: void handle_message_set_mode(mavlink_message_t *msg); void handle_message_set_position_target_local_ned(mavlink_message_t *msg); void handle_message_set_position_target_global_int(mavlink_message_t *msg); + void handle_message_statustext(mavlink_message_t *msg); void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); void handle_message_utm_global_position(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); @@ -227,6 +229,7 @@ private: uORB::Publication _debug_vect_pub{ORB_ID(debug_vect)}; uORB::Publication _follow_target_pub{ORB_ID(follow_target)}; uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; + uORB::Publication _log_message_pub{ORB_ID(log_message)}; 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)};