More work on vehicle connect state machine(s)

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

@@ -1,51 +0,0 @@
-/****************************************************************************
- *
- * (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/ComponentInformation.h

@@ -1,38 +0,0 @@
-/****************************************************************************
- *
- * (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 <QObject>
-
-Q_DECLARE_LOGGING_CATEGORY(ComponentInformationLog)
-
-class Vehicle;
-
-class ComponentInformation : public QObject
-{
-    Q_OBJECT
-
-public:
-    ComponentInformation(Vehicle* vehicle, QObject* parent = nullptr);
-
-    void requestVersionMetaData         (Vehicle* vehicle);
-    bool requestParameterMetaData       (Vehicle* vehicle);
-    void componentInformationReceived   (const mavlink_message_t& message);
-    bool metaDataTypeSupported          (COMP_METADATA_TYPE type);
-
-private:
-    Vehicle*                    _vehicle =                  nullptr;
-    bool                        _versionMetaDataAvailable = false;
-    bool                        _paramMetaDataAvailable =   false;
-    QList<COMP_METADATA_TYPE>   _supportedMetaDataTypes;
-};

src/Vehicle/ComponentInformationManager.cc

@@ -0,0 +1,182 @@
+/****************************************************************************
+ *
+ * (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

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ * (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

@@ -0,0 +1,294 @@
+/****************************************************************************
+ *
+ * (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

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ * (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/RequestMessageTest.cc

@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ * (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

@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ * (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

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ * (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/SendMavCommandWithHandlerTest.h

@@ -0,0 +1,33 @@
+/****************************************************************************
+ *
+ * (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"
+
+class SendMavCommandWithHandlerTest : public UnitTest
+{
+    Q_OBJECT
+    
+private slots:
+    void _performTestCases(void);
+
+private:
+    typedef struct {
+        MAV_CMD     command;
+        MAV_RESULT  expectedCommandResult;
+        bool        expectedNoResponseFromVehicle;
+    } TestCase_t;
+
+    void _testCaseWorker(TestCase_t& testCase);
+
+    static void _mavCmdResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle);
+
+    static TestCase_t _rgTestCases[];
+};

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/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

@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ * (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

@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ * (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)