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

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

src/Vehicle/ComponentInformationManager.cc

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

src/Vehicle/ComponentInformationManager.h

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

src/Vehicle/InitialConnectStateMachine.cc

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

src/Vehicle/InitialConnectStateMachine.h

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

src/Vehicle/RequestMessageTest.cc

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

src/Vehicle/RequestMessageTest.h

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

src/Vehicle/SendMavCommandWithHandlerTest.cc

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

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

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

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

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

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

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

src/Vehicle/StateMachine.cc

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

src/Vehicle/StateMachine.h

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

src/Vehicle/Vehicle.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;