mavlink: refactor mavlink_log

This moves the mavlink_log interface from ioctl to uORB and enables the
mavlink statusmessage output for Snapdragon. The API changes will lead
to changes in all modules that are using it.

msg/mavlink_log.msg

@@ -0,0 +1,4 @@
+uint64 timestamp		# Timestamp in microseconds since boot
+
+uint8[51] text
+uint8 severity

src/modules/mavlink/mavlink_logbuffer.c

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_rate_limiter.cpp
+ * Message rate limiter implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "mavlink_rate_limiter.h"
+
+MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000)
+{
+}
+
+MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval)
+{
+}
+
+MavlinkRateLimiter::~MavlinkRateLimiter()
+{
+}
+
+void
+MavlinkRateLimiter::set_interval(unsigned int interval)
+{
+	_interval = interval;
+}
+
+bool
+MavlinkRateLimiter::check(hrt_abstime t)
+{
+	uint64_t dt = t - _last_sent;
+
+	if (dt > 0 && dt >= _interval) {
+		_last_sent = t;
+		return true;
+	}
+
+	return false;
+}

src/modules/mavlink/mavlink_main.cpp

@@ -69,13 +69,14 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/mcu_version.h>
 #include <systemlib/git_version.h>
+#include <systemlib/mavlink_log.h>
 #include <geo/geo.h>
 #include <dataman/dataman.h>
 //#include <mathlib/mathlib.h>
-#include <mavlink/mavlink_log.h>
 
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/mavlink_log.h>
 
 #include "mavlink_bridge_header.h"
 #include "mavlink_main.h"
@@ -101,11 +102,6 @@ static const int ERROR = -1;
 
 static Mavlink *_mavlink_instances = nullptr;
 
-#ifdef __PX4_NUTTX
-/* TODO: if this is a class member it crashes */
-static struct file_operations fops;
-#endif
-
 static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
 static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
 
@@ -123,14 +119,10 @@ static void usage(void);
 bool Mavlink::_boot_complete = false;
 
 Mavlink::Mavlink() :
-#ifndef __PX4_NUTTX
-	VDev("mavlink-log", MAVLINK_LOG_DEVICE),
-#endif
-	_device_name(DEFAULT_DEVICE_NAME),
 	_task_should_exit(false),
 	next(nullptr),
 	_instance_id(0),
-	_mavlink_fd(-1),
+	_mavlink_log_pub(nullptr),
 	_task_running(false),
 	_hil_enabled(false),
 	_generate_rc(false),
@@ -149,7 +141,7 @@ Mavlink::Mavlink() :
 	_mode(MAVLINK_MODE_NORMAL),
 	_channel(MAVLINK_COMM_0),
 	_radio_id(0),
-	_logbuffer {},
+	_logbuffer(5, sizeof(mavlink_log_s)),
 	_total_counter(0),
 	_receive_thread {},
 	_verbose(false),
@@ -203,10 +195,6 @@ Mavlink::Mavlink() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
 	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
 {
-#ifdef __PX4_NUTTX
-	fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
-#endif
-
 	_instance_id = Mavlink::instance_count();
 
 	/* set channel according to instance id */
@@ -475,60 +463,6 @@ Mavlink::get_channel()
 	return _channel;
 }
 
