Vehicle::sendMavCommand: send command immediately, retries only for limited set of messages

qgroundcontrol.pro

@@ -495,7 +495,6 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
         src/qgcunittest/MultiSignalSpyV2.h \
         src/qgcunittest/UnitTest.h \
         src/Vehicle/FTPManagerTest.h \
-        src/Vehicle/InitialConnectTest.h \
         src/Vehicle/RequestMessageTest.h \
         src/Vehicle/SendMavCommandWithHandlerTest.h \
         src/Vehicle/SendMavCommandWithSignallingTest.h \
@@ -543,7 +542,6 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
         src/qgcunittest/UnitTest.cc \
         src/qgcunittest/UnitTestList.cc \
         src/Vehicle/FTPManagerTest.cc \
-        src/Vehicle/InitialConnectTest.cc \
         src/Vehicle/RequestMessageTest.cc \
         src/Vehicle/SendMavCommandWithHandlerTest.cc \
         src/Vehicle/SendMavCommandWithSignallingTest.cc \

src/Vehicle/InitialConnectStateMachine.cc

@@ -72,17 +72,24 @@ void InitialConnectStateMachine::_stateRequestCapabilities(StateMachine* stateMa
     }
 }
 
-void InitialConnectStateMachine::_capabilitiesCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, bool noResponsefromVehicle)
+void InitialConnectStateMachine::_capabilitiesCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, Vehicle::MavCmdResultFailureCode_t failureCode)
 {
     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 {
+        switch (failureCode) {
+        case Vehicle::MavCmdResultCommandResultOnly:
             qCDebug(InitialConnectStateMachineLog) << QStringLiteral("MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES error(%1)").arg(result);
+            break;
+        case Vehicle::MavCmdResultFailureNoResponseToCommand:
+            qCDebug(InitialConnectStateMachineLog) << "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES no response from vehicle";
+            break;
+        case Vehicle::MavCmdResultFailureDuplicateCommand:
+            qCDebug(InitialConnectStateMachineLog) << "Internal Error: MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES could not be sent due to duplicate command";
+            break;
         }
+
         qCDebug(InitialConnectStateMachineLog) << "Setting no capabilities";
         vehicle->_setCapabilities(0);
         vehicle->_waitForMavlinkMessageClear();
@@ -170,16 +177,22 @@ void InitialConnectStateMachine::_stateRequestProtocolVersion(StateMachine* stat
     }
 }
 
-void InitialConnectStateMachine::_protocolVersionCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, bool noResponsefromVehicle)
+void InitialConnectStateMachine::_protocolVersionCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, Vehicle::MavCmdResultFailureCode_t failureCode)
 {
     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 {
+        switch (failureCode) {
+        case Vehicle::MavCmdResultCommandResultOnly:
             qCDebug(InitialConnectStateMachineLog) << QStringLiteral("MAV_CMD_REQUEST_PROTOCOL_VERSION error(%1)").arg(result);
+            break;
+        case Vehicle::MavCmdResultFailureNoResponseToCommand:
+            qCDebug(InitialConnectStateMachineLog) << "MAV_CMD_REQUEST_PROTOCOL_VERSION no response from vehicle";
+            break;
+        case Vehicle::MavCmdResultFailureDuplicateCommand:
+            qCDebug(InitialConnectStateMachineLog) << "Internal Error: MAV_CMD_REQUEST_PROTOCOL_VERSION could not be sent due to duplicate command";
+            break;
         }
 
         // _mavlinkProtocolRequestMaxProtoVersion stays at 0 to indicate unknown

src/Vehicle/InitialConnectStateMachine.h

@@ -12,6 +12,7 @@
 #include "StateMachine.h"
 #include "QGCMAVLink.h"
 #include "QGCLoggingCategory.h"
+#include "Vehicle.h"
 
 Q_DECLARE_LOGGING_CATEGORY(InitialConnectStateMachineLog)
 
@@ -38,8 +39,8 @@ private:
     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 _capabilitiesCmdResultHandler           (void* resultHandlerData, int compId, MAV_RESULT result, Vehicle::MavCmdResultFailureCode_t failureCode);
+    static void _protocolVersionCmdResultHandler        (void* resultHandlerData, int compId, MAV_RESULT result, Vehicle::MavCmdResultFailureCode_t failureCode);
 
     static void _waitForAutopilotVersionResultHandler   (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message);
     static void _waitForProtocolVersionResultHandler    (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message);

src/Vehicle/InitialConnectTest.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 "InitialConnectTest.h"
-#include "MultiVehicleManager.h"
-#include "QGCApplication.h"
-#include "MockLink.h"
-
-InitialConnectTest::TestCase_t InitialConnectTest::_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 InitialConnectTest::_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 InitialConnectTest::_testCaseWorker(TestCase_t& testCase)
-{
-    _connectMockLink();
-
-    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 InitialConnectTest::_test(void)
-{
-    _connectMockLink();
-
-    //MultiVehicleManager*    vehicleMgr  = qgcApp()->toolbox()->multiVehicleManager();
-    //Vehicle*                vehicle     = vehicleMgr->activeVehicle();
-
-    _disconnectMockLink();
-}
-
-void InitialConnectTest::_performTestCases(void)
-{
-    int index = 0;
-    for (TestCase_t& testCase: _rgTestCases) {
-        qDebug() << "Testing case" << index++;
-        _testCaseWorker(testCase);
-    }
-}

src/Vehicle/InitialConnectTest.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 InitialConnectTest : public UnitTest
-{
-    Q_OBJECT
-
-signals:
-    void resultHandlerCalled(void);
-    
-private slots:
-    void _test(void);
-
-private:
-    void _performTestCases(void);
-    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/RequestMessageTest.cc

@@ -13,11 +13,10 @@
 #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 },
+//    {  MockLink::FailRequestMessageNone,                                MAV_RESULT_ACCEPTED,    Vehicle::RequestMessageNoFailure,                   1,                                  false },
+    {  MockLink::FailRequestMessageCommandAcceptedMsgNotSent,           MAV_RESULT_FAILED,      Vehicle::RequestMessageFailureMessageNotReceived,   1,                                  false },
+    {  MockLink::FailRequestMessageCommandUnsupported,                  MAV_RESULT_UNSUPPORTED, Vehicle::RequestMessageFailureCommandError,         1,                                  false },
+    {  MockLink::FailRequestMessageCommandNoResponse,                   MAV_RESULT_FAILED,      Vehicle::RequestMessageFailureCommandNotAcked,      Vehicle::_mavCommandMaxRetryCount,  false },
 };
 
 void RequestMessageTest::_requestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message)
@@ -39,15 +38,13 @@ void RequestMessageTest::_testCaseWorker(TestCase_t& testCase)
     MultiVehicleManager*    vehicleMgr  = qgcApp()->toolbox()->multiVehicleManager();
     Vehicle*                vehicle     = vehicleMgr->activeVehicle();
 
+    _mockLink->clearSendMavCommandCounts();
     _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);
+
+    vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG);
+    QVERIFY(QTest::qWaitFor([&]() { return testCase.resultHandlerCalled; }, 10000));
+    QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE),   -1);
+    QCOMPARE(_mockLink->sendMavCommandCount(MAV_CMD_REQUEST_MESSAGE),                                   testCase.expectedSendCount);
 
     _disconnectMockLink();
 }
