Merge pull request #1926 from pritamghanghas/apmseveritybugfix

Fixes https://github.com/diydrones/apm_planner/issues/788

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -26,15 +26,100 @@
 
 #include "APMFirmwarePlugin.h"
 #include "Generic/GenericFirmwarePlugin.h"
+#include "QGCMAVLink.h"
 
 #include <QDebug>
 
 IMPLEMENT_QGC_SINGLETON(APMFirmwarePlugin, FirmwarePlugin)
+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)");
+
+// 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*)*");
+
+// minimum firmware versions that don't suffer from mavlink severity inversion bug.
+// https://github.com/diydrones/apm_planner/issues/788
+static const QString MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Copter V3.4.0");
+static const QString MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Plane V3.4.0");
+static const QString MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Rover V2.6.0");
+
+
+/*
+ * @brief APMFirmwareVersion is a small class to represent the firmware version
+ * It encabsules vehicleType, major version, minor version and patch level version
+ * and provides accessors for the same.
+ * isValid() can be used, to know whether version infromation is available or not
+ * supports < operator
+ */
+APMFirmwareVersion::APMFirmwareVersion(const QString &versionText)
+{
+    _major         = 0;
+    _minor         = 0;
+    _patch         = 0;
+
+    _parseVersion(versionText);
+}
+
+bool APMFirmwareVersion::isValid() const
+{
+    return !_versionString.isEmpty();
+}
+
+bool APMFirmwareVersion::isBeta() const
+{
+    return _versionString.contains(QStringLiteral(".rc"));
+}
+
+bool APMFirmwareVersion::isDev() const
+{
+    return _versionString.contains(QStringLiteral(".dev"));
+}
+
+bool APMFirmwareVersion::operator <(const APMFirmwareVersion& other) const
+{
+    int myVersion = _major << 16 | _minor << 8 | _patch ;
+    int otherVersion = other.majorNumber() << 16 | other.minorNumber() << 8 | other.patchNumber();
+    return myVersion < otherVersion;
+}
+
+void APMFirmwareVersion::_parseVersion(const QString &versionText)
+{
+    if (versionText.isEmpty()) {
+        return;
+    }
+
+
+    if (VERSION_REXP.indexIn(versionText) == -1) {
+        qCWarning(APMFirmwarePluginLog) << "firmware version regex didn't match anything"
+                                        << "version text to be parsed" << versionText;
+        return;
+    }
+
+    QStringList capturedTexts = VERSION_REXP.capturedTexts();
+
+    if (capturedTexts.count() < 5) {
+        qCWarning(APMFirmwarePluginLog) << "something wrong with parsing the version text, not hitting anything"
+                                        << VERSION_REXP.captureCount() << VERSION_REXP.capturedTexts();
+        return;
+    }
+
+    // successful extraction of version numbers
+    // even though we could have collected the version string atleast
+    // but if the parsing has faild, not much point
+    _versionString = versionText;
+    _vehicleType   = capturedTexts[1];
+    _major         = capturedTexts[2].toInt();
+    _minor         = capturedTexts[3].toInt();
+    _patch         = capturedTexts[4].toInt();
+}
 
 APMFirmwarePlugin::APMFirmwarePlugin(QObject* parent) :
     FirmwarePlugin(parent)
 {
-    
+     _textSeverityAdjustmentNeeded = false;
 }
 
 bool APMFirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
@@ -120,7 +205,7 @@ void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
                 paramUnion.param_float = paramValue.param_value;
                 break;
             default:
-                qCritical() << "Invalid/Unsupported data type used in parameter:" << paramValue.param_type;
+                qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramValue.param_type;
         }
         
         paramValue.param_value = paramUnion.param_float;
@@ -161,13 +246,80 @@ void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
                 // Already in param_float
                 break;
             default:
-                qCritical() << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type;
+                qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type;
         }
         
         mavlink_msg_param_set_encode(message->sysid, message->compid, message, &paramSet);
     }
 
