diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 408799be41..f9c7fcedf2 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -433,7 +433,24 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 			const char *txt = (const char *)arg;
 			struct mavlink_logmessage msg;
 			strncpy(msg.text, txt, sizeof(msg.text));
-			msg.severity = (unsigned char)cmd;
+
+			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) {
@@ -816,23 +833,23 @@ Mavlink::handle_message(const mavlink_message_t *msg)
 void
 Mavlink::send_statustext_info(const char *string)
 {
-	send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string);
+	send_statustext(MAV_SEVERITY_INFO, string);
 }
 
 void
 Mavlink::send_statustext_critical(const char *string)
 {
-	send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string);
+	send_statustext(MAV_SEVERITY_CRITICAL, string);
 }
 
 void
 Mavlink::send_statustext_emergency(const char *string)
 {
-	send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string);
+	send_statustext(MAV_SEVERITY_EMERGENCY, string);
 }
 
 void
-Mavlink::send_statustext(unsigned severity, const char *string)
+Mavlink::send_statustext(unsigned char severity, const char *string)
 {
 	struct mavlink_logmessage logmsg;
 	strncpy(logmsg.text, string, sizeof(logmsg.text));
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 4121e286e0..38149cf107 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -218,7 +218,7 @@ public:
 	 * @param string the message to send (will be capped by mavlink max string length)
 	 * @param severity the log level
 	 */
-	void			send_statustext(unsigned severity, const char *string);
+	void			send_statustext(unsigned char severity, const char *string);
 
 	MavlinkStream *		get_streams() const { return _streams; }
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index a710cdf6a1..083a6301a9 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -351,24 +351,7 @@ protected:
 			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;
-				}
+				msg.severity = logmsg.severity;
 				strncpy(msg.text, logmsg.text, sizeof(msg.text));
 
 				_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);