@@ -60,3 +57,59 @@ void RequestMessageTest::_performTestCases(void)
         _testCaseWorker(testCase);
     }
 }
+
+void RequestMessageTest::_duplicateCommand(void)
+{
+    _connectMockLinkNoInitialConnectSequence();
+
+    RequestMessageTest::TestCase_t testCase = {
+        MockLink::FailRequestMessageCommandNoResponse, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureDuplicateCommand, 1, false
+    };
+
+    MultiVehicleManager*    vehicleMgr  = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle*                vehicle     = vehicleMgr->activeVehicle();
+
+    _mockLink->clearSendMavCommandCounts();
+    _mockLink->setRequestMessageFailureMode(testCase.failureMode);
+
+    vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG);
+    QVERIFY(QTest::qWaitFor([&]() { return _mockLink->sendMavCommandCount(MAV_CMD_REQUEST_MESSAGE) == 1; }, 10));
+    QCOMPARE(testCase.resultHandlerCalled, false);
+    vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG);
+
+    // Duplicate command returns immediately
+    QCOMPARE(testCase.resultHandlerCalled,                                                              true);
+    QCOMPARE(_mockLink->sendMavCommandCount(MAV_CMD_REQUEST_MESSAGE),                                   testCase.expectedSendCount);
+    QVERIFY(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE) != -1);
+}
+
+void RequestMessageTest::_compIdAllRequestMessageResultHandler(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);
+}
+
+void RequestMessageTest::_compIdAllFailure(void)
+{
+    _connectMockLinkNoInitialConnectSequence();
+
+    RequestMessageTest::TestCase_t testCase = {
+        MockLink::FailRequestMessageCommandNoResponse, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureCommandError, 0, false
+    };
+
+    MultiVehicleManager*    vehicleMgr  = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle*                vehicle     = vehicleMgr->activeVehicle();
+
+    _mockLink->clearSendMavCommandCounts();
+    _mockLink->setRequestMessageFailureMode(testCase.failureMode);
+
+    vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_ALL, MAVLINK_MSG_ID_DEBUG);
+    QCOMPARE(testCase.resultHandlerCalled,                                                      true);
+    QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_ALL, MAV_CMD_REQUEST_MESSAGE),  -1);
+    QCOMPARE(_mockLink->sendMavCommandCount(MAV_CMD_REQUEST_MESSAGE),                           0);
+
+    _disconnectMockLink();
+}

src/Vehicle/RequestMessageTest.h

@@ -22,18 +22,22 @@ signals:
     
 private slots:
     void _performTestCases(void);
+    void _compIdAllFailure(void);
+    void _duplicateCommand(void);
 
 private:
     typedef struct {
         MockLink::RequestMessageFailureMode_t               failureMode;
         MAV_RESULT                                          expectedCommandResult;
         Vehicle::RequestMessageResultHandlerFailureCode_t   expectedFailureCode;
+        int                                                 expectedSendCount;
         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);
+    static void _requestMessageResultHandler            (void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message);
+    static void _compIdAllRequestMessageResultHandler   (void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message);
 
     bool _resultHandlerCalled;
 

src/Vehicle/SendMavCommandWithHandlerTest.cc

@@ -13,20 +13,25 @@
 #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 },