-/****************************************************************************
- * MAVLink text message logger
- ****************************************************************************/
-
-int
-#ifdef __PX4_NUTTX
-Mavlink::mavlink_dev_ioctl(struct file *filp, int cmd, unsigned long arg)
-#else
-Mavlink::ioctl(device::file_t *filp, int cmd, unsigned long arg)
-#endif
-{
-	switch (cmd) {
-	case (int)MAVLINK_IOC_SEND_TEXT_INFO:
-	case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL:
-	case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
-
-			const char *txt = (const char *)arg;
-			struct mavlink_logmessage msg;
-			strncpy(msg.text, txt, sizeof(msg.text));
-
-			switch (cmd) {
-			case MAVLINK_IOC_SEND_TEXT_INFO:
-				msg.severity = MAV_SEVERITY_INFO;
-				break;
-
-			case MAVLINK_IOC_SEND_TEXT_CRITICAL:
-				msg.severity = MAV_SEVERITY_CRITICAL;
-				break;
-
-			case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
-				msg.severity = MAV_SEVERITY_EMERGENCY;
-				break;
-
-			default:
-				msg.severity = MAV_SEVERITY_INFO;
-				break;
-			}
-
-			Mavlink *inst;
-			LL_FOREACH(_mavlink_instances, inst) {
-				if (!inst->_task_should_exit) {
-					mavlink_logbuffer_write(&inst->_logbuffer, &msg);
-					inst->_total_counter++;
-				}
-			}
-
-			return OK;
-		}
-
-	default:
-		return ENOTTY;
-	}
-}
-
 void Mavlink::mavlink_update_system(void)
 {
 	if (!_param_initialized) {
@@ -1088,29 +1022,19 @@ Mavlink::handle_message(const mavlink_message_t *msg)
 void
 Mavlink::send_statustext_info(const char *string)
 {
-	send_statustext(MAV_SEVERITY_INFO, string);
+	mavlink_log_info(&_mavlink_log_pub, string);
 }
 
 void
 Mavlink::send_statustext_critical(const char *string)
 {
-	send_statustext(MAV_SEVERITY_CRITICAL, string);
+	mavlink_log_critical(&_mavlink_log_pub, string);
 }
 
 void
 Mavlink::send_statustext_emergency(const char *string)
 {
-	send_statustext(MAV_SEVERITY_EMERGENCY, string);
-}
-
-void
-Mavlink::send_statustext(unsigned char severity, const char *string)
-{
-	struct mavlink_logmessage logmsg;
-	strncpy(logmsg.text, string, sizeof(logmsg.text));
-	logmsg.severity = severity;
-
-	mavlink_logbuffer_write(&_logbuffer, &logmsg);
+	mavlink_log_emergency(&_mavlink_log_pub, string);
 }
 
 void Mavlink::send_autopilot_capabilites()
@@ -1686,9 +1610,6 @@ Mavlink::task_main(int argc, char *argv[])
 	/* initialize send mutex */
 	pthread_mutex_init(&_send_mutex, NULL);
 
-	/* initialize mavlink text message buffering */
-	mavlink_logbuffer_init(&_logbuffer, 5);
-
 	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
 	if (_forwarding_on || _ftp_on) {
 		/* initialize message buffer if multiplexing is on or its needed for FTP.
@@ -1704,20 +1625,6 @@ Mavlink::task_main(int argc, char *argv[])
 		pthread_mutex_init(&_message_buffer_mutex, NULL);
 	}
 
-	/* create the device node that's used for sending text log messages, etc. */
-#ifdef __PX4_NUTTX
-	register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
-#else
-	int ret;
-	ret = VDev::init();
-	if (ret != OK) {
-		PX4_WARN("VDev setup for mavlink log device failed!\n");
-	}
-#endif
-
-	/* initialize logging device */
-	_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
-
 	/* Initialize system properties */
 	mavlink_update_system();
 
@@ -2112,17 +2019,11 @@ Mavlink::task_main(int argc, char *argv[])
 		::close(_uart_fd);
 	}
 
-	/* close mavlink logging device */
-	px4_close(_mavlink_fd);
-
 	if (_forwarding_on || _ftp_on) {
 		message_buffer_destroy();
 		pthread_mutex_destroy(&_message_buffer_mutex);
 	}
 
-	/* destroy log buffer */
-	mavlink_logbuffer_destroy(&_logbuffer);
-
 	warnx("exiting");
 	_task_running = false;
 
@@ -2390,3 +2291,6 @@ int mavlink_main(int argc, char *argv[])
 
 	return 0;
 }
+
+
+

src/modules/mavlink/mavlink_main.h

@@ -53,7 +53,8 @@
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
 #include <pthread.h>
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
@@ -75,11 +76,7 @@ enum Protocol {
 	TCP,
 };
 
-#ifdef __PX4_NUTTX
 class Mavlink
-#else
-class Mavlink : public device::VDev
-#endif
 {
 
 public:
@@ -251,7 +248,7 @@ public:
 
 	bool			_task_should_exit;	/**< if true, mavlink task should exit */
 
-	int			get_mavlink_fd() { return _mavlink_fd; }
+	orb_advert_t		*get_mavlink_log_pub() { return &_mavlink_log_pub; }
 
 	/**
 	 * Send a status text with loglevel INFO
@@ -329,7 +326,7 @@ public:
 	 */
 	struct telemetry_status_s&	get_rx_status() { return _rstatus; }
 
-	struct mavlink_logbuffer	*get_logbuffer() { return &_logbuffer; }
+	ringbuffer::RingBuffer	*get_logbuffer() { return &_logbuffer; }
 
 	unsigned		get_system_type() { return _system_type; }
 
@@ -362,7 +359,7 @@ protected:
 private:
 	int			_instance_id;
 
-	int			_mavlink_fd;
+	orb_advert_t		_mavlink_log_pub;
 	bool			_task_running;
 	static bool		_boot_complete;
 
@@ -390,7 +387,7 @@ private:
 	mavlink_channel_t	_channel;
 	int32_t			_radio_id;
 
-	struct mavlink_logbuffer _logbuffer;
+	ringbuffer::RingBuffer		_logbuffer;
 	unsigned int		_total_counter;
 
 	pthread_t		_receive_thread;
@@ -514,12 +511,6 @@ private:
 
 	void init_udp();
 
-#ifdef __PX4_NUTTX
-	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
-#else
-	virtual int	ioctl(device::file_t *filp, int cmd, unsigned long arg);
-#endif
-
 	/**
 	 * Main mavlink task.
 	 */

src/modules/mavlink/mavlink_messages.cpp

@@ -75,10 +75,11 @@
 #include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/mavlink_log.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
 #include <systemlib/err.h>
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
 
 #include <mathlib/mathlib.h>
 
@@ -360,90 +361,87 @@ public:
 	}
 
 	unsigned get_size() {
-		return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
+		return _mavlink->get_logbuffer()->empty() ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
 	}
 
 private:
+	MavlinkOrbSubscription *_mavlink_log_sub;
+	uint64_t _mavlink_log_time;
+
 	/* do not allow top copying this class */
 	MavlinkStreamStatustext(MavlinkStreamStatustext &);
 	MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
-	FILE *fp = nullptr;
+
 	unsigned write_err_count = 0;
 	static const unsigned write_err_threshold = 5;
 
 protected:
-	explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
+	explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_mavlink_log_sub(_mavlink->add_orb_subscription(ORB_ID(mavlink_log)))
 	{}
 
-	~MavlinkStreamStatustext() {
-		if (fp) {
-			fclose(fp);
-		}
-	}
-
-#ifndef __PX4_QURT
 	void send(const hrt_abstime t)
 	{
-		if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) {
-			struct mavlink_logmessage logmsg;
-			int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg);
+		struct mavlink_log_s mavlink_log;
 
-			if (lb_ret == OK) {
-				mavlink_statustext_t msg;
+		if (_mavlink_log_sub->update(&_mavlink_log_time, &mavlink_log)) {
 
-				msg.severity = logmsg.severity;
-				strncpy(msg.text, logmsg.text, sizeof(msg.text));
+			mavlink_statustext_t msg;
+			msg.severity = mavlink_log.severity;
 
-				_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
+			strncpy(msg.text, (const char*)mavlink_log.text, sizeof(msg.text));
 
-				/* write log messages in first instance to disk */
-				if (_mavlink->get_instance_id() == 0) {
-					if (fp) {
-						if (EOF == fputs(msg.text, fp)) {
-							write_err_count++;
-						} else {
-							write_err_count = 0;
-						}
+			_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
+		}
 
-						if (write_err_count >= write_err_threshold) {
-							(void)fclose(fp);
-							fp = nullptr;
-						} else {
-							(void)fputs("\n", fp);
-							(void)fsync(fileno(fp));
-						}
+		// TODO: add this again
+#if 0
+		/* write log messages in first instance to disk */
+		if (_mavlink->get_instance_id() == 0) {
+			if (fp) {
+				if (EOF == fputs(msg.text, fp)) {
+					write_err_count++;
+				} else {
+					write_err_count = 0;
+				}
 
-					} else if (write_err_count < write_err_threshold) {
-						/* string to hold the path to the log */
-						char log_file_name[32] = "";
-						char log_file_path[70] = "";
+				if (write_err_count >= write_err_threshold) {
+					(void)fclose(fp);
+					fp = nullptr;
+				} else {
+					(void)fputs("\n", fp);
+					(void)fsync(fileno(fp));
+				}
 
-						timespec ts;
-						px4_clock_gettime(CLOCK_REALTIME, &ts);
-						/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
-						time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
-						struct tm tt;
-						gmtime_r(&gps_time_sec, &tt);
+			} else if (write_err_count < write_err_threshold) {
+				/* string to hold the path to the log */
+				char log_file_name[32] = "";
+				char log_file_path[70] = "";
 
-						// XXX we do not want to interfere here with the SD log app
-						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
-						snprintf(log_file_path, sizeof(log_file_path), PX4_ROOTFSDIR"/fs/microsd/%s", log_file_name);
-						fp = fopen(log_file_path, "ab");
+				timespec ts;
+				px4_clock_gettime(CLOCK_REALTIME, &ts);
+				/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
+				time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
+				struct tm tt;
+				gmtime_r(&gps_time_sec, &tt);
 
-						if (fp != NULL) {
-							/* write first message */
-							fputs(msg.text, fp);
-							fputs("\n", fp);
-						}
-						else {
-							warn("Failed to open %s errno=%d", log_file_path, errno);
-						}
-					}
+				// XXX we do not want to interfere here with the SD log app
+				strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
+				snprintf(log_file_path, sizeof(log_file_path), PX4_ROOTFSDIR"/fs/microsd/%s", log_file_name);
+				fp = fopen(log_file_path, "ab");
+
+				if (fp != NULL) {
+					/* write first message */
+					fputs(msg.text, fp);
+					fputs("\n", fp);
+				}
+				else {
+					warn("Failed to open %s errno=%d", log_file_path, errno);
 				}
 			}
 		}
-	}
 #endif
+	}
 };
 
 class MavlinkStreamCommandLong : public MavlinkStream
