dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

VA args now supported by MAVLink text messages

This commit is contained in:
Lorenz Meier 2013-05-20 14:55:48 +02:00
parent 88ba97816d
commit d720944efe
3 changed files with 56 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
src/include/mavlink/mavlink_log.h
View File

@ -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

26
src/modules/mavlink/mavlink_log.c
View File

@ -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
}

4
src/systemcmds/preflight_check/preflight_check.c
View File

@ -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;
}