+    {  MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED,           MAV_RESULT_ACCEPTED,    Vehicle::MavCmdResultCommandResultOnly,             1 },
+    {  MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED,             MAV_RESULT_FAILED,      Vehicle::MavCmdResultCommandResultOnly,             1 },
+    {  MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED,   MAV_RESULT_ACCEPTED,    Vehicle::MavCmdResultCommandResultOnly,             2 },
+    {  MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED,     MAV_RESULT_FAILED,      Vehicle::MavCmdResultCommandResultOnly,             2 },
+    {  MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE,                      MAV_RESULT_FAILED,      Vehicle::MavCmdResultFailureNoResponseToCommand,    Vehicle::_mavCommandMaxRetryCount },
+    {  MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY,             MAV_RESULT_FAILED,      Vehicle::MavCmdResultFailureNoResponseToCommand,    1 },
 };
 
-void SendMavCommandWithHandlerTest::_mavCmdResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle)
+bool SendMavCommandWithHandlerTest::_handlerCalled = false;
+
+void SendMavCommandWithHandlerTest::_mavCmdResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode)
 {
     TestCase_t* testCase = static_cast<TestCase_t*>(resultHandlerData);
 
-    QVERIFY(compId == MAV_COMP_ID_AUTOPILOT1);
-    QCOMPARE(testCase->expectedCommandResult, commandResult);
-    QCOMPARE(testCase->expectedNoResponseFromVehicle, noResponsefromVehicle);
+    _handlerCalled = true;
+
+    QCOMPARE(compId,                            MAV_COMP_ID_AUTOPILOT1);
+    QCOMPARE(testCase->expectedCommandResult,   commandResult);
+    QCOMPARE(testCase->expectedFailureCode,     failureCode);
 }
 
 void SendMavCommandWithHandlerTest::_testCaseWorker(TestCase_t& testCase)
@@ -36,7 +41,12 @@ void SendMavCommandWithHandlerTest::_testCaseWorker(TestCase_t& testCase)
     MultiVehicleManager*    vehicleMgr  = qgcApp()->toolbox()->multiVehicleManager();
     Vehicle*                vehicle     = vehicleMgr->activeVehicle();
 
-    vehicle->sendMavCommandWithHandler(_mavCmdResultHandler, &testCase, MAV_COMP_ID_ALL, testCase.command);
+    _handlerCalled = false;
+    _mockLink->clearSendMavCommandCounts();
+    vehicle->sendMavCommandWithHandler(_mavCmdResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, testCase.command);
+    QVERIFY(QTest::qWaitFor([&]() { return _handlerCalled; }, 10000));
+    QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, testCase.command),  -1);
+    QCOMPARE(_mockLink->sendMavCommandCount(testCase.command),                  testCase.expectedSendCount);
 
     _disconnectMockLink();
 }
@@ -49,3 +59,57 @@ void SendMavCommandWithHandlerTest::_performTestCases(void)
         _testCaseWorker(testCase);
     }
 }
+
+void SendMavCommandWithHandlerTest::_duplicateCommand(void)
+{
+    _connectMockLinkNoInitialConnectSequence();
+
+    SendMavCommandWithHandlerTest::TestCase_t testCase = {
+        MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, MAV_RESULT_FAILED, Vehicle::MavCmdResultFailureDuplicateCommand, 1
+    };
+
+    MultiVehicleManager*    vehicleMgr  = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle*                vehicle     = vehicleMgr->activeVehicle();
+
+    _handlerCalled = false;
+    _mockLink->clearSendMavCommandCounts();
+    vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1, testCase.command, true /* showError */);
+    QVERIFY(QTest::qWaitFor([&]() { return _mockLink->sendMavCommandCount(testCase.command) == 1; }, 10));
+    QVERIFY(!_handlerCalled);
+    vehicle->sendMavCommandWithHandler(_mavCmdResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, testCase.command);
+
+    // Duplicate command response should happen immediately
+    QVERIFY(_handlerCalled);
+    QVERIFY(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, testCase.command) != -1);
+    QCOMPARE(_mockLink->sendMavCommandCount(testCase.command), 1);
+}
+
+void SendMavCommandWithHandlerTest::_compIdAllMavCmdResultHandler(void* /*resultHandlerData*/, int compId, MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode)
+{
+    _handlerCalled = true;
+
+    QCOMPARE(compId,            MAV_COMP_ID_ALL);
+    QCOMPARE(commandResult,     MAV_RESULT_FAILED);
+    QCOMPARE(failureCode,       Vehicle::MavCmdResultCommandResultOnly);
+}
+
+void SendMavCommandWithHandlerTest::_compIdAllFailure(void)
+{
+    _connectMockLinkNoInitialConnectSequence();
+
+    SendMavCommandWithHandlerTest::TestCase_t testCase = {
+        MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, MAV_RESULT_FAILED, Vehicle::MavCmdResultFailureDuplicateCommand, 0
+    };
+
+    MultiVehicleManager*    vehicleMgr  = qgcApp()->toolbox()->multiVehicleManager();
+    Vehicle*                vehicle     = vehicleMgr->activeVehicle();
+
+    _handlerCalled = false;
+    _mockLink->clearSendMavCommandCounts();
+    vehicle->sendMavCommandWithHandler(_compIdAllMavCmdResultHandler, nullptr, MAV_COMP_ID_ALL, testCase.command);
+    QCOMPARE(_handlerCalled,                                                            true);
+    QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_ALL, testCase.command), -1);
+    QCOMPARE(_mockLink->sendMavCommandCount(testCase.command),                          testCase.expectedSendCount);
+
+    _disconnectMockLink();
+}

src/Vehicle/SendMavCommandWithHandlerTest.h

