diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc index fefdaa87f21b5ed3967444965f328feca22878a9..01c1f606da1bad14ecc07b951f4961da205a5c08 100644 --- a/src/MissionManager/MissionManagerTest.cc +++ b/src/MissionManager/MissionManagerTest.cc @@ -27,9 +27,9 @@ MissionManagerTest::MissionManagerTest(void) } -void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail) +void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult, bool shouldFail) { - _mockLink->setMissionItemFailureMode(failureMode); + _mockLink->setMissionItemFailureMode(failureMode, failureAckResult); // Setup our test case data QList missionItems; @@ -115,11 +115,11 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f _multiSpyMissionManager->clearAllSignals(); } -void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail) +void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult, bool shouldFail) { - _writeItems(MockLinkMissionItemHandler::FailNone, false); + _writeItems(MockLinkMissionItemHandler::FailNone, failureAckResult, false); - _mockLink->setMissionItemFailureMode(failureMode); + _mockLink->setMissionItemFailureMode(failureMode, failureAckResult); // Read the items back from the vehicle _missionManager->loadFromVehicle(); @@ -253,8 +253,8 @@ void MissionManagerTest::_testWriteFailureHandlingWorker(void) for (size_t i=0; ifailureText; - _writeItems(pCase->failureMode, pCase->shouldFail); + qDebug() << "TEST CASE _testWriteFailureHandlingWorker" << pCase->failureText; + _writeItems(pCase->failureMode, MAV_MISSION_ERROR, pCase->shouldFail); _mockLink->resetMissionItemHandler(); } } @@ -298,8 +298,8 @@ void MissionManagerTest::_testReadFailureHandlingWorker(void) for (size_t i=0; ifailureText; - _roundTripItems(pCase->failureMode, pCase->shouldFail); + qDebug() << "TEST CASE _testReadFailureHandlingWorker" << pCase->failureText; + _roundTripItems(pCase->failureMode, MAV_MISSION_ERROR, pCase->shouldFail); _mockLink->resetMissionItemHandler(); _multiSpyMissionManager->clearAllSignals(); } @@ -329,3 +329,34 @@ void MissionManagerTest::_testReadFailureHandlingPX4(void) _initForFirmwareType(MAV_AUTOPILOT_PX4); _testReadFailureHandlingWorker(); } + +void MissionManagerTest::_testErrorAckFailureStrings(void) +{ + _initForFirmwareType(MAV_AUTOPILOT_PX4); + + typedef struct { + const char* ackResultStr; + MAV_MISSION_RESULT ackResult; + } ErrorStringTestCase_t; + + static const ErrorStringTestCase_t rgTestCases[] = { + { "MAV_MISSION_UNSUPPORTED_FRAME", MAV_MISSION_UNSUPPORTED_FRAME }, + { "MAV_MISSION_UNSUPPORTED", MAV_MISSION_UNSUPPORTED }, + { "MAV_MISSION_INVALID_PARAM1", MAV_MISSION_INVALID_PARAM1 }, + { "MAV_MISSION_INVALID_PARAM2", MAV_MISSION_INVALID_PARAM2 }, + { "MAV_MISSION_INVALID_PARAM3", MAV_MISSION_INVALID_PARAM3 }, + { "MAV_MISSION_INVALID_PARAM4", MAV_MISSION_INVALID_PARAM4 }, + { "MAV_MISSION_INVALID_PARAM5_X", MAV_MISSION_INVALID_PARAM5_X }, + { "MAV_MISSION_INVALID_PARAM6_Y", MAV_MISSION_INVALID_PARAM6_Y }, + { "MAV_MISSION_INVALID_PARAM7", MAV_MISSION_INVALID_PARAM7 }, + { "MAV_MISSION_INVALID_SEQUENCE", MAV_MISSION_INVALID_SEQUENCE }, + }; + + for (size_t i=0; iackResultStr; + _writeItems(MockLinkMissionItemHandler::FailWriteRequest1ErrorAck, pCase->ackResult, true /* shouldFail */); + _mockLink->resetMissionItemHandler(); + } + +} diff --git a/src/MissionManager/MissionManagerTest.h b/src/MissionManager/MissionManagerTest.h index 7d3b4d5926077e7aa302c522b5c9a6c1db858dae..0d3d893f5cfb75fd60859b71574f57d9f36772d9 100644 --- a/src/MissionManager/MissionManagerTest.h +++ b/src/MissionManager/MissionManagerTest.h @@ -31,10 +31,11 @@ private slots: void _testWriteFailureHandlingAPM(void); void _testReadFailureHandlingPX4(void); void _testReadFailureHandlingAPM(void); + void _testErrorAckFailureStrings(void); private: - void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail); - void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail); + void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult, bool shouldFail); + void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult, bool shouldFail); void _testWriteFailureHandlingWorker(void); void _testReadFailureHandlingWorker(void); diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 1d5311fcdadb26bba6dde02bbaffdb6c18d8fdd6..5dc76cf761ee29105c79cb1e7c336c94c69fcd5a 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -1013,9 +1013,9 @@ void MockLink::_respondWithAutopilotVersion(void) respondWithMavlinkMessage(msg); } -void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode) +void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult) { - _missionItemHandler.setMissionItemFailureMode(failureMode); + _missionItemHandler.setFailureMode(failureMode, failureAckResult); } void MockLink::_sendHomePosition(void) diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 49cfd9cefdf3aef0e9e8f0e5ff497b91ad729242..c02d5ea99a9eecf043935197d527f4db5ee7cd67 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -134,7 +134,8 @@ public: /// Sets a failure mode for unit testing /// @param failureMode Type of failure to simulate - void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode); + /// @param failureAckResult Error to send if one the ack error modes + void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult); /// Called to send a MISSION_ACK message while the MissionManager is in idle state void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); } diff --git a/src/comm/MockLinkMissionItemHandler.cc b/src/comm/MockLinkMissionItemHandler.cc index a7fcaa76a6d722f918f37f1101ddffc9adb5c1b7..a97b810b526073abea7d2abb1f7ffa98a380079d 100644 --- a/src/comm/MockLinkMissionItemHandler.cc +++ b/src/comm/MockLinkMissionItemHandler.cc @@ -200,7 +200,7 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) || (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) { - _sendAck(MAV_MISSION_ERROR); + _sendAck(_failureAckResult); } else { MissionItemBoth_t missionItemBoth; @@ -327,7 +327,7 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber) if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) || (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) { qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode"; - _sendAck(MAV_MISSION_ERROR); + _sendAck(_failureAckResult); } else { mavlink_message_t message; @@ -408,7 +408,7 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg _writeSequenceIndex++; if (_writeSequenceIndex < _writeSequenceCount) { if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) { - // Send MAV_MISSION_ACCPETED ack too early + // Send MAV_MISSION_ACCEPTED ack too early _sendAck(MAV_MISSION_ACCEPTED); } else { _requestNextMissionItem(_writeSequenceIndex); @@ -448,9 +448,10 @@ void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void) Q_ASSERT(false); } -void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode) +void MockLinkMissionItemHandler::setFailureMode(FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult) { _failureMode = failureMode; + _failureAckResult = failureAckResult; } void MockLinkMissionItemHandler::shutdown(void) diff --git a/src/comm/MockLinkMissionItemHandler.h b/src/comm/MockLinkMissionItemHandler.h index 0424ddc52163d2a068fec61bb79dd9eb9e3de224..625c615bb7eff691d0af7f56a8da406aa1f06547 100644 --- a/src/comm/MockLinkMissionItemHandler.h +++ b/src/comm/MockLinkMissionItemHandler.h @@ -63,7 +63,8 @@ public: /// Sets a failure mode for unit testing /// @param failureMode Type of failure to simulate - void setMissionItemFailureMode(FailureMode_t failureMode); + /// @param failureAckResult Error to send if one the ack error modes + void setFailureMode(FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult); /// Called to send a MISSION_ACK message while the MissionManager is in idle state void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType); @@ -115,6 +116,7 @@ private: QTimer* _missionItemResponseTimer; FailureMode_t _failureMode; + MAV_MISSION_RESULT _failureAckResult; bool _sendHomePositionOnEmptyList; MAVLinkProtocol* _mavlinkProtocol; bool _failReadRequestListFirstResponse;