VA args now supported by MAVLink text messages

3 files changed, 56 insertions, 19 deletions

diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h
index 233a76cb3..a28ff3a68 100644
--- a/src/include/mavlink/mavlink_log.h
+++ b/src/include/mavlink/mavlink_log.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -47,27 +47,42 @@
  */
 #include <sys/ioctl.h>
 
-/*
+/**
  * The mavlink log device node; must be opened before messages
  * can be logged.
  */
 #define MAVLINK_LOG_DEVICE			"/dev/mavlink"
+/**
+ * The maximum string length supported.
+ */
+#define MAVLINK_LOG_MAXLEN			50
 
 #define MAVLINK_IOC_SEND_TEXT_INFO		_IOC(0x1100, 1)
 #define MAVLINK_IOC_SEND_TEXT_CRITICAL		_IOC(0x1100, 2)
 #define MAVLINK_IOC_SEND_TEXT_EMERGENCY		_IOC(0x1100, 3)
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
+#ifdef __cplusplus
+}
+#endif
+
+/*
+ * The va_args implementation here is not beautiful, but obviously we run into the same issues
+ * the GCC devs saw, and are using their solution:
+ *
+ * http://gcc.gnu.org/onlinedocs/cpp/Variadic-Macros.html
+ */
+
 /**
  * Send a mavlink emergency message.
  *
  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
  * @param _text		The text to log;
  */
-#ifdef __cplusplus
-#define mavlink_log_emergency(_fd, _text)	::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
-#else
-#define mavlink_log_emergency(_fd, _text)	ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
-#endif
+#define mavlink_log_emergency(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__);
 
 /**
  * Send a mavlink critical message.
@@ -75,11 +90,7 @@
  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
  * @param _text		The text to log;
  */
-#ifdef __cplusplus
-#define mavlink_log_critical(_fd, _text)	::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
-#else
-#define mavlink_log_critical(_fd, _text)	ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
-#endif
+#define mavlink_log_critical(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__);
 
 /**
  * Send a mavlink info message.
@@ -87,14 +98,10 @@
  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
  * @param _text		The text to log;
  */
-#ifdef __cplusplus
-#define mavlink_log_info(_fd, _text)		::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
-#else
-#define mavlink_log_info(_fd, _text)		ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
-#endif
+#define mavlink_log_info(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
 
 struct mavlink_logmessage {
-	char text[51];
+	char text[MAVLINK_LOG_MAXLEN + 1];
 	unsigned char severity;
 };
 
@@ -116,5 +123,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
 
 int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem);
 
+void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...);
+
 #endif
 
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
+}
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index 7b1c9679e..4bcce18fb 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -41,6 +41,7 @@
 #include <unistd.h>
 #include <stdlib.h>
 #include <stdio.h>
+#include <string.h>
 #include <fcntl.h>
 #include <errno.h>
 
@@ -143,6 +144,7 @@ int preflight_check_main(int argc, char *argv[])
 	float param_min, param_max, param_trim, param_rev, param_dz;
 
 	bool rc_ok = true;
+	char nbuf[20];
 
 	for (int i = 0; i < 12; i++) {
 		/* should the channel be enabled? */
@@ -217,7 +219,7 @@ int preflight_check_main(int argc, char *argv[])
 
 		/* sanity checks pass, enable channel */
 		if (count) {
-			mavlink_log_critical("ERROR: %d config error(s) for RC channel %d.", count, (channel + 1));
+			mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
 			usleep(100000);
 			rc_ok = false;
 		}