Merge pull request #8861 from DonLakeFlyer/ConnectStateMachine

Connect state machine major rework

qgroundcontrol.pro

@@ -498,7 +498,9 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
         src/qgcunittest/TCPLinkTest.h \
         src/qgcunittest/TCPLoopBackServer.h \
         src/qgcunittest/UnitTest.h \
-        src/Vehicle/SendMavCommandTest.h \
+        src/Vehicle/RequestMessageTest.h \
+        src/Vehicle/SendMavCommandWithHandlerTest.h \
+        src/Vehicle/SendMavCommandWithSignallingTest.h \
         #src/qgcunittest/RadioConfigTest.h \
         #src/AnalyzeView/LogDownloadTest.h \
         #src/qgcunittest/FileDialogTest.h \
@@ -541,7 +543,9 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
         src/qgcunittest/TCPLoopBackServer.cc \
         src/qgcunittest/UnitTest.cc \
         src/qgcunittest/UnitTestList.cc \
-        src/Vehicle/SendMavCommandTest.cc \
+        src/Vehicle/RequestMessageTest.cc \
+        src/Vehicle/SendMavCommandWithHandlerTest.cc \
+        src/Vehicle/SendMavCommandWithSignallingTest.cc \
         #src/qgcunittest/RadioConfigTest.cc \
         #src/AnalyzeView/LogDownloadTest.cc \
         #src/qgcunittest/FileDialogTest.cc \
@@ -674,10 +678,13 @@ HEADERS += \
     src/SHPFileHelper.h \
     src/Terrain/TerrainQuery.h \
     src/TerrainTile.h \
-    src/Vehicle/ComponentInformation.h \
+    src/Vehicle/ComponentInformationManager.h \
     src/Vehicle/GPSRTKFactGroup.h \
+    src/Vehicle/InitialConnectStateMachine.h \
+    src/Vehicle/MAVLinkFTPManager.h \
     src/Vehicle/MAVLinkLogManager.h \
     src/Vehicle/MultiVehicleManager.h \
+    src/Vehicle/StateMachine.h \
     src/Vehicle/TerrainFactGroup.h \
     src/Vehicle/TerrainProtocolHandler.h \
     src/Vehicle/TrajectoryPoints.h \
@@ -887,10 +894,13 @@ SOURCES += \
     src/SHPFileHelper.cc \
     src/Terrain/TerrainQuery.cc \
     src/TerrainTile.cc\
-    src/Vehicle/ComponentInformation.cc \
+    src/Vehicle/ComponentInformationManager.cc \
     src/Vehicle/GPSRTKFactGroup.cc \
+    src/Vehicle/InitialConnectStateMachine.cc \
+    src/Vehicle/MAVLinkFTPManager.cc \
     src/Vehicle/MAVLinkLogManager.cc \
     src/Vehicle/MultiVehicleManager.cc \
+    src/Vehicle/StateMachine.cc \
     src/Vehicle/TerrainFactGroup.cc \
     src/Vehicle/TerrainProtocolHandler.cc \
     src/Vehicle/TrajectoryPoints.cc \

src/MissionManager/MavCmdInfoCommon.json

@@ -1005,6 +1005,8 @@
         { "id": 500, "rawName": "MAV_CMD_START_RX_PAIR", "friendlyName": "Bind Spektrum receiver" },
         { "id": 510, "rawName": "MAV_CMD_GET_MESSAGE_INTERVAL", "friendlyName": "Get message interval" },
         { "id": 511, "rawName": "MAV_CMD_SET_MESSAGE_INTERVAL", "friendlyName": "Set message interval" },
+        { "id": 512, "rawName": "MAV_CMD_REQUEST_MESSAGE" },
+        { "id": 519, "rawName": "MAV_CMD_REQUEST_PROTOCOL_VERSION" },
         { "id": 520, "rawName": "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES", "friendlyName": "Get capabilities" },
         {
             "id":           530,

src/Vehicle/ComponentInformation.cc

-/****************************************************************************
- *
- * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
- *
- * QGroundControl is licensed according to the terms in the file
- * COPYING.md in the root of the source code directory.
- *
- ****************************************************************************/
-
-#include "ComponentInformation.h"
-#include "Vehicle.h"
-
-QGC_LOGGING_CATEGORY(ComponentInformation, "ComponentInformation")
-
-ComponentInformation::ComponentInformation(Vehicle* vehicle, QObject* parent)
-    : QObject   (parent)
-    , _vehicle  (vehicle)
-{
-
-}
-
-void ComponentInformation::requestVersionMetaData(Vehicle* vehicle)
-{
-    vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
-                            MAV_CMD_REQUEST_MESSAGE,
-                            false,                                   // No error shown if fails
-                            MAVLINK_MSG_ID_COMPONENT_INFORMATION,
-                            COMP_METADATA_TYPE_VERSION);
-}
-
-bool ComponentInformation::requestParameterMetaData(Vehicle* vehicle)
-{
-    vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
-                            MAV_CMD_REQUEST_MESSAGE,
-                            false,                                   // No error shown if fails
-                            MAVLINK_MSG_ID_COMPONENT_INFORMATION,
-                            COMP_METADATA_TYPE_PARAMETER);
-    return true;
-}
-
-void ComponentInformation::componentInformationReceived(const mavlink_message_t &message)
-{
-    mavlink_component_information_t componentInformation;
-    mavlink_msg_component_information_decode(&message, &componentInformation);    
-    qDebug() << componentInformation.metadata_type << componentInformation.metadata_uri;
-}
-
-bool ComponentInformation::metaDataTypeSupported(COMP_METADATA_TYPE type)
-{
-    return _supportedMetaDataTypes.contains(type);
-}

src/Vehicle/ComponentInformationManager.cc

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "ComponentInformationManager.h"
+#include "Vehicle.h"
+
+QGC_LOGGING_CATEGORY(ComponentInformationManagerLog, "ComponentInformationManagerLog")
+
+ComponentInformationManager::StateFn ComponentInformationManager::_rgStates[]= {
+    ComponentInformationManager::_stateRequestCompInfoVersion,
+    ComponentInformationManager::_stateRequestCompInfoParam,
+    ComponentInformationManager::_stateRequestAllCompInfoComplete
+};
+
+int ComponentInformationManager::_cStates = sizeof(ComponentInformationManager::_rgStates) / sizeof(ComponentInformationManager::_rgStates[0]);
+
+RequestMetaDataTypeStateMachine::StateFn RequestMetaDataTypeStateMachine::_rgStates[]= {
+    RequestMetaDataTypeStateMachine::_stateRequestCompInfo,
+};
+
+int RequestMetaDataTypeStateMachine::_cStates = sizeof(RequestMetaDataTypeStateMachine::_rgStates) / sizeof(RequestMetaDataTypeStateMachine::_rgStates[0]);
+
+ComponentInformationManager::ComponentInformationManager(Vehicle* vehicle)
+    : _vehicle                  (vehicle)
+    , _requestTypeStateMachine  (this)
+{
+
+}
+
+int ComponentInformationManager::stateCount(void) const
+{
+    return _cStates;
+}
+
+const ComponentInformationManager::StateFn* ComponentInformationManager::rgStates(void) const
+{
+    return &_rgStates[0];
+}
+
+void ComponentInformationManager::_componentInformationReceived(const mavlink_message_t& message)
+{
+    mavlink_component_information_t componentInformation;
+    mavlink_msg_component_information_decode(&message, &componentInformation);
+
+    ComponentInformation_t* pCompInfo = nullptr;
+    switch (componentInformation.metadata_type) {
+    case COMP_METADATA_TYPE_VERSION:
+        qCDebug(ComponentInformationManagerLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_VERSION received";
+        _versionCompInfoAvailable = true;
+        pCompInfo = &_versionCompInfo;
+        break;
+    case COMP_METADATA_TYPE_PARAMETER:
+        qCDebug(ComponentInformationManagerLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_PARAMETER received";
+        _paramCompInfoAvailable = true;
+        pCompInfo = &_parameterCompInfo;
+        break;
+    }
+
+    if (pCompInfo) {
+        pCompInfo->metadataUID      = componentInformation.metadata_uid;
+        pCompInfo->metadataURI      = componentInformation.metadata_uri;
+        pCompInfo->translationUID   = componentInformation.translation_uid;
+        pCompInfo->translationURI   = componentInformation.translation_uri;
+    }
+}
+
+void ComponentInformationManager::requestAllComponentInformation(RequestAllCompleteFn requestAllCompletFn, void * requestAllCompleteFnData)
+{
+    _requestAllCompleteFn       = requestAllCompletFn;
+    _requestAllCompleteFnData   = requestAllCompleteFnData;
+    start();
+}
+
+void ComponentInformationManager::_stateRequestCompInfoVersion(StateMachine* stateMachine)
+{
+    ComponentInformationManager* compMgr = static_cast<ComponentInformationManager*>(stateMachine);
+    compMgr->_requestTypeStateMachine.request(COMP_METADATA_TYPE_VERSION);
+}
+
+void ComponentInformationManager::_stateRequestCompInfoComplete(void)
+{
+    advance();
+}
+
+void ComponentInformationManager::_stateRequestCompInfoParam(StateMachine* stateMachine)
+{
+    ComponentInformationManager* compMgr = static_cast<ComponentInformationManager*>(stateMachine);
+    compMgr->_requestTypeStateMachine.request(COMP_METADATA_TYPE_PARAMETER);
+}
+
+void ComponentInformationManager::_stateRequestAllCompInfoComplete(StateMachine* stateMachine)
+{
+    ComponentInformationManager* compMgr = static_cast<ComponentInformationManager*>(stateMachine);
+    (*compMgr->_requestAllCompleteFn)(compMgr->_requestAllCompleteFnData);
+    compMgr->_requestAllCompleteFn      = nullptr;
+    compMgr->_requestAllCompleteFnData  = nullptr;
+}
+
+RequestMetaDataTypeStateMachine::RequestMetaDataTypeStateMachine(ComponentInformationManager* compMgr)
+    : _compMgr(compMgr)
+{
+
+}
+
+void RequestMetaDataTypeStateMachine::request(COMP_METADATA_TYPE type)
+{
+    _type       = type;
+    _stateIndex = -1;
+
+    start();
+}
+
+int RequestMetaDataTypeStateMachine::stateCount(void) const
+{
+    return _cStates;
+}
+
+const RequestMetaDataTypeStateMachine::StateFn* RequestMetaDataTypeStateMachine::rgStates(void) const
+{
+    return &_rgStates[0];
+}
+
+void RequestMetaDataTypeStateMachine::statesCompleted(void) const
+{
+    _compMgr->_stateRequestCompInfoComplete();
+}
+
+QString RequestMetaDataTypeStateMachine::typeToString(void)
+{
+    return _type == COMP_METADATA_TYPE_VERSION ? "COMP_METADATA_TYPE_VERSION" : "COMP_METADATA_TYPE_PARAM";
+}
+
+static void _requestMessageResultHandler(void* resultHandlerData, MAV_RESULT result, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t &message)
+{
+    RequestMetaDataTypeStateMachine* requestMachine = static_cast<RequestMetaDataTypeStateMachine*>(resultHandlerData);
+
+    if (result == MAV_RESULT_ACCEPTED) {
+        requestMachine->compMgr()->_componentInformationReceived(message);
+    } else {
+        switch (failureCode) {
+        case Vehicle::RequestMessageFailureCommandError:
+            qCDebug(ComponentInformationManagerLog) << QStringLiteral("MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION %1 error(%2)").arg(requestMachine->typeToString()).arg(QGCMAVLink::mavResultToString(result));
+            break;
+        case Vehicle::RequestMessageFailureCommandNotAcked:
+            qCDebug(ComponentInformationManagerLog) << QStringLiteral("MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION %1 no response to command from vehicle").arg(requestMachine->typeToString());
+            break;
+        case Vehicle::RequestMessageFailureMessageNotReceived:
+            qCDebug(ComponentInformationManagerLog) << QStringLiteral("MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION %1 vehicle did not send requested message").arg(requestMachine->typeToString());
+            break;
+        default:
+            break;
+        }
+    }
+    requestMachine->advance();
+}
+
+void RequestMetaDataTypeStateMachine::_stateRequestCompInfo(StateMachine* stateMachine)
+{
+    RequestMetaDataTypeStateMachine*    requestMachine  = static_cast<RequestMetaDataTypeStateMachine*>(stateMachine);
+    Vehicle*                            vehicle         = requestMachine->_compMgr->vehicle();
+    LinkInterface*                      link            = vehicle->priorityLink();
+
+    if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
+        qCDebug(ComponentInformationManagerLog) << QStringLiteral("Skipping component information % 1 request due to link type").arg(requestMachine->typeToString());
+        stateMachine->advance();
+    } else {
+        qCDebug(ComponentInformationManagerLog) << "Requesting component information" << requestMachine->typeToString();
+        vehicle->requestMessage(
+                    _requestMessageResultHandler,
+                    stateMachine,
+                    MAV_COMP_ID_AUTOPILOT1,
+                    MAVLINK_MSG_ID_COMPONENT_INFORMATION,
+                    requestMachine->_type);
+    }
+}
+

src/Vehicle/ComponentInformationManager.h

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "QGCLoggingCategory.h"
+#include "QGCMAVLink.h"
+#include "StateMachine.h"
+
+Q_DECLARE_LOGGING_CATEGORY(ComponentInformationManagerLog)
+
+class Vehicle;
+class ComponentInformationManager;
+
+class RequestMetaDataTypeStateMachine : public StateMachine
+{
+public:
+    RequestMetaDataTypeStateMachine(ComponentInformationManager* compMgr);
+
+    void request(COMP_METADATA_TYPE type);
+    ComponentInformationManager* compMgr(void) { return _compMgr; }
+    QString typeToString(void);
+
+    // Overrides from StateMachine
+    int             stateCount      (void) const final;
+    const StateFn*  rgStates        (void) const final;
+    void            statesCompleted (void) const final;
+
+private:
+    static void _stateRequestCompInfo           (StateMachine* stateMachine);
+    static void _stateRequestMetaDataJson       (StateMachine* stateMachine);
+    static void _stateRequestTranslationJson    (StateMachine* stateMachine);
+
+    ComponentInformationManager*    _compMgr;
+    COMP_METADATA_TYPE              _type = COMP_METADATA_TYPE_VERSION;
+
+    static StateFn                  _rgStates[];
+    static int                      _cStates;
+};
+
+class ComponentInformationManager : public StateMachine
+{
+public:
+    ComponentInformationManager(Vehicle* vehicle);
+
+    typedef struct {
+        uint32_t        metadataUID;
+        QString         metadataURI;
+        uint32_t        translationUID;
+        QString         translationURI;
+    } ComponentInformation_t;
+
+    typedef void (*RequestAllCompleteFn)(void* requestAllCompleteFnData);
+
+    void        requestAllComponentInformation  (RequestAllCompleteFn requestAllCompletFn, void * requestAllCompleteFnData);
+    Vehicle*    vehicle                         (void) { return _vehicle; }
+
+    // These methods should only be called by RequestMetaDataTypeStateMachine
+    void _componentInformationReceived(const mavlink_message_t& message);
+    void _stateRequestCompInfoComplete(void);
+
+    // Overrides from StateMachine
+    int             stateCount  (void) const final;
+    const StateFn*  rgStates    (void) const final;
+
+private:
+    static void _stateRequestCompInfoVersion        (StateMachine* stateMachine);
+    static void _stateRequestCompInfoParam          (StateMachine* stateMachine);
+    static void _stateRequestAllCompInfoComplete    (StateMachine* stateMachine);
+
+    Vehicle*                        _vehicle                    = nullptr;
+    RequestMetaDataTypeStateMachine _requestTypeStateMachine;
+    bool                            _versionCompInfoAvailable   = false;
+    ComponentInformation_t          _versionCompInfo;
+    bool                            _paramCompInfoAvailable     = false;
+    ComponentInformation_t          _parameterCompInfo;
+    QList<COMP_METADATA_TYPE>       _supportedMetaDataTypes;
+    RequestAllCompleteFn            _requestAllCompleteFn       = nullptr;
+    void*                           _requestAllCompleteFnData   = nullptr;
+
+    static StateFn                  _rgStates[];
+    static int                      _cStates;
+};

src/Vehicle/InitialConnectStateMachine.cc

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "InitialConnectStateMachine.h"
+#include "Vehicle.h"
+#include "QGCCorePlugin.h"
+#include "QGCOptions.h"
+#include "FirmwarePlugin.h"
+#include "ParameterManager.h"
+#include "ComponentInformationManager.h"
+#include "MissionManager.h"
+
+QGC_LOGGING_CATEGORY(InitialConnectStateMachineLog, "InitialConnectStateMachineLog")
+
+const StateMachine::StateFn InitialConnectStateMachine::_rgStates[] = {
+    InitialConnectStateMachine::_stateRequestCapabilities,
+    InitialConnectStateMachine::_stateRequestProtocolVersion,
+    InitialConnectStateMachine::_stateRequestCompInfo,
+    InitialConnectStateMachine::_stateRequestParameters,
+    InitialConnectStateMachine::_stateRequestMission,
+    InitialConnectStateMachine::_stateRequestGeoFence,
+    InitialConnectStateMachine::_stateRequestRallyPoints,
+    InitialConnectStateMachine::_stateSignalInitialConnectComplete
+};
+
+const int InitialConnectStateMachine::_cStates = sizeof(InitialConnectStateMachine::_rgStates) / sizeof(InitialConnectStateMachine::_rgStates[0]);
+
+InitialConnectStateMachine::InitialConnectStateMachine(Vehicle* vehicle)
+    : _vehicle(vehicle)
+{
+
+}
+
+int InitialConnectStateMachine::stateCount(void) const
+{
+    return _cStates;
+}
+
+const InitialConnectStateMachine::StateFn* InitialConnectStateMachine::rgStates(void) const
+{
+    return &_rgStates[0];
+}
+
+void InitialConnectStateMachine::statesCompleted(void) const
+{
+
+}
+
+void InitialConnectStateMachine::_stateRequestCapabilities(StateMachine* stateMachine)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(stateMachine);
+    Vehicle*                    vehicle         = connectMachine->_vehicle;
+
+    LinkInterface* link = vehicle->priorityLink();
+    if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
+        qCDebug(InitialConnectStateMachineLog) << "Skipping capability request due to link type";
+        connectMachine->advance();
+    } else {
+        qCDebug(InitialConnectStateMachineLog) << "Requesting capabilities";
+        vehicle->_waitForMavlinkMessage(_waitForAutopilotVersionResultHandler, connectMachine, MAVLINK_MSG_ID_AUTOPILOT_VERSION, 1000);
+        vehicle->sendMavCommandWithHandler(_capabilitiesCmdResultHandler,
+                                           connectMachine,
+                                           MAV_COMP_ID_AUTOPILOT1,
+                                           MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
+                                           1);                                      // Request firmware version
+    }
+}
+
+void InitialConnectStateMachine::_capabilitiesCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, bool noResponsefromVehicle)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(resultHandlerData);
+    Vehicle*                    vehicle         = connectMachine->_vehicle;
+
+    if (result != MAV_RESULT_ACCEPTED) {
+        if (noResponsefromVehicle) {
+            qCDebug(InitialConnectStateMachineLog) << "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES no response from vehicle";
+        } else {
+            qCDebug(InitialConnectStateMachineLog) << QStringLiteral("MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES error(%1)").arg(result);
+        }
+        qCDebug(InitialConnectStateMachineLog) << "Setting no capabilities";
+        vehicle->_setCapabilities(0);
+        vehicle->_waitForMavlinkMessageClear();
+        connectMachine->advance();
+    }
+}
+
+void InitialConnectStateMachine::_waitForAutopilotVersionResultHandler(void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(resultHandlerData);
+    Vehicle*                    vehicle         = connectMachine->_vehicle;
+
+    if (noResponsefromVehicle) {
+        qCDebug(InitialConnectStateMachineLog) << "AUTOPILOT_VERSION timeout";
+        qCDebug(InitialConnectStateMachineLog) << "Setting no capabilities";
+        vehicle->_setCapabilities(0);
+    } else {
+        qCDebug(InitialConnectStateMachineLog) << "AUTOPILOT_VERSION received";
+
+        mavlink_autopilot_version_t autopilotVersion;
+        mavlink_msg_autopilot_version_decode(&message, &autopilotVersion);
+
+        vehicle->_uid = (quint64)autopilotVersion.uid;
+        emit vehicle->vehicleUIDChanged();
+
+        if (autopilotVersion.flight_sw_version != 0) {
+            int majorVersion, minorVersion, patchVersion;
+            FIRMWARE_VERSION_TYPE versionType;
+
+            majorVersion = (autopilotVersion.flight_sw_version >> (8*3)) & 0xFF;
+            minorVersion = (autopilotVersion.flight_sw_version >> (8*2)) & 0xFF;
+            patchVersion = (autopilotVersion.flight_sw_version >> (8*1)) & 0xFF;
+            versionType = (FIRMWARE_VERSION_TYPE)((autopilotVersion.flight_sw_version >> (8*0)) & 0xFF);
+            vehicle->setFirmwareVersion(majorVersion, minorVersion, patchVersion, versionType);
+        }
+
+        if (vehicle->px4Firmware()) {
+            // Lower 3 bytes is custom version
+            int majorVersion, minorVersion, patchVersion;
+            majorVersion = autopilotVersion.flight_custom_version[2];
+            minorVersion = autopilotVersion.flight_custom_version[1];
+            patchVersion = autopilotVersion.flight_custom_version[0];
+            vehicle->setFirmwareCustomVersion(majorVersion, minorVersion, patchVersion);
+
+            // PX4 Firmware stores the first 16 characters of the git hash as binary, with the individual bytes in reverse order
+            vehicle->_gitHash = "";
+            for (int i = 7; i >= 0; i--) {
+                vehicle->_gitHash.append(QString("%1").arg(autopilotVersion.flight_custom_version[i], 2, 16, QChar('0')));
+            }
+        } else {
+            // APM Firmware stores the first 8 characters of the git hash as an ASCII character string
+            char nullStr[9];
+            strncpy(nullStr, (char*)autopilotVersion.flight_custom_version, 8);
+            nullStr[8] = 0;
+            vehicle->_gitHash = nullStr;
+        }
+        if (vehicle->_toolbox->corePlugin()->options()->checkFirmwareVersion() && !vehicle->_checkLatestStableFWDone) {
+            vehicle->_checkLatestStableFWDone = true;
+            vehicle->_firmwarePlugin->checkIfIsLatestStable(vehicle);
+        }
+        emit vehicle->gitHashChanged(vehicle->_gitHash);
+
+        vehicle->_setCapabilities(autopilotVersion.capabilities);
+    }
+    connectMachine->advance();
+}
+
+void InitialConnectStateMachine::_stateRequestProtocolVersion(StateMachine* stateMachine)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(stateMachine);
+    Vehicle*                    vehicle         = connectMachine->_vehicle;
+    LinkInterface*              link            = vehicle->priorityLink();
+
+    if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
+        qCDebug(InitialConnectStateMachineLog) << "Skipping protocol version request due to link type";
+        connectMachine->advance();
+    } else {
+        qCDebug(InitialConnectStateMachineLog) << "Requesting protocol version";
+        vehicle->_waitForMavlinkMessage(_waitForProtocolVersionResultHandler, connectMachine, MAVLINK_MSG_ID_PROTOCOL_VERSION, 1000);
+        vehicle->sendMavCommandWithHandler(_protocolVersionCmdResultHandler,
+                                           connectMachine,
+                                           MAV_COMP_ID_AUTOPILOT1,
+                                           MAV_CMD_REQUEST_PROTOCOL_VERSION,
+                                           1);                                      // Request protocol version
+    }
+}
+
+void InitialConnectStateMachine::_protocolVersionCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, bool noResponsefromVehicle)
+{
+    if (result != MAV_RESULT_ACCEPTED) {
+        InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(resultHandlerData);
+        Vehicle*                    vehicle         = connectMachine->_vehicle;
+
+        if (noResponsefromVehicle) {
+            qCDebug(InitialConnectStateMachineLog) << "MAV_CMD_REQUEST_PROTOCOL_VERSION no response from vehicle";
+        } else {
+            qCDebug(InitialConnectStateMachineLog) << QStringLiteral("MAV_CMD_REQUEST_PROTOCOL_VERSION error(%1)").arg(result);
+        }
+
+        // _mavlinkProtocolRequestMaxProtoVersion stays at 0 to indicate unknown
+        vehicle->_mavlinkProtocolRequestComplete = true;
+        vehicle->_setMaxProtoVersionFromBothSources();
+        vehicle->_waitForMavlinkMessageClear();
+        connectMachine->advance();
+    }
+}
+
+void InitialConnectStateMachine::_waitForProtocolVersionResultHandler(void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(resultHandlerData);
+    Vehicle*                    vehicle         = connectMachine->_vehicle;
+
+    if (noResponsefromVehicle) {
+        qCDebug(InitialConnectStateMachineLog) << "PROTOCOL_VERSION timeout";
+        // The PROTOCOL_VERSION message didn't make it through the pipe from Vehicle->QGC.
+        // This means although the vehicle may support mavlink 2, the pipe does not.
+        qCDebug(InitialConnectStateMachineLog) << QStringLiteral("Setting _maxProtoVersion to 100 due to timeout on receiving PROTOCOL_VERSION message.");
+        vehicle->_mavlinkProtocolRequestMaxProtoVersion = 100;
+        vehicle->_mavlinkProtocolRequestComplete = true;
+        vehicle->_setMaxProtoVersionFromBothSources();
+    } else {
+        mavlink_protocol_version_t protoVersion;
+        mavlink_msg_protocol_version_decode(&message, &protoVersion);
+
+        qCDebug(InitialConnectStateMachineLog) << "PROTOCOL_VERSION received mav_version:" << protoVersion.max_version;
+        vehicle->_mavlinkProtocolRequestMaxProtoVersion = protoVersion.max_version;
+        vehicle->_mavlinkProtocolRequestComplete = true;
+        vehicle->_setMaxProtoVersionFromBothSources();
+    }
+    connectMachine->advance();
+}
+
+void InitialConnectStateMachine::_stateRequestCompInfo(StateMachine* stateMachine)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(stateMachine);
+    Vehicle*                    vehicle         = connectMachine->_vehicle;
+
+    qCDebug(InitialConnectStateMachineLog) << "_stateRequestCompInfo";
+    vehicle->_componentInformationManager->requestAllComponentInformation(_stateRequestCompInfoComplete, connectMachine);
+}
+
+void InitialConnectStateMachine::_stateRequestCompInfoComplete(void* requestAllCompleteFnData)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(requestAllCompleteFnData);
+
+    connectMachine->advance();
+}
+
+void InitialConnectStateMachine::_stateRequestParameters(StateMachine* stateMachine)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(stateMachine);
+    Vehicle*                    vehicle         = connectMachine->_vehicle;
+
+    qCDebug(InitialConnectStateMachineLog) << "_stateRequestParameters";
+    vehicle->_parameterManager->refreshAllParameters();
+}
+
+void InitialConnectStateMachine::_stateRequestMission(StateMachine* stateMachine)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(stateMachine);
+    Vehicle*                    vehicle         = connectMachine->_vehicle;
+    LinkInterface*              link            = vehicle->priorityLink();
+
+    if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
+        qCDebug(InitialConnectStateMachineLog) << "_stateRequestMission: Skipping first mission load request due to link type";
+        vehicle->_firstMissionLoadComplete();
+    } else {
+        qCDebug(InitialConnectStateMachineLog) << "_stateRequestMission";
+        vehicle->_missionManager->loadFromVehicle();
+    }
+}
+
+void InitialConnectStateMachine::_stateRequestGeoFence(StateMachine* stateMachine)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(stateMachine);
+    Vehicle*                    vehicle         = connectMachine->_vehicle;
+
+    qCDebug(InitialConnectStateMachineLog) << "_stateRequestGeoFence";
+    if (vehicle->_geoFenceManager->supported()) {
+        vehicle->_geoFenceManager->loadFromVehicle();
+    } else {
+        qCDebug(InitialConnectStateMachineLog) << "_stateRequestGeoFence: skipped due to no support";
+        vehicle->_firstGeoFenceLoadComplete();
+    }
+}
+
+void InitialConnectStateMachine::_stateRequestRallyPoints(StateMachine* stateMachine)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(stateMachine);
+    Vehicle*                    vehicle         = connectMachine->_vehicle;
+
+    qCDebug(InitialConnectStateMachineLog) << "_stateRequestRallyPoints";
+    if (vehicle->_rallyPointManager->supported()) {
+        vehicle->_rallyPointManager->loadFromVehicle();
+    } else {
+        qCDebug(InitialConnectStateMachineLog) << "_stateRequestRallyPoints: skipping due to no support";
+        vehicle->_firstRallyPointLoadComplete();
+    }
+}
+
+void InitialConnectStateMachine::_stateSignalInitialConnectComplete(StateMachine* stateMachine)
+{
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(stateMachine);
+    Vehicle*                    vehicle         = connectMachine->_vehicle;
+
+    qCDebug(InitialConnectStateMachineLog) << "Signalling initialConnectComplete";
+    emit vehicle->initialConnectComplete();
+}
+

