From 563189ba4d27925b0ef65848de6656201fc4aa1d Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Sun, 10 Jul 2016 10:13:52 -0700
Subject: [PATCH] Warn user on requiring 1.4.1 or above version

---
 src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 54 +++++++++++++++++++++++++++++
 src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h  |  8 +++++
 2 files changed, 62 insertions(+)

diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
index 0c781d9..0fa69e6 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
@@ -69,6 +69,12 @@ static const struct Modes2Name rgModes2Name[] = {
     { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,       PX4FirmwarePlugin::takeoffFlightMode,       false,  true,   true },
 };
 
+PX4FirmwarePlugin::PX4FirmwarePlugin(void)
+    : _versionNotified(false)
+{
+
+}
+
 QList<VehicleComponent*> PX4FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
 {
     Q_UNUSED(vehicle);
@@ -365,3 +371,51 @@ bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
     return (vehicle->flightMode() == holdFlightMode || vehicle->flightMode() == takeoffFlightMode
             || vehicle->flightMode() == landingFlightMode);
 }
+
+bool PX4FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
+{
+    //-- Don't process messages to/from UDP Bridge. It doesn't suffer from these issues
+    if (message->compid == MAV_COMP_ID_UDP_BRIDGE) {
+        return true;
+    }
+
+    switch (message->msgid) {
+    case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
+        _handleAutopilotVersion(vehicle, message);
+        break;
+    }
+
+    return true;
+}
+
+void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message)
+{
+    Q_UNUSED(vehicle);
+
+    if (!_versionNotified) {
+        bool notifyUser = false;
+        int supportedMajorVersion = 1;
+        int supportedMinorVersion = 4;
+        int supportedPatchVersion = 1;
+
+        mavlink_autopilot_version_t version;
+        mavlink_msg_autopilot_version_decode(message, &version);
+
+        if (version.flight_sw_version != 0) {
+            int majorVersion, minorVersion, patchVersion;
+
+            majorVersion = (version.flight_sw_version >> (8*3)) & 0xFF;
+            minorVersion = (version.flight_sw_version >> (8*2)) & 0xFF;
+            patchVersion = (version.flight_sw_version >> (8*1)) & 0xFF;
+
+            notifyUser = majorVersion < supportedMajorVersion || minorVersion < supportedMinorVersion || patchVersion < supportedPatchVersion;
+        } else {
+            notifyUser = true;
+        }
+
+        if (notifyUser) {
+            _versionNotified = true;
+            qgcApp()->showMessage(QString("QGroundControl supports PX4 Pro firmware Version %1.%2.%3 and above. You are using a version prior to that which will lead to unpredictable results. Please upgrade your firmware.").arg(supportedMajorVersion).arg(supportedMinorVersion).arg(supportedPatchVersion));
+        }
+    }
+}
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
index a5c2229..6a712f9 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
@@ -23,6 +23,8 @@ class PX4FirmwarePlugin : public FirmwarePlugin
     Q_OBJECT
 
 public:
+    PX4FirmwarePlugin(void);
+
     // Overrides from FirmwarePlugin
 
     QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle) final;
@@ -50,6 +52,7 @@ public:
     QString     internalParameterMetaDataFile   (void) final { return QString(":/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml"); }
     void        getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
     QObject*    loadParameterMetaData           (const QString& metaDataFile);
+    bool        adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
 
     // Use these constants to set flight modes using setFlightMode method. Don't use hardcoded string names since the
     // names may change.
@@ -69,6 +72,11 @@ public:
     static const char* landingFlightMode;
     static const char* rtgsFlightMode;
     static const char* followMeFlightMode;
+
+private:
+    void _handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message);
+
+    bool _versionNotified;  ///< true: user notified over version issue
 };
 
 #endif