From 90e4b7fec91019a818d736746bc5b813785a915a Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Fri, 15 Jan 2016 15:01:55 -0800
Subject: [PATCH] Fix more incorrect severity problems

---
 src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 44 ++++++++++++++++++---
 src/FirmwarePlugin/APM/APMFirmwarePlugin.h  |  2 +
 2 files changed, 40 insertions(+), 6 deletions(-)

diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
index 3471a7735..90556a352 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
@@ -35,6 +35,9 @@ QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog")
 static const QRegExp APM_COPTER_REXP("^(ArduCopter|APM:Copter)");
 static const QRegExp APM_PLANE_REXP("^(ArduPlane|APM:Plane)");
 static const QRegExp APM_ROVER_REXP("^(ArduRover|APM:Rover)");
+static const QRegExp APM_PX4NUTTX_REXP("^PX4: .*NuttX: .*");
+static const QRegExp APM_FRAME_REXP("^Frame: ");
+static const QRegExp APM_SYSID_REXP("^PX4v2 ");
 
 // Regex to parse version text coming from APM, gives out firmware type, major, minor and patch level numbers
 static const QRegExp VERSION_REXP("^(APM:Copter|APM:Plane|APM:Rover|ArduCopter|ArduPlane|ArduRover) +[vV](\\d*)\\.*(\\d*)*\\.*(\\d*)*");
@@ -294,16 +297,13 @@ void APMFirmwarePlugin::adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t
     }
 
     if (message->msgid == MAVLINK_MSG_ID_STATUSTEXT) {
+        QString messageText;
+
         mavlink_statustext_t statusText;
         mavlink_msg_statustext_decode(message, &statusText);
 
         if (!_firmwareVersion.isValid() || statusText.severity < MAV_SEVERITY_NOTICE) {
-            QByteArray b;
-            b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
-            mavlink_msg_statustext_get_text(message, b.data());
-            // Ensure NUL-termination
-            b[b.length()-1] = '\0';
-            QString messageText = QString(b);
+            messageText = _getMessageText(message);
             qCDebug(APMFirmwarePluginLog) << messageText;
 
             if (!_firmwareVersion.isValid()) {
@@ -358,9 +358,32 @@ void APMFirmwarePlugin::adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t
         if (_textSeverityAdjustmentNeeded) {
             _adjustSeverity(message);
         }
+
+        if (messageText.isEmpty()) {
+            messageText = _getMessageText(message);
+        }
+
+        // The following messages are incorrectly labeled as warning message.
+        // Fixed in newer firmware (unreleased at this point), but still in older firmware.
+        if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) ||
+                messageText.contains(APM_PX4NUTTX_REXP) || messageText.contains(APM_FRAME_REXP) || messageText.contains(APM_SYSID_REXP)) {
+            _setInfoSeverity(message);
+        }
     }
 }
 
+QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message) const
+{
+    QByteArray b;
+
+    b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
+    mavlink_msg_statustext_get_text(message, b.data());
+
+    // Ensure NUL-termination
+    b[b.length()-1] = '\0';
+    return QString(b);
+}
+
 bool APMFirmwarePlugin::_isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion)
 {
    if (!firmwareVersion.isValid()) {
@@ -405,6 +428,15 @@ void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
     mavlink_msg_statustext_encode(message->sysid, message->compid, message, &statusText);
 }
 
+void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message) const
+{
+    mavlink_statustext_t statusText;
+    mavlink_msg_statustext_decode(message, &statusText);
+
+    statusText.severity = MAV_SEVERITY_INFO;
+    mavlink_msg_statustext_encode(message->sysid, message->compid, message, &statusText);
+}
+
 void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* message) const
 {
     mavlink_statustext_t statusText;
diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
index 775949927..9395404b3 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
@@ -102,6 +102,8 @@ private:
     void _adjustSeverity(mavlink_message_t* message) const;
     void _adjustCalibrationMessageSeverity(mavlink_message_t* message) const;
     static bool _isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion);
+    void _setInfoSeverity(mavlink_message_t* message) const;
+    QString _getMessageText(mavlink_message_t* message) const;
 
     APMFirmwareVersion      _firmwareVersion;
     bool                    _textSeverityAdjustmentNeeded;
-- 
2.22.0