@@ -2699,7 +2697,7 @@ protected:
 
 			if (land_detected.landed) {
 				_msg.landed_state = MAV_LANDED_STATE_ON_GROUND;
-			
+
 			} else {
 				_msg.landed_state = MAV_LANDED_STATE_IN_AIR;
 			}

src/modules/mavlink/mavlink_receiver.cpp

@@ -75,9 +75,9 @@
 
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 #include <systemlib/airspeed.h>
-#include <mavlink/mavlink_log.h>
 #include <commander/px4_custom_mode.h>
 #include <geo/geo.h>
 
@@ -1882,7 +1882,7 @@ MavlinkReceiver::receive_thread(void *arg)
 			struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address();
 			int localhost = (127 << 24) + 1;
 			if (!_mavlink->get_client_source_initialized()) {
-				
+
 				// set the address either if localhost or if 3 seconds have passed
 				// this ensures that a GCS running on localhost can get a hold of
 				// the system within the first N seconds

src/modules/systemlib/mavlink_log.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -44,93 +44,40 @@
 #include <stdio.h>
 #include <stdarg.h>
 
-#include <mavlink/mavlink_log.h>
+#include <uORB/topics/mavlink_log.h>
+#include "mavlink_log.h"
 
-__EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
+
+
+__EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, const char *fmt, ...)
 {
-	lb->size  = size;
-	lb->start = 0;
-	lb->count = 0;
-	lb->elems = calloc(lb->size, sizeof(struct mavlink_logmessage));
-}
+	// TODO: add compile check for maxlen
 