@@ -10,6 +10,7 @@
 #pragma once
 
 #include "UnitTest.h"
+#include "Vehicle.h"
 
 class SendMavCommandWithHandlerTest : public UnitTest
 {
@@ -17,17 +18,23 @@ class SendMavCommandWithHandlerTest : public UnitTest
     
 private slots:
     void _performTestCases(void);
+    void _compIdAllFailure(void);
+    void _duplicateCommand(void);
 
 private:
     typedef struct {
-        MAV_CMD     command;
-        MAV_RESULT  expectedCommandResult;
-        bool        expectedNoResponseFromVehicle;
+        MAV_CMD                             command;
+        MAV_RESULT                          expectedCommandResult;
+        Vehicle::MavCmdResultFailureCode_t  expectedFailureCode;
+        int                                 expectedSendCount;
     } TestCase_t;
 
     void _testCaseWorker(TestCase_t& testCase);
 
-    static void _mavCmdResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle);
+    static void _mavCmdResultHandler            (void* resultHandlerData, int compId, MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode);
+    static void _compIdAllMavCmdResultHandler   (void* resultHandlerData, int compId, MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode);
+
+    static bool _handlerCalled;
 
     static TestCase_t _rgTestCases[];
 };

src/Vehicle/SendMavCommandWithSignallingTest.cc

@@ -7,13 +7,21 @@
  *
  ****************************************************************************/
 
-
 #include "SendMavCommandWithSignallingTest.h"
 #include "MultiVehicleManager.h"
 #include "QGCApplication.h"
 #include "MockLink.h"
 
-void SendMavCommandWithSignallingTest::_noFailure(void)
+SendMavCommandWithSignallingTest::TestCase_t SendMavCommandWithSignallingTest::_rgTestCases[] = {
+    {  MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED,           MAV_RESULT_ACCEPTED,    Vehicle::MavCmdResultCommandResultOnly,             1 },
+    {  MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED,             MAV_RESULT_FAILED,      Vehicle::MavCmdResultCommandResultOnly,             1 },
+    {  MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED,   MAV_RESULT_ACCEPTED,    Vehicle::MavCmdResultCommandResultOnly,             2 },
+    {  MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED,     MAV_RESULT_FAILED,      Vehicle::MavCmdResultCommandResultOnly,             2 },
+    {  MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE,                      MAV_RESULT_FAILED,      Vehicle::MavCmdResultFailureNoResponseToCommand,    Vehicle::_mavCommandMaxRetryCount },
+    {  MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY,             MAV_RESULT_FAILED,      Vehicle::MavCmdResultFailureNoResponseToCommand,    1 },
+};
+
+void SendMavCommandWithSignallingTest::_testCaseWorker(TestCase_t& testCase)
 {
     _connectMockLinkNoInitialConnectSequence();
 
@@ -21,146 +29,53 @@ void SendMavCommandWithSignallingTest::_noFailure(void)
     Vehicle*                vehicle     = vehicleMgr->activeVehicle();
     QSignalSpy              spyResult(vehicle, &Vehicle::mavCommandResult);
 
-    vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, true /* showError */);
-
-    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)MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED);
-    QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_ACCEPTED);
-    QCOMPARE(arguments.at(4).toBool(), false);
-}
-
-void SendMavCommandWithSignallingTest::_failureShowError(void)
-{
-    // Will pop error about request failure
-    setExpectedMessageBox(QMessageBox::Ok);
-
-    _connectMockLinkNoInitialConnectSequence();
-
-    MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
-    Vehicle* vehicle = vehicleMgr->activeVehicle();
-    QVERIFY(vehicle);
-
-    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)MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED);
-    QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
-    QCOMPARE(arguments.at(4).toBool(), false);
-
-    // User should have been notified
-    checkExpectedMessageBox();
-}
-
-void SendMavCommandWithSignallingTest::_failureNoShowError(void)
-{
-    _connectMockLinkNoInitialConnectSequence();
-
-    MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
-    Vehicle* vehicle = vehicleMgr->activeVehicle();
-    QVERIFY(vehicle);
-
-    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)MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED);
-    QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
-    QCOMPARE(arguments.at(4).toBool(), false);
-}
-
-void SendMavCommandWithSignallingTest::_noFailureAfterRetry(void)
-{
-    _connectMockLinkNoInitialConnectSequence();
+    _mockLink->clearSendMavCommandCounts();
 
-    MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
-    Vehicle* vehicle = vehicleMgr->activeVehicle();
-    QVERIFY(vehicle);
+    vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1, testCase.command, 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)MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED);
-    QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_ACCEPTED);
-    QCOMPARE(arguments.at(4).toBool(), false);
+    QCOMPARE(arguments.count(),                                             5);
+    QCOMPARE(arguments.at(0).toInt(),                                       vehicle->id());
+    QCOMPARE(arguments.at(1).toInt(),                                       MAV_COMP_ID_AUTOPILOT1);
+    QCOMPARE(arguments.at(2).toInt(),                                       testCase.command);
+    QCOMPARE(arguments.at(3).toInt(),                                       testCase.expectedCommandResult);
+    QCOMPARE(arguments.at(4).value<Vehicle::MavCmdResultFailureCode_t>(),   testCase.expectedFailureCode);
+    QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED), -1);
+    QCOMPARE(_mockLink->sendMavCommandCount(testCase.command),              testCase.expectedSendCount);
+
+    _disconnectMockLink();
 }
 
