Methods to peek and skip messages in queue.

Allows receiver to not consume a message until after it's handled. And that enables the transmitter to block until the queue is empty, knowing that when unblocked, all messages in queue have been handled.

firmware/common/fifo.hpp

@@ -114,6 +114,26 @@ public:
 		return len;
 	}
 
+	bool skip() {
+		if( is_empty() ) {
+			return false;
+		}
+
+		size_t len = peek_n();
+		_out += len + recsize();
+		return true;
+	}
+
+	size_t peek_r(void* const buf, size_t len) {
+		if( is_empty() ) {
+			return 0;
+		}
+
+		size_t n;
+		len = out_copy_r((T*)buf, len, &n);
+		return len;
+	}
+
 	size_t out_r(void* const buf, size_t len) {
 		if( is_empty() ) {
 			return 0;

firmware/common/message_queue.hpp

@@ -47,6 +47,25 @@ public:
 		return push(&message, sizeof(message));
 	}
 
+	template<typename T>
+	bool push_and_wait(const T& message) {
+		const bool result = push(message);
+		if( result ) {
+			// TODO: More graceful method of waiting for empty? Maybe sleep for a bit?
+			while( !is_empty() );
+		}
+		return result;
+	}
+
+	Message* peek(std::array<uint8_t, Message::MAX_SIZE>& buf) {
+		Message* const p = reinterpret_cast<Message*>(buf.data());
+		return fifo.peek_r(buf.data(), buf.size()) ? p : nullptr;
+	}
+
+	bool skip() {
+		return fifo.skip();
+	}
+
 	Message* pop(std::array<uint8_t, Message::MAX_SIZE>& buf) {
 		Message* const p = reinterpret_cast<Message*>(buf.data());
 		return fifo.out_r(buf.data(), buf.size()) ? p : nullptr;