src/Vehicle/InitialConnectStateMachine.h

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "StateMachine.h"
+#include "QGCMAVLink.h"
+#include "QGCLoggingCategory.h"
+
+Q_DECLARE_LOGGING_CATEGORY(InitialConnectStateMachineLog)
+
+class Vehicle;
+
+class InitialConnectStateMachine : public StateMachine
+{
+public:
+    InitialConnectStateMachine(Vehicle* vehicle);
+
+    // Overrides from StateMachine
+    int             stateCount      (void) const final;
+    const StateFn*  rgStates        (void) const final;
+    void            statesCompleted (void) const final;
+
+private:
+    static void _stateRequestCapabilities               (StateMachine* stateMachine);
+    static void _stateRequestProtocolVersion            (StateMachine* stateMachine);
+    static void _stateRequestCompInfo                   (StateMachine* stateMachine);
+    static void _stateRequestCompInfoComplete           (void* requestAllCompleteFnData);
+    static void _stateRequestParameters                 (StateMachine* stateMachine);
+    static void _stateRequestMission                    (StateMachine* stateMachine);
+    static void _stateRequestGeoFence                   (StateMachine* stateMachine);
+    static void _stateRequestRallyPoints                (StateMachine* stateMachine);
+    static void _stateSignalInitialConnectComplete      (StateMachine* stateMachine);
+
+    static void _capabilitiesCmdResultHandler           (void* resultHandlerData, int compId, MAV_RESULT result, bool noResponsefromVehicle);
+    static void _protocolVersionCmdResultHandler        (void* resultHandlerData, int compId, MAV_RESULT result, bool noResponsefromVehicle);
+
+    static void _waitForAutopilotVersionResultHandler   (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message);
+    static void _waitForProtocolVersionResultHandler    (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message);
+
+    Vehicle* _vehicle;
+
+    static const StateFn    _rgStates[];
+    static const int        _cStates;
+};

src/Vehicle/MAVLinkFTPManager.cc

