logging: publish a message for PX4_{WARN,ERR} & store them to the ulog file

- ulog file message rate limited to 50Hz
- queuing with size 2
- this replaces the mavlink log message in the ulog
  (but the mavlink warnings & errors still go to the ulog)

msg/CMakeLists.txt

@@ -65,6 +65,7 @@ set(msg_file_names
 	hil_sensor.msg
 	home_position.msg
 	input_rc.msg
+	log_message.msg
 	manual_control_setpoint.msg
 	mavlink_log.msg
 	mc_att_ctrl_status.msg

msg/log_message.msg

@@ -0,0 +1,5 @@
+# A logging message, output with PX4_{WARN,ERR,INFO}
+
+uint8 severity # log level (same as in the linux kernel, starting with 0)
+uint8[127] text
+

src/modules/logger/logger.cpp

@@ -43,7 +43,7 @@
 #include <uORB/uORB.h>
 #include <uORB/uORBTopics.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/mavlink_log.h>
+#include <uORB/topics/log_message.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -584,7 +584,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));
+	uORB::Subscription<log_message_s> log_message_sub(ORB_ID(log_message), 20);
 
 	int ntopics = add_topics_from_file(PX4_ROOTFSDIR "/fs/microsd/etc/logging/logger_topics.txt");
 
@@ -735,10 +735,10 @@ void Logger::run()
 				}
 			}
 
-			//check for new mavlink log message
-			if (mavlink_log_sub.check_updated()) {
-				mavlink_log_sub.update();
-				const char *message = (const char *)mavlink_log_sub.get().text;
+			//check for new logging message(s)
+			if (log_message_sub.check_updated()) {
+				log_message_sub.update();
+				const char *message = (const char *)log_message_sub.get().text;
 				int message_len = strlen(message);
 
 				if (message_len > 0) {
@@ -747,8 +747,8 @@ void Logger::run()
 					_msg_buffer[0] = (uint8_t)write_msg_size;
 					_msg_buffer[1] = (uint8_t)(write_msg_size >> 8);
 					_msg_buffer[2] = static_cast<uint8_t>(ULogMessageType::LOGGING);
-					_msg_buffer[3] = mavlink_log_sub.get().severity + '0';
-					memcpy(_msg_buffer + 4, &mavlink_log_sub.get().timestamp, sizeof(ulog_message_logging_s::timestamp));
+					_msg_buffer[3] = log_message_sub.get().severity + '0';
+					memcpy(_msg_buffer + 4, &log_message_sub.get().timestamp, sizeof(ulog_message_logging_s::timestamp));
 					strncpy((char *)(_msg_buffer + 12), message, sizeof(ulog_message_logging_s::message));
 
 					write(_msg_buffer, write_msg_size + ULOG_MSG_HEADER_LEN);

src/modules/uORB/Subscription.cpp

@@ -49,6 +49,7 @@
 #include "topics/vehicle_status.h"
 #include "topics/manual_control_setpoint.h"
 #include "topics/mavlink_log.h"
+#include "topics/log_message.h"
 #include "topics/vehicle_local_position_setpoint.h"
 #include "topics/vehicle_local_position.h"
 #include "topics/vehicle_attitude_setpoint.h"
@@ -166,6 +167,7 @@ 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<log_message_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>;

src/modules/uORB/uORBMain.cpp

@@ -36,6 +36,7 @@
 #include "uORBManager.hpp"
 #include "uORB.h"
 #include "uORBCommon.hpp"
+#include <px4_log.h>
 
 extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); }
 
@@ -78,6 +79,8 @@ uorb_main(int argc, char *argv[])
 			return -errno;
 		}
 
+		px4_log_initialize();
+
 		return OK;
 	}
 

src/platforms/posix/px4_layer/px4_log.c

@@ -3,12 +3,34 @@
 #ifdef __PX4_POSIX
 #include <execinfo.h>
 #endif
+#include <uORB/uORB.h>
+#include <uORB/topics/log_message.h>
+#include <drivers/drv_hrt.h>
 
+static orb_advert_t orb_log_message_pub = NULL;
 
 __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" };
 __EXPORT const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] =
 { PX4_ANSI_COLOR_RESET, PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_YELLOW, PX4_ANSI_COLOR_RED, PX4_ANSI_COLOR_RED };
 
+
+void px4_log_initialize(void)
+{
+	ASSERT(orb_log_message_pub == NULL);
+
+	/* we need to advertise with a valid message */
+	struct log_message_s log_message;
+	log_message.timestamp = hrt_absolute_time();
+	log_message.severity = 6; //info
+	strcpy((char *)log_message.text, "initialized uORB logging");
+
+	orb_log_message_pub = orb_advertise_queue(ORB_ID(log_message), &log_message, 2);
+
+	if (!orb_log_message_pub) {
+		PX4_ERR("failed to advertise log_message");
+	}
+}
+
 void px4_backtrace()
 {
 #ifdef __PX4_POSIX
@@ -43,5 +65,33 @@ __EXPORT void px4_log_modulename(int level, const char *moduleName, const char *
 	va_end(argptr);
 	PX4_LOG_COLOR_END
 	printf("\n");
+
+	/* publish an orb log message */
+	if (level >= _PX4_LOG_LEVEL_WARN && orb_log_message_pub) { //only publish important messages
+
+		struct log_message_s log_message;
+		const unsigned max_length = sizeof(log_message.text);
+		log_message.timestamp = hrt_absolute_time();
+
+		const uint8_t log_level_table[] = {
+			6, /* _PX4_LOG_LEVEL_ALWAYS */
+			7, /* _PX4_LOG_LEVEL_DEBUG */
+			4, /* _PX4_LOG_LEVEL_WARN */
+			3, /* _PX4_LOG_LEVEL_ERROR */
+			0  /* _PX4_LOG_LEVEL_PANIC */
+		};
+		log_message.severity = log_level_table[level];
+
+		unsigned pos = 0;
+
+		pos += snprintf((char *)log_message.text + pos, max_length - pos, __px4__log_modulename_pfmt, moduleName);
+		va_start(argptr, fmt);
+		pos += vsnprintf((char *)log_message.text + pos, max_length - pos, fmt, argptr);
+		va_end(argptr);
+		log_message.text[max_length - 1] = 0; //ensure 0-termination
+
+		orb_publish(ORB_ID(log_message), orb_log_message_pub, &log_message);
+
+	}
 }
 

src/platforms/px4_log.h

@@ -128,6 +128,11 @@ static inline void do_nothing(int level, ...)
 
 __BEGIN_DECLS
 
+/**
+ * initialize the orb logging. Logging to console still works without or before calling this.
+ */
+__EXPORT extern void px4_log_initialize(void);
+
 __EXPORT extern const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1];
 __EXPORT extern const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1];
 __EXPORT extern void px4_backtrace(void);