console buffer: write to log as 'boot_console_output' message

platforms/nuttx/src/px4_layer/console_buffer.cpp

@@ -36,7 +36,7 @@
 #include <px4_defines.h>
 #include <px4_sem.h>
 #include <pthread.h>
-
+#include <string.h>
 #include <fcntl.h>
 
 #ifdef BOARD_ENABLE_CONSOLE_BUFFER
@@ -55,6 +55,10 @@ public:
 
 	void print(bool follow);
 
+	int size();
+
+	int read(char *buffer, int buffer_length, int *offset);
+
 private:
 	void		lock() { do {} while (px4_sem_wait(&_lock) != 0); }
 	void		unlock() { px4_sem_post(&_lock); }
@@ -121,6 +125,69 @@ void ConsoleBuffer::write(const char *buffer, size_t len)
 	unlock();
 }
 
+int ConsoleBuffer::size()
+{
+	lock();
+	int size;
+
+	if (_head <= _tail) {
+		size = _tail - _head;
+
+	} else {
+		size = BOARD_CONSOLE_BUFFER_SIZE - (_head - _tail);
+	}
+
+	unlock();
+	return size;
+}
+
+int ConsoleBuffer::read(char *buffer, int buffer_length, int *offset)
+{
+	lock();
+
+	if (*offset == -1) {
+		*offset = _head;
+	}
+
+	int size = 0;
+
+	if (*offset < _tail) {
+		size = _tail - *offset;
+
+		if (size > buffer_length) {
+			size = buffer_length;
+		}
+
+		memcpy(buffer, _buffer + *offset, size);
+
+	} else if (_tail < *offset) {
+		size = BOARD_CONSOLE_BUFFER_SIZE - *offset;
+
+		if (size > buffer_length) {
+			size = buffer_length;
+		}
+
+		memcpy(buffer, _buffer + *offset, size);
+		buffer += size;
+		buffer_length -= size;
+
+		int size_secondary = _tail;
+
+		if (size_secondary > buffer_length) {
+			size_secondary = buffer_length;
+		}
+
+		if (size_secondary > 0) {
+			memcpy(buffer, _buffer, size_secondary);
+			size += size_secondary;
+		}
+	}
+
+	unlock();
+	*offset = (*offset + size) % BOARD_CONSOLE_BUFFER_SIZE;
+	return size;
+}
+
 static ConsoleBuffer g_console_buffer;
 
 
@@ -162,10 +229,14 @@ int px4_console_buffer_init()
 	return register_driver(CONSOLE_BUFFER_DEVICE, &g_console_buffer_fops, 0666, NULL);
 }
 
-#else
-
-int px4_console_buffer_init()
+int px4_console_buffer_size()
 {
-	return 0;
+	return g_console_buffer.size();
 }
+
+int px4_console_buffer_read(char *buffer, int buffer_length, int *offset)
+{
+	return g_console_buffer.read(buffer, buffer_length, offset);
+}
+
 #endif /* BOARD_ENABLE_CONSOLE_BUFFER */

src/modules/logger/logger.cpp

@@ -32,6 +32,7 @@
  ****************************************************************************/
 
 #include <px4_config.h>
+#include <px4_console_buffer.h>
 #include "logger.h"
 #include "messages.h"
 #include "watchdog.h"
@@ -53,6 +54,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 
 #include <drivers/drv_hrt.h>
+#include <mathlib/math/Limits.hpp>
 #include <px4_getopt.h>
 #include <px4_log.h>
 #include <px4_posix.h>
@@ -1628,6 +1630,7 @@ void Logger::start_log_file(LogType type)
 	if (type == LogType::Full) {
 		write_parameters(type);
 		write_perf_data(true);
+		write_console_output();
 	}
 	write_all_add_logged_msg(type);
 	_writer.set_need_reliable_transfer(false);
@@ -1675,6 +1678,7 @@ void Logger::start_log_mavlink()
 	write_formats(LogType::Full);
 	write_parameters(LogType::Full);
 	write_perf_data(true);
+	write_console_output();
 	write_all_add_logged_msg(LogType::Full);
 	_writer.set_need_reliable_transfer(false);
 	_writer.unselect_write_backend();
@@ -1700,7 +1704,7 @@ struct perf_callback_data_t {
 void Logger::perf_iterate_callback(perf_counter_t handle, void *user)
 {
 	perf_callback_data_t *callback_data = (perf_callback_data_t *)user;
-	const int buffer_length = 256;
+	const int buffer_length = 220;
 	char buffer[buffer_length];
 	const char *perf_name;
 
@@ -1790,6 +1794,25 @@ void Logger::write_load_output()
 	_writer.set_need_reliable_transfer(false);
 }
 
+void Logger::write_console_output()
+{
+	const int buffer_length = 220;
+	char buffer[buffer_length];
+	int size = px4_console_buffer_size();
+	int offset = -1;
+	bool first = true;
+	while (size > 0) {
+		int read_size = px4_console_buffer_read(buffer, buffer_length-1, &offset);
+		if (read_size <= 0) { break; }
+		buffer[math::min(read_size, size)] = '\0';
+		write_info_multiple(LogType::Full, "boot_console_output", buffer, !first);
+
+		size -= read_size;
+		first = false;
+	}
+
+}
+
 void Logger::write_format(LogType type, const orb_metadata &meta, WrittenFormats &written_formats, ulog_message_format_s& msg, int level)
 {
 	if (level > 3) {
@@ -1996,6 +2019,8 @@ void Logger::write_info_multiple(LogType type, const char *name, const char *val
 		msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN;
 
 		write_message(type, buffer, msg_size);
+	} else {
+		PX4_ERR("info_multiple str too long (%i), key=%s", msg.key_len, msg.key);
 	}
 
 	_writer.unlock();

src/modules/logger/logger.h

@@ -267,6 +267,11 @@ private:
 	 */
 	void write_perf_data(bool preflight);
 
+	/**
+	 * write bootup console output
+	 */
+	void write_console_output();
+
 	/**
 	 * callback to write the performance counters
 	 */

src/platforms/px4_console_buffer.h

@@ -40,8 +40,13 @@
  * to the original console.
  */
 
+#include <px4_config.h>
+
+
 #define CONSOLE_BUFFER_DEVICE "/dev/console_buf"
 
+#ifdef BOARD_ENABLE_CONSOLE_BUFFER
+
 __BEGIN_DECLS
 
 /**
@@ -57,4 +62,38 @@ int px4_console_buffer_init();
  */
 void px4_console_buffer_print(bool follow);
 
+/**
+ * Get the current used buffer size
+ */
+int px4_console_buffer_size();
+
+/**
+ * Read (chunks) of the console buffer.
+ * Note that no lock is held between reading multiple chunks, so the buffer could get
+ * updated meanwhile. Use px4_console_buffer_size() to read no more than expected.
+ * @param buffer output buffer
+ * @param buffer_length output buffer length
+ * @param offset input and output argument for the offset. Initially set this to -1.
+ * @return number of bytes written to the buffer (or <0 on error)
+ */
+int px4_console_buffer_read(char *buffer, int buffer_length, int *offset);
+
 __END_DECLS
+
+#else
+
+static inline int px4_console_buffer_init()
+{
+	return 0;
+}
+
+static inline int px4_console_buffer_size()
+{
+	return 0;
+}
+
+static inline int px4_console_buffer_read(char *buffer, int buffer_length, int *offset)
+{
+	return 0;
+}
+#endif /* BOARD_ENABLE_CONSOLE_BUFFER */