Merge pull request #8861 from DonLakeFlyer/ConnectStateMachine

Connect state machine major rework

qgroundcontrol.pro

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

src/MissionManager/MavCmdInfoCommon.json

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

src/Vehicle/ComponentInformation.cc

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

src/Vehicle/ComponentInformationManager.cc

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

src/Vehicle/ComponentInformationManager.h

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

src/Vehicle/InitialConnectStateMachine.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
     
-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 slots:
+    void _performTestCases(void);
 
 private:
-    Vehicle*                    _vehicle =                  nullptr;
-    bool                        _versionMetaDataAvailable = false;
-    bool                        _paramMetaDataAvailable =   false;
-    QList<COMP_METADATA_TYPE>   _supportedMetaDataTypes;
+    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/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);
+    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/comm/MockLink.cc

@@ -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,12 +1485,12 @@ 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();
@@ -1475,6 +1499,32 @@ bool MockLink::_handleRequestMessage(const mavlink_command_long_t& request)
             _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);
+    }
+        return true;
+    }
 
     return false;
 }

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,6 +146,8 @@ 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(); }
 
@@ -155,11 +156,29 @@ public:
 
     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);
 
+    if (autopilot != MAV_AUTOPILOT_INVALID) {
         // Wait for initial connect sequence to complete
-    QSignalSpy spyPlan(_vehicle, SIGNAL(initialConnectComplete()));
-    QCOMPARE(spyPlan.wait(10000), true);
+        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)