+/****************************************************************************
+ *
+ *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+
+#include "MAVLinkFTPManager.h"
+#include "Vehicle.h"
+#include "QGCApplication.h"
+
+#include <QFile>
+#include <QDir>
+#include <string>
+
+QGC_LOGGING_CATEGORY(MAVLinkFTPManagerLog, "MAVLinkFTPManagerLog")
+
+MAVLinkFTPManager::MAVLinkFTPManager(Vehicle* vehicle)
+    : QObject   (vehicle)
+    , _vehicle  (vehicle)
+{
+    connect(&_ackTimer, &QTimer::timeout, this, &MAVLinkFTPManager::_ackTimeout);
+    
+    _lastOutgoingRequest.hdr.seqNumber = 0;
+
+    _systemIdServer = _vehicle->id();
+    
+    // Make sure we don't have bad structure packing
+    Q_ASSERT(sizeof(RequestHeader) == 12);
+}
+
+/// Respond to the Ack associated with the Open command with the next read command.
+void MAVLinkFTPManager::_openAckResponse(Request* openAck)
+{
+    qCDebug(MAVLinkFTPManagerLog) << QString("_openAckResponse: _currentOperation(%1) _readFileLength(%2)").arg(_currentOperation).arg(openAck->openFileLength);
+    
+	Q_ASSERT(_currentOperation == kCOOpenRead || _currentOperation == kCOOpenBurst);
+	_currentOperation = _currentOperation == kCOOpenRead ? kCORead : kCOBurst;
+    _activeSession = openAck->hdr.session;
+    
+    // File length comes back in data
+    Q_ASSERT(openAck->hdr.size == sizeof(uint32_t));
+    _downloadFileSize = openAck->openFileLength;
+    
+    // Start the sequence of read commands
+
+    _downloadOffset = 0;            // Start reading at beginning of file
+    _readFileAccumulator.clear();   // Start with an empty file
+    _missingDownloadedBytes = 0;
+    _downloadingMissingParts = false;
+    _missingData.clear();
+
+    Request request;
+    request.hdr.session = _activeSession;
+	Q_ASSERT(_currentOperation == kCORead || _currentOperation == kCOBurst);
+	request.hdr.opcode = _currentOperation == kCORead ? kCmdReadFile : kCmdBurstReadFile;
+    request.hdr.offset = _downloadOffset;
+    request.hdr.size = sizeof(request.data);
+
+    _sendRequest(&request);
+}
+
+/// request the next missing part of a (partially) downloaded file
+void MAVLinkFTPManager::_requestMissingData()
+{
+    if (_missingData.empty()) {
+        _downloadingMissingParts = false;
+        _missingDownloadedBytes = 0;
+        // there might be more data missing at the end
+        if ((uint32_t)_readFileAccumulator.length() != _downloadFileSize) {
+            _downloadOffset = _readFileAccumulator.length();
+            qCDebug(MAVLinkFTPManagerLog) << QString("_requestMissingData: missing parts done, downloadOffset(%1) downloadFileSize(%2)")
+                    .arg(_downloadOffset).arg(_downloadFileSize);
+        } else {
+            _closeDownloadSession(true);
+            return;
+        }
+    } else {
+        qCDebug(MAVLinkFTPManagerLog) << QString("_requestMissingData: offset(%1) size(%2)").arg(_missingData.head().offset).arg(_missingData.head().size);
+
+        _downloadOffset = _missingData.head().offset;
+    }
+
+    Request request;
+    _currentOperation = kCORead;
+    request.hdr.session = _activeSession;
+    request.hdr.opcode = kCmdReadFile;
+    request.hdr.offset = _downloadOffset;
+    request.hdr.size = sizeof(request.data);
+
+    _sendRequest(&request);
+}
+
+/// Closes out a download session by writing the file and doing cleanup.
+///     @param success true: successful download completion, false: error during download
+void MAVLinkFTPManager::_closeDownloadSession(bool success)
+{
+    qCDebug(MAVLinkFTPManagerLog) << QString("_closeDownloadSession: success(%1) missingBytes(%2)").arg(success).arg(_missingDownloadedBytes);
+    
+    _currentOperation = kCOIdle;
+    
+    if (success) {
+        if (_missingDownloadedBytes > 0 || (uint32_t)_readFileAccumulator.length() < _downloadFileSize) {
+            // we're not done yet: request the missing parts individually (either we had missing parts or
+            // the last (few) packets right before the EOF got dropped)
+            _downloadingMissingParts = true;
+            _requestMissingData();
+            return;
+        }
+
+        QString downloadFilePath = _readFileDownloadDir.absoluteFilePath(_readFileDownloadFilename);
+
+        QFile file(downloadFilePath);
+        if (!file.open(QIODevice::WriteOnly | QIODevice::Truncate)) {
+            _emitErrorMessage(tr("Unable to open local file for writing (%1)").arg(downloadFilePath));
+            return;
+        }
+
+        qint64 bytesWritten = file.write((const char *)_readFileAccumulator, _readFileAccumulator.length());
+        if (bytesWritten != _readFileAccumulator.length()) {
+            file.close();
+            _emitErrorMessage(tr("Unable to write data to local file (%1)").arg(downloadFilePath));
+            return;
+        }
+        file.close();
+
+        emit commandComplete();
+    }
+    
+    _readFileAccumulator.clear();
+    
+    // Close the open session
+    _sendResetCommand();
+}
+
+/// Closes out an upload session doing cleanup.
+///     @param success true: successful upload completion, false: error during download
+void MAVLinkFTPManager::_closeUploadSession(bool success)
+{
+    qCDebug(MAVLinkFTPManagerLog) << QString("_closeUploadSession: success(%1)").arg(success);
+    
+    _currentOperation = kCOIdle;
+    _writeFileAccumulator.clear();
+    _writeFileSize = 0;
+    
+    if (success) {
+        emit commandComplete();
+    }
+    
+    // Close the open session
+    _sendResetCommand();
+}
+
+/// Respond to the Ack associated with the Read or Stream commands.
+///		@param readFile: true: read file, false: stream file
+void MAVLinkFTPManager::_downloadAckResponse(Request* readAck, bool readFile)
+{
+    if (readAck->hdr.session != _activeSession) {
+        _closeDownloadSession(false /* failure */);
+        _emitErrorMessage(tr("Download: Incorrect session returned"));
+        return;
+    }
+
+    if (readAck->hdr.offset != _downloadOffset) {
+        if (readFile) {
+            _closeDownloadSession(false /* failure */);
+            _emitErrorMessage(tr("Download: Offset returned (%1) differs from offset requested/expected (%2)").arg(readAck->hdr.offset).arg(_downloadOffset));
+            return;
+        } else { // burst
+            if (readAck->hdr.offset < _downloadOffset) { // old data: ignore it
+                _setupAckTimeout();
+                return;
+            }
+            // keep track of missing data chunks
+            MissingData missingData;
+            missingData.offset = _downloadOffset;
+            missingData.size = readAck->hdr.offset - _downloadOffset;
+            _missingData.push_back(missingData);
+            _missingDownloadedBytes += readAck->hdr.offset - _downloadOffset;
+            qCDebug(MAVLinkFTPManagerLog) << QString("_downloadAckResponse: missing data: offset(%1) size(%2)").arg(missingData.offset).arg(missingData.size);
+            _downloadOffset = readAck->hdr.offset;
+            _readFileAccumulator.resize(_downloadOffset); // placeholder for the missing data
+        }
+    }
+    
+    qCDebug(MAVLinkFTPManagerLog) << QString("_downloadAckResponse: offset(%1) size(%2) burstComplete(%3)").arg(readAck->hdr.offset).arg(readAck->hdr.size).arg(readAck->hdr.burstComplete);
+
+    if (_downloadingMissingParts) {
+        Q_ASSERT(_missingData.head().offset == _downloadOffset);
+        _missingDownloadedBytes -= readAck->hdr.size;
+        _readFileAccumulator.replace(_downloadOffset, readAck->hdr.size, (const char*)readAck->data, readAck->hdr.size);
+        if (_missingData.head().size <= readAck->hdr.size) {
+            _missingData.pop_front();
+        } else {
+            _missingData.head().size -= readAck->hdr.size;
+            _missingData.head().offset += readAck->hdr.size;
+        }
+    } else {
+        _downloadOffset += readAck->hdr.size;
+        _readFileAccumulator.append((const char*)readAck->data, readAck->hdr.size);
+    }
+    
+    if (_downloadFileSize != 0) {
+        emit commandProgress(100 * ((float)(_readFileAccumulator.length() - _missingDownloadedBytes) / (float)_downloadFileSize));
+    }
+
+    if (_downloadingMissingParts) {
+        _requestMissingData();
+    } else if (readFile || readAck->hdr.burstComplete) {
+        // Possibly still more data to read, send next read request
+
+        Request request;
+        request.hdr.session = _activeSession;
+        request.hdr.opcode = readFile ? kCmdReadFile : kCmdBurstReadFile;
+        request.hdr.offset = _downloadOffset;
+        request.hdr.size = 0;
+
+        _sendRequest(&request);
+    } else if (!readFile) {
+        // Streaming, so next ack should come automatically
+        _setupAckTimeout();
+    }
+}
+
+/// @brief Respond to the Ack associated with the List command.
+void MAVLinkFTPManager::_listAckResponse(Request* listAck)
+{
+    if (listAck->hdr.offset != _listOffset) {
+        // this is a real error (directory listing is synchronous), no need to retransmit
+        _currentOperation = kCOIdle;
+        _emitErrorMessage(tr("List: Offset returned (%1) differs from offset requested (%2)").arg(listAck->hdr.offset).arg(_listOffset));
+        return;
+    }
+
+    uint8_t offset = 0;
+    uint8_t cListEntries = 0;
+    uint8_t cBytes = listAck->hdr.size;
+
+    // parse filenames out of the buffer
+    while (offset < cBytes) {
+        const char * ptr = ((const char *)listAck->data) + offset;
+
+        // get the length of the name
+        uint8_t cBytesLeft = cBytes - offset;
+        uint8_t nlen = static_cast<uint8_t>(strnlen(ptr, cBytesLeft));
+        if ((*ptr == 'S' && nlen > 1) || (*ptr != 'S' && nlen < 2)) {
+            _currentOperation = kCOIdle;
+            _emitErrorMessage(tr("Incorrectly formed list entry: '%1'").arg(ptr));
+            return;
+        } else if (nlen == cBytesLeft) {
+            _currentOperation = kCOIdle;
+            _emitErrorMessage(tr("Missing NULL termination in list entry"));
+            return;
+        }
+
+        // Returned names are prepended with D for directory, F for file, S for skip
+        if (*ptr == 'F' || *ptr == 'D') {
+            // put it in the view
+            _emitListEntry(ptr);
+        } else if (*ptr == 'S') {
+            // do nothing
+        } else {
+            qDebug() << "unknown entry" << ptr;
+        }
+
+        // account for the name + NUL
+        offset += nlen + 1;
+
+        cListEntries++;
+    }
+
+    if (listAck->hdr.size == 0 || cListEntries == 0) {
+        // Directory is empty, we're done
+        Q_ASSERT(listAck->hdr.opcode == kRspAck);
+        _currentOperation = kCOIdle;
+        emit commandComplete();
+    } else {
+        // Possibly more entries to come, need to keep trying till we get EOF
+        _currentOperation = kCOList;
+        _listOffset += cListEntries;
+        _sendListCommand();
+    }
+}
+
+/// @brief Respond to the Ack associated with the create command.
+void MAVLinkFTPManager::_createAckResponse(Request* createAck)
+{
+    qCDebug(MAVLinkFTPManagerLog) << "_createAckResponse";
+    
+    _currentOperation = kCOWrite;
+    _activeSession = createAck->hdr.session;
+
+    // Start the sequence of write commands from the beginning of the file
+
+    _writeOffset = 0;
+    _writeSize = 0;
+    
+    _writeFileDatablock();
+}
+
+/// @brief Respond to the Ack associated with the write command.
+void MAVLinkFTPManager::_writeAckResponse(Request* writeAck)
+{
+    if(_writeOffset + _writeSize >= _writeFileSize){
+        _closeUploadSession(true /* success */);
+        return;
+    }
+
+    if (writeAck->hdr.session != _activeSession) {
+        _closeUploadSession(false /* failure */);
+        _emitErrorMessage(tr("Write: Incorrect session returned"));
+        return;
+    }
+
+    if (writeAck->hdr.offset != _writeOffset) {
+        _closeUploadSession(false /* failure */);
+        _emitErrorMessage(tr("Write: Offset returned (%1) differs from offset requested (%2)").arg(writeAck->hdr.offset).arg(_writeOffset));
+        return;
+    }
+
+    if (writeAck->hdr.size != sizeof(uint32_t)) {
+        _closeUploadSession(false /* failure */);
+        _emitErrorMessage(tr("Write: Returned invalid size of write size data"));
+        return;
+    }
+
+
+    if( writeAck->writeFileLength !=_writeSize) {
+        _closeUploadSession(false /* failure */);
+        _emitErrorMessage(tr("Write: Size returned (%1) differs from size requested (%2)").arg(writeAck->writeFileLength).arg(_writeSize));
+        return;
+    }
+
+    _writeFileDatablock();
+}
+
+/// @brief Send next write file data block.
+void MAVLinkFTPManager::_writeFileDatablock(void)
+{
+    if (_writeOffset + _writeSize >= _writeFileSize){
+        _closeUploadSession(true /* success */);
+        return;
+    }
+
+    _writeOffset += _writeSize;
+
+    Request request;
+    request.hdr.session = _activeSession;
+    request.hdr.opcode = kCmdWriteFile;
+    request.hdr.offset = _writeOffset;
+
+    if(_writeFileSize -_writeOffset > sizeof(request.data) )
+        _writeSize = sizeof(request.data);
+    else
+        _writeSize = _writeFileSize - _writeOffset;
+
+    request.hdr.size = _writeSize;
+
+    memcpy(request.data, &_writeFileAccumulator.data()[_writeOffset], _writeSize);
+
+    _sendRequest(&request);
+}
+
+void MAVLinkFTPManager::receiveMessage(mavlink_message_t message)
+{
+    // receiveMessage is signalled will all mavlink messages so we need to filter everything else out but ours.
+    if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
+        return;
+    }
+
+    mavlink_file_transfer_protocol_t data;
+    mavlink_msg_file_transfer_protocol_decode(&message, &data);
+	
+    // Make sure we are the target system
+    if (data.target_system != _systemIdQGC) {
+        qDebug() << "Received MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL with incorrect target_system:" <<  data.target_system << "expected:" << _systemIdQGC;
+        return;
+    }
+    
+    Request* request = (Request*)&data.payload[0];
+
+    uint16_t incomingSeqNumber = request->hdr.seqNumber;
+    
+    // Make sure we have a good sequence number
+    uint16_t expectedSeqNumber = _lastOutgoingRequest.hdr.seqNumber + 1;
+
+    // ignore old/reordered packets (handle wrap-around properly)
+    if ((uint16_t)((expectedSeqNumber - 1) - incomingSeqNumber) < (std::numeric_limits<uint16_t>::max()/2)) {
+        qDebug() << "Received old packet: expected seq:" << expectedSeqNumber << "got:" << incomingSeqNumber;
+        return;
+    }
+
+    _clearAckTimeout();
+    
+    qCDebug(MAVLinkFTPManagerLog) << "receiveMessage" << request->hdr.opcode;
+	
+    if (incomingSeqNumber != expectedSeqNumber) {
+        bool doAbort = true;
+        switch (_currentOperation) {
+            case kCOBurst: // burst download drops are handled in _downloadAckResponse()
+                doAbort = false;
+                break;
+            case kCORead:
+                _closeDownloadSession(false /* failure */);
+                break;
+            
+            case kCOWrite:
+                _closeUploadSession(false /* failure */);
+                break;
+                
+            case kCOOpenRead:
+            case kCOOpenBurst:
+            case kCOCreate:
+                // We could have an open session hanging around
+                _currentOperation = kCOIdle;
+                _sendResetCommand();
+                break;
+                
+            default:
+                // Don't need to do anything special
+                _currentOperation = kCOIdle;
+                break;
+        }
+        
+        if (doAbort) {
+            _emitErrorMessage(tr("Bad sequence number on received message: expected(%1) received(%2)").arg(expectedSeqNumber).arg(incomingSeqNumber));
+            return;
+        }
+    }
+    
+    // Move past the incoming sequence number for next request
+    _lastOutgoingRequest.hdr.seqNumber = incomingSeqNumber;
+
+    if (request->hdr.opcode == kRspAck) {
+        switch (request->hdr.req_opcode) {
+			case kCmdListDirectory:
+				_listAckResponse(request);
+				break;
+				
+			case kCmdOpenFileRO:
+            case kCmdOpenFileWO:
+				_openAckResponse(request);
+				break;
+				
+			case kCmdReadFile:
+				_downloadAckResponse(request, true /* read file */);
+				break;
+				
+			case kCmdBurstReadFile:
+				_downloadAckResponse(request, false /* stream file */);
+				break;
+				
+            case kCmdCreateFile:
+                _createAckResponse(request);
+                break;
+                
+            case kCmdWriteFile:
+                _writeAckResponse(request);
+                break;
+                
+			default:
+				// Ack back from operation which does not require additional work
+				_currentOperation = kCOIdle;
+				break;
+		}
+    } else if (request->hdr.opcode == kRspNak) {
+        uint8_t errorCode = request->data[0];
+
+        // Nak's normally have 1 byte of data for error code, except for kErrFailErrno which has additional byte for errno
+        Q_ASSERT((errorCode == kErrFailErrno && request->hdr.size == 2) || request->hdr.size == 1);
+        
+        _currentOperation = kCOIdle;
+
+        if (request->hdr.req_opcode == kCmdListDirectory && errorCode == kErrEOF) {
+            // This is not an error, just the end of the list loop
+            emit commandComplete();
+            return;
+        } else if ((request->hdr.req_opcode == kCmdReadFile || request->hdr.req_opcode == kCmdBurstReadFile) && errorCode == kErrEOF) {
+            // This is not an error, just the end of the download loop
+            _closeDownloadSession(true /* success */);
+            return;
+        } else if (request->hdr.req_opcode == kCmdCreateFile) {
+            _emitErrorMessage(tr("Nak received creating file, error: %1").arg(errorString(request->data[0])));
+            return;
+        } else if (request->hdr.req_opcode == kCmdCreateDirectory) {
+            _emitErrorMessage(tr("Nak received creating directory, error: %1").arg(errorString(request->data[0])));
+            return;
+        } else {
+            // Generic Nak handling
+            if (request->hdr.req_opcode == kCmdReadFile || request->hdr.req_opcode == kCmdBurstReadFile) {
+                // Nak error during download loop, download failed
+                _closeDownloadSession(false /* failure */);
+            } else if (request->hdr.req_opcode == kCmdWriteFile) {
+                // Nak error during upload loop, upload failed
+                _closeUploadSession(false /* failure */);
+            }
+            _emitErrorMessage(tr("Nak received, error: %1").arg(errorString(request->data[0])));
+        }
+    } else {
+        // Note that we don't change our operation state. If something goes wrong beyond this, the operation
+        // will time out.
+        _emitErrorMessage(tr("Unknown opcode returned from server: %1").arg(request->hdr.opcode));
+    }
+}
+
+void MAVLinkFTPManager::listDirectory(const QString& dirPath)
+{
+    if (_currentOperation != kCOIdle) {
+        _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete."));
+        return;
+    }
+
+    _dedicatedLink = _vehicle->priorityLink();
+    if (!_dedicatedLink) {
+        _emitErrorMessage(tr("Command not sent. No Vehicle links."));
+        return;
+    }
+
+    // initialise the lister
+    _listPath = dirPath;
+    _listOffset = 0;
+    _currentOperation = kCOList;
+
+    // and send the initial request
+    _sendListCommand();
+}
+
+void MAVLinkFTPManager::_fillRequestWithString(Request* request, const QString& str)
+{
+    strncpy((char *)&request->data[0], str.toStdString().c_str(), sizeof(request->data));
+    request->hdr.size = static_cast<uint8_t>(strnlen((const char *)&request->data[0], sizeof(request->data)));
+}
+
+void MAVLinkFTPManager::_sendListCommand(void)
+{
+    Request request;
+
+    request.hdr.session = 0;
+    request.hdr.opcode = kCmdListDirectory;
+    request.hdr.offset = _listOffset;
+    request.hdr.size = 0;
+
+    _fillRequestWithString(&request, _listPath);
+
+    qCDebug(MAVLinkFTPManagerLog) << "listDirectory: path:" << _listPath << "offset:" <<  _listOffset;
+    
+    _sendRequest(&request);
+}
+
+void MAVLinkFTPManager::downloadPath(const QString& vehicleFilePath, const QDir& downloadDir)
+{
+    if (_currentOperation != kCOIdle) {
+        _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete."));
+        return;
+    }
+
+    _dedicatedLink = _vehicle->priorityLink();
+    if (!_dedicatedLink) {
+        _emitErrorMessage(tr("Command not sent. No Vehicle links."));
+        return;
+    }
+    
+    qCDebug(MAVLinkFTPManagerLog) << "downloadPath vehicleFilePath:" << vehicleFilePath << "to:" << downloadDir;
+    _downloadWorker(vehicleFilePath, downloadDir, true /* read file */);
+}
+
+void MAVLinkFTPManager::streamPath(const QString& from, const QDir& downloadDir)
+{
+    if (_currentOperation != kCOIdle) {
+        _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete."));
+        return;
+    }
+
+    _dedicatedLink = _vehicle->priorityLink();
+    if (!_dedicatedLink) {
+        _emitErrorMessage(tr("Command not sent. No Vehicle links."));
+        return;
+    }
+    
+    qCDebug(MAVLinkFTPManagerLog) << "streamPath from:" << from << "to:" << downloadDir;
+	_downloadWorker(from, downloadDir, false /* stream file */);
+}
+
+void MAVLinkFTPManager::_downloadWorker(const QString& vehicleFilePath, const QDir& downloadDir, bool readFile)
+{
+    if (vehicleFilePath.isEmpty()) {
+		return;
+	}
+	
+	_readFileDownloadDir.setPath(downloadDir.absolutePath());
+	
+	// We need to strip off the file name from the fully qualified path. We can't use the usual QDir
+	// routines because this path does not exist locally.
+	int i;
+    for (i=vehicleFilePath.size()-1; i>=0; i--) {
+        if (vehicleFilePath[i] == '/') {
+			break;
+		}
+	}
+	i++; // move past slash
+    _readFileDownloadFilename = vehicleFilePath.right(vehicleFilePath.size() - i);
+	
+	_currentOperation = readFile ? kCOOpenRead : kCOOpenBurst;
+	
+	Request request;
+	request.hdr.session = 0;
+	request.hdr.opcode = kCmdOpenFileRO;
+	request.hdr.offset = 0;
+	request.hdr.size = 0;
+    _fillRequestWithString(&request, vehicleFilePath);
+	_sendRequest(&request);
+}
+
+/// @brief Uploads the specified file.
+///     @param toPath File in UAS to upload to, fully qualified path
+///     @param uploadFile Local file to upload from
+void MAVLinkFTPManager::uploadPath(const QString& toPath, const QFileInfo& uploadFile)
+{
+    if(_currentOperation != kCOIdle){
+        _emitErrorMessage(tr("UAS File manager busy. Try again later"));
+        return;
+    }
+
+    _dedicatedLink = _vehicle->priorityLink();
+    if (!_dedicatedLink) {
+        _emitErrorMessage(tr("Command not sent. No Vehicle links."));
+        return;
+    }
+
+    if (toPath.isEmpty()) {
+        return;
+    }
+
+    if (!uploadFile.isReadable()){
+        _emitErrorMessage(tr("File (%1) is not readable for upload").arg(uploadFile.path()));
+        return;
+    }
+
+    QFile file(uploadFile.absoluteFilePath());
+    if (!file.open(QIODevice::ReadOnly)) {
+            _emitErrorMessage(tr("Unable to open local file for upload (%1)").arg(uploadFile.absoluteFilePath()));
+            return;
+        }
+
+    _writeFileAccumulator = file.readAll();
+    _writeFileSize = _writeFileAccumulator.size();
+
+    file.close();
+
+    if (_writeFileAccumulator.size() == 0) {
+        _emitErrorMessage(tr("Unable to read data from local file (%1)").arg(uploadFile.absoluteFilePath()));
+        return;
+    }
+
+    _currentOperation = kCOCreate;
+
+    Request request;
+    request.hdr.session = 0;
+    request.hdr.opcode = kCmdCreateFile;
+    request.hdr.offset = 0;
+    request.hdr.size = 0;
+    _fillRequestWithString(&request, toPath + "/" + uploadFile.fileName());
+    _sendRequest(&request);
+}
+
+void MAVLinkFTPManager::createDirectory(const QString& directory)
+{
+    if(_currentOperation != kCOIdle){
+        _emitErrorMessage(tr("UAS File manager busy. Try again later"));
+        return;
+    }
+
+    _currentOperation = kCOCreateDir;
+
+    Request request;
+    request.hdr.session = 0;
+    request.hdr.opcode = kCmdCreateDirectory;
+    request.hdr.offset = 0;
+    request.hdr.size = 0;
+    _fillRequestWithString(&request, directory);
+    _sendRequest(&request);
+}
+
+QString MAVLinkFTPManager::errorString(uint8_t errorCode)
+{
+    switch(errorCode) {
+        case kErrNone:
+            return QString("no error");
+        case kErrFail:
+            return QString("unknown error");
+        case kErrEOF:
+            return QString("read beyond end of file");
+        case kErrUnknownCommand:
+            return QString("unknown command");
+        case kErrFailErrno:
+            return QString("command failed");
+        case kErrInvalidDataSize:
+            return QString("invalid data size");
+        case kErrInvalidSession:
+            return QString("invalid session");
+        case kErrNoSessionsAvailable:
+            return QString("no sessions available");
+        case kErrFailFileExists:
+            return QString("File already exists on target");
+        case kErrFailFileProtected:
+            return QString("File is write protected");
+        default:
+            return QString("unknown error code");
+    }
+}
+
+/// @brief Sends a command which only requires an opcode and no additional data
+///     @param opcode Opcode to send
+///     @param newOpState State to put state machine into
+/// @return TRUE: command sent, FALSE: command not sent, waiting for previous command to finish
+bool MAVLinkFTPManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpState)
+{
+    if (_currentOperation != kCOIdle) {
+        // Can't have multiple commands in play at the same time
+        return false;
+    }
+
+    Request request;
+    request.hdr.session = 0;
+    request.hdr.opcode = opcode;
+    request.hdr.offset = 0;
+    request.hdr.size = 0;
+
+    _currentOperation = newOpState;
+
+    _sendRequest(&request);
+
+    return true;
+}
+
+/// @brief Starts the ack timeout timer
+void MAVLinkFTPManager::_setupAckTimeout(void)
+{
+    qCDebug(MAVLinkFTPManagerLog) << "_setupAckTimeout";
+
+    Q_ASSERT(!_ackTimer.isActive());
+
+    _ackNumTries = 0;
+    _ackTimer.setSingleShot(false);
+    _ackTimer.start(ackTimerTimeoutMsecs);
+}
+
+/// @brief Clears the ack timeout timer
+void MAVLinkFTPManager::_clearAckTimeout(void)
+{
+    qCDebug(MAVLinkFTPManagerLog) << "_clearAckTimeout";
+    _ackTimer.stop();
+}
+
+/// @brief Called when ack timeout timer fires
+void MAVLinkFTPManager::_ackTimeout(void)
+{
+    qCDebug(MAVLinkFTPManagerLog) << "_ackTimeout";
+    
+    if (++_ackNumTries <= ackTimerMaxRetries) {
+        qCDebug(MAVLinkFTPManagerLog) << "ack timeout - retrying";
+        if (_currentOperation == kCOBurst) {
+            // for burst downloads try to initiate a new burst
+            Request request;
+            request.hdr.session = _activeSession;
+            request.hdr.opcode = kCmdBurstReadFile;
+            request.hdr.offset = _downloadOffset;
+            request.hdr.size = 0;
+            request.hdr.seqNumber = ++_lastOutgoingRequest.hdr.seqNumber;
+
+            _sendRequestNoAck(&request);
+        } else {
+            _sendRequestNoAck(&_lastOutgoingRequest);
+        }
+        return;
+    }
+
+    _clearAckTimeout();
+
+    // Make sure to set _currentOperation state before emitting error message. Code may respond
+    // to error message signal by sending another command, which will fail if state is not back
+    // to idle. FileView UI works this way with the List command.
+
+    switch (_currentOperation) {
+        case kCORead:
+        case kCOBurst:
+            _closeDownloadSession(false /* failure */);
+            _emitErrorMessage(tr("Timeout waiting for ack: Download failed"));
+            break;
+            
+        case kCOOpenRead:
+        case kCOOpenBurst:
+            _currentOperation = kCOIdle;
+            _emitErrorMessage(tr("Timeout waiting for ack: Download failed"));
+            _sendResetCommand();
+            break;
+            
+        case kCOCreate:
+            _currentOperation = kCOIdle;
+            _emitErrorMessage(tr("Timeout waiting for ack: Upload failed"));
+            _sendResetCommand();
+            break;
+            
+        case kCOWrite:
+            _closeUploadSession(false /* failure */);
+            _emitErrorMessage(tr("Timeout waiting for ack: Upload failed"));
+            break;
+			
+        default:
+        {
+            OperationState currentOperation = _currentOperation;
+            _currentOperation = kCOIdle;
+            _emitErrorMessage(QString("Timeout waiting for ack: Command failed (%1)").arg(currentOperation));
+        }
+            break;
+    }
+}
+
+void MAVLinkFTPManager::_sendResetCommand(void)
+{
+    Request request;
+    request.hdr.opcode = kCmdResetSessions;
+    request.hdr.size = 0;
+    _sendRequest(&request);
+}
+
+void MAVLinkFTPManager::_emitErrorMessage(const QString& msg)
+{
+    qCDebug(MAVLinkFTPManagerLog) << "Error:" << msg;
+    emit commandError(msg);
+}
+
+void MAVLinkFTPManager::_emitListEntry(const QString& entry)
+{
+    qCDebug(MAVLinkFTPManagerLog) << "_emitListEntry" << entry;
+    emit listEntry(entry);
+}
+
+/// @brief Sends the specified Request out to the UAS.
+void MAVLinkFTPManager::_sendRequest(Request* request)
+{
+
+    _setupAckTimeout();
+    
+    request->hdr.seqNumber = ++_lastOutgoingRequest.hdr.seqNumber;
+    // store the current request
+    if (request->hdr.size <= sizeof(request->data)) {
+        memcpy(&_lastOutgoingRequest, request, sizeof(RequestHeader) + request->hdr.size);
+    } else {
+        qCCritical(MAVLinkFTPManagerLog) << "request length too long:" << request->hdr.size;
+    }
+    
+    qCDebug(MAVLinkFTPManagerLog) << "_sendRequest opcode:" << request->hdr.opcode << "seqNumber:" << request->hdr.seqNumber;
+    
+    if (_systemIdQGC == 0) {
+        _systemIdQGC = qgcApp()->toolbox()->mavlinkProtocol()->getSystemId();
+    }
+    _sendRequestNoAck(request);
+}
+
+/// @brief Sends the specified Request out to the UAS, without ack timeout handling
+void MAVLinkFTPManager::_sendRequestNoAck(Request* request)
+{
+    mavlink_message_t message;
+
+    // Unit testing code can end up here without _dedicateLink set since it tests inidividual commands.
+    LinkInterface* link;
+    if (_dedicatedLink) {
+        link = _dedicatedLink;
+    } else {
+        link = _vehicle->priorityLink();
+    }
+    
+    mavlink_msg_file_transfer_protocol_pack_chan(_systemIdQGC,       // QGC System ID
+                                                 0,                  // QGC Component ID
+                                                 link->mavlinkChannel(),
+                                                 &message,           // Mavlink Message to pack into
+                                                 0,                  // Target network
+                                                 _systemIdServer,    // Target system
+                                                 0,                  // Target component
+                                                 (uint8_t*)request); // Payload
+    
+    _vehicle->sendMessageOnLinkThreadSafe(link, message);
+}

