mavlink: STATUSTEXT implemented via streams API

3 files changed, 119 insertions, 81 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 0cf708017..7d3fb97e1 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -452,7 +452,6 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 	case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
 
 			const char *txt = (const char *)arg;
-//		printf("logmsg: %s\n", txt);
 			struct mavlink_logmessage msg;
 			strncpy(msg.text, txt, sizeof(msg.text));
 			msg.severity = (unsigned char)cmd;
@@ -782,67 +781,32 @@ Mavlink::handle_message(const mavlink_message_t *msg)
 	}
 }
 
-int
+void
 Mavlink::send_statustext_info(const char *string)
 {
-	return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string);
+	send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string);
 }
 
-int
+void
 Mavlink::send_statustext_critical(const char *string)
 {
-	return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string);
+	send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string);
 }
 
-int
+void
 Mavlink::send_statustext_emergency(const char *string)
 {
-	return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string);
+	send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string);
 }
 
-int
+void
 Mavlink::send_statustext(unsigned severity, const char *string)
 {
-	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
-	mavlink_statustext_t statustext;
-
-	int i = 0;
-
-	while (i < len - 1) {
-		statustext.text[i] = string[i];
-
-		if (string[i] == '\0') {
-			break;
-		}
-
-		i++;
-	}
-
-	if (i > 1) {
-		/* Enforce null termination */
-		statustext.text[i] = '\0';
+	struct mavlink_logmessage logmsg;
+	strncpy(logmsg.text, string, sizeof(logmsg.text));
+	logmsg.severity = severity;
 
-		/* Map severity */
-		switch (severity) {
-		case MAVLINK_IOC_SEND_TEXT_INFO:
-			statustext.severity = MAV_SEVERITY_INFO;
-			break;
-
-		case MAVLINK_IOC_SEND_TEXT_CRITICAL:
-			statustext.severity = MAV_SEVERITY_CRITICAL;
-			break;
-
-		case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
-			statustext.severity = MAV_SEVERITY_EMERGENCY;
-			break;
-		}
-
-		send_message(MAVLINK_MSG_ID_STATUSTEXT, &statustext);
-		return OK;
-
-	} else {
-		return ERROR;
-	}
+	mavlink_logbuffer_write(&_logbuffer, &logmsg);
 }
 
 MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
@@ -1312,6 +1276,9 @@ Mavlink::task_main(int argc, char *argv[])
 	/* HEARTBEAT is constant rate stream, rate never adjusted */
 	configure_stream("HEARTBEAT", 1.0f);
 
+	/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
+	configure_stream("STATUSTEXT", 20.0f);
+
 	/* COMMAND_LONG stream: use high rate to avoid commands skipping */
 	configure_stream("COMMAND_LONG", 100.0f);
 
@@ -1367,7 +1334,7 @@ Mavlink::task_main(int argc, char *argv[])
 		hrt_abstime t = hrt_absolute_time();
 
 		update_rate_mult();
-		warnx("rate mult %.2f", (double)_rate_mult);
+		warnx("rate mult %.2f rate %.3f err %.3f", (double)_rate_mult, (double)_rate_tx, (double)_rate_txerr);
 
 		if (param_sub->update(&param_time, nullptr)) {
 			/* parameters updated */
@@ -1407,15 +1374,6 @@ Mavlink::task_main(int argc, char *argv[])
 		if (fast_rate_limiter.check(t)) {
 			// TODO missions
 			//_mission_manager->eventloop();
-
-			if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
-				struct mavlink_logmessage msg;
-				int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
-
-				if (lb_ret == OK) {
-					send_statustext(msg.severity, msg.text);
-				}
-			}
 		}
 
 		/* pass messages from other UARTs or FTP worker */
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 7fcbae72e..87bb9d9ad 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -188,29 +188,33 @@ public:
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
 	 */
-	int			send_statustext_info(const char *string);
+	void			send_statustext_info(const char *string);
 
 	/**
 	 * Send a status text with loglevel CRITICAL
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
 	 */
-	int			send_statustext_critical(const char *string);
+	void			send_statustext_critical(const char *string);
 
 	/**
 	 * Send a status text with loglevel EMERGENCY
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
 	 */
