From d720944efed1c5cde2b6feed170ade7b2bc9ada3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 May 2013 14:55:48 +0200 Subject: [PATCH] VA args now supported by MAVLink text messages --- src/include/mavlink/mavlink_log.h | 45 +++++++++++-------- src/modules/mavlink/mavlink_log.c | 26 +++++++++++ .../preflight_check/preflight_check.c | 4 +- 3 files changed, 56 insertions(+), 19 deletions(-) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 233a76cb31..a28ff3a687 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 * * Redistribution and use in source and binary forms, with or without @@ -47,27 +47,42 @@ */ #include -/* +/** * 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 fa974dc0b6..6395ab214a 100644 --- a/src/modules/mavlink/mavlink_log.c +++ b/src/modules/mavlink/mavlink_log.c @@ -41,6 +41,7 @@ #include #include +#include #include @@ -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 7b1c9679ef..4bcce18fb8 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -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; }