logger: log dropout events

src/modules/logger/log_writer.cpp

@@ -32,6 +32,7 @@
  ****************************************************************************/
 
 #include "log_writer.h"
+#include "messages.h"
 #include <fcntl.h>
 #include <string.h>
 
@@ -241,17 +242,35 @@ void LogWriter::run()
 	}
 }
 
-bool LogWriter::write(void *ptr, size_t size)
+bool LogWriter::write(void *ptr, size_t size, uint64_t dropout_start)
 {
 
 	// Bytes available to write
 	size_t available = _buffer_size - _count;
+	size_t dropout_size = 0;
 
-	if (size > available) {
+	if (dropout_start) {
+		dropout_size = sizeof(message_dropout_s);
+	}
+
+	if (size + dropout_size > available) {
 		// buffer overflow
 		return false;
 	}
 
+	if (dropout_start) {
+		//write dropout msg
+		message_dropout_s dropout_msg;
+		dropout_msg.duration = (uint16_t)(hrt_elapsed_time(&dropout_start) / 1000);
+		write_no_check(&dropout_msg, sizeof(dropout_msg));
+	}
+
+	write_no_check(ptr, size);
+	return true;
+}
+
+void LogWriter::write_no_check(void *ptr, size_t size)
+{
 	size_t n = _buffer_size - _head;	// bytes to end of the buffer
 
 	uint8_t *buffer_c = reinterpret_cast<uint8_t *>(ptr);
@@ -270,7 +289,6 @@ bool LogWriter::write(void *ptr, size_t size)
 	memcpy(&(_buffer[_head]), &(buffer_c[n]), p);
 	_head = (_head + p) % _buffer_size;
 	_count += size;
-	return true;
 }
 
 size_t LogWriter::get_read_ptr(void **ptr, bool *is_part)

src/modules/logger/log_writer.h

@@ -67,9 +67,10 @@ public:
 
 	/**
 	 * Write data to be logged. The caller must call lock() before calling this.
+	 * @param dropout_start timestamp when lastest dropout occured. 0 if no dropout at the moment.
 	 * @return true on success, false if not enough space in the buffer left
 	 */
-	bool write(void *ptr, size_t size);
+	bool write(void *ptr, size_t size, uint64_t dropout_start = 0);
 
 	void lock()
 	{
@@ -113,6 +114,11 @@ private:
 		_count -= n;
 	}
 
+	/**
+	 * Write to the buffer but assuming there is enough space
+	 */
+	inline void write_no_check(void *ptr, size_t size);
+
 	/* 512 didn't seem to work properly, 4096 should match the FAT cluster size */
 	static constexpr size_t	_min_write_chunk = 4096;
 

src/modules/logger/logger.cpp

@@ -32,6 +32,7 @@
  ****************************************************************************/
 
 #include "logger.h"
+#include "messages.h"
 
 #include <sys/stat.h>
 #include <errno.h>
@@ -282,74 +283,6 @@ void Logger::run_trampoline(int argc, char *argv[])
 	logger_task = -1;
 }
 
-enum class MessageType : uint8_t {
-	FORMAT = 'F',
-	DATA = 'D',
-	INFO = 'I',
-	PARAMETER = 'P',
-	ADD_LOGGED_MSG = 'A',
-	REMOVE_LOGGED_MSG = 'R',
-	SYNC = 'S',
-	DROPOUT = 'O',
-};
-
-/* declare message data structs with byte alignment (no padding) */
-#pragma pack(push, 1)
-
-#define MSG_HEADER_LEN 3 //accounts for msg_size and msg_type
-
-/** first bytes of the file */
-struct message_file_header_s {
-	uint8_t magic[8];
-	uint64_t timestamp;
-};
-
-struct message_format_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::FORMAT);
-
-	char format[2096];
-};
-
-struct message_add_logged_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::ADD_LOGGED_MSG);
-
-	uint8_t multi_id;
-	uint16_t msg_id;
-	char message_name[255];
-};
-
-struct message_remove_logged_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::REMOVE_LOGGED_MSG);
-
-	uint16_t msg_id;
-};
-struct message_data_header_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::DATA);
-
-	uint16_t msg_id;
-};
-
-struct message_info_header_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::INFO);
-
-	uint8_t key_len;
-	char key[255];
-};
-
-struct message_parameter_header_s {
-	uint16_t msg_size;
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
-
-	uint8_t key_len;
-	char key[255];
-};
-#pragma pack(pop)
-
 
 static constexpr size_t MAX_DATA_SIZE = 740;
 
@@ -611,7 +544,7 @@ 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)) {
+						if (_writer.write(buffer, msg_size, _dropout_start)) {
 
 #ifdef DBGPRINT
 							total_bytes += msg_size;

src/modules/logger/messages.h

+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+enum class MessageType : uint8_t {
+	FORMAT = 'F',
+	DATA = 'D',
+	INFO = 'I',
+	PARAMETER = 'P',
+	ADD_LOGGED_MSG = 'A',
+	REMOVE_LOGGED_MSG = 'R',
+	SYNC = 'S',
+	DROPOUT = 'O',
+};
+
+
+/* declare message data structs with byte alignment (no padding) */
+#pragma pack(push, 1)
+
+#define MSG_HEADER_LEN 3 //accounts for msg_size and msg_type
+
+/** first bytes of the file */
+struct message_file_header_s {
+	uint8_t magic[8];
+	uint64_t timestamp;
+};
+
+struct message_format_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::FORMAT);
+
+	char format[2096];
+};
+
+struct message_add_logged_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::ADD_LOGGED_MSG);
+
+	uint8_t multi_id;
+	uint16_t msg_id;
+	char message_name[255];
+};
+
+struct message_remove_logged_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::REMOVE_LOGGED_MSG);
+
+	uint16_t msg_id;
+};
+
+struct message_sync_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::SYNC);
+
+	uint8_t sync_magic[8];
+};
+
+struct message_dropout_s {
+	uint16_t msg_size = sizeof(uint16_t); //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::DROPOUT);
+
+	uint16_t duration; //in ms
+};
+
+struct message_data_header_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::DATA);
+
+	uint16_t msg_id;
+};
+
+struct message_info_header_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::INFO);
+
+	uint8_t key_len;
+	char key[255];
+};
+
+struct message_parameter_header_s {
+	uint16_t msg_size;
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
+
+	uint8_t key_len;
+	char key[255];
+};
+#pragma pack(pop)