src/Vehicle/MAVLinkFTPManager.h

+/****************************************************************************
+ *
+ *   (c) 2009-2018 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+
+#pragma once
+
+#include <QObject>
+#include <QDir>
+#include <QTimer>
+#include <QQueue>
+
+#include "UASInterface.h"
+#include "QGCLoggingCategory.h"
+
+#ifdef __GNUC__
+#define PACKED_STRUCT( __Declaration__ ) __Declaration__ __attribute__((packed))
+#else
+#define PACKED_STRUCT( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) )
+#endif
+
+
+Q_DECLARE_LOGGING_CATEGORY(MAVLinkFTPManagerLog)
+
+class Vehicle;
+
+class MAVLinkFTPManager : public QObject
+{
+    Q_OBJECT
+
+public:
+    MAVLinkFTPManager(Vehicle* vehicle);
+
+    /// These methods are only used for testing purposes.
+    bool _sendCmdTestAck(void) { return _sendOpcodeOnlyCmd(kCmdNone, kCOAck); };
+    bool _sendCmdTestNoAck(void) { return _sendOpcodeOnlyCmd(kCmdTestNoAck, kCOAck); };
+
+    /// Timeout in msecs to wait for an Ack time come back. This is public so we can write unit tests which wait long enough
+    /// for the MAVLinkFTPManager to timeout.
+    static const int ackTimerTimeoutMsecs = 50;
+
+    static const int ackTimerMaxRetries = 6;
+
+    /// Downloads the specified file.
+    ///     @param vehicleFilePath File to download from UAS, fully qualified path
+    ///     @param downloadDir Local directory to download file to
+    void downloadPath(const QString& vehicleFilePath, const QDir& downloadDir);
+
+    /// Stream downloads the specified file.
+    ///     @param from File to download from UAS, fully qualified path
+    ///     @param downloadDir Local directory to download file to
+    void streamPath(const QString& from, const QDir& downloadDir);
+
+    /// Lists the specified directory. Emits listEntry signal for each entry, followed by listComplete signal.
+    ///		@param dirPath Fully qualified path to list
+    void listDirectory(const QString& dirPath);
+
+    /// Upload the specified file to the specified location
+    void uploadPath(const QString& toPath, const QFileInfo& uploadFile);
+
+    /// Create a remote directory
+    void createDirectory(const QString& directory);
+
+signals:
+    // Signals associated with the listDirectory method
+
+    /// Signalled to indicate a new directory entry was received.
+    void listEntry(const QString& entry);
+
+    // Signals associated with all commands
+
+    /// Signalled after a command has completed
+    void commandComplete(void);
+
+    /// Signalled when an error occurs during a command. In this case a commandComplete signal will
+    /// not be sent.
+    void commandError(const QString& msg);
+
+    /// Signalled during a lengthy command to show progress
+    ///     @param value Amount of progress: 0.0 = none, 1.0 = complete
+    void commandProgress(int value);
+
+public slots:
+    void receiveMessage(mavlink_message_t message);
+
+private slots:
+    void _ackTimeout(void);
+
+private:
+    /// @brief This is the fixed length portion of the protocol data.
+    /// This needs to be packed, because it's typecasted from mavlink_file_transfer_protocol_t.payload, which starts
+    /// at a 3 byte offset, causing an unaligned access to seq_number and offset
+    PACKED_STRUCT(
+            typedef struct _RequestHeader
+    {
+                uint16_t    seqNumber;      ///< sequence number for message
+                uint8_t     session;        ///< Session id for read and write commands
+                uint8_t     opcode;         ///< Command opcode
+                uint8_t     size;           ///< Size of data
+                uint8_t     req_opcode;     ///< Request opcode returned in kRspAck, kRspNak message
+                uint8_t     burstComplete;  ///< Only used if req_opcode=kCmdBurstReadFile - 1: set of burst packets complete, 0: More burst packets coming.
+                uint8_t     padding;        ///< 32 bit aligment padding
+                uint32_t    offset;         ///< Offsets for List and Read commands
+            }) RequestHeader;
+
+    PACKED_STRUCT(
+            typedef struct _Request
+    {
+                RequestHeader hdr;
+
+                // We use a union here instead of just casting (uint32_t)&payload[0] to not break strict aliasing rules
+                union {
+                    // The entire Request must fit into the payload member of the mavlink_file_transfer_protocol_t structure. We use as many leftover bytes
+                    // after we use up space for the RequestHeader for the data portion of the Request.
+                    uint8_t data[sizeof(((mavlink_file_transfer_protocol_t*)0)->payload) - sizeof(RequestHeader)];
+
+                    // File length returned by Open command
+                    uint32_t openFileLength;
+
+                    // Length of file chunk written by write command
+                    uint32_t writeFileLength;
+                };
+            }) Request;
+
+    enum Opcode
+    {
+        kCmdNone,				///< ignored, always acked
+        kCmdTerminateSession,	///< Terminates open Read session
+        kCmdResetSessions,		///< Terminates all open Read sessions
+        kCmdListDirectory,		///< List files in <path> from <offset>
+        kCmdOpenFileRO,			///< Opens file at <path> for reading, returns <session>
+        kCmdReadFile,			///< Reads <size> bytes from <offset> in <session>
+        kCmdCreateFile,			///< Creates file at <path> for writing, returns <session>
+        kCmdWriteFile,			///< Writes <size> bytes to <offset> in <session>
+        kCmdRemoveFile,			///< Remove file at <path>
+        kCmdCreateDirectory,	///< Creates directory at <path>
+        kCmdRemoveDirectory,	///< Removes Directory at <path>, must be empty
+        kCmdOpenFileWO,			///< Opens file at <path> for writing, returns <session>
+        kCmdTruncateFile,		///< Truncate file at <path> to <offset> length
+        kCmdRename,				///< Rename <path1> to <path2>
+        kCmdCalcFileCRC32,		///< Calculate CRC32 for file at <path>
+        kCmdBurstReadFile,      ///< Burst download session file
+
+        kRspAck = 128,          ///< Ack response
+        kRspNak,                ///< Nak response
+
+        // Used for testing only, not part of protocol
+        kCmdTestNoAck,          ///< ignored, ack not sent back, should timeout waiting for ack
+    };
+
+    /// @brief Error codes returned in Nak response PayloadHeader.data[0].
+    enum ErrorCode
+    {
+        kErrNone,
+        kErrFail,                   ///< Unknown failure
+        kErrFailErrno,              ///< errno sent back in PayloadHeader.data[1]
+        kErrInvalidDataSize,		///< PayloadHeader.size is invalid
+        kErrInvalidSession,         ///< Session is not currently open
+        kErrNoSessionsAvailable,	///< All available Sessions in use
+        kErrEOF,                    ///< Offset past end of file for List and Read commands
+        kErrUnknownCommand,          ///< Unknown command opcode
+        kErrFailFileExists,         ///< File exists already
+        kErrFailFileProtected       ///< File is write protected
+    };
+
+    enum OperationState
+    {
+        kCOIdle,		// not doing anything
+        kCOAck,			// waiting for an Ack
+        kCOList,		// waiting for List response
+        kCOOpenRead,    // waiting for Open response followed by Read download
+        kCOOpenBurst,   // waiting for Open response, followed by Burst download
+        kCORead,		// waiting for Read response
+        kCOBurst,		// waiting for Burst response
+        kCOWrite,       // waiting for Write response
+        kCOCreate,      // waiting for Create response
+        kCOCreateDir,   // waiting for Create Directory response
+    };
+
+    bool _sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpState);
+    void _setupAckTimeout(void);
+    void _clearAckTimeout(void);
+    void _emitErrorMessage(const QString& msg);
+    void _emitListEntry(const QString& entry);
+    void _sendRequest(Request* request);
+    void _sendRequestNoAck(Request* request);
+    void _fillRequestWithString(Request* request, const QString& str);
+    void _openAckResponse(Request* openAck);
+    void _downloadAckResponse(Request* readAck, bool readFile);
+    void _listAckResponse(Request* listAck);
+    void _createAckResponse(Request* createAck);
+    void _writeAckResponse(Request* writeAck);
+    void _writeFileDatablock(void);
+    void _sendListCommand(void);
+    void _sendResetCommand(void);
+    void _closeDownloadSession(bool success);
+    void _closeUploadSession(bool success);
+    void _downloadWorker(const QString& vehicleFilePath, const QDir& downloadDir, bool readFile);
+    void _requestMissingData();
+
+    static QString errorString(uint8_t errorCode);
+
+    OperationState  _currentOperation = kCOIdle;              ///< Current operation of state machine
+    QTimer          _ackTimer;                      ///< Used to signal a timeout waiting for an ack
+    int             _ackNumTries = 0;                   ///< current number of tries
+
+    Vehicle*        _vehicle = nullptr;
+    LinkInterface*  _dedicatedLink = nullptr; ///< Link to use for communication
+
+    Request  _lastOutgoingRequest; ///< contains the last outgoing packet
+
+    unsigned    _listOffset;    ///< offset for the current List operation
+    QString     _listPath;      ///< path for the current List operation
+
+    uint8_t     _activeSession = 0;             ///< currently active session, 0 for none
+
+    uint32_t    _readOffset;                ///< current read offset
+
+    uint32_t    _writeOffset;               ///< current write offset
+    uint32_t    _writeSize;                 ///< current write data size
+    uint32_t    _writeFileSize;             ///< Size of file being uploaded
+    QByteArray  _writeFileAccumulator;      ///< Holds file being uploaded
+
+    struct MissingData {
+        uint32_t offset;
+        uint32_t size;
+    };
+    uint32_t    _downloadOffset;            ///< current download offset
+    uint32_t    _missingDownloadedBytes = 0;    ///< number of missing bytes for burst download
+    QQueue<MissingData> _missingData;       ///< missing chunks of downloaded file (for burst downloads)
+    bool        _downloadingMissingParts = false;   ///< true if we are currently downloading missing parts
+    QByteArray  _readFileAccumulator;       ///< Holds file being downloaded
+    QDir        _readFileDownloadDir;       ///< Directory to download file to
+    QString     _readFileDownloadFilename;  ///< Filename (no path) for download file
+    uint32_t    _downloadFileSize;          ///< Size of file being downloaded
+
+    uint8_t     _systemIdQGC = 0;               ///< System ID for QGC
+    uint8_t     _systemIdServer = 0;            ///< System ID for server
+
+    // We give MockLinkFileServer friend access so that it can use the data structures and opcodes
+    // to build a mock mavlink file server for testing.
+    friend class MockLinkFileServer;
+};
+

src/Vehicle/RequestMessageTest.cc

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "RequestMessageTest.h"
+#include "MultiVehicleManager.h"
+#include "QGCApplication.h"
+#include "MockLink.h"
+
+RequestMessageTest::TestCase_t RequestMessageTest::_rgTestCases[] = {
+    {  MockLink::FailRequestMessageNone,                                MAV_RESULT_ACCEPTED,    Vehicle::RequestMessageNoFailure,                   false },
+    {  MockLink::FailRequestMessageCommandAcceptedMsgNotSent,           MAV_RESULT_FAILED,      Vehicle::RequestMessageFailureMessageNotReceived,   false },
+    {  MockLink::FailRequestMessageCommandUnsupported,                  MAV_RESULT_UNSUPPORTED, Vehicle::RequestMessageFailureCommandError,         false },
+    {  MockLink::FailRequestMessageCommandNoResponse,                   MAV_RESULT_FAILED,      Vehicle::RequestMessageFailureCommandNotAcked,      false },
+    //{  MockLink::FailRequestMessageCommandAcceptedSecondAttempMsgSent,  MAV_RESULT_FAILED,      Vehicle::RequestMessageNoFailure,   false },
+};
+
+void RequestMessageTest::_requestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message)
+{
+    TestCase_t* testCase = static_cast<TestCase_t*>(resultHandlerData);
+
+    testCase->resultHandlerCalled = true;
+    QCOMPARE((int)testCase->expectedCommandResult, (int)commandResult);
+    QCOMPARE((int)testCase->expectedFailureCode, (int)failureCode);
+    if (testCase->expectedFailureCode == Vehicle::RequestMessageNoFailure) {
+        QVERIFY(message.msgid == MAVLINK_MSG_ID_DEBUG);
+    }
+}
+
+void RequestMessageTest::_testCaseWorker(TestCase_t& testCase)
+{
+    _connectMockLinkNoInitialConnectSequence();
+
+    MultiVehicleManager*    vehicleMgr  = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle*                vehicle     = vehicleMgr->activeVehicle();
+
+    _mockLink->setRequestMessageFailureMode(testCase.failureMode);
+    vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_ALL, MAVLINK_MSG_ID_DEBUG);
+    for (int i=0; i<100; i++) {
+        QTest::qWait(100);
+        if (testCase.resultHandlerCalled) {
+            break;
+        }
+    }
+    QVERIFY(testCase.resultHandlerCalled);
+
+    _disconnectMockLink();
+}
+
+void RequestMessageTest::_performTestCases(void)
+{
+    int index = 0;
+    for (TestCase_t& testCase: _rgTestCases) {
+        qDebug() << "Testing case" << index++;
+        _testCaseWorker(testCase);
+    }
+}

src/Vehicle/RequestMessageTest.h

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "UnitTest.h"
+#include "MockLink.h"
+#include "Vehicle.h"
+
+class RequestMessageTest : public UnitTest
+{
+    Q_OBJECT
+
+signals:
+    void resultHandlerCalled(void);
+    
+private slots:
+    void _performTestCases(void);
+
+private:
+    typedef struct {
+        MockLink::RequestMessageFailureMode_t               failureMode;
+        MAV_RESULT                                          expectedCommandResult;
+        Vehicle::RequestMessageResultHandlerFailureCode_t   expectedFailureCode;
+        bool                                                resultHandlerCalled;
+    } TestCase_t;
+
+    void _testCaseWorker(TestCase_t& testCase);
+
+    static void _requestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message);
+
+    bool _resultHandlerCalled;
+
+    static TestCase_t _rgTestCases[];
+};

src/Vehicle/SendMavCommandWithHandlerTest.cc

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "SendMavCommandWithHandlerTest.h"
+#include "MultiVehicleManager.h"
+#include "QGCApplication.h"
+#include "MockLink.h"
+
+SendMavCommandWithHandlerTest::TestCase_t SendMavCommandWithHandlerTest::_rgTestCases[] = {
+    {  MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED,           MAV_RESULT_ACCEPTED, false },
+    {  MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED,             MAV_RESULT_ACCEPTED, false },
+    {  MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED,   MAV_RESULT_ACCEPTED, false },
+    {  MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED,     MAV_RESULT_ACCEPTED, false },
+    {  MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE,                      MAV_RESULT_ACCEPTED, false },
+};
+
+void SendMavCommandWithHandlerTest::_mavCmdResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle)
+{
+    TestCase_t* testCase = static_cast<TestCase_t*>(resultHandlerData);
+
+    QVERIFY(compId == MAV_COMP_ID_AUTOPILOT1);
+    QCOMPARE(testCase->expectedCommandResult, commandResult);
+    QCOMPARE(testCase->expectedNoResponseFromVehicle, noResponsefromVehicle);
+}
+
+void SendMavCommandWithHandlerTest::_testCaseWorker(TestCase_t& testCase)
+{
+    _connectMockLinkNoInitialConnectSequence();
+
+    MultiVehicleManager*    vehicleMgr  = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle*                vehicle     = vehicleMgr->activeVehicle();
+
+    vehicle->sendMavCommandWithHandler(_mavCmdResultHandler, &testCase, MAV_COMP_ID_ALL, testCase.command);
+
+    _disconnectMockLink();
+}
+
+void SendMavCommandWithHandlerTest::_performTestCases(void)
+{
+    int index = 0;
+    for (TestCase_t& testCase: _rgTestCases) {
+        qDebug() << "Testing case" << index++;
+        _testCaseWorker(testCase);
+    }
+}

src/Vehicle/ComponentInformation.h → src/Vehicle/SendMavCommandWithHandlerTest.h

@@ -9,30 +9,25 @@
 
 #pragma once
 
-#include "QGCLoggingCategory.h"
-#include "QGCMAVLink.h"
+#include "UnitTest.h"
 
-#include <QObject>
-
-Q_DECLARE_LOGGING_CATEGORY(ComponentInformationLog)
-
-class Vehicle;
-
-class ComponentInformation : public QObject
+class SendMavCommandWithHandlerTest : public UnitTest
 {
     Q_OBJECT
+    
+private slots:
+    void _performTestCases(void);
 
-public:
-    ComponentInformation(Vehicle* vehicle, QObject* parent = nullptr);
+private:
+    typedef struct {
+        MAV_CMD     command;
+        MAV_RESULT  expectedCommandResult;
+        bool        expectedNoResponseFromVehicle;
+    } TestCase_t;
 
-    void requestVersionMetaData         (Vehicle* vehicle);
-    bool requestParameterMetaData       (Vehicle* vehicle);
-    void componentInformationReceived   (const mavlink_message_t& message);
-    bool metaDataTypeSupported          (COMP_METADATA_TYPE type);
+    void _testCaseWorker(TestCase_t& testCase);
 
-private:
-    Vehicle*                    _vehicle =                  nullptr;
-    bool                        _versionMetaDataAvailable = false;
-    bool                        _paramMetaDataAvailable =   false;
-    QList<COMP_METADATA_TYPE>   _supportedMetaDataTypes;
+    static void _mavCmdResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle);
+
+    static TestCase_t _rgTestCases[];
 };

src/Vehicle/SendMavCommandTest.cc → src/Vehicle/SendMavCommandWithSignallingTest.cc

@@ -8,49 +8,49 @@
  ****************************************************************************/
 
 