-void SendMavCommandWithSignallingTest::_failureAfterRetry(void)
+void SendMavCommandWithSignallingTest::_performTestCases(void)
 {
-    // Will pop error about request failure
-    setExpectedMessageBox(QMessageBox::Ok);
-
-    _connectMockLinkNoInitialConnectSequence();
-
-    MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
-    Vehicle* vehicle = vehicleMgr->activeVehicle();
-    QVERIFY(vehicle);
-
-    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)MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED);
-    QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
-    QCOMPARE(arguments.at(4).toBool(), false);
-
-    // User should have been notified
-    checkExpectedMessageBox();
+    int index = 0;
+    for (TestCase_t& testCase: _rgTestCases) {
+        qDebug() << "Testing case" << index++;
+        _testCaseWorker(testCase);
+    }
 }
 
-void SendMavCommandWithSignallingTest::_failureAfterNoReponse(void)
-{
-    // Will pop error about request failure
-    setExpectedMessageBox(QMessageBox::Ok);
-
-    _connectMockLinkNoInitialConnectSequence();
-
-    MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
-    Vehicle* vehicle = vehicleMgr->activeVehicle();
-    QVERIFY(vehicle);
-
-    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)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)
+void SendMavCommandWithSignallingTest::_duplicateCommand(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);
+    vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1, MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, true /* showError */);
+    QVERIFY(QTest::qWaitFor([&]() { return _mockLink->sendMavCommandCount(MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE) == 1; }, 10));
+    QSignalSpy spyResult(vehicle, &Vehicle::mavCommandResult);
+    vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1, MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, true /* showError */);
 
-    _mockLink->sendUnexpectedCommandAck(MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, MAV_RESULT_FAILED);
-    QCOMPARE(spyResult.wait(100), false);
+    // Duplicate command returns immediately
+    QCOMPARE(spyResult.count(),                                                         1);
+    QList<QVariant> arguments = spyResult.takeFirst();
+    QCOMPARE(arguments.count(),                                                         5);
+    QCOMPARE(arguments.at(0).toInt(),                                                   vehicle->id());
+    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).value<Vehicle::MavCmdResultFailureCode_t>(),               Vehicle::MavCmdResultFailureDuplicateCommand);
+    QCOMPARE(_mockLink->sendMavCommandCount(MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE),    1);
+    QVERIFY(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE) != -1);
 }

src/Vehicle/SendMavCommandWithSignallingTest.h

@@ -10,19 +10,25 @@
 #pragma once
 
 #include "UnitTest.h"
+#include "Vehicle.h"
 
 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 _unexpectedAck         (void);
+    void _performTestCases(void);
+    void _duplicateCommand(void);
 
 private:
+    typedef struct {
+        MAV_CMD                             command;
+        MAV_RESULT                          expectedCommandResult;
+        Vehicle::MavCmdResultFailureCode_t  expectedFailureCode;
+        int                                 expectedSendCount;
+    } TestCase_t;
+
+    void _testCaseWorker(TestCase_t& testCase);
+
+    static TestCase_t _rgTestCases[];
 };

src/Vehicle/Vehicle.h

@@ -39,7 +39,6 @@
 #include "GeoFenceManager.h"
 #include "RallyPointManager.h"
 #include "FTPManager.h"
-#include "InitialConnectStateMachine.h"
 
 class UAS;
 class UASInterface;
@@ -57,8 +56,12 @@ class TrajectoryPoints;
 class TerrainProtocolHandler;
 class ComponentInformationManager;
 class VehicleBatteryFactGroup;
+class SendMavCommandWithSignallingTest;
+class SendMavCommandWithHandlerTest;
+class RequestMessageTest;
 class LinkInterface;
 class LinkManager;
+class InitialConnectStateMachine;
 
 #if defined(QGC_AIRMAP_ENABLED)
 class AirspaceVehicleManager;
@@ -70,6 +73,14 @@ class Vehicle : public FactGroup
 {
     Q_OBJECT
 
+    friend class InitialConnectStateMachine;
+    friend class VehicleLinkManager;
+    friend class VehicleBatteryFactGroup;           // Allow VehicleBatteryFactGroup to call _addFactGroup
+    friend class SendMavCommandWithSignallingTest;  // Unit test
+    friend class SendMavCommandWithHandlerTest;     // Unit test
+    friend class RequestMessageTest;                // Unit test
+
+
 public:
     Vehicle(LinkInterface*          link,
             int                     vehicleId,
@@ -617,25 +628,19 @@ public:
     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 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(
-            compId, static_cast<MAV_CMD>(command),
-            showError,
-            static_cast<float>(param1),
-            static_cast<float>(param2),
-            static_cast<float>(param3),
-            static_cast<float>(param4),
-            static_cast<float>(param5),
-            static_cast<float>(param6),
-            static_cast<float>(param7));
-    }
-
-    /// Callback for sendMavCommandWithHandle
+    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);
+
+    typedef enum {
+        MavCmdResultCommandResultOnly,          ///< commandResult specifies full success/fail info
+        MavCmdResultFailureNoResponseToCommand, ///< No response from vehicle to command
+        MavCmdResultFailureDuplicateCommand,    ///< Unabled to send command since duplicate is already being waited on for response
+    } MavCmdResultFailureCode_t;
+
+    /// Callback for sendMavCommandWithHandler
     ///     @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);