-	int			send_statustext_emergency(const char *string);
+	void			send_statustext_emergency(const char *string);
 
 	/**
-	 * Send a status text with loglevel
+	 * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent
+	 * only on this mavlink connection. Useful for reporting communication specific, not system-wide info
+	 * only to client interested in it. Message will be not sent immediately but queued in buffer as
+	 * for mavlink_log_xxx().
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
-	 * @param severity the log level, one of 
+	 * @param severity the log level
 	 */
-	int			send_statustext(unsigned severity, const char *string);
+	void			send_statustext(unsigned severity, const char *string);
+
 	MavlinkStream *		get_streams() const { return _streams; }
 
 	float			get_rate_mult();
@@ -259,6 +263,8 @@ public:
 	 */
 	struct telemetry_status_s&	get_rx_status() { return _rstatus; }
 
+	struct mavlink_logbuffer	*get_logbuffer() { return &_logbuffer; }
+
 protected:
 	Mavlink			*next;
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 863b7a1d4..4333369fe 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -40,9 +40,9 @@
  */
 
 #include <stdio.h>
+
 #include <commander/px4_custom_mode.h>
 #include <lib/geo/geo.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -72,8 +72,8 @@
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_range_finder.h>
-
 #include <systemlib/err.h>
+#include <mavlink/mavlink_log.h>
 
 #include "mavlink_messages.h"
 #include "mavlink_main.h"
@@ -306,6 +306,77 @@ protected:
 	}
 };
 
+class MavlinkStreamStatustext : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamStatustext::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "STATUSTEXT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_STATUSTEXT;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamStatustext(mavlink);
+	}
+
+	unsigned get_size() {
+		return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
+	}
+
+private:
+	/* do not allow top copying this class */
+	MavlinkStreamStatustext(MavlinkStreamStatustext &);
+	MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
+
+protected:
+	explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	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);
+
+			if (lb_ret == OK) {
+				mavlink_statustext_t msg;
+
+				/* map severity */
+				switch (logmsg.severity) {
+				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;
+				}
+				strncpy(msg.text, logmsg.text, sizeof(msg.text));
+
+				_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
+			}
+		}
+	}
+};
+
 class MavlinkStreamCommandLong : public MavlinkStream
 {
 public:
@@ -329,7 +400,9 @@ public:
 		return new MavlinkStreamCommandLong(mavlink);
 	}
 
-	unsigned get_size() { return 0; }	// commands stream is not regular and not predictable
+	unsigned get_size() {
+		return 0;	// commands stream is not regular and not predictable
+	}
 
 private:
 	MavlinkOrbSubscription *_cmd_sub;
@@ -352,21 +425,21 @@ protected:
 		if (_cmd_sub->update(&_cmd_time, &cmd)) {
 			/* only send commands for other systems/components */
 			if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
-				mavlink_command_long_t mavcmd;
-
-				mavcmd.target_system = cmd.target_system;
-				mavcmd.target_component = cmd.target_component;
-				mavcmd.command = cmd.command;
-				mavcmd.confirmation = cmd.confirmation;
-				mavcmd.param1 = cmd.param1;
-				mavcmd.param2 = cmd.param2;
-				mavcmd.param3 = cmd.param3;
-				mavcmd.param4 = cmd.param4;
-				mavcmd.param5 = cmd.param5;
-				mavcmd.param6 = cmd.param6;
-				mavcmd.param7 = cmd.param7;
-
-				_mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &mavcmd);
+				mavlink_command_long_t msg;
+
+				msg.target_system = cmd.target_system;
+				msg.target_component = cmd.target_component;
+				msg.command = cmd.command;
+				msg.confirmation = cmd.confirmation;
+				msg.param1 = cmd.param1;
+				msg.param2 = cmd.param2;
+				msg.param3 = cmd.param3;
+				msg.param4 = cmd.param4;
+				msg.param5 = cmd.param5;
+				msg.param6 = cmd.param6;
+				msg.param7 = cmd.param7;
+
+				_mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg);
 			}
 		}
 	}
@@ -1996,6 +2069,7 @@ protected:
 
 StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
+	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static),
 	new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static),
 	new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static),
 	new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static),