VA args now supported by MAVLink text messages

1 files changed, 26 insertions, 0 deletions

diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c
index fa974dc0b..6395ab214 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,28 @@ 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);
+}
+
+__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...)
+{
+	va_list ap;
+	va_start(ap, fmt);
+	struct mavlink_logmessage msg;
+	msg.severity = severity;
+	vsnprintf(msg.text, sizeof(msg.text), fmt, ap);
+	va_end(ap);
+	#ifdef __cplusplus
+	::ioctl(_fd, msg.severity, (unsigned long)msg.text);
+	#else
+	ioctl(_fd, msg.severity, (unsigned long)msg.text);
+	#endif
+}