-#include "SendMavCommandTest.h"
+#include "SendMavCommandWithSignallingTest.h"
 #include "MultiVehicleManager.h"
 #include "QGCApplication.h"
+#include "MockLink.h"
 
-void SendMavCommandTest::_noFailure(void)
+void SendMavCommandWithSignallingTest::_noFailure(void)
 {
-    _connectMockLink(MAV_AUTOPILOT_ARDUPILOTMEGA);
+    _connectMockLinkNoInitialConnectSequence();
 
-    MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
-    Vehicle* vehicle = vehicleMgr->activeVehicle();
-    QVERIFY(vehicle);
+    MultiVehicleManager*    vehicleMgr  = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle*                vehicle     = vehicleMgr->activeVehicle();
+    QSignalSpy              spyResult(vehicle, &Vehicle::mavCommandResult);
 
-    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_1, true /* showError */);
+    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, true /* showError */);
 
-    QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
     QCOMPARE(spyResult.wait(10000), true);
     QList<QVariant> arguments = spyResult.takeFirst();
     QCOMPARE(arguments.count(), 5);
     QCOMPARE(arguments.at(0).toInt(), vehicle->id());
-    QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_1);
+    QCOMPARE(arguments.at(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED);
     QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_ACCEPTED);
     QCOMPARE(arguments.at(4).toBool(), false);
 }
 
-void SendMavCommandTest::_failureShowError(void)
+void SendMavCommandWithSignallingTest::_failureShowError(void)
 {
     // Will pop error about request failure
     setExpectedMessageBox(QMessageBox::Ok);
 
-    _connectMockLink(MAV_AUTOPILOT_ARDUPILOTMEGA);
+    _connectMockLinkNoInitialConnectSequence();
 
     MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
     Vehicle* vehicle = vehicleMgr->activeVehicle();
     QVERIFY(vehicle);
 
-    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_2, true /* showError */);
+    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED, true /* showError */);
 
     QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
     QCOMPARE(spyResult.wait(10000), true);
     QList<QVariant> arguments = spyResult.takeFirst();
     QCOMPARE(arguments.count(), 5);
     QCOMPARE(arguments.at(0).toInt(), vehicle->id());
-    QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_2);
+    QCOMPARE(arguments.at(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED);
     QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
     QCOMPARE(arguments.at(4).toBool(), false);
 
@@ -58,65 +58,65 @@ void SendMavCommandTest::_failureShowError(void)
     checkExpectedMessageBox();
 }
 
-void SendMavCommandTest::_failureNoShowError(void)
+void SendMavCommandWithSignallingTest::_failureNoShowError(void)
 {
-    _connectMockLink(MAV_AUTOPILOT_ARDUPILOTMEGA);
+    _connectMockLinkNoInitialConnectSequence();
 
     MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
     Vehicle* vehicle = vehicleMgr->activeVehicle();
     QVERIFY(vehicle);
 
-    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_2, false /* showError */);
+    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED, false /* showError */);
 
     QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
     QCOMPARE(spyResult.wait(10000), true);
     QList<QVariant> arguments = spyResult.takeFirst();
     QCOMPARE(arguments.count(), 5);
     QCOMPARE(arguments.at(0).toInt(), vehicle->id());
-    QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_2);
+    QCOMPARE(arguments.at(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED);
     QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
     QCOMPARE(arguments.at(4).toBool(), false);
 }
 
-void SendMavCommandTest::_noFailureAfterRetry(void)
+void SendMavCommandWithSignallingTest::_noFailureAfterRetry(void)
 {
-    _connectMockLink(MAV_AUTOPILOT_ARDUPILOTMEGA);
+    _connectMockLinkNoInitialConnectSequence();
 
     MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
     Vehicle* vehicle = vehicleMgr->activeVehicle();
     QVERIFY(vehicle);
 
-    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_3, true /* showError */);
+    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED, true /* showError */);
 
     QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
     QCOMPARE(spyResult.wait(10000), true);
     QList<QVariant> arguments = spyResult.takeFirst();
     QCOMPARE(arguments.count(), 5);
     QCOMPARE(arguments.at(0).toInt(), vehicle->id());
-    QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_3);
+    QCOMPARE(arguments.at(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED);
     QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_ACCEPTED);
     QCOMPARE(arguments.at(4).toBool(), false);
 }
 
-void SendMavCommandTest::_failureAfterRetry(void)
+void SendMavCommandWithSignallingTest::_failureAfterRetry(void)
 {
     // Will pop error about request failure
     setExpectedMessageBox(QMessageBox::Ok);
 
-    _connectMockLink(MAV_AUTOPILOT_ARDUPILOTMEGA);
+    _connectMockLinkNoInitialConnectSequence();
 
     MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
     Vehicle* vehicle = vehicleMgr->activeVehicle();
     QVERIFY(vehicle);
 
-    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_4, true /* showError */);
+    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED, true /* showError */);
 
     QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
     QCOMPARE(spyResult.wait(10000), true);
     QList<QVariant> arguments = spyResult.takeFirst();
     QCOMPARE(arguments.count(), 5);
     QCOMPARE(arguments.at(0).toInt(), vehicle->id());
-    QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_4);
+    QCOMPARE(arguments.at(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED);
     QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
     QCOMPARE(arguments.at(4).toBool(), false);
 
@@ -124,28 +124,43 @@ void SendMavCommandTest::_failureAfterRetry(void)
     checkExpectedMessageBox();
 }
 
-void SendMavCommandTest::_failureAfterNoReponse(void)
+void SendMavCommandWithSignallingTest::_failureAfterNoReponse(void)
 {
     // Will pop error about request failure
     setExpectedMessageBox(QMessageBox::Ok);
 
-    _connectMockLink(MAV_AUTOPILOT_ARDUPILOTMEGA);
+    _connectMockLinkNoInitialConnectSequence();
 
     MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
     Vehicle* vehicle = vehicleMgr->activeVehicle();
     QVERIFY(vehicle);
 
-    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_5, true /* showError */);
+    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, true /* showError */);
 
     QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
     QCOMPARE(spyResult.wait(20000), true);
     QList<QVariant> arguments = spyResult.takeFirst();
     QCOMPARE(arguments.count(), 5);
     QCOMPARE(arguments.at(0).toInt(), vehicle->id());
-    QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_5);
+    QCOMPARE(arguments.at(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE);
     QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
     QCOMPARE(arguments.at(4).toBool(), true);
 
     // User should have been notified
     checkExpectedMessageBox();
 }
+
+void SendMavCommandWithSignallingTest::_unexpectedAck(void)
+{
+    _connectMockLinkNoInitialConnectSequence();
+
+    MultiVehicleManager*    vehicleMgr  = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle*                vehicle     = vehicleMgr->activeVehicle();
+    QSignalSpy              spyResult(vehicle, &Vehicle::mavCommandResult);
+
+    _mockLink->sendUnexpectedCommandAck(MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, MAV_RESULT_ACCEPTED);
+    QCOMPARE(spyResult.wait(100), false);
+
+    _mockLink->sendUnexpectedCommandAck(MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, MAV_RESULT_FAILED);
+    QCOMPARE(spyResult.wait(100), false);
+}

src/Vehicle/SendMavCommandTest.h → src/Vehicle/SendMavCommandWithSignallingTest.h

@@ -7,25 +7,22 @@
  *
  ****************************************************************************/
 
-
-#ifndef SendMavCommandTest_H
-#define SendMavCommandTest_H
+#pragma once
 
 #include "UnitTest.h"
 
-class SendMavCommandTest : public UnitTest
+class SendMavCommandWithSignallingTest : public UnitTest
 {
     Q_OBJECT
     
 private slots:
-    void _noFailure(void);
-    void _failureShowError(void);
-    void _failureNoShowError(void);
-    void _noFailureAfterRetry(void);
-    void _failureAfterRetry(void);
-    void _failureAfterNoReponse(void);
+    void _noFailure             (void);
+    void _failureShowError      (void);
+    void _failureNoShowError    (void);
+    void _noFailureAfterRetry   (void);
+    void _failureAfterRetry     (void);
+    void _failureAfterNoReponse (void);
+    void _unexpectedAck         (void);
 
 private:
 };
-
-#endif

src/Vehicle/StateMachine.cc

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "StateMachine.h"
+
+StateMachine::StateMachine(void)
+{
+
+}
+
+void StateMachine::start(void)
+{
+    _active = true;
+    advance();
+}
+
+void StateMachine::advance(void)
+{
+    if (_active) {
+        _stateIndex++;
+        if (_stateIndex < stateCount()) {
+            (*rgStates()[_stateIndex])(this);
+        } else {
+            _active = false;
+            statesCompleted();
+        }
+    }
+}
+
+void StateMachine::move(StateFn stateFn)
+{
+    if (_active) {
+        for (int i=0; i<stateCount(); i++) {
+            if (rgStates()[i] == stateFn) {
+                _stateIndex = i;
+                (*rgStates()[_stateIndex])(this);
+                break;
+            }
+        }
+    }
+}
+
+void StateMachine::statesCompleted(void) const
+{
+
+}

src/Vehicle/StateMachine.h

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+class StateMachine
+{
+public:
+    typedef void (*StateFn)(StateMachine* stateMachine);
+
+    StateMachine(void);
+
+    /// Start the state machine with the first step
+    void start(void);
+
+    /// Advance the state machine to the next state and call the state function
+    void advance(void);
+
+    /// Move the state machine to the specified state and call the state function
+    void move(StateFn stateFn);
+
+    /// @return The number of states in the rgStates array
+    virtual int stateCount(void) const = 0;
+
+    /// @return Array of states to execute
+    virtual const StateFn* rgStates(void) const = 0;
+
+    /// Called when all states have completed
+    virtual void statesCompleted(void) const;
+
+protected:
+    bool    _active = false;
+    int     _stateIndex  = -1;
+};

src/Vehicle/Vehicle.cc

@@ -45,6 +45,9 @@
 #include "QGCGeo.h"
 #include "TerrainProtocolHandler.h"
 #include "ParameterManager.h"
+#include "MAVLinkFTPManager.h"
+#include "ComponentInformationManager.h"
+#include "InitialConnectStateMachine.h"
 
 #if defined(QGC_AIRMAP_ENABLED)
 #include "AirspaceVehicleManager.h"
@@ -93,18 +96,6 @@ const char* Vehicle::_distanceSensorFactGroupName =     "distanceSensor";
 const char* Vehicle::_estimatorStatusFactGroupName =    "estimatorStatus";
 const char* Vehicle::_terrainFactGroupName =            "terrain";
 
-const Vehicle::StateFn Vehicle::_rgInitialConnectStates[_cInitialConnectStateEntries] = {
-    Vehicle::_stateRequestCapabilities,
-    Vehicle::_stateRequestProtocolVersion,
-    Vehicle::_stateRequestCompInfoVersion,
-    Vehicle::_stateRequestCompInfoParam,
-    Vehicle::_stateRequestParameters,
-    Vehicle::_stateRequestMission,
-    Vehicle::_stateRequestGeoFence,
-    Vehicle::_stateRequestRallyPoints,
-    Vehicle::_stateSignalInitialConnectComplete
-};
-
 // Standard connected vehicle
 Vehicle::Vehicle(LinkInterface*             link,
                  int                        vehicleId,
@@ -199,7 +190,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _orbitActive(false)
     , _pidTuningTelemetryMode(false)
     , _pidTuningWaitingForRates(false)
-    , _componentInformation(this)
     , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
     , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
     , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)
@@ -277,7 +267,11 @@ Vehicle::Vehicle(LinkInterface*             link,
     connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged,  this, &Vehicle::_handleTextMessage);
     connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageReceived,      this, &Vehicle::_handletextMessageReceived);
 
-    _advanceInitialConnectStateMachine();
+    // MAV_TYPE_GENERIC is used by unit test for creating a vehicle which doesn't do the connect sequence. This
+    // way we can test the methods that are used within the connect sequence.
+    if (!qgcApp()->runningUnitTests() || _vehicleType != MAV_TYPE_GENERIC) {
+        _initialConnectStateMachine->start();
+    }
 
     _firmwarePlugin->initializeVehicle(this);
     for(auto& factName: factNames()) {
@@ -386,7 +380,6 @@ Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
     , _orbitActive(false)
     , _pidTuningTelemetryMode(false)
     , _pidTuningWaitingForRates(false)
-    , _componentInformation(this)
     , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
     , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
     , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)
@@ -472,6 +465,10 @@ void Vehicle::_commonInit()
     _parameterManager = new ParameterManager(this);
     connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
 
+    _componentInformationManager    = new ComponentInformationManager(this);
+    _initialConnectStateMachine     = new InitialConnectStateMachine(this);
+    _mavlinkFTPManager              = new MAVLinkFTPManager(this);
+
     _objectAvoidance = new VehicleObjectAvoidance(this, this);
 
     // GeoFenceManager needs to access ParameterManager so make sure to create after
@@ -719,17 +716,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
         return;
     }
 
-    if (_waitForMavlinkMessageId != 0) {
-        if (_waitForMavlinkMessageId == message.msgid) {
-            WaitForMavlinkMessageMessageHandler handler = _waitForMavlinkMessageMessageHandler;
-            _waitForMavlinkMessageClear();
-            (*handler)(this, message);
-        } else if (_waitForMavlinkMessageElapsed.elapsed() > _waitForMavlinkMessageTimeoutMsecs) {
-            WaitForMavlinkMessageTimeoutHandler handler = _waitForMavlinkMessageTimeoutHandler;
-            _waitForMavlinkMessageClear();
-            (*handler)(this);
-        }
-    }
+    _waitForMavlinkMessageMessageReceived(message);
 
     switch (message.msgid) {
     case MAVLINK_MSG_ID_HOME_POSITION:
@@ -2573,13 +2560,13 @@ void Vehicle::_updateFlightTime()
 void Vehicle::_firstMissionLoadComplete()
 {
     disconnect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_firstMissionLoadComplete);
-    _advanceInitialConnectStateMachine();
+    _initialConnectStateMachine->advance();
 }
 
 void Vehicle::_firstGeoFenceLoadComplete()
 {
     disconnect(_geoFenceManager, &GeoFenceManager::loadComplete,   this, &Vehicle::_firstGeoFenceLoadComplete);
-    _advanceInitialConnectStateMachine();
+    _initialConnectStateMachine->advance();
 }
 
 void Vehicle::_firstRallyPointLoadComplete()
@@ -2587,19 +2574,21 @@ void Vehicle::_firstRallyPointLoadComplete()
     disconnect(_rallyPointManager, &RallyPointManager::loadComplete,   this, &Vehicle::_firstRallyPointLoadComplete);
     _initialPlanRequestComplete = true;
     emit initialPlanRequestCompleteChanged(true);
-    _advanceInitialConnectStateMachine();
+    _initialConnectStateMachine->advance();
 }
 
 void Vehicle::_parametersReady(bool parametersReady)
 {
     qDebug() << "_parametersReady" << parametersReady;
+
     // Try to set current unix time to the vehicle
     _sendQGCTimeToVehicle();
     // Send time twice, more likely to get to the vehicle on a noisy link
     _sendQGCTimeToVehicle();
     if (parametersReady) {
+        disconnect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
         _setupAutoDisarmSignalling();
-        _advanceInitialConnectStateMachine();
+        _initialConnectStateMachine->advance();
     }
 }
 
@@ -3151,75 +3140,66 @@ void Vehicle::setCurrentMissionSequence(int seq)
     sendMessageOnLinkThreadSafe(priorityLink(), msg);
 }
 
