mavlink_receiver: write STATUSTEXT messages from the same system to log

PX4 · Nov 4, 2019 · 7b83da7 · 7b83da7
1 parent 2416d0f
commit 7b83da7
Show file tree

Hide file tree

Showing 2 changed files with 51 additions and 0 deletions.
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
@@ -46,6 +46,7 @@
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_tone_alarm.h>
 #include <ecl/geo/geo.h>
+#include <systemlib/px4_macros.h>
 
 #ifdef CONFIG_NET
 #include <net/if.h>
@@ -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
@@ -68,6 +68,7 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/landing_target_pose.h>
+#include <uORB/topics/log_message.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/obstacle_distance.h>
 #include <uORB/topics/offboard_control_mode.h>
@@ -163,6 +164,7 @@ class MavlinkReceiver : public ModuleParams
 	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 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Publication<debug_vect_s>				_debug_vect_pub{ORB_ID(debug_vect)};
 	uORB::Publication<follow_target_s>			_follow_target_pub{ORB_ID(follow_target)};
 	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<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)};