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