-void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
+void Vehicle::sendMavCommand(int compId, MAV_CMD command, bool showError, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
 {
-    MavCommandQueueEntry_t entry;
-
-    entry.commandInt = false;
-    entry.component = component;
-    entry.command = command;
-    entry.showError = showError;
-    entry.resultHandler = nullptr;
-    entry.rgParam[0] = static_cast<double>(param1);
-    entry.rgParam[1] = static_cast<double>(param2);
-    entry.rgParam[2] = static_cast<double>(param3);
-    entry.rgParam[3] = static_cast<double>(param4);
-    entry.rgParam[4] = static_cast<double>(param5);
-    entry.rgParam[5] = static_cast<double>(param6);
-    entry.rgParam[6] = static_cast<double>(param7);
-
-    _mavCommandQueue.append(entry);
-
-    if (_mavCommandQueue.count() == 1) {
-        _mavCommandRetryCount = 0;
-        _sendMavCommandAgain();
-    }
+    _sendMavCommandWorker(false,            // commandInt
+                          false,            // requestMessage
+                          showError,
+                          nullptr,          // resultHandler
+                          nullptr,          // resultHandlerData
+                          compId,
+                          command,
+                          MAV_FRAME_GLOBAL,
+                          param1, param2, param3, param4, param5, param6, param7);
 }
 
-void Vehicle::sendMavCommand(int component, MAV_CMD command, MavCmdResultHandler resultHandler, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
+void Vehicle::sendMavCommandWithHandler(MavCmdResultHandler resultHandler, void *resultHandlerData, int compId, MAV_CMD command, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
 {
-    MavCommandQueueEntry_t entry;
-
-    entry.commandInt = false;
-    entry.component = component;
-    entry.command = command;
-    entry.showError = false;
-    entry.resultHandler = resultHandler;
-    entry.rgParam[0] = static_cast<double>(param1);
-    entry.rgParam[1] = static_cast<double>(param2);
-    entry.rgParam[2] = static_cast<double>(param3);
-    entry.rgParam[3] = static_cast<double>(param4);
-    entry.rgParam[4] = static_cast<double>(param5);
-    entry.rgParam[5] = static_cast<double>(param6);
-    entry.rgParam[6] = static_cast<double>(param7);
-
-    _mavCommandQueue.append(entry);
+    _sendMavCommandWorker(false,                // commandInt
+                          false,                // requestMessage,
+                          false,                // showError
+                          resultHandler,
+                          resultHandlerData,
+                          compId,
+                          command,
+                          MAV_FRAME_GLOBAL,
+                          param1, param2, param3, param4, param5, param6, param7);
+}
 
-    if (_mavCommandQueue.count() == 1) {
-        _mavCommandRetryCount = 0;
-        _sendMavCommandAgain();
-    }
+void Vehicle::sendMavCommandInt(int compId, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7)
+{
+    _sendMavCommandWorker(true,         // commandInt
+                          false,        // requestMessage
+                          showError,
+                          nullptr,      // resultHandler
+                          nullptr,      // resultHandlerData
+                          compId,
+                          command,
+                          frame,
+                          param1, param2, param3, param4, param5, param6, param7);
 }
 
-void Vehicle::sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7)
+void Vehicle::_sendMavCommandWorker(bool commandInt, bool requestMessage, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
 {
     MavCommandQueueEntry_t entry;
 
-    entry.commandInt = true;
-    entry.component = component;
-    entry.command = command;
-    entry.frame = frame;
-    entry.showError = showError;
-    entry.resultHandler = nullptr;
-    entry.rgParam[0] = static_cast<double>(param1);
-    entry.rgParam[1] = static_cast<double>(param2);
-    entry.rgParam[2] = static_cast<double>(param3);
-    entry.rgParam[3] = static_cast<double>(param4);
-    entry.rgParam[4] = param5;
-    entry.rgParam[5] = param6;
-    entry.rgParam[6] = static_cast<double>(param7);
-
-    _mavCommandQueue.append(entry);
+    entry.commandInt        = commandInt;
+    entry.compId            = compId;
+    entry.command           = command;
+    entry.frame             = frame;
+    entry.showError         = showError;
+    entry.requestMessage    = requestMessage;
+    entry.resultHandler     = resultHandler;
+    entry.resultHandlerData = resultHandlerData;
+    entry.rgParam[0]        = param1;
+    entry.rgParam[1]        = param2;
+    entry.rgParam[2]        = param3;
+    entry.rgParam[3]        = param4;
+    entry.rgParam[4]        = param5;
+    entry.rgParam[5]        = param6;
+    entry.rgParam[6]        = param7;
+
+    _mavCommandQueue.enqueue(entry);
 
     if (_mavCommandQueue.count() == 1) {
         _mavCommandRetryCount = 0;
@@ -3229,23 +3209,25 @@ void Vehicle::sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame,
 
 void Vehicle::_sendMavCommandAgain()
 {
-    if(!_mavCommandQueue.size()) {
+    if(_mavCommandQueue.isEmpty()) {
         qWarning() << "Command resend with no commands in queue";
         _mavCommandAckTimer.stop();
         return;
     }
 
-    MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0];
+    MavCommandQueueEntry_t& queuedCommand   = _mavCommandQueue[0];
+    QString                 rawCommandName  =_toolbox->missionCommandTree()->rawName(queuedCommand.command);
 
-    if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) {
-        emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
+    if (_mavCommandRetryCount++ >= _mavCommandMaxRetryCount) {
         if (queuedCommand.resultHandler) {
-            (*queuedCommand.resultHandler)(this, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
+            (*queuedCommand.resultHandler)(queuedCommand.resultHandlerData, queuedCommand.compId, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
+        } else {
+            emit mavCommandResult(_id, queuedCommand.compId, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
         }
         if (queuedCommand.showError) {
-            qgcApp()->showAppMessage(tr("Vehicle did not respond to command: %1").arg(_toolbox->missionCommandTree()->friendlyName(queuedCommand.command)));
+            qgcApp()->showAppMessage(tr("Vehicle did not respond to command: %1").arg(rawCommandName));
         }
-        _mavCommandQueue.removeFirst();
+        _mavCommandQueue.dequeue();
         _sendNextQueuedMavCommand();
         return;
     }
@@ -3256,12 +3238,17 @@ void Vehicle::_sendMavCommandAgain()
             // we aren't really sure whether they are correct or not.
             return;
         }
-        qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount;
+        qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << rawCommandName << _mavCommandRetryCount;
     }
 
     _mavCommandAckTimer.start();
 
-    qCDebug(VehicleLog) << "_sendMavCommandAgain sending" << queuedCommand.command;
+    if (queuedCommand.requestMessage) {
+        RequestMessageInfo_t* pInfo = static_cast<RequestMessageInfo_t*>(queuedCommand.resultHandlerData);
+        _waitForMavlinkMessage(_requestMessageWaitForMessageResultHandler, pInfo, pInfo->msgId, 1000);
+    }
+
+    qCDebug(VehicleLog) << "_sendMavCommandAgain sending name:retry" << rawCommandName << _mavCommandRetryCount;
 
     mavlink_message_t       msg;
     if (queuedCommand.commandInt) {
@@ -3269,7 +3256,7 @@ void Vehicle::_sendMavCommandAgain()
 
         memset(&cmd, 0, sizeof(cmd));
         cmd.target_system =     _id;
-        cmd.target_component =  queuedCommand.component;
+        cmd.target_component =  queuedCommand.compId;
         cmd.command =           queuedCommand.command;
         cmd.frame =             queuedCommand.frame;
         cmd.param1 =            queuedCommand.rgParam[0];
@@ -3289,7 +3276,7 @@ void Vehicle::_sendMavCommandAgain()
 
         memset(&cmd, 0, sizeof(cmd));
         cmd.target_system =     _id;
-        cmd.target_component =  queuedCommand.component;
+        cmd.target_component =  queuedCommand.compId;
         cmd.command =           queuedCommand.command;
         cmd.confirmation =      0;
         cmd.param1 =            queuedCommand.rgParam[0];
@@ -3311,7 +3298,7 @@ void Vehicle::_sendMavCommandAgain()
 
 void Vehicle::_sendNextQueuedMavCommand()
 {
-    if (_mavCommandQueue.count()) {
+    if (!_mavCommandQueue.isEmpty()) {
         _mavCommandRetryCount = 0;
         _sendMavCommandAgain();
     }
@@ -3322,7 +3309,8 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
     mavlink_command_ack_t ack;
     mavlink_msg_command_ack_decode(&message, &ack);
 
-    qCDebug(VehicleLog) << QStringLiteral("_handleCommandAck command(%1) result(%2)").arg(ack.command).arg(ack.result);
+    QString rawCommandName  =_toolbox->missionCommandTree()->rawName(static_cast<MAV_CMD>(ack.command));
+    qCDebug(VehicleLog) << QStringLiteral("_handleCommandAck command(%1) result(%2)").arg(rawCommandName).arg(QGCMAVLink::mavResultToString(static_cast<MAV_RESULT>(ack.result)));
 
     if (ack.command == MAV_CMD_DO_SET_ROI_LOCATION) {
         if (ack.result == MAV_RESULT_ACCEPTED) {
@@ -3344,43 +3332,149 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
     }
 #endif
 
-    bool                showError =     false;
-    MavCmdResultHandler resultHandler = nullptr;
-    if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
+    if (!_mavCommandQueue.isEmpty() && ack.command == _mavCommandQueue.head().command) {
+        bool                    sendNextCommand = false;
+        MavCommandQueueEntry_t  commandEntry = _mavCommandQueue.head();
         _mavCommandAckTimer.stop();
-        showError = _mavCommandQueue[0].showError;
-        resultHandler = _mavCommandQueue[0].resultHandler;
-        _mavCommandQueue.removeFirst();
-        _sendNextQueuedMavCommand();
-    }
 
-    emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);
-    if (resultHandler) {
-        (*resultHandler)(this, static_cast<MAV_RESULT>(ack.result), false /* noResponsefromVehicle */);
+        if (commandEntry.requestMessage) {
+            RequestMessageInfo_t* pInfo = static_cast<RequestMessageInfo_t*>(commandEntry.resultHandlerData);
+            pInfo->commandAckReceived = true;
+            if (ack.result == MAV_RESULT_ACCEPTED) {
+                if (pInfo->messageReceived) {
+                    delete pInfo;
+                    sendNextCommand = true;
+                } else {
+                    // We dont set sendNextCommand because we wait for the message wait to complete before sending next message
+                    _waitForMavlinkMessageTimeoutActive = true;
+                    _waitForMavlinkMessageElapsed.restart();
+                }
+            } else {
+                sendNextCommand = true;
+                if (pInfo->messageReceived) {
+                    qCWarning(VehicleLog) << "Internal Error: _handleCommandAck for requestMessage with result failure, but message already received";
+                } else {
+                    _waitForMavlinkMessageClear();
+                    (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast<MAV_RESULT>(ack.result), false /* noResponsefromVehicle */);
+                }
+            }
+        } else {
+            if (commandEntry.resultHandler) {
+                (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast<MAV_RESULT>(ack.result), false /* noResponsefromVehicle */);
+            } else {
+                if (commandEntry.showError) {
+                    switch (ack.result) {
+                    case MAV_RESULT_TEMPORARILY_REJECTED:
+                        qgcApp()->showAppMessage(tr("%1 command temporarily rejected").arg(rawCommandName));
+                        break;
+                    case MAV_RESULT_DENIED:
+                        qgcApp()->showAppMessage(tr("%1 command denied").arg(rawCommandName));
+                        break;
+                    case MAV_RESULT_UNSUPPORTED:
+                        qgcApp()->showAppMessage(tr("%1 command not supported").arg(rawCommandName));
+                        break;
+                    case MAV_RESULT_FAILED:
+                        qgcApp()->showAppMessage(tr("%1 command failed").arg(rawCommandName));
+                        break;
+                    default:
+                        // Do nothing
+                        break;
+                    }
+                }
+                emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);
+            }
+            sendNextCommand = true;
+        }
+
+        _mavCommandQueue.dequeue();
+        if (sendNextCommand) {
+            _sendNextQueuedMavCommand();
+        }
+    } else {
+        qCDebug(VehicleLog) << "_handleCommandAck Ack not in queue" << rawCommandName;
     }
+}
 
-    if (showError) {
-        QString commandName = _toolbox->missionCommandTree()->friendlyName(static_cast<MAV_CMD>(ack.command));
-        switch (ack.result) {
-        case MAV_RESULT_TEMPORARILY_REJECTED:
-            qgcApp()->showAppMessage(tr("%1 command temporarily rejected").arg(commandName));
-            break;
-        case MAV_RESULT_DENIED:
-            qgcApp()->showAppMessage(tr("%1 command denied").arg(commandName));
-            break;
-        case MAV_RESULT_UNSUPPORTED:
-            qgcApp()->showAppMessage(tr("%1 command not supported").arg(commandName));
-            break;
-        case MAV_RESULT_FAILED:
-            qgcApp()->showAppMessage(tr("%1 command failed").arg(commandName));
-            break;
-        default:
-            // Do nothing
-            break;
+void Vehicle::_waitForMavlinkMessage(WaitForMavlinkMessageResultHandler resultHandler, void* resultHandlerData, int messageId, int timeoutMsecs)
+{
+    qCDebug(VehicleLog) << "_waitForMavlinkMessage msg:timeout" << messageId << timeoutMsecs;
+    _waitForMavlinkMessageResultHandler     = resultHandler;
+    _waitForMavlinkMessageResultHandlerData = resultHandlerData;
+    _waitForMavlinkMessageId                = messageId;
+    _waitForMavlinkMessageTimeoutActive     = false;
+    _waitForMavlinkMessageTimeoutMsecs      = timeoutMsecs;
+}
+
+void Vehicle::_waitForMavlinkMessageClear(void)
+{
+    qCDebug(VehicleLog) << "_waitForMavlinkMessageClear";
+    _waitForMavlinkMessageResultHandler     = nullptr;
+    _waitForMavlinkMessageResultHandlerData = nullptr;
+    _waitForMavlinkMessageId                = 0;
+}
+
+void Vehicle::_waitForMavlinkMessageMessageReceived(const mavlink_message_t& message)
+{
+    if (_waitForMavlinkMessageId != 0) {
+        if (_waitForMavlinkMessageId == message.msgid) {
+            WaitForMavlinkMessageResultHandler  resultHandler       = _waitForMavlinkMessageResultHandler;
+            void*                               resultHandlerData   = _waitForMavlinkMessageResultHandlerData;
+            qCDebug(VehicleLog) << "_waitForMavlinkMessageMessageReceived message received" << _waitForMavlinkMessageId;
+            _waitForMavlinkMessageClear();
+            (*resultHandler)(resultHandlerData, false /* noResponseFromVehicle */, message);
+        } else if (_waitForMavlinkMessageTimeoutActive && _waitForMavlinkMessageElapsed.elapsed() > _waitForMavlinkMessageTimeoutMsecs) {
+            WaitForMavlinkMessageResultHandler  resultHandler       = _waitForMavlinkMessageResultHandler;
+            void*                               resultHandlerData   = _waitForMavlinkMessageResultHandlerData;
+            qCDebug(VehicleLog) << "_waitForMavlinkMessageMessageReceived message timed out" << _waitForMavlinkMessageId;
+            _waitForMavlinkMessageClear();
+            (*resultHandler)(resultHandlerData, true /* noResponseFromVehicle */, message);
         }
     }
 }
 
+void Vehicle::requestMessage(RequestMessageResultHandler resultHandler, void* resultHandlerData, int compId, int messageId, float param1, float param2, float param3, float param4, float param5)
+{
+    RequestMessageInfo_t* pInfo = new RequestMessageInfo_t;
+
+    pInfo->msgId                = messageId;
+    pInfo->compId               = compId;
+    pInfo->resultHandler        = resultHandler;
+    pInfo->resultHandlerData    = resultHandlerData;
+
+    _sendMavCommandWorker(false,                                    // commandInt
+                          true,                                     // requestMessage,
+                          false,                                    // showError
+                          _requestMessageCmdResultHandler,
+                          pInfo,
+                          compId,
+                          MAV_CMD_REQUEST_MESSAGE,
+                          MAV_FRAME_GLOBAL,
+                          messageId,
+                          param1, param2, param3, param4, param5, 0);
+}
+
+void Vehicle::_requestMessageCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, bool noResponsefromVehicle)
+{
+    RequestMessageInfo_t* pInfo   = static_cast<RequestMessageInfo_t*>(resultHandlerData);
+
+    pInfo->commandAckReceived = true;
+    if (result != MAV_RESULT_ACCEPTED) {
+        mavlink_message_t message;
+        (*pInfo->resultHandler)(pInfo->resultHandlerData, result,  noResponsefromVehicle ? RequestMessageFailureCommandNotAcked : RequestMessageFailureCommandError, message);
+    }
+    if (pInfo->messageReceived) {
+        delete pInfo;
+    }
+}
+
+void Vehicle::_requestMessageWaitForMessageResultHandler(void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message)
+{
+    RequestMessageInfo_t* pInfo = static_cast<RequestMessageInfo_t*>(resultHandlerData);
+
+    pInfo->messageReceived = true;
+    (*pInfo->resultHandler)(pInfo->resultHandlerData, noResponsefromVehicle ? MAV_RESULT_FAILED : MAV_RESULT_ACCEPTED, noResponsefromVehicle ? RequestMessageFailureMessageNotReceived : RequestMessageNoFailure, message);
+}
+
 void Vehicle::setPrearmError(const QString& prearmError)
 {
     _prearmError = prearmError;
@@ -4235,328 +4329,6 @@ void Vehicle::sendJoystickDataThreadSafe(float roll, float pitch, float yaw, flo
     sendMessageOnLinkThreadSafe(priorityLink(), message);
 }
 
-void Vehicle::_advanceInitialConnectStateMachine(void)
-{
-    _currentInitialConnectState++;
-    if (_currentInitialConnectState < _cInitialConnectStateEntries) {
-        (*_rgInitialConnectStates[_currentInitialConnectState])(this);
-    }
-}
-
-void Vehicle::_advanceInitialConnectStateMachine(StateFn stateFn)
-{
-    for (int i=0; i<_cInitialConnectStateEntries; i++) {
-        if (_rgInitialConnectStates[i] == stateFn) {
-            _currentInitialConnectState = i;
-            (*_rgInitialConnectStates[_currentInitialConnectState])(this);
-            break;
-        }
-    }
-}
-
-void Vehicle::_waitForMavlinkMessage(int messageId, int timeoutMsecs, WaitForMavlinkMessageMessageHandler messageHandler, WaitForMavlinkMessageTimeoutHandler timeoutHandler)
-{
-    _waitForMavlinkMessageId =              messageId;
-    _waitForMavlinkMessageTimeoutMsecs =    timeoutMsecs;
-    _waitForMavlinkMessageTimeoutHandler =  timeoutHandler;
-    _waitForMavlinkMessageMessageHandler =  messageHandler;
-    _waitForMavlinkMessageElapsed.restart();
-}
-
-void Vehicle::_waitForMavlinkMessageClear(void)
-{
-    _waitForMavlinkMessageId =              0;
-    _waitForMavlinkMessageTimeoutMsecs =    0;
-    _waitForMavlinkMessageTimeoutHandler =  nullptr;
-    _waitForMavlinkMessageMessageHandler =  nullptr;
-}
-
-void Vehicle::_stateRequestCapabilities(Vehicle* vehicle)
-{
-    LinkInterface* link = vehicle->priorityLink();
-    if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
-        qCDebug(VehicleLog) << "Skipping capability request due to link type";
-        vehicle->_advanceInitialConnectStateMachine();
-    } else {
-        qCDebug(VehicleLog) << "Requesting capabilities";
-        vehicle->_waitForMavlinkMessage(MAVLINK_MSG_ID_AUTOPILOT_VERSION, 1000, _waitForAutopilotVersionMessageHandler, _waitForAutopilotVersionTimeoutHandler);
-        vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
-                                MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
-                                _capabilitiesCmdResultHandler,
-                                1);                                      // Request firmware version
-    }
-}
-
-void Vehicle::_capabilitiesCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle)
-{
-    if (result != MAV_RESULT_ACCEPTED) {
-        if (noResponsefromVehicle) {
-            qCDebug(VehicleLog) << "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES no response from vehicle";
-        } else {
-            qCDebug(VehicleLog) << QStringLiteral("MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES error(%1)").arg(result);
-        }
-        qCDebug(VehicleLog) << "Setting no capabilities";
-        vehicle->_setCapabilities(0);
-        vehicle->_waitForMavlinkMessageClear();
-        vehicle->_advanceInitialConnectStateMachine(_stateRequestProtocolVersion);
-    }
-}
-
-void Vehicle::_waitForAutopilotVersionMessageHandler(Vehicle* vehicle, const mavlink_message_t& message)
-{
-    qCDebug(VehicleLog) << "AUTOPILOT_VERSION received";
-
-    mavlink_autopilot_version_t autopilotVersion;
-    mavlink_msg_autopilot_version_decode(&message, &autopilotVersion);
-
-    vehicle->_uid = (quint64)autopilotVersion.uid;
-    emit vehicle->vehicleUIDChanged();
-
-    if (autopilotVersion.flight_sw_version != 0) {
-        int majorVersion, minorVersion, patchVersion;
-        FIRMWARE_VERSION_TYPE versionType;
-
-        majorVersion = (autopilotVersion.flight_sw_version >> (8*3)) & 0xFF;
-        minorVersion = (autopilotVersion.flight_sw_version >> (8*2)) & 0xFF;
-        patchVersion = (autopilotVersion.flight_sw_version >> (8*1)) & 0xFF;
-        versionType = (FIRMWARE_VERSION_TYPE)((autopilotVersion.flight_sw_version >> (8*0)) & 0xFF);
-        vehicle->setFirmwareVersion(majorVersion, minorVersion, patchVersion, versionType);
-    }
-
-    if (vehicle->px4Firmware()) {
-        // Lower 3 bytes is custom version
-        int majorVersion, minorVersion, patchVersion;
-        majorVersion = autopilotVersion.flight_custom_version[2];
-        minorVersion = autopilotVersion.flight_custom_version[1];
-        patchVersion = autopilotVersion.flight_custom_version[0];
-        vehicle->setFirmwareCustomVersion(majorVersion, minorVersion, patchVersion);
-
-        // PX4 Firmware stores the first 16 characters of the git hash as binary, with the individual bytes in reverse order
-        vehicle->_gitHash = "";
-        for (int i = 7; i >= 0; i--) {
-            vehicle->_gitHash.append(QString("%1").arg(autopilotVersion.flight_custom_version[i], 2, 16, QChar('0')));
-        }
-    } else {
-        // APM Firmware stores the first 8 characters of the git hash as an ASCII character string
-        char nullStr[9];
-        strncpy(nullStr, (char*)autopilotVersion.flight_custom_version, 8);
-        nullStr[8] = 0;
-        vehicle->_gitHash = nullStr;
-    }
-    if (vehicle->_toolbox->corePlugin()->options()->checkFirmwareVersion() && !vehicle->_checkLatestStableFWDone) {
-        vehicle->_checkLatestStableFWDone = true;
-        vehicle->_firmwarePlugin->checkIfIsLatestStable(vehicle);
-    }
-    emit vehicle->gitHashChanged(vehicle->_gitHash);
-
-    vehicle->_setCapabilities(autopilotVersion.capabilities);
-    vehicle->_advanceInitialConnectStateMachine();
-}
-
-void Vehicle::_waitForAutopilotVersionTimeoutHandler(Vehicle* vehicle)
-{
-    qCDebug(VehicleLog) << "AUTOPILOT_VERSION timeout";
-    qCDebug(VehicleLog) << "Setting no capabilities";
-    vehicle->_setCapabilities(0);
-    vehicle->_advanceInitialConnectStateMachine();
-}
-
-void Vehicle::_stateRequestProtocolVersion(Vehicle* vehicle)
-{
-    LinkInterface* link = vehicle->priorityLink();
-    if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
-        qCDebug(VehicleLog) << "Skipping protocol version request due to link type";
-        vehicle->_advanceInitialConnectStateMachine();
-    } else {
-        qCDebug(VehicleLog) << "Requesting protocol version";
-        vehicle->_waitForMavlinkMessage(MAVLINK_MSG_ID_PROTOCOL_VERSION, 1000, _waitForProtocolVersionMessageHandler, _waitForProtocolVersionTimeoutHandler);
-        vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
-                                MAV_CMD_REQUEST_PROTOCOL_VERSION,
-                                _protocolVersionCmdResultHandler,
-                                1);                                      // Request protocol version
-    }
-}
-
-void Vehicle::_protocolVersionCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle)
-{
-    if (result != MAV_RESULT_ACCEPTED) {
-        if (noResponsefromVehicle) {
-            qCDebug(VehicleLog) << "MAV_CMD_REQUEST_PROTOCOL_VERSION no response from vehicle";
-        } else {
-            qCDebug(VehicleLog) << QStringLiteral("MAV_CMD_REQUEST_PROTOCOL_VERSION error(%1)").arg(result);
-        }
-
-        // _mavlinkProtocolRequestMaxProtoVersion stays at 0 to indicate unknown
-        vehicle->_mavlinkProtocolRequestComplete = true;
-        vehicle->_setMaxProtoVersionFromBothSources();
-        vehicle->_waitForMavlinkMessageClear();
-        vehicle->_advanceInitialConnectStateMachine(_stateRequestCompInfoVersion);
-    }
-}
-
-void Vehicle::_waitForProtocolVersionMessageHandler(Vehicle* vehicle, const mavlink_message_t& message)
-{
-    mavlink_protocol_version_t protoVersion;
-    mavlink_msg_protocol_version_decode(&message, &protoVersion);
-
-    qCDebug(VehicleLog) << "PROTOCOL_VERSION received mav_version:" << protoVersion.max_version;
-    vehicle->_mavlinkProtocolRequestMaxProtoVersion = protoVersion.max_version;
-    vehicle->_mavlinkProtocolRequestComplete = true;
-    vehicle->_setMaxProtoVersionFromBothSources();
-    vehicle->_advanceInitialConnectStateMachine();
-}
-
-
-void Vehicle::_waitForProtocolVersionTimeoutHandler(Vehicle* vehicle)
-{
-    qCDebug(VehicleLog) << "PROTOCOL_VERSION timeout";
-    // The PROTOCOL_VERSION message didn't make it through the pipe from Vehicle->QGC.
-    // This means although the vehicle may support mavlink 2, the pipe does not.
-    qCDebug(VehicleLog) << QStringLiteral("Setting _maxProtoVersion to 100 due to timeout on receiving PROTOCOL_VERSION message.");
-    vehicle->_mavlinkProtocolRequestMaxProtoVersion = 100;
-    vehicle->_mavlinkProtocolRequestComplete = true;
-    vehicle->_setMaxProtoVersionFromBothSources();
-    vehicle->_advanceInitialConnectStateMachine();
-}
-
-
-void Vehicle::_stateRequestCompInfoVersion(Vehicle* vehicle)
-{
-    LinkInterface* link = vehicle->priorityLink();
-    if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
-        qCDebug(VehicleLog) << "Skipping version component information request due to link type";
-        vehicle->_advanceInitialConnectStateMachine();
-    } else {
-        qCDebug(VehicleLog) << "Requesting version component information";
-        vehicle->_waitForMavlinkMessage(MAVLINK_MSG_ID_COMPONENT_INFORMATION, 1000, _waitForCompInfoVersionMessageHandler, _waitForCompInfoVersionTimeoutHandler);
-        vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
-                                MAV_CMD_REQUEST_MESSAGE,
-                                _compInfoVersionCmdResultHandler,
-                                MAVLINK_MSG_ID_COMPONENT_INFORMATION,
-                                COMP_METADATA_TYPE_VERSION);
-    }
-}
-
-void Vehicle::_compInfoVersionCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle)
-{
-    if (result != MAV_RESULT_ACCEPTED) {
-        if (noResponsefromVehicle) {
-            qCDebug(VehicleLog) << "MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION COMP_METADATA_TYPE_VERSION no response from vehicle";
-        } else {
-            qCDebug(VehicleLog) << QStringLiteral("MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION COMP_METADATA_TYPE_VERSION error(%1)").arg(result);
-        }
-        vehicle->_waitForMavlinkMessageClear();
-        vehicle->_advanceInitialConnectStateMachine(_stateRequestParameters);
-    }
-}
-
-void Vehicle::_waitForCompInfoVersionMessageHandler(Vehicle* vehicle, const mavlink_message_t& message)
-{
-    qCDebug(VehicleLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_VERSION received";
-    vehicle->_componentInformation.componentInformationReceived(message);
-    vehicle->_advanceInitialConnectStateMachine();
-}
-
-void Vehicle::_waitForCompInfoVersionTimeoutHandler(Vehicle* vehicle)
-{
-    qCDebug(VehicleLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_VERSION timeout";
-    vehicle->_advanceInitialConnectStateMachine(_stateRequestParameters);
-}
-
-void Vehicle::_stateRequestCompInfoParam(Vehicle* vehicle)
-{
-    LinkInterface* link = vehicle->priorityLink();
-    if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
-        qCDebug(VehicleLog) << "Skipping parameter component information request due to link type";
-        vehicle->_advanceInitialConnectStateMachine();
-    } else {
-        if (vehicle->_componentInformation.metaDataTypeSupported(COMP_METADATA_TYPE_PARAMETER)) {
-            qCDebug(VehicleLog) << "Requesting parameter component information";
-            vehicle->_waitForMavlinkMessage(MAVLINK_MSG_ID_COMPONENT_INFORMATION, 1000, _waitForCompInfoParamMessageHandler, _waitForCompInfoParamTimeoutHandler);
-            vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
-                                    MAV_CMD_REQUEST_MESSAGE,
-                                    _compInfoParamCmdResultHandler,
-                                    MAVLINK_MSG_ID_COMPONENT_INFORMATION,
-                                    COMP_METADATA_TYPE_PARAMETER);
-        } else {
-            qCDebug(VehicleLog) << "Skipping parameter component information request due to lack of support";
-            vehicle->_advanceInitialConnectStateMachine(_stateRequestParameters);
-        }
-    }
-}
-
-void Vehicle::_compInfoParamCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle)
-{
-    if (result != MAV_RESULT_ACCEPTED) {
-        if (noResponsefromVehicle) {
-            qCDebug(VehicleLog) << "MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION COMP_METADATA_TYPE_PARAMETER no response from vehicle";
-        } else {
-            qCDebug(VehicleLog) << QStringLiteral("MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION COMP_METADATA_TYPE_PARAMETER error(%1)").arg(result);
-        }
-        vehicle->_waitForMavlinkMessageClear();
-        vehicle->_advanceInitialConnectStateMachine(_stateRequestParameters);
-    }
-}
-
-void Vehicle::_waitForCompInfoParamMessageHandler(Vehicle* vehicle, const mavlink_message_t& message)
-{
-    qCDebug(VehicleLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_PARAMETER received";
-    vehicle->_componentInformation.componentInformationReceived(message);
-    vehicle->_advanceInitialConnectStateMachine();
-}
-
-void Vehicle::_waitForCompInfoParamTimeoutHandler(Vehicle* vehicle)
-{
-    qCDebug(VehicleLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_PARAMETER timeout";
-    vehicle->_advanceInitialConnectStateMachine();
-}
-
-void Vehicle::_stateRequestParameters(Vehicle* vehicle)
-{
-    qCDebug(VehicleLog) << "Requesting parameters";
-    vehicle->_parameterManager->refreshAllParameters();
-}
-
-void Vehicle::_stateRequestMission(Vehicle* vehicle)
-{
-    LinkInterface* link = vehicle->priorityLink();
-    if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
-        qCDebug(VehicleLog) << "Skipping first mission load request due to link type";
-        vehicle->_firstMissionLoadComplete();
-    } else {
-        qCDebug(VehicleLog) << "Requesting first mission load";
-        vehicle->_missionManager->loadFromVehicle();
-    }
-}
-
-void Vehicle::_stateRequestGeoFence(Vehicle* vehicle)
-{
-    qCDebug(VehicleLog) << "Requesting first geofence load";
-    if (vehicle->_geoFenceManager->supported()) {
-        vehicle->_geoFenceManager->loadFromVehicle();
-    } else {
-        qCDebug(VehicleLog) << "Geofence load skipped";
-        vehicle->_firstGeoFenceLoadComplete();
-    }
-}
-
-void Vehicle::_stateRequestRallyPoints(Vehicle* vehicle)
-{
-    qCDebug(VehicleLog) << "Requesting first rally point load";
-    if (vehicle->_rallyPointManager->supported()) {
-        vehicle->_rallyPointManager->loadFromVehicle();
-    } else {
-        qCDebug(VehicleLog) << "Rally Point load skipped";
-        vehicle->_firstRallyPointLoadComplete();
-    }
-}
-
-void Vehicle::_stateSignalInitialConnectComplete(Vehicle* vehicle)
-{
-    qCDebug(VehicleLog) << "Signalling initialConnectComplete";
-    emit vehicle->initialConnectComplete();
-}
-
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 

src/Vehicle/Vehicle.h

@@ -14,6 +14,7 @@
 #include <QVariantList>
 #include <QGeoCoordinate>
 #include <QTime>
+#include <QQueue>
 
 #include "FactGroup.h"
 #include "LinkInterface.h"
@@ -24,7 +25,6 @@
 #include "SettingsFact.h"
 #include "QGCMapCircle.h"
 #include "TerrainFactGroup.h"
-#include "ComponentInformation.h"
 
 class UAS;
 class UASInterface;
@@ -43,6 +43,9 @@ class Joystick;
 class VehicleObjectAvoidance;
 class TrajectoryPoints;
 class TerrainProtocolHandler;
+class ComponentInformationManager;
+class MAVLinkFTPManager;
+class InitialConnectStateMachine;
 
 #if defined(QGC_AIRMAP_ENABLED)
 class AirspaceVehicleManager;
@@ -1004,31 +1007,29 @@ public:
 
     void setConnectionLostEnabled(bool connectionLostEnabled);
 
-    ParameterManager*       parameterManager() { return _parameterManager; }
-    ParameterManager*       parameterManager() const { return _parameterManager; }
-    VehicleObjectAvoidance* objectAvoidance()  { return _objectAvoidance; }
+    ParameterManager*       parameterManager    () { return _parameterManager; }
+    ParameterManager*       parameterManager    () const { return _parameterManager; }
+    MAVLinkFTPManager*      mavlinkFTPManager   () { return _mavlinkFTPManager; }
+    VehicleObjectAvoidance* objectAvoidance     () { return _objectAvoidance; }
 
     static const int cMaxRcChannels = 18;
 
     bool containsLink(LinkInterface* link) { return _links.contains(link); }
 
-    typedef void (*MavCmdResultHandler)(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
-
     /// Sends the specified MAV_CMD to the vehicle. If no Ack is received command will be retried. If a sendMavCommand is already in progress
     /// the command will be queued and sent when the previous command completes.
-    ///     @param component Component to send to
+    ///     @param compId Component to send to.
     ///     @param command MAV_CMD to send
     ///     @param showError true: Display error to user if command failed, false:  no error shown
     /// Signals: mavCommandResult on success or failure
-    void sendMavCommand(int component, MAV_CMD command, bool showError, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
-    void sendMavCommand(int component, MAV_CMD command, MavCmdResultHandler errorHandler, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
-    void sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7);
+    void sendMavCommand(int compId, MAV_CMD command, bool showError, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
+    void sendMavCommandInt(int compId, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7);
 
     /// Same as sendMavCommand but available from Qml.
-    Q_INVOKABLE void sendCommand(int component, int command, bool showError, double param1 = 0.0, double param2 = 0.0, double param3 = 0.0, double param4 = 0.0, double param5 = 0.0, double param6 = 0.0, double param7 = 0.0)
+    Q_INVOKABLE void sendCommand(int compId, int command, bool showError, double param1 = 0.0, double param2 = 0.0, double param3 = 0.0, double param4 = 0.0, double param5 = 0.0, double param6 = 0.0, double param7 = 0.0)
     {
         sendMavCommand(
-            component, static_cast<MAV_CMD>(command),
+            compId, static_cast<MAV_CMD>(command),
             showError,
             static_cast<float>(param1),
             static_cast<float>(param2),
@@ -1039,6 +1040,36 @@ public:
             static_cast<float>(param7));
     }
 
+    /// Callback for sendMavCommandWithHandle
+    ///     @param resultHandleData     Opaque data passed in to sendMavCommand call
+    ///     @param commandResult        Ack result for command send
+    ///     @param noReponseFromVehicle true: The vehicle did not responsed to the COMMAND_LONG message
+    typedef void (*MavCmdResultHandler)(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle);
+
+    /// Sends the command and calls the callback with the result
+    ///     @param resultHandler    Callback for result, nullptr for no callback
+    ///     @param resultHandleData Opaque data passed through callback
+    void sendMavCommandWithHandler(MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
+
+    typedef enum {
+        RequestMessageNoFailure,
+        RequestMessageFailureCommandError,
+        RequestMessageFailureCommandNotAcked,
+        RequestMessageFailureMessageNotReceived,
+    } RequestMessageResultHandlerFailureCode_t;
+
+    /// Callback for requestMessage
+    ///     @param resultHandlerData    Opaque data which was passed in to requestMessage call
+    ///     @param commandResult        Result from ack to REQUEST_MESSAGE command
+    ///     @param noResponseToCommand  true: Vehicle never acked the REQUEST_MESSAGE command
+    ///     @param messageNotReceived   true: Vehicle never sent requested message
+    typedef void (*RequestMessageResultHandler)(void* resultHandlerData, MAV_RESULT commandResult, RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message);
+
+    /// Requests the vehicle to send the specified message. Will retry a number of times.
+    ///     @param resultHandler Callback for result
+    ///     @param resultHandlerData Opaque data passed back to resultHandler
+    void requestMessage(RequestMessageResultHandler resultHandler, void* resultHandlerData, int compId, int messageId, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f);
+
     int firmwareMajorVersion() const { return _firmwareMajorVersion; }
     int firmwareMinorVersion() const { return _firmwareMinorVersion; }
     int firmwarePatchVersion() const { return _firmwarePatchVersion; }
@@ -1414,23 +1445,6 @@ private:
 
     QGCCameraManager* _cameras;
 
-    typedef struct {
-        int                 component;
-        bool                commandInt; // true: use COMMAND_INT, false: use COMMAND_LONG
-        MAV_CMD             command;
-        MAV_FRAME           frame;
-        double              rgParam[7];
-        bool                showError;
-        MavCmdResultHandler resultHandler;
-    } MavCommandQueueEntry_t;
-
-    QList<MavCommandQueueEntry_t>   _mavCommandQueue;
-    QTimer                          _mavCommandAckTimer;
-    int                             _mavCommandRetryCount;
-    static const int                _mavCommandMaxRetryCount =              3;
-    static const int                _mavCommandAckTimeoutMSecs =            3000;
-    static const int                _mavCommandAckTimeoutMSecsHighLatency = 120000;
-
     QString             _prearmError;
     QTimer              _prearmErrorTimer;
     static const int    _prearmErrorTimeoutMSecs = 35 * 1000;   ///< Take away prearm error after 35 seconds
@@ -1445,8 +1459,11 @@ private:
     GeoFenceManager*    _geoFenceManager;
     RallyPointManager*  _rallyPointManager;
 
-    ParameterManager*       _parameterManager   = nullptr;
-    VehicleObjectAvoidance* _objectAvoidance    = nullptr;
+    ParameterManager*               _parameterManager               = nullptr;
+    MAVLinkFTPManager*              _mavlinkFTPManager              = nullptr;
+    ComponentInformationManager*    _componentInformationManager    = nullptr;
+    InitialConnectStateMachine*     _initialConnectStateMachine     = nullptr;
+    VehicleObjectAvoidance*         _objectAvoidance                = nullptr;
 
 #if defined(QGC_AIRMAP_ENABLED)
     AirspaceVehicleManager* _airspaceVehicleManager;
@@ -1544,53 +1561,58 @@ private:
     QMap<uint8_t /* compId */, ChunkedStatusTextInfo_t> _chunkedStatusTextInfoMap;
     QTimer _chunkedStatusTextTimer;
 
-    ComponentInformation _componentInformation;
-
-    typedef void (*WaitForMavlinkMessageTimeoutHandler)(Vehicle* vehicle);
-    typedef void (*WaitForMavlinkMessageMessageHandler)(Vehicle* vehicle, const mavlink_message_t& message);
+    /// Callback for waitForMavlinkMessage
+    ///     @param resultHandleData     Opaque data passed in to waitForMavlinkMessage call
+    ///     @param commandResult        Ack result for command send
+    ///     @param noReponseFromVehicle true: The vehicle did not responsed to the COMMAND_LONG message
+    typedef void (*WaitForMavlinkMessageResultHandler)(void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message);
 
     /// Waits for the specified msecs for the message to be received. Calls timeoutHandler if not received.
-    void _waitForMavlinkMessage     (int messageId, int timeoutMsecs, WaitForMavlinkMessageMessageHandler messageHandler, WaitForMavlinkMessageTimeoutHandler timeoutHandler);
+    void _waitForMavlinkMessage     (WaitForMavlinkMessageResultHandler resultHandler, void* resultHandlerData, int messageId, int timeoutMsecs);
     void _waitForMavlinkMessageClear(void);
 
-    int                                 _waitForMavlinkMessageId =              0;
-    int                                 _waitForMavlinkMessageTimeoutMsecs =    0;
+    int                                 _waitForMavlinkMessageId                = 0;
+    bool                                _waitForMavlinkMessageTimeoutActive     = false;
+    int                                 _waitForMavlinkMessageTimeoutMsecs      = 0;
     QElapsedTimer                       _waitForMavlinkMessageElapsed;
-    WaitForMavlinkMessageTimeoutHandler _waitForMavlinkMessageTimeoutHandler =  nullptr;
-    WaitForMavlinkMessageMessageHandler _waitForMavlinkMessageMessageHandler =  nullptr;
-
-    // Initial connection state machine
-    typedef void (*StateFn)(Vehicle* vehicle);
-    static const int _cInitialConnectStateEntries = 9;
-    static const StateFn _rgInitialConnectStates[_cInitialConnectStateEntries];
-    int _currentInitialConnectState = -1;
-    void _advanceInitialConnectStateMachine(void);
-    void _advanceInitialConnectStateMachine(StateFn stateFn);
-
-    static void _stateRequestCapabilities           (Vehicle* vehicle);
-    static void _stateRequestProtocolVersion        (Vehicle* vehicle);
-    static void _stateRequestCompInfoVersion        (Vehicle* vehicle);
-    static void _stateRequestCompInfoParam          (Vehicle* vehicle);
-    static void _stateRequestParameters             (Vehicle* vehicle);
-    static void _stateRequestMission                (Vehicle* vehicle);
-    static void _stateRequestGeoFence               (Vehicle* vehicle);
-    static void _stateRequestRallyPoints            (Vehicle* vehicle);
-    static void _stateSignalInitialConnectComplete  (Vehicle* vehicle);
-
-    static void _capabilitiesCmdResultHandler   (Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
-    static void _protocolVersionCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
-    static void _compInfoVersionCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
-    static void _compInfoParamCmdResultHandler  (Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
-
-    static void _waitForAutopilotVersionMessageHandler  (Vehicle* vehicle, const mavlink_message_t& message);
-    static void _waitForProtocolVersionMessageHandler   (Vehicle* vehicle, const mavlink_message_t& message);
-    static void _waitForCompInfoVersionMessageHandler   (Vehicle* vehicle, const mavlink_message_t& message);
-    static void _waitForCompInfoParamMessageHandler     (Vehicle* vehicle, const mavlink_message_t& message);
-
-    static void _waitForAutopilotVersionTimeoutHandler  (Vehicle* vehicle);
-    static void _waitForProtocolVersionTimeoutHandler   (Vehicle* vehicle);
-    static void _waitForCompInfoVersionTimeoutHandler   (Vehicle* vehicle);
-    static void _waitForCompInfoParamTimeoutHandler     (Vehicle* vehicle);
+    WaitForMavlinkMessageResultHandler  _waitForMavlinkMessageResultHandler     = nullptr;
+    void*                               _waitForMavlinkMessageResultHandlerData = nullptr;
+
+    void _waitForMavlinkMessageMessageReceived(const mavlink_message_t& message);
+
+    // requestMessage handling
+    typedef struct {
+        bool                        commandAckReceived  = false;    // We keep track of the ack/message being received since the order in which this will come in is random
+        bool                        messageReceived     = false;    // We only delete the allocated RequestMessageInfo_t when both happen (or the message wait times out)
+        int                         msgId               = 0;
+        int                         compId              = 0;
+        RequestMessageResultHandler resultHandler       = nullptr;
+        void*                       resultHandlerData   = nullptr;
+    } RequestMessageInfo_t;
+
+    static void _requestMessageCmdResultHandler             (void* resultHandlerData, int compId, MAV_RESULT result, bool noResponsefromVehicle);
+    static void _requestMessageWaitForMessageResultHandler  (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message);
+
+    typedef struct {
+        int                 compId;
+        bool                commandInt;     // true: use COMMAND_INT, false: use COMMAND_LONG
+        MAV_CMD             command;
+        MAV_FRAME           frame;
+        float               rgParam[7];
+        bool                showError;
+        bool                requestMessage; // true: this is from a requestMessage call
+        MavCmdResultHandler resultHandler;
+        void*               resultHandlerData;
+    } MavCommandQueueEntry_t;
+
+    QQueue<MavCommandQueueEntry_t>  _mavCommandQueue;
+    QTimer                          _mavCommandAckTimer;
+    int                             _mavCommandRetryCount;
+    static const int                _mavCommandMaxRetryCount =              3;
+    static const int                _mavCommandAckTimeoutMSecs =            3000;
+    static const int                _mavCommandAckTimeoutMSecsHighLatency = 120000;
+
+    void _sendMavCommandWorker(bool commandInt, bool requestMessage, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, float param5, float param6, float param7);
 
     // FactGroup facts
 
@@ -1666,4 +1688,6 @@ private:
     // Settings keys
     static const char* _settingsGroup;
     static const char* _joystickEnabledSettingsKey;
+
+    friend class InitialConnectStateMachine;
 };

src/comm/MockLink.cc

@@ -367,18 +367,18 @@ void MockLink::_sendSysStatus(void)
     }
     mavlink_message_t   msg;
     mavlink_msg_sys_status_pack_chan(
-        _vehicleSystemId,
-        _vehicleComponentId,
-        static_cast<uint8_t>(_mavlinkChannel),
-        &msg,
-        0,          // onboard_control_sensors_present
-        0,          // onboard_control_sensors_enabled
-        0,          // onboard_control_sensors_health
-        250,        // load
-        4200 * 4,   // voltage_battery
-        8000,       // current_battery
-        _batteryRemaining, // battery_remaining
-        0,0,0,0,0,0);
+                _vehicleSystemId,
+                _vehicleComponentId,
+                static_cast<uint8_t>(_mavlinkChannel),
+                &msg,
+                0,          // onboard_control_sensors_present
+                0,          // onboard_control_sensors_enabled
+                0,          // onboard_control_sensors_health
+                250,        // load
+                4200 * 4,   // voltage_battery
+                8000,       // current_battery
+                _batteryRemaining, // battery_remaining
+                0,0,0,0,0,0);
     respondWithMavlinkMessage(msg);
 }
 
@@ -429,7 +429,6 @@ void MockLink::_writeBytesQueued(const QByteArray bytes)
 
         _handleIncomingMavlinkBytes((uint8_t *)bytes.constData(), bytes.count());
     }
-
 }
 
 /// @brief Handle incoming bytes which are meant to be interpreted by the NuttX shell
@@ -909,6 +908,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
     static bool firstCmdUser3 = true;
     static bool firstCmdUser4 = true;
 
+    bool noAck = false;
     mavlink_command_long_t request;
     uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
 
@@ -935,19 +935,22 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
         _respondWithAutopilotVersion();
         break;
     case MAV_CMD_REQUEST_MESSAGE:
-        if (request.param1 == MAVLINK_MSG_ID_COMPONENT_INFORMATION && _handleRequestMessage(request)) {
+        if (_handleRequestMessage(request, noAck)) {
+            if (noAck) {
+                return;
+            }
             commandResult = MAV_RESULT_ACCEPTED;
         }
         break;
-    case MAV_CMD_USER_1:
+    case MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED:
         // Test command which always returns MAV_RESULT_ACCEPTED
         commandResult = MAV_RESULT_ACCEPTED;
         break;
-    case MAV_CMD_USER_2:
+    case MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED:
         // Test command which always returns MAV_RESULT_FAILED
         commandResult = MAV_RESULT_FAILED;
         break;
-    case MAV_CMD_USER_3:
+    case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED:
         // Test command which returns MAV_RESULT_ACCEPTED on second attempt
         if (firstCmdUser3) {
             firstCmdUser3 = false;
@@ -957,7 +960,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
             commandResult = MAV_RESULT_ACCEPTED;
         }
         break;
-    case MAV_CMD_USER_4:
+    case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED:
         // Test command which returns MAV_RESULT_FAILED on second attempt
         if (firstCmdUser4) {
             firstCmdUser4 = false;
@@ -967,7 +970,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
             commandResult = MAV_RESULT_FAILED;
         }
         break;
-    case MAV_CMD_USER_5:
+    case MAV_CMD_MOCKLINK_NO_RESPONSE:
         // No response
         return;
         break;
@@ -987,6 +990,22 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
     respondWithMavlinkMessage(commandAck);
 }
 
+void MockLink::sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult)
+{
+    mavlink_message_t commandAck;
+    mavlink_msg_command_ack_pack_chan(_vehicleSystemId,
+                                      _vehicleComponentId,
+                                      _mavlinkChannel,
+                                      &commandAck,
+                                      command,
+                                      ackResult,
+                                      0,    // progress
+                                      0,    // result_param2
+                                      0,    // target_system
+                                      0);   // target_component
+    respondWithMavlinkMessage(commandAck);
+}
+
 void MockLink::_respondWithAutopilotVersion(void)
 {
     mavlink_message_t msg;
@@ -1147,7 +1166,7 @@ void MockLink::_sendStatusTextMessages(void)
     { MAV_SEVERITY_NOTICE,      "Status text notice" },
     { MAV_SEVERITY_INFO,        "Status text info" },
     { MAV_SEVERITY_DEBUG,       "Status text debug" },
-    };
+};
 
     mavlink_message_t msg;
 
