Merge pull request #3770 from DonLakeFlyer/VersionCheck

Warn user on requiring 1.4.1 or above version

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));
+        }
+    }
+}

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

src/comm/MockLink.cc

@@ -794,24 +794,34 @@ void MockLink::_respondWithAutopilotVersion(void)
 {
     mavlink_message_t msg;
 
-    uint8_t customVersion[8];
-    memset(customVersion, 0, sizeof(customVersion));
+    uint8_t customVersion[8] = { };
+    uint32_t flightVersion = 0;
+    if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+        flightVersion |= 3 << (8*3);
+        flightVersion |= 3 << (8*2);
+        flightVersion |= 0 << (8*1);
+        flightVersion |= FIRMWARE_VERSION_TYPE_DEV << (8*0);
+    } else if (_firmwareType == MAV_AUTOPILOT_PX4) {
+        flightVersion |= 1 << (8*3);
+        flightVersion |= 4 << (8*2);
+        flightVersion |= 1 << (8*1);
+        flightVersion |= FIRMWARE_VERSION_TYPE_DEV << (8*0);
+    }
 
-    // Only flight_sw_version is supported a this point
     mavlink_msg_autopilot_version_pack(_vehicleSystemId,
                                        _vehicleComponentId,
                                        &msg,
-                                       0,                                           // capabilities,
-                                       (1 << (8*3)) | FIRMWARE_VERSION_TYPE_DEV,    // flight_sw_version,
-                                       0,                                           // middleware_sw_version,
-                                       0,                                           // os_sw_version,
-                                       0,                                           // board_version,
-                                       (uint8_t *)&customVersion,                   // flight_custom_version,
-                                       (uint8_t *)&customVersion,                   // middleware_custom_version,
-                                       (uint8_t *)&customVersion,                   // os_custom_version,
-                                       0,                                           // vendor_id,
-                                       0,                                           // product_id,
-                                       0);                                          // uid
+                                       0,                               // capabilities,
+                                       flightVersion,                   // flight_sw_version,
+                                       0,                               // middleware_sw_version,
+                                       0,                               // os_sw_version,
+                                       0,                               // board_version,
+                                       (uint8_t *)&customVersion,       // flight_custom_version,
+                                       (uint8_t *)&customVersion,       // middleware_custom_version,
+                                       (uint8_t *)&customVersion,       // os_custom_version,
+                                       0,                               // vendor_id,
+                                       0,                               // product_id,
+                                       0);                              // uid
     respondWithMavlinkMessage(msg);
 }