logger: subscribe to mavlink_log messages and write them to the log

msg/mavlink_log.msg

@@ -1,3 +1,3 @@
 
 uint8[50] text
-uint8 severity
+uint8 severity # log level (same as in the linux kernel, starting with 0)

src/modules/logger/logger.cpp

@@ -41,6 +41,8 @@
 
 #include <uORB/uORB.h>
 #include <uORB/uORBTopics.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -439,6 +441,7 @@ void Logger::run()
 
 	uORB::Subscription<vehicle_status_s> vehicle_status_sub(ORB_ID(vehicle_status));
 	uORB::Subscription<parameter_update_s> parameter_update_sub(ORB_ID(parameter_update));
+	uORB::Subscription<mavlink_log_s> mavlink_log_sub(ORB_ID(mavlink_log));
 
 
 	add_topic("sensor_gyro", 0);
@@ -547,38 +550,37 @@ void Logger::run()
 
 						//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size);
 
-						if (_writer.write(buffer, msg_size, _dropout_start)) {
+						if (write(buffer, msg_size)) {
 
 #ifdef DBGPRINT
 							total_bytes += msg_size;
 #endif /* DBGPRINT */
 
-							if (_dropout_start) {
-								float dropout_duration = (float)(hrt_elapsed_time(&_dropout_start) / 1000) / 1.e3f;
-
-								if (dropout_duration > _max_dropout_duration) {
-									_max_dropout_duration = dropout_duration;
-								}
-
-								_dropout_start = 0;
-							}
-
 							data_written = true;
 
 						} else {
-
-							if (!_dropout_start) {
-								_dropout_start = hrt_absolute_time();
-								++_write_dropouts;
-								_high_water = 0;
-							}
-
 							break;	// Write buffer overflow, skip this record
 						}
 					}
 				}
 			}
 
+			//check for new mavlink log message
+			if (mavlink_log_sub.check_updated()) {
+				mavlink_log_sub.update();
+				message_logging_s log_msg;
+				log_msg.log_level = mavlink_log_sub.get().severity + '0';
+				log_msg.timestamp = mavlink_log_sub.get().timestamp;
+				const char *message = (const char *)mavlink_log_sub.get().text;
+				int message_len = strlen(message);
+
+				if (message_len > 0) {
+					strncpy(log_msg.message, message, sizeof(log_msg.message));
+					log_msg.msg_size = sizeof(log_msg) - sizeof(log_msg.message) - MSG_HEADER_LEN + message_len;
+					write(&log_msg, log_msg.msg_size + MSG_HEADER_LEN);
+				}
+			}
+
 			if (!_dropout_start && _writer.get_buffer_fill_count() > _high_water) {
 				_high_water = _writer.get_buffer_fill_count();
 			}
@@ -642,6 +644,32 @@ void Logger::run()
 	}
 }
 
+bool Logger::write(void *ptr, size_t size)
+{
+	if (_writer.write(ptr, size, _dropout_start)) {
+
+		if (_dropout_start) {
+			float dropout_duration = (float)(hrt_elapsed_time(&_dropout_start) / 1000) / 1.e3f;
+
+			if (dropout_duration > _max_dropout_duration) {
+				_max_dropout_duration = dropout_duration;
+			}
+
+			_dropout_start = 0;
+		}
+
+		return true;
+	}
+
+	if (!_dropout_start) {
+		_dropout_start = hrt_absolute_time();
+		++_write_dropouts;
+		_high_water = 0;
+	}
+
+	return false;
+}
+
 int Logger::create_log_dir(tm *tt)
 {
 	/* create dir on sdcard if needed */

src/modules/logger/logger.h

@@ -164,6 +164,13 @@ private:
 	 */
 	bool write_wait(void *ptr, size_t size);
 
+	/**
+	 * Write data to the logger and handle dropouts.
+	 * Must be called with _writer.lock() held.
+	 * @return true if data written, false otherwise (on overflow)
+	 */
+	bool write(void *ptr, size_t size);
+
 	/**
 	 * Get the time for log file name
 	 * @param tt returned time

src/modules/logger/messages.h

@@ -42,6 +42,7 @@ enum class MessageType : uint8_t {
 	REMOVE_LOGGED_MSG = 'R',
 	SYNC = 'S',
 	DROPOUT = 'O',
+	LOGGING = 'L',
 };
 
 
@@ -108,6 +109,15 @@ struct message_info_header_s {
 	char key[255];
 };
 
+struct message_logging_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::LOGGING);
+
+	uint8_t log_level; //same levels as in the linux kernel
+	uint64_t timestamp;
+	char message[255];
+};
+
 struct message_parameter_header_s {
 	uint16_t msg_size;
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::PARAMETER);

src/modules/systemlib/mavlink_log.c

@@ -64,6 +64,7 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
 	struct mavlink_log_s log_msg;
 
 	log_msg.severity = severity;
+	log_msg.timestamp = hrt_absolute_time();
 
 	va_list ap;
 

src/modules/uORB/Subscription.cpp

@@ -49,6 +49,7 @@
 #include "topics/position_setpoint_triplet.h"
 #include "topics/vehicle_status.h"
 #include "topics/manual_control_setpoint.h"
+#include "topics/mavlink_log.h"
 #include "topics/vehicle_local_position_setpoint.h"
 #include "topics/vehicle_local_position.h"
 #include "topics/vehicle_attitude_setpoint.h"
@@ -165,6 +166,7 @@ template class __EXPORT Subscription<encoders_s>;
 template class __EXPORT Subscription<position_setpoint_triplet_s>;
 template class __EXPORT Subscription<vehicle_status_s>;
 template class __EXPORT Subscription<manual_control_setpoint_s>;
+template class __EXPORT Subscription<mavlink_log_s>;
 template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
 template class __EXPORT Subscription<vehicle_local_position_s>;
 template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;