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)

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);