+    ///     @param failureCode          Failure reason
+    typedef void (*MavCmdResultHandler)(void* resultHandlerData, int compId, MAV_RESULT commandResult, MavCmdResultFailureCode_t failureCode);
 
     /// Sends the command and calls the callback with the result
     ///     @param resultHandler    Callback for result, nullptr for no callback
@@ -647,13 +652,14 @@ public:
         RequestMessageFailureCommandError,
         RequestMessageFailureCommandNotAcked,
         RequestMessageFailureMessageNotReceived,
+        RequestMessageFailureDuplicateCommand,    ///< Unabled to send command since duplicate is already being waited on for response
     } 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
+    ///     @param failureCode          Failure code
+    ///     @param message              Received message which was requested
     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.
@@ -829,12 +835,12 @@ signals:
     void mavlinkLogData                 (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t first_message, QByteArray data, bool acked);
 
     /// Signalled in response to usage of sendMavCommand
-    ///     @param vehicleId Vehicle which command was sent to
-    ///     @param component Component which command was sent to
-    ///     @param command MAV_CMD Command which was sent
-    ///     @param result MAV_RESULT returned in ack
-    ///     @param noResponseFromVehicle true: vehicle did not respond to command, false: vehicle responsed, MAV_RESULT in result
-    void mavCommandResult               (int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
+    ///     @param vehicleId        Vehicle which command was sent to
+    ///     @param targetComponent  Component which command was sent to
+    ///     @param command          Command which was sent
+    ///     @param ackResult        MAV_RESULT returned in ack
+    ///     @param failureCode      More detailed failure code Vehicle::MavCmdResultFailureCode_t
+    void mavCommandResult               (int vehicleId, int targetComponent, int command, int ackResult, int failureCode);
 
     // MAVlink Serial Data
     void mavlinkSerialControl           (uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);
@@ -866,7 +872,7 @@ private slots:
     void _firstMissionLoadComplete          ();
     void _firstGeoFenceLoadComplete         ();
     void _firstRallyPointLoadComplete       ();
-    void _sendMavCommandAgain               ();
+    void _sendMavCommandResponseTimeoutCheck();
     void _clearCameraTriggerPoints          ();
     void _updateDistanceHeadingToHome       ();
     void _updateMissionItemIndex            ();
@@ -922,7 +928,6 @@ private:
     void _handleMavlinkLoggingData      (mavlink_message_t& message);
     void _handleMavlinkLoggingDataAcked (mavlink_message_t& message);
     void _ackMavlinkLogData             (uint16_t sequence);
-    void _sendNextQueuedMavCommand      ();
     void _commonInit                    ();
     void _setupAutoDisarmSignalling     ();
     void _setCapabilities               (uint64_t capabilityBits);
@@ -936,7 +941,7 @@ private:
     void _chunkedStatusTextTimeout      (void);
     void _chunkedStatusTextCompleted    (uint8_t compId);
 
-    static void _rebootCommandResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle);
+    static void _rebootCommandResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, MavCmdResultFailureCode_t failureCode);
 
     int     _id;                    ///< Mavlink system id
     int     _defaultComponentId;
@@ -1137,29 +1142,36 @@ private:
         void*                       resultHandlerData;
     } RequestMessageInfo_t;
 
-    static void _requestMessageCmdResultHandler             (void* resultHandlerData, int compId, MAV_RESULT result, bool noResponsefromVehicle);
+    static void _requestMessageCmdResultHandler             (void* resultHandlerData, int compId, MAV_RESULT result, MavCmdResultFailureCode_t failureCode);
     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
+        int                 targetCompId        = MAV_COMP_ID_AUTOPILOT1;
+        bool                useCommandInt       = false;
         MAV_CMD             command;
         MAV_FRAME           frame;
-        float               rgParam[7];
-        bool                showError;
-        bool                requestMessage; // true: this is from a requestMessage call
+        float               rgParam[7]          = { 0 };
+        bool                showError           = true;
+        bool                requestMessage      = false;                        // 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);
+        void*               resultHandlerData   = nullptr;
+        int                 maxTries            = _mavCommandMaxRetryCount;
+        int                 tryCount            = 0;
+        QElapsedTimer       elapsedTimer;
+        int                 ackTimeoutMSecs     = _mavCommandAckTimeoutMSecs;
+    } MavCommandListEntry_t;
+
+    QList<MavCommandListEntry_t>    _mavCommandList;
+    QTimer                          _mavCommandResponseCheckTimer;
+    static const int                _mavCommandMaxRetryCount                = 3;
+    static const int                _mavCommandResponseCheckTimeoutMSecs    = 500;
+    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);
+    void _sendMavCommandFromList(MavCommandListEntry_t& commandEntry);
+    int  _findMavCommandListEntryIndex(int targetCompId, MAV_CMD command);
+    bool _sendMavCommandShouldRetry(MAV_CMD command);
 
     QMap<uint8_t /* batteryId */, uint8_t /* MAV_BATTERY_CHARGE_STATE_OK */> _lowestBatteryChargeStateAnnouncedMap;
 
@@ -1243,8 +1255,6 @@ private:
     // Settings keys
     static const char* _settingsGroup;
     static const char* _joystickEnabledSettingsKey;
-
-    friend class InitialConnectStateMachine;
-    friend class VehicleBatteryFactGroup;       // Allow VehicleBatteryFactGroup to call _addFactGroup
-    friend class VehicleLinkManager;
 };
+
+Q_DECLARE_METATYPE(Vehicle::MavCmdResultFailureCode_t)

src/comm/MockLink.cc