-    // FIXME: Need to implement mavlink message severity adjustment
+    if (message->msgid == MAVLINK_MSG_ID_STATUSTEXT)
+    {
+        if (!_firmwareVersion.isValid()) {
+            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 text = QString(b);
+            qCDebug(APMFirmwarePluginLog) << text;
+
+            // if don't know firmwareVersion yet, try and see this message contains it
+            if (text.contains(APM_COPTER_REXP) || text.contains(APM_PLANE_REXP) || text.contains(APM_ROVER_REXP)) {
+                // found version string
+                _firmwareVersion = APMFirmwareVersion(text);
+                _textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(_firmwareVersion);
+            }
+        }
+
+        // adjust mesasge if needed
+        if (_textSeverityAdjustmentNeeded) {
+            _adjustSeverity(message);
+        }
+    }
+}
+
+bool APMFirmwarePlugin::_isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion)
+{
+   if (!firmwareVersion.isValid()) {
+       return false;
+   }
+
+   bool adjustmentNeeded = false;
+   if (firmwareVersion.vehicleType().contains(APM_COPTER_REXP)) {
+       if (firmwareVersion < APMFirmwareVersion(MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
+           adjustmentNeeded = true;
+       }
+   } else if (firmwareVersion.vehicleType().contains(APM_PLANE_REXP)) {
+       if (firmwareVersion < APMFirmwareVersion(MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
+           adjustmentNeeded = true;
+       }
+   } else if (firmwareVersion.vehicleType().contains(APM_ROVER_REXP)) {
+       if (firmwareVersion < APMFirmwareVersion(MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
+           adjustmentNeeded = true;
+       }
+   }
+
+   return adjustmentNeeded;
+}
+
+void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
+{
+    // lets make QGC happy with right severity values
+    mavlink_statustext_t statusText;
+    mavlink_msg_statustext_decode(message, &statusText);
+    switch(statusText.severity) {
+        case MAV_SEVERITY_ALERT:    /* SEVERITY_LOW according to old codes */
+            statusText.severity = MAV_SEVERITY_WARNING;
+            break;
+        case MAV_SEVERITY_CRITICAL: /*SEVERITY_MEDIUM according to old codes  */
+            statusText.severity = MAV_SEVERITY_ALERT;
+            break;
+        case MAV_SEVERITY_ERROR:    /*SEVERITY_HIGH according to old codes */
+            statusText.severity = MAV_SEVERITY_CRITICAL;
+            break;
+    }
+
+    mavlink_msg_statustext_encode(message->sysid, message->compid, message, &statusText);
 }
 
 void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -29,6 +29,31 @@
 
 #include "FirmwarePlugin.h"
 
+Q_DECLARE_LOGGING_CATEGORY(APMFirmwarePluginLogsLog)
+
+class APMFirmwareVersion
+{
+public:
+    APMFirmwareVersion(const QString &versionText = "");
+    bool isValid() const;
+    bool isBeta() const;
+    bool isDev() const;
+    bool operator<(const APMFirmwareVersion& other) const;
+    QString versionString() const { return _versionString; }
+    QString vehicleType() const { return _vehicleType; }
+    int majorNumber() const { return _major; }
+    int minorNumber() const { return _minor; }
+    int patchNumber() const { return _patch; }
+
+private:
+    void _parseVersion(const QString &versionText);
+    QString _versionString;
+    QString _vehicleType;
+    int     _major;
+    int     _minor;
+    int     _patch;
+};
+
 class APMFirmwarePlugin : public FirmwarePlugin
 {
     Q_OBJECT
@@ -49,6 +74,12 @@ public:
 private:
     /// All access to singleton is through AutoPilotPluginManager::instance
     APMFirmwarePlugin(QObject* parent = NULL);
+    void _adjustSeverity(mavlink_message_t* message) const;
+    static bool _isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion);
+
+    APMFirmwareVersion _firmwareVersion;
+    bool               _textSeverityAdjustmentNeeded;
+
 };
 
 #endif