2 files changed, 68 insertions, 17 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 0d685c3d8..cd0581af4 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -492,6 +492,7 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 //		printf("logmsg: %s\n", txt);
 			struct mavlink_logmessage msg;
 			strncpy(msg.text, txt, sizeof(msg.text));
+			msg.severity = (unsigned char)cmd;
 
 			Mavlink *inst;
 			LL_FOREACH(_mavlink_instances, inst) {
@@ -840,7 +841,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 					(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
 				/* Start sending parameters */
 				mavlink_pm_start_queued_send();
-				send_statustext("[mavlink pm] sending list");
+				send_statustext_info("[pm] sending list");
 			}
 		} break;
 
@@ -864,7 +865,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 					if (param == PARAM_INVALID) {
 						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
 						sprintf(buf, "[pm] unknown: %s", name);
-						send_statustext(buf);
+						send_statustext_info(buf);
 
 					} else {
 						/* set and send parameter */
@@ -901,17 +902,28 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 }
 
 int
-Mavlink::send_statustext(const char *string)
+Mavlink::send_statustext_info(const char *string)
 {
-	return send_statustext(MAV_SEVERITY_INFO, string);
+	return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string);
 }
 
 int
-Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string)
+Mavlink::send_statustext_critical(const char *string)
+{
+	return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string);
+}
+
+int
+Mavlink::send_statustext_emergency(const char *string)
+{
+	return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string);
+}
+
+int
+Mavlink::send_statustext(unsigned severity, const char *string)
 {
 	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
 	mavlink_statustext_t statustext;
-	statustext.severity = severity;
 
 	int i = 0;
 
@@ -929,6 +941,19 @@ Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string)
 		/* Enforce null termination */
 		statustext.text[i] = '\0';
 
+		/* 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;
+		}
+
 		mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
 		return OK;
 
@@ -1449,7 +1474,7 @@ Mavlink::task_main(int argc, char *argv[])
 				int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
 
 				if (lb_ret == OK) {
-					send_statustext(msg.text);
+					send_statustext(msg.severity, msg.text);
 				}
 			}
 		}
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1f4fb759f..acfc8b90e 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -157,9 +157,9 @@ public:
 	 */
 	int			set_hil_enabled(bool hil_enabled);
 
-	void	send_message(const mavlink_message_t *msg);
+	void			send_message(const mavlink_message_t *msg);
 
-	void	handle_message(const mavlink_message_t *msg);
+	void			handle_message(const mavlink_message_t *msg);
 
 	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
 
@@ -174,17 +174,43 @@ public:
 
 	mavlink_channel_t	get_channel();
 
-	void configure_stream_threadsafe(const char *stream_name, float rate);
+	void			configure_stream_threadsafe(const char *stream_name, float rate);
 
 	bool			_task_should_exit;	/**< if true, mavlink task should exit */
 
 	int			get_mavlink_fd() { return _mavlink_fd; }
 
-	int send_statustext(const char *string);
-	int send_statustext(enum MAV_SEVERITY severity, const char *string);
-	MavlinkStream * get_streams() const { return _streams; }
+	/**
+	 * Send a status text with loglevel INFO
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 */
+	int			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);
+
+	/**
+	 * 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);
+
+	/**
+	 * Send a status text with loglevel
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 * @param severity the log level, one of 
+	 */
+	int			send_statustext(unsigned severity, const char *string);
+	MavlinkStream *		get_streams() const { return _streams; }
 
-	float get_rate_mult();
+	float			get_rate_mult();
 
 	/* Functions for waiting to start transmission until message received. */
 	void			set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
@@ -195,13 +221,13 @@ public:
 
 	bool			message_buffer_write(const void *ptr, int size);
     
-    void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
-    void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
+	void			lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
+	void			unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
 
 	/**
 	 * Count a transmision error
 	 */
-	void count_txerr();
+	void			count_txerr();
 
 protected:
 	Mavlink			*next;