diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 3590ce30d38e8f9666798ff398427eb6d232c0bd..03c3491e768066d333f729f21c42865c9e814b70 100644 --- a/qgroundcontrol.pro +++ b/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 \ diff --git a/src/Vehicle/InitialConnectStateMachine.cc b/src/Vehicle/InitialConnectStateMachine.cc index e2dd953002a1340dc8d9c91a2b30a8222a870e6d..2a393fb03e276167e10529beb157855e05286426 100644 --- a/src/Vehicle/InitialConnectStateMachine.cc +++ b/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(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(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 diff --git a/src/Vehicle/InitialConnectStateMachine.h b/src/Vehicle/InitialConnectStateMachine.h index ab196c11a84ea16f3619506e3ac66c84ffd0335e..461cb1f17796404eafc3990327961002260647b2 100644 --- a/src/Vehicle/InitialConnectStateMachine.h +++ b/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); diff --git a/src/Vehicle/InitialConnectTest.cc b/src/Vehicle/InitialConnectTest.cc deleted file mode 100644 index 38a515f2e6ac54d06a4084ab4c40996d119ebd49..0000000000000000000000000000000000000000 --- a/src/Vehicle/InitialConnectTest.cc +++ /dev/null @@ -1,72 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * 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(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); - } -} diff --git a/src/Vehicle/InitialConnectTest.h b/src/Vehicle/InitialConnectTest.h deleted file mode 100644 index ab70af4574d505bfa25ec7e1dbcf8e95c4ef3dfe..0000000000000000000000000000000000000000 --- a/src/Vehicle/InitialConnectTest.h +++ /dev/null @@ -1,42 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * 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[]; -}; diff --git a/src/Vehicle/RequestMessageTest.cc b/src/Vehicle/RequestMessageTest.cc index c955470015303d45a1190d9e90b02b389ef93047..a09727065d8312a167f255b14ef498a4da5453db 100644 --- a/src/Vehicle/RequestMessageTest.cc +++ b/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(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(); +} diff --git a/src/Vehicle/RequestMessageTest.h b/src/Vehicle/RequestMessageTest.h index af01f4a4549a764887ee03608e9aafc554db07d3..3bef4ac8b41bfd1527c892a048d47b0d305858a2 100644 --- a/src/Vehicle/RequestMessageTest.h +++ b/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; diff --git a/src/Vehicle/SendMavCommandWithHandlerTest.cc b/src/Vehicle/SendMavCommandWithHandlerTest.cc index 68e4e7eb4eed150aad3ea505ccb79aa0e87d86eb..6dfbf64211275a98975e3c28d253c3588bd31700 100644 --- a/src/Vehicle/SendMavCommandWithHandlerTest.cc +++ b/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(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(); +} diff --git a/src/Vehicle/SendMavCommandWithHandlerTest.h b/src/Vehicle/SendMavCommandWithHandlerTest.h index 99e8cb5921668b09ca43f9b6c49219c18de0fb49..ba1da91fad04fb089213e112b06eb168cebd82e2 100644 --- a/src/Vehicle/SendMavCommandWithHandlerTest.h +++ b/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[]; }; diff --git a/src/Vehicle/SendMavCommandWithSignallingTest.cc b/src/Vehicle/SendMavCommandWithSignallingTest.cc index 3ff14a1c1d321a4d637e05e2801af69e61d13bd8..a5b0139bcc064fea39e0630f9539d8068aa30ef6 100644 --- a/src/Vehicle/SendMavCommandWithSignallingTest.cc +++ b/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 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 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 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 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(), 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 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 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 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::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); } diff --git a/src/Vehicle/SendMavCommandWithSignallingTest.h b/src/Vehicle/SendMavCommandWithSignallingTest.h index 48c23cc58eea28be15169b13165a7eac959b8bbd..40b01e38676e1b8a45ed3219bbda6afb52e91a30 100644 --- a/src/Vehicle/SendMavCommandWithSignallingTest.h +++ b/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[]; }; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index ece41e82ffa4889bcbfcede5abcc065e887d7ec3..d9f98063364deccc2f9efc75990ea2e3d1e58b6b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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(command), + showError, + static_cast(param1), + static_cast(param2), + static_cast(param3), + static_cast(param4), + static_cast(param5), + static_cast(param6), + static_cast(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(queuedCommand.resultHandlerData); + if (commandEntry.requestMessage) { + RequestMessageInfo_t* pInfo = static_cast(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(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(ack.command)); + if (entryIndex != -1) { + const MavCommandListEntry_t& commandEntry = _mavCommandList[entryIndex]; + if (commandEntry.command == ack.command) { + if (commandEntry.requestMessage) { + RequestMessageInfo_t* pInfo = static_cast(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(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(ack.result), MavCmdResultCommandResultOnly); } else { - _waitForMavlinkMessageClear(); - (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast(ack.result), false /* noResponsefromVehicle */); - } - } - } else { - if (commandEntry.resultHandler) { - (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast(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(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(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 { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index cb495c8d9374f838ae338a0baf346bbdc930fcdf..b306fc81670dea1ecf5badbbdc46be3003f3c4b8 100644 --- a/src/Vehicle/Vehicle.h +++ b/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(command), - showError, - static_cast(param1), - static_cast(param2), - static_cast(param3), - static_cast(param4), - static_cast(param5), - static_cast(param6), - static_cast(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 _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 _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 _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) diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index eef2da11afb0dca1dbfff597c56675ec06dbb1d5..59a61209dcf35c0da8c79c7ad2fc2da0f8b41a35 100644 --- a/src/comm/MockLink.cc +++ b/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(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; diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 0cca6338e867ba952e45d1afe522c888a22e4f6f..ef86a6b147945a92d8d56c81168d833db641c9da 100644 --- a/src/comm/MockLink.h +++ b/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_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 _sendMavCommandCountMap; QMap> _mapParamName2Value; QMap> _mapParamName2MavParamType; diff --git a/src/main.cc b/src/main.cc index 945d2143a8fe1a3464867e880dfd18437cb9fd48..f84831aea2063c903ab4348ede002d6959bf4121 100644 --- a/src/main.cc +++ b/src/main.cc @@ -289,6 +289,8 @@ int main(int argc, char *argv[]) #endif #endif + qRegisterMetaType("Vehicle::MavCmdResultFailureCode_t"); + // We statically link our own QtLocation plugin #ifdef Q_OS_WIN diff --git a/src/qgcunittest/UnitTestList.cc b/src/qgcunittest/UnitTestList.cc index f5c7b43540c8a6e503ecd70b657fcbf1b5c7af36..98c43aa60089045db4da36c7fcdc7b07cb1d1af6 100644 --- a/src/qgcunittest/UnitTestList.cc +++ b/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) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 8a1accec95c0129a6e049f595a2ea04392793d8e..0a7928cfc6ba071f8d3aa835a6e61cfa106de731 100644 --- a/src/uas/UAS.cc +++ b/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: { diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 0a3e6dfbbc576a356d76933271a47c91c5855512..7504a0b56136d883aa4e2f2190b6ee1c3f44f927 100644 --- a/src/uas/UAS.h +++ b/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); - QMapcomponentID; - QMapcomponentMulti; - 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 diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index b612372ada3910e63f1bc735ecfa77e9d0fce35f..601a15497ef404b68b7bce53537d434fb0c21afc 100644 --- a/src/uas/UASInterface.h +++ b/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); /**