-__EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb)
-{
-	lb->size  = 0;
-	lb->start = 0;
-	lb->count = 0;
-	free(lb->elems);
-}
-
-__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
-{
-	return lb->count == (int)lb->size;
-}
-
-__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
-{
-	return lb->count == 0;
-}
-
-__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
-{
-	int end = (lb->start + lb->count) % lb->size;
-	memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage));
-
-	if (mavlink_logbuffer_is_full(lb)) {
-		lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
-
-	} else {
-		++lb->count;
-	}
-}
-
-__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
-{
-	if (!mavlink_logbuffer_is_empty(lb)) {
-		memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
-		lb->start = (lb->start + 1) % lb->size;
-		--lb->count;
-
-		return 0;
-
-	} else {
-		return 1;
-	}
-}
-
-__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
-{
 	if (!fmt) {
 		return;
 	}
 
-	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[end].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, ...)
-{
-	if (!fmt) {
+	if (mavlink_log_pub == NULL) {
 		return;
 	}
 
+	struct mavlink_log_s log_msg;
+
+	log_msg.severity = severity;
+
 	va_list ap;
+
 	va_start(ap, fmt);
-	char text[MAVLINK_LOG_MAXLEN + 1];
-	vsnprintf(text, sizeof(text), fmt, ap);
+
+	vsnprintf((char *)log_msg.text, sizeof(log_msg.text), fmt, ap);
+
 	va_end(ap);
-	px4_ioctl(_fd, severity, (unsigned long)&text[0]);
+
+	if (*mavlink_log_pub != NULL) {
+		orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg);
+
+	} else {
+		*mavlink_log_pub = orb_advertise(ORB_ID(mavlink_log), &log_msg);
+	}
 }
+

src/modules/systemlib/mavlink_log.h

@@ -36,35 +36,22 @@
  * MAVLink text logging.
  *
  * @author Lorenz Meier <lorenz@px4.io>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#ifndef MAVLINK_LOG