@@ -48,6 +48,13 @@ const char* MockConfiguration::_sendStatusTextKey       = "SendStatusText";
 const char* MockConfiguration::_incrementVehicleIdKey   = "IncrementVehicleId";
 const char* MockConfiguration::_failureModeKey          = "FailureMode";
 
+constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED;
+constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED;
+constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED;
+constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED;
+constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE;
+constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY;
+
 MockLink::MockLink(SharedLinkConfigurationPtr& config)
     : LinkInterface                         (config)
     , _missionItemHandler                   (this, qgcApp()->toolbox()->mavlinkProtocol())
@@ -985,6 +992,8 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
 
     mavlink_msg_command_long_decode(&msg, &request);
 
+    _sendMavCommandCountMap[static_cast<MAV_CMD>(request.command)]++;
+
     switch (request.command) {
     case MAV_CMD_COMPONENT_ARM_DISARM:
         if (request.param1 == 0.0f) {
@@ -1051,9 +1060,9 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
         }
         break;
     case MAV_CMD_MOCKLINK_NO_RESPONSE:
+    case MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY:
         // No response
         return;
-        break;
     }
 
     mavlink_message_t commandAck;

src/comm/MockLink.h

@@ -153,12 +153,16 @@ public:
     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 commands for testing COMMAND_LONG handlers. By default all commands except for MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY should retry.
+    static constexpr MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED            = MAV_CMD_USER_1;
+    static constexpr MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED              = MAV_CMD_USER_2;
+    static constexpr MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED    = MAV_CMD_USER_3;
+    static constexpr MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED      = MAV_CMD_USER_4;
+    static constexpr MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE                       = MAV_CMD_USER_5;
+    static constexpr MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY              = static_cast<MAV_CMD>(MAV_CMD_USER_5 + 1);
+
+    void clearSendMavCommandCounts(void) { _sendMavCommandCountMap.clear(); }
+    int sendMavCommandCount(MAV_CMD command) { return _sendMavCommandCountMap[command]; }
 
     // Special message ids for testing requestMessage support
     typedef enum {
@@ -237,7 +241,7 @@ private:
     int                         _mavlinkChannel;
 
     uint8_t                     _vehicleSystemId;
-    uint8_t                     _vehicleComponentId;
+    uint8_t                     _vehicleComponentId             = MAV_COMP_ID_AUTOPILOT1;
 
     bool                        _inNSH;
     bool                        _mavlinkStarted;
@@ -287,6 +291,7 @@ private:
 
     RequestMessageFailureMode_t _requestMessageFailureMode = FailRequestMessageNone;
 
+    QMap<MAV_CMD, int>  _sendMavCommandCountMap;
     QMap<int, QMap<QString, QVariant>>          _mapParamName2Value;
     QMap<int, QMap<QString, MAV_PARAM_TYPE>>    _mapParamName2MavParamType;
 

src/main.cc

@@ -289,6 +289,8 @@ int main(int argc, char *argv[])
 #endif
 #endif
 
+    qRegisterMetaType<Vehicle::MavCmdResultFailureCode_t>("Vehicle::MavCmdResultFailureCode_t");
+
     // We statically link our own QtLocation plugin
 
 #ifdef Q_OS_WIN

src/qgcunittest/UnitTestList.cc

@@ -44,7 +44,6 @@
 #include "CameraCalcTest.h"
 #include "FWLandingPatternTest.h"
 #include "RequestMessageTest.h"
-#include "InitialConnectTest.h"
 #include "FTPManagerTest.h"
 #include "MissionCommandTreeEditorTest.h"
 #include "VehicleLinkManagerTest.h"
@@ -60,7 +59,6 @@ UT_REGISTER_TEST(SendMavCommandWithSignallingTest)
 UT_REGISTER_TEST(SendMavCommandWithHandlerTest)
 UT_REGISTER_TEST(RequestMessageTest)
 UT_REGISTER_TEST(FTPManagerTest)
-UT_REGISTER_TEST(InitialConnectTest)
 UT_REGISTER_TEST(MissionItemTest)
 UT_REGISTER_TEST(SimpleMissionItemTest)
 UT_REGISTER_TEST(MissionControllerTest)

src/uas/UAS.cc

@@ -67,7 +67,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
     imagePackets(0),    // We must initialize to 0, otherwise extended data packets maybe incorrectly thought to be images
 
     blockHomePositionChanges(false),
-    receivedMode(false),
 
     // Note variances calculated from flight case from this log: http://dash.oznet.ch/view/MRjW8NUNYQSuSZkbn8dEjY
     // TODO: calibrate stand-still pixhawk variances
@@ -144,93 +143,8 @@ void UAS::receiveMessage(mavlink_message_t message)
     // and we already got one attitude packet
     if (message.sysid == uasId && (!attitudeStamped || lastAttitude != 0 || message.msgid == MAVLINK_MSG_ID_ATTITUDE))
     {
-        bool multiComponentSourceDetected = false;
-        bool wrongComponent = false;
-
-        switch (message.compid)
-        {
-        case MAV_COMP_ID_IMU_2:
-            // Prefer IMU 2 over IMU 1 (FIXME)
-            componentID[message.msgid] = MAV_COMP_ID_IMU_2;
-            break;
-        default:
-            // Do nothing
-            break;
-        }
-
-        // Store component ID
-        if (!componentID.contains(message.msgid))
-        {
-            // Prefer the first component
-            componentID[message.msgid] = message.compid;
-            componentMulti[message.msgid] = false;
-        }
-        else
-        {
-            // Got this message already
-            if (componentID[message.msgid] != message.compid)
-            {
-                componentMulti[message.msgid] = true;
-                wrongComponent = true;
-            }
-        }
-
-        if (componentMulti[message.msgid] == true) {
-            multiComponentSourceDetected = true;
-        }
-
-
         switch (message.msgid)
         {
-        case MAVLINK_MSG_ID_HEARTBEAT:
-        {
-            if (multiComponentSourceDetected && wrongComponent)
-            {
-                break;
-            }
-            mavlink_heartbeat_t state;
-            mavlink_msg_heartbeat_decode(&message, &state);
-
-            // Send the base_mode and system_status values to the plotter. This uses the ground time
-            // so the Ground Time checkbox must be ticked for these values to display
-            quint64 time = getUnixTime();
-            QString name = QString("M%1:HEARTBEAT.%2").arg(message.sysid);
-            emit valueChanged(uasId, name.arg("base_mode"), "bits", state.base_mode, time);
-            emit valueChanged(uasId, name.arg("custom_mode"), "bits", state.custom_mode, time);
-            emit valueChanged(uasId, name.arg("system_status"), "-", state.system_status, time);
-
-            // We got the mode
-            receivedMode = true;
-        }
-
-            break;
-
-        case MAVLINK_MSG_ID_SYS_STATUS:
-        {
-            if (multiComponentSourceDetected && wrongComponent)
-            {
-                break;
-            }
-            mavlink_sys_status_t state;
-            mavlink_msg_sys_status_decode(&message, &state);
-
-            // Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present.
-            quint64 time = getUnixTime();
-            QString name = QString("M%1:SYS_STATUS.%2").arg(message.sysid);
-            emit valueChanged(uasId, name.arg("sensors_enabled"), "bits", state.onboard_control_sensors_enabled, time);
-            emit valueChanged(uasId, name.arg("sensors_health"), "bits", state.onboard_control_sensors_health, time);
-            emit valueChanged(uasId, name.arg("errors_comm"), "-", state.errors_comm, time);
-            emit valueChanged(uasId, name.arg("errors_count1"), "-", state.errors_count1, time);
-            emit valueChanged(uasId, name.arg("errors_count2"), "-", state.errors_count2, time);
-            emit valueChanged(uasId, name.arg("errors_count3"), "-", state.errors_count3, time);
-            emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time);
-
-            // Process CPU load.
-            emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time);
-            emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time);
-        }
-            break;
-
         case MAVLINK_MSG_ID_PARAM_VALUE:
         {
             mavlink_param_value_t rawValue;
@@ -246,20 +160,6 @@ void UAS::receiveMessage(mavlink_message_t message)
             processParamValueMsg(message, parameterName,rawValue,paramVal);
          }
             break;
-        case MAVLINK_MSG_ID_ATTITUDE_TARGET:
-        {
-            mavlink_attitude_target_t out;
-            mavlink_msg_attitude_target_decode(&message, &out);
-            float roll, pitch, yaw;
-            mavlink_quaternion_to_euler(out.q, &roll, &pitch, &yaw);
-            quint64 time = getUnixTimeFromMs(out.time_boot_ms);
-
-            // For plotting emit roll sp, pitch sp and yaw sp values
-            emit valueChanged(uasId, "roll sp", "rad", roll, time);
-            emit valueChanged(uasId, "pitch sp", "rad", pitch, time);
-            emit valueChanged(uasId, "yaw sp", "rad", yaw, time);
-        }
-            break;
 
         case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE:
         {

src/uas/UAS.h

@@ -149,7 +149,6 @@ protected:
     QImage image;               ///< Image data of last completely transmitted image
     quint64 imageStart;
     bool blockHomePositionChanges;   ///< Block changes to the home position
-    bool receivedMode;          ///< True if mode was retrieved from current conenction to UAS
 
     /// SIMULATION NOISE
     float xacc_var;             ///< variance of x acclerometer noise for HIL sim (mg)
@@ -199,9 +198,6 @@ protected:
 
     virtual void processParamValueMsg(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue);
 
-    QMap<int, int>componentID;
-    QMap<int, bool>componentMulti;
-
     bool connectionLost; ///< Flag indicates a timed out connection
     quint64 connectionLossTime; ///< Time the connection was interrupted
     quint64 lastVoltageWarning; ///< Time at which the last voltage warning occurred

src/uas/UASInterface.h

@@ -49,20 +49,6 @@ signals:
     /** @brief The robot is disconnected **/
     void disconnected();
 
-    /** @brief A value of the robot has changed.
-      *
-      * Typically this is used to send lowlevel information like the battery voltage to the plotting facilities of
-      * the groundstation. The data here should be converted to human-readable values before being passed, so ideally
-      * SI units.
-      *
-      * @param uasId ID of this system
-      * @param name name of the value, e.g. "battery voltage"
-      * @param unit The units this variable is in as an abbreviation. For system-dependent (such as raw ADC values) use "raw", for bitfields use "bits", for true/false or on/off use "bool", for unitless values use "-".
-      * @param value the value that changed
-      * @param msec the timestamp of the message, in milliseconds
-      */
-    void valueChanged(const int uasid, const QString& name, const QString& unit, const QVariant &value,const quint64 msecs);
-
     void parameterUpdate(int uas, int component, QString parameterName, int parameterCount, int parameterId, int type, QVariant value);
 
     /**