@@ -1276,6 +1295,11 @@ MockLink*  MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration
     return _startMockLinkWorker("Generic MockLink", MAV_AUTOPILOT_GENERIC, MAV_TYPE_QUADROTOR, sendStatusText, failureMode);
 }
 
+MockLink* MockLink::startNoInitialConnectMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
+{
+    return _startMockLinkWorker("No Initial Connect MockLink", MAV_AUTOPILOT_PX4, MAV_TYPE_GENERIC, sendStatusText, failureMode);
+}
+
 MockLink*  MockLink::startAPMArduCopterMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
 {
     return _startMockLinkWorker("ArduCopter MockLink",MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_QUADROTOR, sendStatusText, failureMode);
@@ -1461,18 +1485,44 @@ void MockLink::_sendADSBVehicles(void)
     respondWithMavlinkMessage(responseMsg);
 }
 
-bool MockLink::_handleRequestMessage(const mavlink_command_long_t& request)
+bool MockLink::_handleRequestMessage(const mavlink_command_long_t& request, bool& noAck)
 {
-    if (request.param1 != MAVLINK_MSG_ID_COMPONENT_INFORMATION) {
-        return false;
+    noAck = false;
+
+    switch ((int)request.param1) {
+    case MAVLINK_MSG_ID_COMPONENT_INFORMATION:
+        switch (static_cast<int>(request.param2)) {
+        case COMP_METADATA_TYPE_VERSION:
+            _sendVersionMetaData();
+            return true;
+        case COMP_METADATA_TYPE_PARAMETER:
+            _sendParameterMetaData();
+            return true;
+        }
+        break;
+    case MAVLINK_MSG_ID_DEBUG:
+        switch (_requestMessageFailureMode) {
+        case FailRequestMessageNone:
+            break;
+        case FailRequestMessageCommandAcceptedMsgNotSent:
+            return true;
+        case FailRequestMessageCommandUnsupported:
+            return false;
+        case FailRequestMessageCommandNoResponse:
+            noAck = true;
+            return true;
+        case FailRequestMessageCommandAcceptedSecondAttempMsgSent:
+            return true;
+        }
+    {
+        mavlink_message_t   responseMsg;
+        mavlink_msg_debug_pack_chan(_vehicleSystemId,
+                                    _vehicleComponentId,
+                                    _mavlinkChannel,
+                                    &responseMsg,
+                                    0, 0, 0);
+        respondWithMavlinkMessage(responseMsg);
     }
-
-    switch (static_cast<int>(request.param2)) {
-    case COMP_METADATA_TYPE_VERSION:
-        _sendVersionMetaData();
-        return true;
-    case COMP_METADATA_TYPE_PARAMETER:
-        _sendParameterMetaData();
         return true;
     }
 

src/comm/MockLink.h

@@ -121,12 +121,11 @@ public:
 
     MockLinkFileServer* getFileServer(void) { return _fileServer; }
 
-    // Virtuals from LinkInterface
-    virtual QString getName(void) const { return _name; }
-    virtual void requestReset(void){ }
-    virtual bool isConnected(void) const { return _connected; }
-    virtual qint64 getConnectionSpeed(void) const { return 100000000; }
-    virtual qint64 bytesAvailable(void) { return 0; }
+    // Overrides from LinkInterface
+    QString getName             (void) const override { return _name; }
+    void    requestReset        (void) override { }
+    bool    isConnected         (void) const override { return _connected; }
+    qint64  getConnectionSpeed  (void) const override { return 100000000; }
 
     // These are left unimplemented in order to cause linker errors which indicate incorrect usage of
     // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
@@ -147,18 +146,38 @@ public:
     /// Called to send a MISSION_REQUEST message while the MissionManager is in idle state
     void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); }
 
+    void sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult);
+
     /// Reset the state of the MissionItemHandler to no items, no transactions in progress.
     void resetMissionItemHandler(void) { _missionItemHandler.reset(); }
 
     /// Returns the filename for the simulated log file. Only available after a download is requested.
     QString logDownloadFile(void) { return _logDownloadFilename; }
 