-#define MAVLINK_LOG
+#pragma once
 
-/*
- * IOCTL interface for sending log messages.
- */
-#include <px4_defines.h>
-#include <sys/ioctl.h>
+#include <uORB/uORB.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		_PX4_IOC(0x1100, 1)
-#define MAVLINK_IOC_SEND_TEXT_CRITICAL		_PX4_IOC(0x1100, 2)
-#define MAVLINK_IOC_SEND_TEXT_EMERGENCY		_PX4_IOC(0x1100, 3)
-
 #ifdef __cplusplus
 extern "C" {
 #endif
-__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
+__EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, const char *fmt, ...);
 #ifdef __cplusplus
 }
 #endif
@@ -79,59 +66,65 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
 /**
  * Send a mavlink emergency message.
  *
- * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
+ * @param _pub		Pointer to the uORB advert;
  * @param _text		The text to log;
  */
-#define mavlink_log_emergency(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__);
+#define mavlink_log_emergency(_pub, _text, ...)	mavlink_vasprintf(3, _pub, _text, ##__VA_ARGS__);
 
 /**
  * Send a mavlink critical message.
  *
- * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
+ * @param _pub		Pointer to the uORB advert;
  * @param _text		The text to log;
  */
-#define mavlink_log_critical(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__);
+#define mavlink_log_critical(_pub, _text, ...)	mavlink_vasprintf(2, _pub, _text, ##__VA_ARGS__);
 
 /**
  * Send a mavlink info message.
  *
- * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
+ * @param _pub		Pointer to the uORB advert;
  * @param _text		The text to log;
  */
-#define mavlink_log_info(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
+#define mavlink_log_info(_pub, _text, ...)	mavlink_vasprintf(1, _pub, _text, ##__VA_ARGS__);
 
 /**
  * Send a mavlink emergency message and print to console.
  *
- * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
+ * @param _pub		Pointer to the uORB advert;
  * @param _text		The text to log;
  */
-#define mavlink_and_console_log_emergency(_fd, _text, ...)		do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \
+#define mavlink_and_console_log_emergency(_pub, _text, ...) \
+	do { \
+		mavlink_log_emergency(_pub, _text, ##__VA_ARGS__); \
 		PX4_ERR("telem> "); \
 		PX4_ERR(_text, ##__VA_ARGS__); \
-		PX4_ERR("\n"); } while(0);
+	} while(0);
 
 /**
  * Send a mavlink critical message and print to console.
  *
- * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
+ * @param _pub		Pointer to the uORB advert;
  * @param _text		The text to log;
  */
-#define mavlink_and_console_log_critical(_fd, _text, ...)		do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \
+#define mavlink_and_console_log_critical(_pub, _text, ...) \
+	do { \
+		mavlink_log_critical(_pub, _text, ##__VA_ARGS__); \
 		PX4_WARN("telem> "); \
 		PX4_WARN(_text, ##__VA_ARGS__); \
-		PX4_WARN("\n");  } while(0);
+	} while(0);
 
 /**
  * Send a mavlink emergency message and print to console.
  *
- * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
+ * @param _pub		Pointer to the uORB advert;
  * @param _text		The text to log;
  */
-#define mavlink_and_console_log_info(_fd, _text, ...)			do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \
+#define mavlink_and_console_log_info(_pub, _text, ...)			\
+	do { \
+		mavlink_log_info(_pub, _text, ##__VA_ARGS__); \
 		PX4_INFO("telem> "); \
 		PX4_INFO(_text, ##__VA_ARGS__); \
-		PX4_INFO("\n"); } while(0);
+	} while(0);
 
 struct mavlink_logmessage {
 	char text[MAVLINK_LOG_MAXLEN + 1];
@@ -145,21 +138,3 @@ struct mavlink_logbuffer {
 	struct mavlink_logmessage *elems;
 };
 
-__BEGIN_DECLS
-void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size);
-
-void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb);
-
-int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb);
-
-int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb);
-
-void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem);
-
-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, ...);
-__END_DECLS
-
-#endif
-

src/modules/uORB/objects_common.cpp

@@ -282,3 +282,6 @@ ORB_DEFINE(ekf2_replay, struct ekf2_replay_s);
 
 #include "topics/qshell_req.h"
 ORB_DEFINE(qshell_req, struct qshell_req_s);
+
+#include "topics/mavlink_log.h"
+ORB_DEFINE(mavlink_log, struct mavlink_log_s);