From 04f50e70eccf91c1484087d43d92299a1d4a9f33 Mon Sep 17 00:00:00 2001 From: DoinLakeFlyer Date: Sun, 8 Mar 2020 11:25:13 -0700 Subject: [PATCH 1/2] Much better error reporting for Mission transfer errors --- src/MissionManager/PlanManager.cc | 132 +++++++++++++++++++++----------------- src/MissionManager/PlanManager.h | 12 ++-- src/Vehicle/Vehicle.cc | 6 +- 3 files changed, 84 insertions(+), 66 deletions(-) diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc index 8231f90..3ab585b 100644 --- a/src/MissionManager/PlanManager.cc +++ b/src/MissionManager/PlanManager.cc @@ -23,6 +23,7 @@ QGC_LOGGING_CATEGORY(PlanManagerLog, "PlanManagerLog") PlanManager::PlanManager(Vehicle* vehicle, MAV_MISSION_TYPE planType) : _vehicle (vehicle) + , _missionCommandTree (qgcApp()->toolbox()->missionCommandTree()) , _planType (planType) , _dedicatedLink (nullptr) , _ackTimeoutTimer (nullptr) @@ -178,7 +179,7 @@ void PlanManager::_ackTimeout(void) case AckMissionCount: // MISSION_COUNT message expected if (_retryCount > _maxRetryCount) { - _sendError(VehicleError, tr("Mission request list failed, maximum retries exceeded.")); + _sendError(MaxRetryExceeded, tr("Mission request list failed, maximum retries exceeded.")); _finishTransaction(false); } else { _retryCount++; @@ -189,7 +190,7 @@ void PlanManager::_ackTimeout(void) case AckMissionItem: // MISSION_ITEM expected if (_retryCount > _maxRetryCount) { - _sendError(VehicleError, tr("Mission read failed, maximum retries exceeded.")); + _sendError(MaxRetryExceeded, tr("Mission read failed, maximum retries exceeded.")); _finishTransaction(false); } else { _retryCount++; @@ -201,12 +202,12 @@ void PlanManager::_ackTimeout(void) // MISSION_REQUEST is expected, or MISSION_ACK to end sequence if (_itemIndicesToWrite.count() == 0) { // Vehicle did not send final MISSION_ACK at end of sequence - _sendError(VehicleError, tr("Mission write failed, vehicle failed to send final ack.")); + _sendError(ProtocolError, tr("Mission write failed, vehicle failed to send final ack.")); _finishTransaction(false); } else if (_itemIndicesToWrite[0] == 0) { // Vehicle did not respond to MISSION_COUNT, try again if (_retryCount > _maxRetryCount) { - _sendError(VehicleError, tr("Mission write mission count failed, maximum retries exceeded.")); + _sendError(MaxRetryExceeded, tr("Mission write mission count failed, maximum retries exceeded.")); _finishTransaction(false); } else { _retryCount++; @@ -215,7 +216,7 @@ void PlanManager::_ackTimeout(void) } } else { // Vehicle did not request all items from ground station - _sendError(AckTimeoutError, tr("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck))); + _sendError(ProtocolError, tr("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck))); _expectedAck = AckNone; _finishTransaction(false); } @@ -223,7 +224,7 @@ void PlanManager::_ackTimeout(void) case AckMissionClearAll: // MISSION_ACK expected if (_retryCount > _maxRetryCount) { - _sendError(VehicleError, tr("Mission remove all, maximum retries exceeded.")); + _sendError(MaxRetryExceeded, tr("Mission remove all, maximum retries exceeded.")); _finishTransaction(false); } else { _retryCount++; @@ -341,7 +342,7 @@ void PlanManager::_handleMissionCount(const mavlink_message_t& message) void PlanManager::_requestNextMissionItem(void) { if (_itemIndicesToRead.count() == 0) { - _sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read"); + _sendError(InternalError, tr("Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read")); return; } @@ -623,47 +624,51 @@ void PlanManager::_handleMissionAck(const mavlink_message_t& message) switch (savedExpectedAck) { case AckNone: // State machine is idle. Vehicle is confused. - qCDebug(PlanManagerLog) << QStringLiteral("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)); + qCDebug(PlanManagerLog) << QStringLiteral("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)); break; case AckMissionCount: // MISSION_COUNT message expected - _sendError(VehicleError, tr("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + // FIXME: Protocol error + _sendError(VehicleAckError, _missionResultToString((MAV_MISSION_RESULT)missionAck.type)); _finishTransaction(false); break; case AckMissionItem: // MISSION_ITEM expected - _sendError(VehicleError, tr("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + // FIXME: Protocol error + _sendError(VehicleAckError, _missionResultToString((MAV_MISSION_RESULT)missionAck.type)); _finishTransaction(false); break; case AckMissionRequest: - // MISSION_REQUEST is expected, or MISSION_ACK to end sequence + // MISSION_REQUEST is expected, or MAV_MISSION_ACCEPTED to end sequence if (missionAck.type == MAV_MISSION_ACCEPTED) { if (_itemIndicesToWrite.count() == 0) { qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck write sequence complete %1").arg(_planTypeString()); _finishTransaction(true); } else { - _sendError(MissingRequestsError, tr("Vehicle did not request all items during write sequence, missed count %1.").arg(_itemIndicesToWrite.count())); + // FIXME: Protocol error + _sendError(VehicleAckError, _missionResultToString((MAV_MISSION_RESULT)missionAck.type)); _finishTransaction(false); } } else { - _sendError(VehicleError, tr("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + _sendError(VehicleAckError, _missionResultToString((MAV_MISSION_RESULT)missionAck.type)); _finishTransaction(false); } break; case AckMissionClearAll: - // MISSION_ACK expected + // MAV_MISSION_ACCEPTED expected if (missionAck.type != MAV_MISSION_ACCEPTED) { - _sendError(VehicleError, tr("Vehicle returned error: %1. Vehicle remove all failed.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + _sendError(VehicleAckError, tr("Vehicle remove all failed. Error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); } _finishTransaction(missionAck.type == MAV_MISSION_ACCEPTED); break; case AckGuidedItem: - // MISSION_REQUEST is expected, or MISSION_ACK to end sequence + // MISSION_REQUEST is expected, or MAV_MISSION_ACCEPTED to end sequence if (missionAck.type == MAV_MISSION_ACCEPTED) { qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck %1 guided mode item accepted").arg(_planTypeString()); _finishTransaction(true, true /* apmGuidedItemWrite */); } else { - _sendError(VehicleError, tr("Vehicle returned error: %1. %2Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + // FIXME: Protocol error + _sendError(VehicleAckError, tr("Vehicle returned error: %1. %2Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); _finishTransaction(false, true /* apmGuidedItemWrite */); } break; @@ -702,7 +707,7 @@ void PlanManager::_mavlinkMessageReceived(const mavlink_message_t& message) void PlanManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) { - qCDebug(PlanManagerLog) << QStringLiteral("Sending %1 error").arg(_planTypeString()) << errorCode << errorMsg; + qCDebug(PlanManagerLog) << QStringLiteral("Sending error - _planTypeString(%1) errorCode(%2) errorMsg(%4)").arg(_planTypeString()).arg(errorCode).arg(errorMsg); emit error(errorCode, errorMsg); } @@ -728,104 +733,115 @@ QString PlanManager::_ackTypeToString(AckType_t ackType) QString PlanManager::_lastMissionReqestString(MAV_MISSION_RESULT result) { + QString prefix; + QString postfix; + if (_lastMissionRequest >= 0 && _lastMissionRequest < _writeMissionItems.count()) { MissionItem* item = _writeMissionItems[_lastMissionRequest]; + prefix = tr("Item #%1 Command: %2").arg(_lastMissionRequest).arg(_missionCommandTree->friendlyName(item->command())); + switch (result) { case MAV_MISSION_UNSUPPORTED_FRAME: - return QString(". Frame: %1").arg(item->frame()); + postfix = tr("Frame: %1").arg(item->frame()); + break; case MAV_MISSION_UNSUPPORTED: - { - const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, item->command()); - QString friendlyName; - QString rawName; - if (uiInfo) { - friendlyName = uiInfo->friendlyName(); - rawName = uiInfo->rawName(); - } - return QString(". Command: (%1, %2, %3)").arg(friendlyName).arg(rawName).arg(item->command()); - } + // All we need is the prefix + break; case MAV_MISSION_INVALID_PARAM1: - return QString(". Param1: %1").arg(item->param1()); + postfix = tr("Value: %1").arg(item->param1()); + break; case MAV_MISSION_INVALID_PARAM2: - return QString(". Param2: %1").arg(item->param2()); + postfix = tr("Value: %1").arg(item->param2()); + break; case MAV_MISSION_INVALID_PARAM3: - return QString(". Param3: %1").arg(item->param3()); + postfix = tr("Value: %1").arg(item->param3()); + break; case MAV_MISSION_INVALID_PARAM4: - return QString(". Param4: %1").arg(item->param4()); + postfix = tr("Value: %1").arg(item->param4()); + break; case MAV_MISSION_INVALID_PARAM5_X: - return QString(". Param5: %1").arg(item->param5()); + postfix = tr("Value: %1").arg(item->param5()); + break; case MAV_MISSION_INVALID_PARAM6_Y: - return QString(". Param6: %1").arg(item->param6()); + postfix = tr("Value: %1").arg(item->param6()); + break; case MAV_MISSION_INVALID_PARAM7: - return QString(". Param7: %1").arg(item->param7()); + postfix = tr("Value: %1").arg(item->param7()); + break; case MAV_MISSION_INVALID_SEQUENCE: - return QString(". Sequence: %1").arg(item->sequenceNumber()); + // All we need is the prefix + break; default: break; } } - return QString(); + return prefix + (postfix.isEmpty() ? QStringLiteral("") : QStringLiteral(" ")) + postfix; } QString PlanManager::_missionResultToString(MAV_MISSION_RESULT result) { - QString resultString; - QString lastRequestString = _lastMissionReqestString(result); + QString error; switch (result) { case MAV_MISSION_ACCEPTED: - resultString = tr("Mission accepted (MAV_MISSION_ACCEPTED)"); + error = tr("Mission accepted."); break; case MAV_MISSION_ERROR: - resultString = tr("Unspecified error (MAV_MISSION_ERROR)"); + error = tr("Unspecified error."); break; case MAV_MISSION_UNSUPPORTED_FRAME: - resultString = tr("Coordinate frame is not supported (MAV_MISSION_UNSUPPORTED_FRAME)"); + error = tr("Coordinate frame is not supported."); break; case MAV_MISSION_UNSUPPORTED: - resultString = tr("Command is not supported (MAV_MISSION_UNSUPPORTED)"); + error = tr("Command is not supported."); break; case MAV_MISSION_NO_SPACE: - resultString = tr("Mission item exceeds storage space (MAV_MISSION_NO_SPACE)"); + error = tr("Mission item exceeds storage space."); break; case MAV_MISSION_INVALID: - resultString = tr("One of the parameters has an invalid value (MAV_MISSION_INVALID)"); + error = tr("One of the parameters has an invalid value."); break; case MAV_MISSION_INVALID_PARAM1: - resultString = tr("Param1 has an invalid value (MAV_MISSION_INVALID_PARAM1)"); + error = tr("Param 1 invalid value."); break; case MAV_MISSION_INVALID_PARAM2: - resultString = tr("Param2 has an invalid value (MAV_MISSION_INVALID_PARAM2)"); + error = tr("Param 2 invalid value."); break; case MAV_MISSION_INVALID_PARAM3: - resultString = tr("Param3 has an invalid value (MAV_MISSION_INVALID_PARAM3)"); + error = tr("Param 3 invalid value."); break; case MAV_MISSION_INVALID_PARAM4: - resultString = tr("Param4 has an invalid value (MAV_MISSION_INVALID_PARAM4)"); + error = tr("Param 4 invalid value."); break; case MAV_MISSION_INVALID_PARAM5_X: - resultString = tr("X/Param5 has an invalid value (MAV_MISSION_INVALID_PARAM5_X)"); + error = tr("Param 5 invalid value."); break; case MAV_MISSION_INVALID_PARAM6_Y: - resultString = tr("Y/Param6 has an invalid value (MAV_MISSION_INVALID_PARAM6_Y)"); + error = tr("Param 6 invalid value."); break; case MAV_MISSION_INVALID_PARAM7: - resultString = tr("Param7 has an invalid value (MAV_MISSION_INVALID_PARAM7)"); + error = tr("Param 7 invalid value."); break; case MAV_MISSION_INVALID_SEQUENCE: - resultString = tr("Received mission item out of sequence (MAV_MISSION_INVALID_SEQUENCE)"); + error = tr("Received mission item out of sequence."); break; case MAV_MISSION_DENIED: - resultString = tr("Not accepting any mission commands (MAV_MISSION_DENIED)"); + error = tr("Not accepting any mission commands."); break; default: - qWarning(PlanManagerLog) << QStringLiteral("Fell off end of switch statement %1").arg(_planTypeString()); - resultString = tr("QGC Internal Error"); + qWarning(PlanManagerLog) << QStringLiteral("Fell off end of switch statement %1 %2").arg(_planTypeString()).arg(result); + error = tr("Unknown error: %1.").arg(result); + break; + } + + QString lastRequestString = _lastMissionReqestString(result); + if (!lastRequestString.isEmpty()) { + error += QStringLiteral(" ") + lastRequestString; } - return resultString + lastRequestString; + return error; } void PlanManager::_finishTransaction(bool success, bool apmGuidedItemWrite) diff --git a/src/MissionManager/PlanManager.h b/src/MissionManager/PlanManager.h index 88fabe9..4914488 100644 --- a/src/MissionManager/PlanManager.h +++ b/src/MissionManager/PlanManager.h @@ -20,6 +20,7 @@ #include "LinkInterface.h" class Vehicle; +class MissionCommandTree; Q_DECLARE_LOGGING_CATEGORY(PlanManagerLog) @@ -60,10 +61,10 @@ public: typedef enum { InternalError, AckTimeoutError, ///< Timed out waiting for response from vehicle - ProtocolOrderError, ///< Incorrect protocol sequence from vehicle + ProtocolError, ///< Incorrect protocol sequence from vehicle RequestRangeError, ///< Vehicle requested item out of range ItemMismatchError, ///< Vehicle returned item with seq # different than requested - VehicleError, ///< Vehicle returned error + VehicleAckError, ///< Vehicle returned error in ack MissingRequestsError, ///< Vehicle did not request all items during write sequence MaxRetryExceeded, ///< Retry failed MissionTypeMismatch, ///< MAV_MISSION_TYPE does not match _planType @@ -134,11 +135,12 @@ protected: QString _planTypeString(void); protected: - Vehicle* _vehicle; + Vehicle* _vehicle = nullptr; + MissionCommandTree* _missionCommandTree = nullptr; MAV_MISSION_TYPE _planType; - LinkInterface* _dedicatedLink; + LinkInterface* _dedicatedLink = nullptr; - QTimer* _ackTimeoutTimer; + QTimer* _ackTimeoutTimer = nullptr; AckType_t _expectedAck; int _retryCount; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index b863f3a..5ed7a64 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2572,19 +2572,19 @@ void Vehicle::sendMessageMultiple(mavlink_message_t message) void Vehicle::_missionManagerError(int errorCode, const QString& errorMsg) { Q_UNUSED(errorCode); - qgcApp()->showMessage(tr("Mission transfer failed. Retry transfer. Error: %1").arg(errorMsg)); + qgcApp()->showMessage(tr("Mission transfer failed. Error: %1").arg(errorMsg)); } void Vehicle::_geoFenceManagerError(int errorCode, const QString& errorMsg) { Q_UNUSED(errorCode); - qgcApp()->showMessage(tr("GeoFence transfer failed. Retry transfer. Error: %1").arg(errorMsg)); + qgcApp()->showMessage(tr("GeoFence transfer failed. Error: %1").arg(errorMsg)); } void Vehicle::_rallyPointManagerError(int errorCode, const QString& errorMsg) { Q_UNUSED(errorCode); - qgcApp()->showMessage(tr("Rally Point transfer failed. Retry transfer. Error: %1").arg(errorMsg)); + qgcApp()->showMessage(tr("Rally Point transfer failed. Error: %1").arg(errorMsg)); } void Vehicle::_clearCameraTriggerPoints() From ffb55f92337637741e9a8f5fac3585e19959f174 Mon Sep 17 00:00:00 2001 From: DoinLakeFlyer Date: Sun, 8 Mar 2020 11:25:25 -0700 Subject: [PATCH 2/2] Unit test for mission transfer error string checking --- src/MissionManager/MissionManagerTest.cc | 49 ++++++++++++++++++++++++++------ src/MissionManager/MissionManagerTest.h | 5 ++-- src/comm/MockLink.cc | 4 +-- src/comm/MockLink.h | 3 +- src/comm/MockLinkMissionItemHandler.cc | 9 +++--- src/comm/MockLinkMissionItemHandler.h | 4 ++- 6 files changed, 55 insertions(+), 19 deletions(-) diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc index fefdaa8..01c1f60 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 7d3b4d5..0d3d893 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 1d5311f..5dc76cf 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 49cfd9c..c02d5ea 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 a7fcaa7..a97b810 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 0424ddc..625c615 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;