Merge remote-tracking branch 'upstream/master' into new_state_machine

Conflicts:
	src/examples/fixedwing_control/main.c

2 files changed, 34 insertions, 5 deletions

diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c
index fa974dc0b..d9416a08b 100644
--- a/src/modules/mavlink/mavlink_log.c
+++ b/src/modules/mavlink/mavlink_log.c
@@ -41,6 +41,7 @@
 
 #include <string.h>
 #include <stdlib.h>
+#include <stdarg.h>
 
 #include <mavlink/mavlink_log.h>
 
@@ -87,3 +88,31 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa
 		return 1;
 	}
 }
+
+void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
+{
+	va_list ap;
+	va_start(ap, fmt);
+	int end = (lb->start + lb->count) % lb->size;
+	lb->elems[end].severity = severity;
+	vsnprintf(lb->elems[end].text, sizeof(lb->elems[0].text), fmt, ap);
+	va_end(ap);
+
+	/* increase count */
+	if (mavlink_logbuffer_is_full(lb)) {
+		lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
+
+	} else {
+		++lb->count;
+	}
+}
+
+__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...)
+{
+	va_list ap;
+	va_start(ap, fmt);
+	char text[MAVLINK_LOG_MAXLEN + 1];
+	vsnprintf(text, sizeof(text), fmt, ap);
+	va_end(ap);
+	ioctl(_fd, severity, (unsigned long)&text[0]);
+}
diff --git a/src/modules/mavlink/mavlink_receiver.c b/src/modules/mavlink/mavlink_receiver.c
index a42612f9e..940d030b2 100644
--- a/src/modules/mavlink/mavlink_receiver.c
+++ b/src/modules/mavlink/mavlink_receiver.c
@@ -429,11 +429,11 @@ handle_message(mavlink_message_t *msg)
 			hil_frames += 1 ;
 
 			// output
-			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil gps at %d hz\n", hil_frames/10);
-				old_timestamp = timestamp;
-				hil_frames = 0;
-			}
+			// if ((timestamp - old_timestamp) > 10000000) {
+			// 	printf("receiving hil gps at %d hz\n", hil_frames/10);
+			// 	old_timestamp = timestamp;
+			// 	hil_frames = 0;
+			// }
 		}
 
 		if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {