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.cc

@@ -49,6 +49,9 @@
 #include "ComponentInformationManager.h"
 #include "InitialConnectStateMachine.h"
 #include "VehicleBatteryFactGroup.h"
+#ifdef QT_DEBUG
+#include "MockLink.h"
+#endif
 
 #if defined(QGC_AIRMAP_ENABLED)
 #include "AirspaceVehicleManager.h"
@@ -201,9 +204,10 @@ Vehicle::Vehicle(LinkInterface*             link,
     _prearmErrorTimer.setSingleShot(true);
 
     // Send MAV_CMD ack timer
-    _mavCommandAckTimer.setSingleShot(true);
-    _mavCommandAckTimer.setInterval(link->linkConfiguration()->isHighLatency() ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
-    connect(&_mavCommandAckTimer, &QTimer::timeout, this, &Vehicle::_sendMavCommandAgain);
+    _mavCommandResponseCheckTimer.setSingleShot(false);
+    _mavCommandResponseCheckTimer.setInterval(_mavCommandResponseCheckTimeoutMSecs);
+    _mavCommandResponseCheckTimer.start();
+    connect(&_mavCommandResponseCheckTimer, &QTimer::timeout, this, &Vehicle::_sendMavCommandResponseTimeoutCheck);
 
     // Chunked status text timeout timer
     _chunkedStatusTextTimer.setSingleShot(true);
@@ -2511,6 +2515,20 @@ void Vehicle::sendMavCommand(int compId, MAV_CMD command, bool showError, float
                           param1, param2, param3, param4, param5, param6, param7);
 }
 
+void Vehicle::sendCommand(int compId, int command, bool showError, double param1, double param2, double param3, double param4, double param5, double param6, double param7)
+{
+    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));
+}
+
 void Vehicle::sendMavCommandWithHandler(MavCmdResultHandler resultHandler, void *resultHandlerData, int compId, MAV_CMD command, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
 {
     _sendMavCommandWorker(false,                // commandInt
@@ -2537,12 +2555,77 @@ void Vehicle::sendMavCommandInt(int compId, MAV_CMD command, MAV_FRAME frame, bo
                           param1, param2, param3, param4, param5, param6, param7);
 }
 
-void Vehicle::_sendMavCommandWorker(bool commandInt, bool requestMessage, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
+int Vehicle::_findMavCommandListEntryIndex(int targetCompId, MAV_CMD command)
+{
+    for (int i=0; i<_mavCommandList.count(); i++) {
+        const MavCommandListEntry_t& entry = _mavCommandList[i];
+        if (entry.targetCompId == targetCompId && entry.command == command) {
+            return i;
+        }
+    }
+
+    return -1;
+}
+
+bool Vehicle::_sendMavCommandShouldRetry(MAV_CMD command)
+{
+    switch (command) {
+#ifdef QT_DEBUG
+    // These MockLink command should be retried so we can create unit tests to test retry code
+    case MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED:
+    case MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED:
+    case MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED:
+    case MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED:
+    case MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE:
+        return true;
+#endif
+
+    // In general we should not retry any commands. This is for safety reasons. For example you don't want an ARM command
+    // to timeout with no response over a noisy link twice and then suddenly have the third try work 6 seconds later. At that
+    // point the user could have walked up to the vehicle to see what is going wrong.
+    //
+    // We do retry commands which are part of the initial vehicle connect sequence. This makes this process work better over noisy
+    // links where commands could be lost. Also these commands tend to just be requesting status so if they end up being delayed
+    // there are no safety concerns that could occur.
+    case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
+    case MAV_CMD_REQUEST_PROTOCOL_VERSION:
+    case MAV_CMD_REQUEST_MESSAGE:
+    case MAV_CMD_PREFLIGHT_STORAGE:
+        return true;
+
+    default:
+        return false;
+    }
+}
+
+void Vehicle::_sendMavCommandWorker(bool commandInt, bool requestMessage, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int targetCompId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
 {
-    MavCommandQueueEntry_t entry;
+    int entryIndex = _findMavCommandListEntryIndex(targetCompId, command);
+    if (entryIndex != -1 || targetCompId == MAV_COMP_ID_ALL) {
+        bool    compIdAll       = targetCompId == MAV_COMP_ID_ALL;
+        QString rawCommandName  = _toolbox->missionCommandTree()->rawName(command);
+
+        qCDebug(VehicleLog) << QStringLiteral("_sendMavCommandWorker failing %1").arg(compIdAll ? "MAV_COMP_ID_ALL not supportded" : "duplicate command") << rawCommandName;
+
+        // If we send multiple versions of the same command to a component there is no way to discern which COMMAND_ACK we get back goes with which.
+        // Because of this we fail in that case.
+        MavCmdResultFailureCode_t failureCode = compIdAll ? MavCmdResultCommandResultOnly : MavCmdResultFailureDuplicateCommand;
+        if (resultHandler) {
+            (*resultHandler)(resultHandlerData, targetCompId, MAV_RESULT_FAILED, failureCode);
+        } else {
+            emit mavCommandResult(_id, targetCompId, command, MAV_RESULT_FAILED, failureCode);
+        }
+        if (showError) {
+            qgcApp()->showAppMessage(tr("Unabled to send command: %1. %2").arg(compIdAll ? tr("Internal error") : tr("Waiting on previous response to same command.")));
+        }
+
+        return;
+    }
 
-    entry.commandInt        = commandInt;
-    entry.compId            = compId;
+    MavCommandListEntry_t entry;
+
+    entry.useCommandInt     = commandInt;
+    entry.targetCompId      = targetCompId;
     entry.command           = command;
     entry.frame             = frame;
     entry.showError         = showError;
@@ -2556,74 +2639,61 @@ void Vehicle::_sendMavCommandWorker(bool commandInt, bool requestMessage, bool s
     entry.rgParam[4]        = param5;
     entry.rgParam[5]        = param6;
     entry.rgParam[6]        = param7;
+    entry.maxTries          = _sendMavCommandShouldRetry(command) ? _mavCommandMaxRetryCount : 1;
+    entry.ackTimeoutMSecs   = _vehicleLinkManager->primaryLink()->linkConfiguration()->isHighLatency() ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs;
+    entry.elapsedTimer.start();
 
-    _mavCommandQueue.enqueue(entry);
-
-    if (_mavCommandQueue.count() == 1) {
-        _mavCommandRetryCount = 0;
-        _sendMavCommandAgain();
-    }
+    _mavCommandList.append(entry);
+    _sendMavCommandFromList(_mavCommandList.last());
 }
 
-void Vehicle::_sendMavCommandAgain()
+void Vehicle::_sendMavCommandFromList(MavCommandListEntry_t& commandEntry)
 {
-    if(_mavCommandQueue.isEmpty()) {
-        qWarning() << "Command resend with no commands in queue";
-        _mavCommandAckTimer.stop();
-        return;
-    }
-
-    MavCommandQueueEntry_t& queuedCommand   = _mavCommandQueue[0];
-    QString                 rawCommandName  =_toolbox->missionCommandTree()->rawName(queuedCommand.command);
+    QString rawCommandName  = _toolbox->missionCommandTree()->rawName(commandEntry.command);
 
-    if (_mavCommandRetryCount++ >= _mavCommandMaxRetryCount) {
-        if (queuedCommand.resultHandler) {
-            (*queuedCommand.resultHandler)(queuedCommand.resultHandlerData, queuedCommand.compId, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
+    if (++commandEntry.tryCount > commandEntry.maxTries) {
+        qCDebug(VehicleLog) << "_sendMavCommandFromList giving up after max retries" << rawCommandName;
+        if (commandEntry.resultHandler) {
+            (*commandEntry.resultHandler)(commandEntry.resultHandlerData, commandEntry.targetCompId, MAV_RESULT_FAILED, MavCmdResultFailureNoResponseToCommand);
         } else {
-            emit mavCommandResult(_id, queuedCommand.compId, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
+            emit mavCommandResult(_id, commandEntry.targetCompId, commandEntry.command, MAV_RESULT_FAILED, MavCmdResultFailureNoResponseToCommand);
         }
-        if (queuedCommand.showError) {
+        if (commandEntry.showError) {
             qgcApp()->showAppMessage(tr("Vehicle did not respond to command: %1").arg(rawCommandName));
         }
-        _mavCommandQueue.dequeue();
-        _sendNextQueuedMavCommand();
+        _mavCommandList.removeAt(_findMavCommandListEntryIndex(commandEntry.targetCompId, commandEntry.command));
         return;
     }
 
-    if (_mavCommandRetryCount > 1) {
-        if (!px4Firmware() && queuedCommand.command == MAV_CMD_START_RX_PAIR) {
-            // The implementation of this command comes from the IO layer and is shared across stacks. So for other firmwares
-            // we aren't really sure whether they are correct or not.
-            return;
-        }
-        qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << rawCommandName << _mavCommandRetryCount;
+    if (commandEntry.tryCount > 1 && !px4Firmware() && commandEntry.command == MAV_CMD_START_RX_PAIR) {
+        // The implementation of this command comes from the IO layer and is shared across stacks. So for other firmwares
+        // we aren't really sure whether they are correct or not.
+        return;
     }
 
-    _mavCommandAckTimer.start();
-
-    if (queuedCommand.requestMessage) {
-        RequestMessageInfo_t* pInfo = static_cast<RequestMessageInfo_t*>(queuedCommand.resultHandlerData);
+    if (commandEntry.requestMessage) {
+        RequestMessageInfo_t* pInfo = static_cast<RequestMessageInfo_t*>(commandEntry.resultHandlerData);
         _waitForMavlinkMessage(_requestMessageWaitForMessageResultHandler, pInfo, pInfo->msgId, 1000);
     }
 
-    qCDebug(VehicleLog) << "_sendMavCommandAgain sending name:retry" << rawCommandName << _mavCommandRetryCount;
+    qCDebug(VehicleLog) << "_sendMavCommandFromList command:tryCount" << rawCommandName << commandEntry.tryCount;
 
     mavlink_message_t       msg;
-    if (queuedCommand.commandInt) {
+    if (commandEntry.useCommandInt) {
         mavlink_command_int_t  cmd;
 
         memset(&cmd, 0, sizeof(cmd));
         cmd.target_system =     _id;
-        cmd.target_component =  queuedCommand.compId;
-        cmd.command =           queuedCommand.command;
-        cmd.frame =             queuedCommand.frame;
-        cmd.param1 =            queuedCommand.rgParam[0];
-        cmd.param2 =            queuedCommand.rgParam[1];
-        cmd.param3 =            queuedCommand.rgParam[2];
-        cmd.param4 =            queuedCommand.rgParam[3];
-        cmd.x =                 queuedCommand.rgParam[4] * qPow(10.0, 7.0);
-        cmd.y =                 queuedCommand.rgParam[5] * qPow(10.0, 7.0);
-        cmd.z =                 queuedCommand.rgParam[6];
+        cmd.target_component =  commandEntry.targetCompId;
+        cmd.command =           commandEntry.command;
+        cmd.frame =             commandEntry.frame;
+        cmd.param1 =            commandEntry.rgParam[0];
+        cmd.param2 =            commandEntry.rgParam[1];
+        cmd.param3 =            commandEntry.rgParam[2];
+        cmd.param4 =            commandEntry.rgParam[3];
+        cmd.x =                 commandEntry.rgParam[4] * qPow(10.0, 7.0);
+        cmd.y =                 commandEntry.rgParam[5] * qPow(10.0, 7.0);
+        cmd.z =                 commandEntry.rgParam[6];
         mavlink_msg_command_int_encode_chan(_mavlink->getSystemId(),
                                             _mavlink->getComponentId(),
                                             vehicleLinkManager()->primaryLink()->mavlinkChannel(),
@@ -2634,16 +2704,16 @@ void Vehicle::_sendMavCommandAgain()
 
         memset(&cmd, 0, sizeof(cmd));
         cmd.target_system =     _id;
-        cmd.target_component =  queuedCommand.compId;
-        cmd.command =           queuedCommand.command;
+        cmd.target_component =  commandEntry.targetCompId;
+        cmd.command =           commandEntry.command;
         cmd.confirmation =      0;
-        cmd.param1 =            queuedCommand.rgParam[0];
-        cmd.param2 =            queuedCommand.rgParam[1];
-        cmd.param3 =            queuedCommand.rgParam[2];
-        cmd.param4 =            queuedCommand.rgParam[3];
-        cmd.param5 =            queuedCommand.rgParam[4];
-        cmd.param6 =            queuedCommand.rgParam[5];
-        cmd.param7 =            queuedCommand.rgParam[6];
+        cmd.param1 =            commandEntry.rgParam[0];
+        cmd.param2 =            commandEntry.rgParam[1];
+        cmd.param3 =            commandEntry.rgParam[2];
+        cmd.param4 =            commandEntry.rgParam[3];
+        cmd.param5 =            commandEntry.rgParam[4];
+        cmd.param6 =            commandEntry.rgParam[5];
+        cmd.param7 =            commandEntry.rgParam[6];
         mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
                                              _mavlink->getComponentId(),
                                              vehicleLinkManager()->primaryLink()->mavlinkChannel(),
@@ -2654,11 +2724,19 @@ void Vehicle::_sendMavCommandAgain()
     sendMessageOnLinkThreadSafe(vehicleLinkManager()->primaryLink(), msg);
 }
 
-void Vehicle::_sendNextQueuedMavCommand()
+void Vehicle::_sendMavCommandResponseTimeoutCheck(void)
 {
-    if (!_mavCommandQueue.isEmpty()) {
-        _mavCommandRetryCount = 0;
-        _sendMavCommandAgain();
+    if (_mavCommandList.isEmpty()) {
+        return;
+    }
+
+    // Walk the list backwards since _sendMavCommandFromList can remove entries
+    for (int i=_mavCommandList.count()-1; i>=0; i--) {
+        MavCommandListEntry_t& commandEntry = _mavCommandList[i];
+        if (commandEntry.elapsedTimer.elapsed() > commandEntry.ackTimeoutMSecs) {
+            // Try sending command again
+            _sendMavCommandFromList(commandEntry);
+        }
     }
 }
 
@@ -2690,67 +2768,61 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
     }
 #endif
 
-    if (!_mavCommandQueue.isEmpty() && ack.command == _mavCommandQueue.head().command) {
-        bool                    sendNextCommand = false;
-        MavCommandQueueEntry_t  commandEntry = _mavCommandQueue.head();
-        _mavCommandAckTimer.stop();
-
-        if (commandEntry.requestMessage) {
-            RequestMessageInfo_t* pInfo = static_cast<RequestMessageInfo_t*>(commandEntry.resultHandlerData);
-            pInfo->commandAckReceived = true;
-            if (ack.result == MAV_RESULT_ACCEPTED) {
-                if (pInfo->messageReceived) {
-                    delete pInfo;
-                    sendNextCommand = true;
+    int entryIndex = _findMavCommandListEntryIndex(message.compid, static_cast<MAV_CMD>(ack.command));
+    if (entryIndex != -1) {
+        const MavCommandListEntry_t& commandEntry = _mavCommandList[entryIndex];
+        if (commandEntry.command == ack.command) {
+            if (commandEntry.requestMessage) {
+                RequestMessageInfo_t* pInfo = static_cast<RequestMessageInfo_t*>(commandEntry.resultHandlerData);
+                pInfo->commandAckReceived = true;
+                if (ack.result == MAV_RESULT_ACCEPTED) {
+                    if (pInfo->messageReceived) {
+                        delete pInfo;
+                    } else {
+                        _waitForMavlinkMessageTimeoutActive = true;
+                        _waitForMavlinkMessageElapsed.restart();
+                    }
                 } else {
-                    // We dont set sendNextCommand because we wait for the message wait to complete before sending next message
-                    _waitForMavlinkMessageTimeoutActive = true;
-                    _waitForMavlinkMessageElapsed.restart();
+                    if (pInfo->messageReceived) {
+                        qCWarning(VehicleLog) << "Internal Error: _handleCommandAck for requestMessage with result failure, but message already received";
+                    } else {
+                        _waitForMavlinkMessageClear();
+                        (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast<MAV_RESULT>(ack.result), MavCmdResultCommandResultOnly);
+                    }
                 }
             } else {
-                sendNextCommand = true;
-                if (pInfo->messageReceived) {
-                    qCWarning(VehicleLog) << "Internal Error: _handleCommandAck for requestMessage with result failure, but message already received";
+                if (commandEntry.resultHandler) {
+                    (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast<MAV_RESULT>(ack.result), MavCmdResultCommandResultOnly);
                 } else {
-                    _waitForMavlinkMessageClear();
-                    (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast<MAV_RESULT>(ack.result), false /* noResponsefromVehicle */);
-                }
-            }
-        } else {
-            if (commandEntry.resultHandler) {
-                (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast<MAV_RESULT>(ack.result), false /* noResponsefromVehicle */);
-            } else {
-                if (commandEntry.showError) {
-                    switch (ack.result) {
-                    case MAV_RESULT_TEMPORARILY_REJECTED:
-                        qgcApp()->showAppMessage(tr("%1 command temporarily rejected").arg(rawCommandName));
-                        break;
-                    case MAV_RESULT_DENIED:
-                        qgcApp()->showAppMessage(tr("%1 command denied").arg(rawCommandName));
-                        break;
-                    case MAV_RESULT_UNSUPPORTED:
-                        qgcApp()->showAppMessage(tr("%1 command not supported").arg(rawCommandName));
-                        break;
-                    case MAV_RESULT_FAILED:
-                        qgcApp()->showAppMessage(tr("%1 command failed").arg(rawCommandName));
-                        break;
-                    default:
-                        // Do nothing
-                        break;
+                    if (commandEntry.showError) {
+                        switch (ack.result) {
+                        case MAV_RESULT_TEMPORARILY_REJECTED:
+                            qgcApp()->showAppMessage(tr("%1 command temporarily rejected").arg(rawCommandName));
+                            break;
+                        case MAV_RESULT_DENIED:
+                            qgcApp()->showAppMessage(tr("%1 command denied").arg(rawCommandName));
+                            break;
+                        case MAV_RESULT_UNSUPPORTED:
+                            qgcApp()->showAppMessage(tr("%1 command not supported").arg(rawCommandName));
+                            break;
+                        case MAV_RESULT_FAILED:
+                            qgcApp()->showAppMessage(tr("%1 command failed").arg(rawCommandName));
+                            break;
+                        default:
+                            // Do nothing
+                            break;
+                        }
                     }
+                    emit mavCommandResult(_id, message.compid, ack.command, ack.result, MavCmdResultCommandResultOnly);
                 }
-                emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);
             }
-            sendNextCommand = true;
-        }
 
-        _mavCommandQueue.dequeue();
-        if (sendNextCommand) {
-            _sendNextQueuedMavCommand();
+            _mavCommandList.removeAt(entryIndex);
+            return;
         }
-    } else {
-        qCDebug(VehicleLog) << "_handleCommandAck Ack not in queue" << rawCommandName;
     }
+
+    qCDebug(VehicleLog) << "_handleCommandAck Ack not in list" << rawCommandName;
 }
 
 void Vehicle::_waitForMavlinkMessage(WaitForMavlinkMessageResultHandler resultHandler, void* resultHandlerData, int messageId, int timeoutMsecs)
@@ -2804,7 +2876,7 @@ void Vehicle::requestMessage(RequestMessageResultHandler resultHandler, void* re
                           true,                                     // requestMessage,
                           false,                                    // showError
                           _requestMessageCmdResultHandler,
-                          pInfo,
+                          pInfo,                                    // resultHandlerData
                           compId,
                           MAV_CMD_REQUEST_MESSAGE,
                           MAV_FRAME_GLOBAL,
@@ -2812,14 +2884,28 @@ void Vehicle::requestMessage(RequestMessageResultHandler resultHandler, void* re
                           param1, param2, param3, param4, param5, 0);
 }
 
-void Vehicle::_requestMessageCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, bool noResponsefromVehicle)
+void Vehicle::_requestMessageCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, MavCmdResultFailureCode_t failureCode)
 {
     RequestMessageInfo_t* pInfo   = static_cast<RequestMessageInfo_t*>(resultHandlerData);
 
     pInfo->commandAckReceived = true;
     if (result != MAV_RESULT_ACCEPTED) {
-        mavlink_message_t message;
-        (*pInfo->resultHandler)(pInfo->resultHandlerData, result,  noResponsefromVehicle ? RequestMessageFailureCommandNotAcked : RequestMessageFailureCommandError, message);
+        mavlink_message_t                           message;
+        RequestMessageResultHandlerFailureCode_t    requestMessageFailureCode;
+
+        switch (failureCode) {
+        case Vehicle::MavCmdResultCommandResultOnly:
+            requestMessageFailureCode = RequestMessageFailureCommandError;
+            break;
+        case Vehicle::MavCmdResultFailureNoResponseToCommand:
+            requestMessageFailureCode = RequestMessageFailureCommandNotAcked;
+            break;
+        case Vehicle::MavCmdResultFailureDuplicateCommand:
+            requestMessageFailureCode = RequestMessageFailureDuplicateCommand;
+            break;
+        }
+
+        (*pInfo->resultHandler)(pInfo->resultHandlerData, result,  requestMessageFailureCode, message);
     }
     if (pInfo->messageReceived) {
         delete pInfo;
@@ -2882,15 +2968,21 @@ QString Vehicle::firmwareVersionTypeString() const
     }
 }
 
-void Vehicle::_rebootCommandResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT commandResult, bool noResponsefromVehicle)
+void Vehicle::_rebootCommandResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT commandResult, MavCmdResultFailureCode_t failureCode)
 {
     Vehicle* vehicle = static_cast<Vehicle*>(resultHandlerData);
 
     if (commandResult != MAV_RESULT_ACCEPTED) {
-        if (noResponsefromVehicle) {
-            qCDebug(VehicleLog) << "MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN no response from vehicle";
-        } else {
+        switch (failureCode) {
+        case MavCmdResultCommandResultOnly:
             qCDebug(VehicleLog) << QStringLiteral("MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN error(%1)").arg(commandResult);
+            break;
+        case MavCmdResultFailureNoResponseToCommand:
+            qCDebug(VehicleLog) << "MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN failed: no response from vehicle";
+            break;
+        case MavCmdResultFailureDuplicateCommand:
+            qCDebug(VehicleLog) << "MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN failed: duplicate command";
+            break;
         }
         qgcApp()->showAppMessage(tr("Vehicle reboot failed."));
     } else {

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);
 
     /**