-    static MockLink* startPX4MockLink            (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
-    static MockLink* startGenericMockLink        (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
-    static MockLink* startAPMArduCopterMockLink  (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
-    static MockLink* startAPMArduPlaneMockLink   (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
-    static MockLink* startAPMArduSubMockLink     (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
-    static MockLink* startAPMArduRoverMockLink   (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
+    static MockLink* startPX4MockLink               (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
+    static MockLink* startGenericMockLink           (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
+    static MockLink* startNoInitialConnectMockLink  (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
+    static MockLink* startAPMArduCopterMockLink     (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
+    static MockLink* startAPMArduPlaneMockLink      (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
+    static MockLink* startAPMArduSubMockLink        (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
+    static MockLink* startAPMArduRoverMockLink      (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
+
+    // Special commands for testing COMMAND_LONG handlers
+    static const MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED            = MAV_CMD_USER_1;
+    static const MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED              = MAV_CMD_USER_2;
+    static const MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED    = MAV_CMD_USER_3;
+    static const MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED      = MAV_CMD_USER_4;
+    static const MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE                       = MAV_CMD_USER_5;
+
+    // Special message ids for testing requestMessage support
+    typedef enum {
+        FailRequestMessageNone,
+        FailRequestMessageCommandAcceptedMsgNotSent,
+        FailRequestMessageCommandUnsupported,
+        FailRequestMessageCommandNoResponse,
+        FailRequestMessageCommandAcceptedSecondAttempMsgSent,
+    } RequestMessageFailureMode_t;
+    void setRequestMessageFailureMode(RequestMessageFailureMode_t failureMode) { _requestMessageFailureMode = failureMode; }
 
 signals:
     void writeBytesQueuedSignal(const QByteArray bytes);
@@ -174,11 +193,11 @@ private slots:
 
 private:
     // From LinkInterface
-    virtual bool _connect(void);
-    virtual void _disconnect(void);
+    bool _connect(void) final;
+    void _disconnect(void) final;
 
     // QThread override
-    virtual void run(void);
+    void run(void) final;
 
     // MockLink methods
     void _sendHeartBeat                 (void);
@@ -198,7 +217,7 @@ private:
     void _handleLogRequestList          (const mavlink_message_t& msg);
     void _handleLogRequestData          (const mavlink_message_t& msg);
     void _handleParamMapRC              (const mavlink_message_t& msg);
-    bool _handleRequestMessage          (const mavlink_command_long_t& request);
+    bool _handleRequestMessage          (const mavlink_command_long_t& request, bool& noAck);
     float _floatUnionForParam           (int componentId, const QString& paramName);
     void _setParamFloatUnionIntoMap     (int componentId, const QString& paramName, float paramFloat);
     void _sendHomePosition              (void);
@@ -269,6 +288,8 @@ private:
     QGeoCoordinate  _adsbVehicleCoordinate;
     double          _adsbAngle;
 
+    RequestMessageFailureMode_t _requestMessageFailureMode = FailRequestMessageNone;
+
     static double       _defaultVehicleLatitude;
     static double       _defaultVehicleLongitude;
     static double       _defaultVehicleAltitude;

src/comm/MockLinkFileServer.cc

@@ -135,6 +135,13 @@ void MockLinkFileServer::_openCommand(uint8_t senderSystemId, uint8_t senderComp
             break;
         }
     }
+
+    if (!found) {
+        if (path == "/version.json") {
+            qDebug() << "Requesting version json";
+        }
+    }
+
     if (!found) {
         _sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdOpenFileRO);
         return;

src/comm/QGCMAVLink.cc

@@ -60,3 +60,23 @@ bool QGCMAVLink::isVTOL(MAV_TYPE mavType)
         return false;
     }
 }
+
+QString  QGCMAVLink::mavResultToString(MAV_RESULT result)
+{
+    switch (result) {
+    case MAV_RESULT_ACCEPTED:
+        return QStringLiteral("MAV_RESULT_ACCEPTED");
+    case MAV_RESULT_TEMPORARILY_REJECTED:
+        return QStringLiteral("MAV_RESULT_TEMPORARILY_REJECTED");
+    case MAV_RESULT_DENIED:
+        return QStringLiteral("MAV_RESULT_DENIED");
+    case MAV_RESULT_UNSUPPORTED:
+        return QStringLiteral("MAV_RESULT_UNSUPPORTED");
+    case MAV_RESULT_FAILED:
+        return QStringLiteral("MAV_RESULT_FAILED");
+    case MAV_RESULT_IN_PROGRESS:
+        return QStringLiteral("MAV_RESULT_IN_PROGRESS");
+    default:
+        return QStringLiteral("MAV_RESULT unknown %1").arg(result);
+    }
+}

src/comm/QGCMAVLink.h

@@ -7,15 +7,10 @@
  *
  ****************************************************************************/
 
-
-/**
- * @file
- *   @brief MAVLink header file for QGroundControl
- *   @author Lorenz Meier <pixhawk@switched.com>
- */
-
 #pragma once
 
+#include <QString>
+
 #define MAVLINK_USE_MESSAGE_INFO
 #define MAVLINK_EXTERNAL_RX_STATUS  // Single m_mavlink_status instance is in QGCApplication.cc
 #include <stddef.h>                 // Hack workaround for Mav 2.0 header problem with respect to offsetof usage
@@ -47,11 +42,12 @@ extern mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
 
 class QGCMAVLink {
 public:
-    static bool isFixedWing(MAV_TYPE mavType);
-    static bool isRover(MAV_TYPE mavType);
-    static bool isSub(MAV_TYPE mavType);
-    static bool isMultiRotor(MAV_TYPE mavType);
-    static bool isVTOL(MAV_TYPE mavType);
+    static bool     isFixedWing         (MAV_TYPE mavType);
+    static bool     isRover             (MAV_TYPE mavType);
+    static bool     isSub               (MAV_TYPE mavType);
+    static bool     isMultiRotor        (MAV_TYPE mavType);
+    static bool     isVTOL              (MAV_TYPE mavType);
+    static QString  mavResultToString   (MAV_RESULT result);
 };
 
 

src/qgcunittest/UnitTest.cc

@@ -389,21 +389,25 @@ void UnitTest::_connectMockLink(MAV_AUTOPILOT autopilot)
     case MAV_AUTOPILOT_GENERIC:
         _mockLink = MockLink::startGenericMockLink(false);
         break;
+    case MAV_AUTOPILOT_INVALID:
+        _mockLink = MockLink::startNoInitialConnectMockLink(false);
+        break;
     default:
         qWarning() << "Type not supported";
         break;
     }
 
     // Wait for the Vehicle to get created
-    QSignalSpy spyVehicle(qgcApp()->toolbox()->multiVehicleManager(), SIGNAL(parameterReadyVehicleAvailableChanged(bool)));
+    QSignalSpy spyVehicle(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleChanged);
     QCOMPARE(spyVehicle.wait(10000), true);
-    QVERIFY(qgcApp()->toolbox()->multiVehicleManager()->parameterReadyVehicleAvailable());
     _vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
     QVERIFY(_vehicle);
 
-    // Wait for initial connect sequence to complete
-    QSignalSpy spyPlan(_vehicle, SIGNAL(initialConnectComplete()));
-    QCOMPARE(spyPlan.wait(10000), true);
+    if (autopilot != MAV_AUTOPILOT_INVALID) {
+        // Wait for initial connect sequence to complete
+        QSignalSpy spyPlan(_vehicle, &Vehicle::initialConnectComplete);
+        QCOMPARE(spyPlan.wait(30000), true);
+    }
 }
 
 void UnitTest::_disconnectMockLink(void)

src/qgcunittest/UnitTest.h

@@ -120,6 +120,7 @@ protected slots:
 
 protected:
     void _connectMockLink(MAV_AUTOPILOT autopilot = MAV_AUTOPILOT_PX4);
+    void _connectMockLinkNoInitialConnectSequence(void) { _connectMockLink(MAV_AUTOPILOT_INVALID); }
     void _disconnectMockLink(void);
     void _createMainWindow(void);
     void _closeMainWindow(bool cancelExpected = false);

src/qgcunittest/UnitTestList.cc

@@ -30,7 +30,8 @@
 #include "ParameterManagerTest.h"
 #include "MissionCommandTreeTest.h"
 //#include "LogDownloadTest.h"
-#include "SendMavCommandTest.h"
+#include "SendMavCommandWithSignallingTest.h"
+#include "SendMavCommandWithHandlerTest.h"
 #include "VisualMissionItemTest.h"
 #include "CameraSectionTest.h"
 #include "SpeedSectionTest.h"
@@ -44,6 +45,7 @@
 #include "TransectStyleComplexItemTest.h"
 #include "CameraCalcTest.h"
 #include "FWLandingPatternTest.h"
+#include "RequestMessageTest.h"
 
 UT_REGISTER_TEST(FactSystemTestGeneric)
 UT_REGISTER_TEST(FactSystemTestPX4)
@@ -51,6 +53,9 @@ UT_REGISTER_TEST(FactSystemTestPX4)
 UT_REGISTER_TEST(GeoTest)
 UT_REGISTER_TEST(LinkManagerTest)
 //UT_REGISTER_TEST(MessageBoxTest)
+UT_REGISTER_TEST(SendMavCommandWithSignallingTest)
+UT_REGISTER_TEST(SendMavCommandWithHandlerTest)
+UT_REGISTER_TEST(RequestMessageTest)
 UT_REGISTER_TEST(MissionItemTest)
 UT_REGISTER_TEST(SimpleMissionItemTest)
 UT_REGISTER_TEST(MissionControllerTest)
@@ -61,7 +66,6 @@ UT_REGISTER_TEST(TCPLinkTest)
 UT_REGISTER_TEST(ParameterManagerTest)
 UT_REGISTER_TEST(MissionCommandTreeTest)
 //UT_REGISTER_TEST(LogDownloadTest)
-UT_REGISTER_TEST(SendMavCommandTest)
 UT_REGISTER_TEST(SurveyComplexItemTest)
 UT_REGISTER_TEST(CameraSectionTest)
 UT_REGISTER_TEST(SpeedSectionTest)