From 570548a4d147b03702b8f1a4a7609797275f80f6 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 6 Nov 2023 13:32:25 -0800 Subject: [PATCH] Fix up support for MAV_RESULT_IN_PROGRESS * Update unit tests to test for in progress * Split result handler into regular ack and in progress ack callbacks --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 16 ++-- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 19 ++-- src/Vehicle/Actuators/ActuatorActions.cc | 12 ++- src/Vehicle/Actuators/ActuatorActions.h | 3 +- src/Vehicle/Actuators/ActuatorTesting.cc | 12 ++- src/Vehicle/Actuators/ActuatorTesting.h | 3 +- src/Vehicle/Actuators/MotorAssignment.cc | 12 ++- src/Vehicle/Actuators/MotorAssignment.h | 3 +- src/Vehicle/Autotune.cpp | 46 +++++++--- src/Vehicle/Autotune.h | 3 +- src/Vehicle/RequestMessageTest.cc | 18 ++-- src/Vehicle/SendMavCommandWithHandlerTest.cc | 117 ++++++++++++++++-------- src/Vehicle/SendMavCommandWithHandlerTest.h | 10 +- src/Vehicle/SendMavCommandWithSignallingTest.cc | 11 ++- src/Vehicle/Vehicle.cc | 102 +++++++++++++-------- src/Vehicle/Vehicle.h | 80 ++++++++++------ src/comm/MockLink.cc | 63 ++++++++++++- src/comm/MockLink.h | 12 ++- 18 files changed, 360 insertions(+), 182 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index f38ffea..987826a 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -777,10 +777,8 @@ typedef struct { Vehicle* vehicle; } MAV_CMD_DO_REPOSITION_HandlerData_t; -static void _MAV_CMD_DO_REPOSITION_ResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT commandResult, uint8_t progress, Vehicle::MavCmdResultFailureCode_t failureCode) +static void _MAV_CMD_DO_REPOSITION_ResultHandler(void* resultHandlerData, int /*compId*/, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t /*failureCode*/) { - Q_UNUSED(progress); - auto* data = (MAV_CMD_DO_REPOSITION_HandlerData_t*)resultHandlerData; auto* vehicle = data->vehicle; auto* instanceData = qobject_cast(vehicle->firmwarePluginInstanceData()); @@ -791,8 +789,8 @@ static void _MAV_CMD_DO_REPOSITION_ResultHandler(void* resultHandlerData, int /* goto out; } - instanceData->MAV_CMD_DO_REPOSITION_supported = (commandResult == MAV_RESULT_ACCEPTED); - instanceData->MAV_CMD_DO_REPOSITION_unsupported = (commandResult == MAV_RESULT_UNSUPPORTED); + instanceData->MAV_CMD_DO_REPOSITION_supported = (ack.result == MAV_RESULT_ACCEPTED); + instanceData->MAV_CMD_DO_REPOSITION_unsupported = (ack.result == MAV_RESULT_UNSUPPORTED); out: delete data; @@ -818,9 +816,13 @@ void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord auto* result_handler_data = new MAV_CMD_DO_REPOSITION_HandlerData_t{ vehicle }; + + Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {}; + handlerInfo.resultHandler = _MAV_CMD_DO_REPOSITION_ResultHandler; + handlerInfo.resultHandlerData = result_handler_data; + vehicle->sendMavCommandIntWithHandler( - _MAV_CMD_DO_REPOSITION_ResultHandler, - result_handler_data, + &handlerInfo, vehicle->defaultComponentId(), MAV_CMD_DO_REPOSITION, MAV_FRAME_GLOBAL, diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 1d75579..b8fab27 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -494,14 +494,12 @@ typedef struct { double newAMSLAlt; } PauseVehicleThenChangeAltData_t; -static void _pauseVehicleThenChangeAltResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT commandResult, uint8_t progress, Vehicle::MavCmdResultFailureCode_t failureCode) +static void _pauseVehicleThenChangeAltResultHandler(void* resultHandlerData, int /*compId*/, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode) { - Q_UNUSED(progress); - - if (commandResult != MAV_RESULT_ACCEPTED) { + if (ack.result != MAV_RESULT_ACCEPTED) { switch (failureCode) { case Vehicle::MavCmdResultCommandResultOnly: - qDebug() << QStringLiteral("MAV_CMD_DO_REPOSITION error(%1)").arg(commandResult); + qDebug() << QStringLiteral("MAV_CMD_DO_REPOSITION error(%1)").arg(ack.result); break; case Vehicle::MavCmdResultFailureNoResponseToCommand: qDebug() << "MAV_CMD_DO_REPOSITION no response from vehicle"; @@ -513,7 +511,7 @@ static void _pauseVehicleThenChangeAltResultHandler(void* resultHandlerData, int } PauseVehicleThenChangeAltData_t* pData = static_cast(resultHandlerData); - pData->plugin->_changeAltAfterPause(resultHandlerData, commandResult == MAV_RESULT_ACCEPTED /* pauseSucceeded */); + pData->plugin->_changeAltAfterPause(resultHandlerData, ack.result == MAV_RESULT_ACCEPTED /* pauseSucceeded */); } void PX4FirmwarePlugin::_changeAltAfterPause(void* resultHandlerData, bool pauseSucceeded) @@ -557,9 +555,12 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu resultData->newAMSLAlt = vehicle->homePosition().altitude() + newAltRel; if (pauseVehicle) { + Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {}; + handlerInfo.resultHandler = _pauseVehicleThenChangeAltResultHandler; + handlerInfo.resultHandlerData = resultData; + vehicle->sendMavCommandWithHandler( - _pauseVehicleThenChangeAltResultHandler, - resultData, + &handlerInfo, vehicle->defaultComponentId(), MAV_CMD_DO_REPOSITION, -1.0f, // Don't change groundspeed @@ -736,4 +737,4 @@ bool PX4FirmwarePlugin::hasGripper(const Vehicle* vehicle) const return _hasGripper; } return false; -} \ No newline at end of file +} diff --git a/src/Vehicle/Actuators/ActuatorActions.cc b/src/Vehicle/Actuators/ActuatorActions.cc index 81259ae..555ac42 100644 --- a/src/Vehicle/Actuators/ActuatorActions.cc +++ b/src/Vehicle/Actuators/ActuatorActions.cc @@ -39,11 +39,10 @@ void Action::trigger() sendMavlinkRequest(); } -void Action::ackHandlerEntry(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, - Vehicle::MavCmdResultFailureCode_t failureCode) +void Action::ackHandlerEntry(void* resultHandlerData, int /*compId*/, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode) { Action* action = (Action*)resultHandlerData; - action->ackHandler(commandResult, failureCode); + action->ackHandler(static_cast(ack.result), failureCode); } void Action::ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode) @@ -58,9 +57,12 @@ void Action::sendMavlinkRequest() { qCDebug(ActuatorsConfigLog) << "Sending actuator action, function:" << _outputFunction << "type:" << (int)_type; + Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {}; + handlerInfo.resultHandler = ackHandlerEntry; + handlerInfo.resultHandlerData = this; + _vehicle->sendMavCommandWithHandler( - ackHandlerEntry, // Ack callback - this, // Ack callback data + &handlerInfo, MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot MAV_CMD_CONFIGURE_ACTUATOR, // the mavlink command (int)_type, // action type diff --git a/src/Vehicle/Actuators/ActuatorActions.h b/src/Vehicle/Actuators/ActuatorActions.h index c96c502..ff27065 100644 --- a/src/Vehicle/Actuators/ActuatorActions.h +++ b/src/Vehicle/Actuators/ActuatorActions.h @@ -48,8 +48,7 @@ public: Q_INVOKABLE void trigger(); private: - static void ackHandlerEntry(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, - Vehicle::MavCmdResultFailureCode_t failureCode); + static void ackHandlerEntry(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode); void ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode); void sendMavlinkRequest(); diff --git a/src/Vehicle/Actuators/ActuatorTesting.cc b/src/Vehicle/Actuators/ActuatorTesting.cc index bee1fd3..ad648b9 100644 --- a/src/Vehicle/Actuators/ActuatorTesting.cc +++ b/src/Vehicle/Actuators/ActuatorTesting.cc @@ -120,11 +120,10 @@ void ActuatorTest::setActive(bool active) _active = active; } -void ActuatorTest::ackHandlerEntry(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, - Vehicle::MavCmdResultFailureCode_t failureCode) +void ActuatorTest::ackHandlerEntry(void* resultHandlerData, int /*compId*/, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode) { ActuatorTest* actuatorTest = (ActuatorTest*)resultHandlerData; - actuatorTest->ackHandler(commandResult, failureCode); + actuatorTest->ackHandler(static_cast(ack.result), failureCode); } void ActuatorTest::ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode) @@ -190,9 +189,12 @@ void ActuatorTest::sendMavlinkRequest(int function, float value, float timeout) // TODO: consider using a lower command timeout + Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {}; + handlerInfo.resultHandler = ackHandlerEntry; + handlerInfo.resultHandlerData = this; + _vehicle->sendMavCommandWithHandler( - ackHandlerEntry, // Ack callback - this, // Ack callback data + &handlerInfo, MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot MAV_CMD_ACTUATOR_TEST, // the mavlink command value, // value diff --git a/src/Vehicle/Actuators/ActuatorTesting.h b/src/Vehicle/Actuators/ActuatorTesting.h index 1c52f38..de75249 100644 --- a/src/Vehicle/Actuators/ActuatorTesting.h +++ b/src/Vehicle/Actuators/ActuatorTesting.h @@ -110,8 +110,7 @@ private: void resetStates(); - static void ackHandlerEntry(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, - Vehicle::MavCmdResultFailureCode_t failureCode); + static void ackHandlerEntry(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode); void ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode); void sendMavlinkRequest(int function, float value, float timeout); diff --git a/src/Vehicle/Actuators/MotorAssignment.cc b/src/Vehicle/Actuators/MotorAssignment.cc index b7e087a..ea64793 100644 --- a/src/Vehicle/Actuators/MotorAssignment.cc +++ b/src/Vehicle/Actuators/MotorAssignment.cc @@ -197,11 +197,10 @@ void MotorAssignment::spinTimeout() spinCurrentMotor(); } -void MotorAssignment::ackHandlerEntry(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, - Vehicle::MavCmdResultFailureCode_t failureCode) +void MotorAssignment::ackHandlerEntry(void* resultHandlerData, int /*compId*/, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode) { MotorAssignment* motorAssignment = (MotorAssignment*)resultHandlerData; - motorAssignment->ackHandler(commandResult, failureCode); + motorAssignment->ackHandler(static_cast(ack.result), failureCode); } void MotorAssignment::ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode) @@ -217,9 +216,12 @@ void MotorAssignment::sendMavlinkRequest(int function, float value) { qCDebug(ActuatorsConfigLog) << "Sending actuator test function:" << function << "value:" << value; + Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {}; + handlerInfo.resultHandler = ackHandlerEntry; + handlerInfo.resultHandlerData = this; + _vehicle->sendMavCommandWithHandler( - ackHandlerEntry, // Ack callback - this, // Ack callback data + &handlerInfo, MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot MAV_CMD_ACTUATOR_TEST, // the mavlink command value, // value diff --git a/src/Vehicle/Actuators/MotorAssignment.h b/src/Vehicle/Actuators/MotorAssignment.h index 1fea827..7b7fb60 100644 --- a/src/Vehicle/Actuators/MotorAssignment.h +++ b/src/Vehicle/Actuators/MotorAssignment.h @@ -62,8 +62,7 @@ private: static constexpr int _spinTimeoutDefaultSec = 1000; static constexpr int _spinTimeoutHighSec = 3000; ///< wait a bit longer after assigning motors, so ESCs can initialize - static void ackHandlerEntry(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, - Vehicle::MavCmdResultFailureCode_t failureCode); + static void ackHandlerEntry(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode); void ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode); void sendMavlinkRequest(int function, float value); diff --git a/src/Vehicle/Autotune.cpp b/src/Vehicle/Autotune.cpp index 8b6e18b..2c7fea1 100644 --- a/src/Vehicle/Autotune.cpp +++ b/src/Vehicle/Autotune.cpp @@ -41,7 +41,7 @@ void Autotune::autotuneRequest() //----------------------------------------------------------------------------- -void Autotune::ackHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, Vehicle::MavCmdResultFailureCode_t failureCode) +void Autotune::ackHandler(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode) { Q_UNUSED(compId); Q_UNUSED(failureCode); @@ -49,23 +49,38 @@ void Autotune::ackHandler(void* resultHandlerData, int compId, MAV_RESULT comman auto * autotune = static_cast(resultHandlerData); if (autotune->_autotuneInProgress) { - if ((commandResult == MAV_RESULT_IN_PROGRESS) || (commandResult == MAV_RESULT_ACCEPTED)) { - autotune->handleAckStatus(progress); - } - else if (commandResult == MAV_RESULT_FAILED) { + if (failureCode == Vehicle::MavCmdResultCommandResultOnly) { + if ((ack.result == MAV_RESULT_IN_PROGRESS) || (ack.result == MAV_RESULT_ACCEPTED)) { + autotune->handleAckStatus(ack.progress); + } + else if (ack.result == MAV_RESULT_FAILED) { + autotune->handleAckFailure(); + } + else { + autotune->handleAckError(ack.result); + } + } else { autotune->handleAckFailure(); } - else { - autotune->handleAckError(commandResult); - } - emit autotune->autotuneChanged(); - } - else { + } else { qWarning() << "Ack received for a command different from MAV_CMD_DO_AUTOTUNE_ENABLE ot wrong UI state."; } } +void Autotune::progressHandler(void* progressHandlerData, int compId, const mavlink_command_ack_t& ack) +{ + Q_UNUSED(compId); + + auto * autotune = static_cast(progressHandlerData); + + if (autotune->_autotuneInProgress) { + autotune->handleAckStatus(ack.progress); + emit autotune->autotuneChanged(); + } else { + qWarning() << "Ack received for a command different from MAV_CMD_DO_AUTOTUNE_ENABLE ot wrong UI state."; + } +} //----------------------------------------------------------------------------- bool Autotune::autotuneEnabled() @@ -162,9 +177,14 @@ void Autotune::stopTimers() //----------------------------------------------------------------------------- void Autotune::sendMavlinkRequest() { + Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {}; + handlerInfo.resultHandler = ackHandler; + handlerInfo.resultHandlerData = this; + handlerInfo.progressHandler = progressHandler; + handlerInfo.progressHandlerData = this; + _vehicle->sendMavCommandWithHandler( - ackHandler, // Ack callback - this, // Ack callback data + &handlerInfo, MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot MAV_CMD_DO_AUTOTUNE_ENABLE, // the mavlink command 1, // request autotune diff --git a/src/Vehicle/Autotune.h b/src/Vehicle/Autotune.h index 6da1883..e7019fd 100644 --- a/src/Vehicle/Autotune.h +++ b/src/Vehicle/Autotune.h @@ -28,7 +28,8 @@ public: Q_INVOKABLE void autotuneRequest (); - static void ackHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, Vehicle::MavCmdResultFailureCode_t failureCode); + static void ackHandler (void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode); + static void progressHandler (void* progressHandlerData, int compId, const mavlink_command_ack_t& ack); bool autotuneEnabled (); bool autotuneInProgress () { return _autotuneInProgress; } diff --git a/src/Vehicle/RequestMessageTest.cc b/src/Vehicle/RequestMessageTest.cc index 12e90c5..c422475 100644 --- a/src/Vehicle/RequestMessageTest.cc +++ b/src/Vehicle/RequestMessageTest.cc @@ -38,21 +38,21 @@ void RequestMessageTest::_testCaseWorker(TestCase_t& testCase) MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); Vehicle* vehicle = vehicleMgr->activeVehicle(); - _mockLink->clearSendMavCommandCounts(); + _mockLink->clearReceivedMavCommandCounts(); _mockLink->setRequestMessageFailureMode(testCase.failureMode); 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); + QCOMPARE(_mockLink->receivedMavCommandCount(MAV_CMD_REQUEST_MESSAGE), testCase.expectedSendCount); // We should be able to do it twice in a row without any duplicate command problems testCase.resultHandlerCalled = false; - _mockLink->clearSendMavCommandCounts(); + _mockLink->clearReceivedMavCommandCounts(); 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); + QCOMPARE(_mockLink->receivedMavCommandCount(MAV_CMD_REQUEST_MESSAGE), testCase.expectedSendCount); _disconnectMockLink(); } @@ -77,19 +77,19 @@ void RequestMessageTest::_duplicateCommand(void) MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); Vehicle* vehicle = vehicleMgr->activeVehicle(); - _mockLink->clearSendMavCommandCounts(); + _mockLink->clearReceivedMavCommandCounts(); _mockLink->setRequestMessageFailureMode(testCase.failureMode); QVERIFY(false == vehicle->isMavCommandPending(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE)); vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG); - QVERIFY(QTest::qWaitFor([&]() { return _mockLink->sendMavCommandCount(MAV_CMD_REQUEST_MESSAGE) == 1; }, 10)); + QVERIFY(QTest::qWaitFor([&]() { return _mockLink->receivedMavCommandCount(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); + QCOMPARE(_mockLink->receivedMavCommandCount(MAV_CMD_REQUEST_MESSAGE), testCase.expectedSendCount); QVERIFY(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE) != -1); QVERIFY(true == vehicle->isMavCommandPending(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE)); @@ -123,13 +123,13 @@ void RequestMessageTest::_compIdAllFailure(void) MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); Vehicle* vehicle = vehicleMgr->activeVehicle(); - _mockLink->clearSendMavCommandCounts(); + _mockLink->clearReceivedMavCommandCounts(); _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); + QCOMPARE(_mockLink->receivedMavCommandCount(MAV_CMD_REQUEST_MESSAGE), 0); _disconnectMockLink(); } diff --git a/src/Vehicle/SendMavCommandWithHandlerTest.cc b/src/Vehicle/SendMavCommandWithHandlerTest.cc index bf403ac..6f6023c 100644 --- a/src/Vehicle/SendMavCommandWithHandlerTest.cc +++ b/src/Vehicle/SendMavCommandWithHandlerTest.cc @@ -13,28 +13,47 @@ #include "MockLink.h" SendMavCommandWithHandlerTest::TestCase_t SendMavCommandWithHandlerTest::_rgTestCases[] = { - { MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, MAV_RESULT_ACCEPTED, 0, Vehicle::MavCmdResultCommandResultOnly, 1 }, - { MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED, MAV_RESULT_FAILED, 0, Vehicle::MavCmdResultCommandResultOnly, 1 }, - { MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED, MAV_RESULT_ACCEPTED, 0, Vehicle::MavCmdResultCommandResultOnly, 2 }, - { MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED, MAV_RESULT_FAILED, 0, Vehicle::MavCmdResultCommandResultOnly, 2 }, - { MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, MAV_RESULT_FAILED, 0, Vehicle::MavCmdResultFailureNoResponseToCommand, Vehicle::_mavCommandMaxRetryCount }, - { MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY, MAV_RESULT_FAILED, 0, Vehicle::MavCmdResultFailureNoResponseToCommand, 1 }, + { MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, MAV_RESULT_ACCEPTED, false, Vehicle::MavCmdResultCommandResultOnly, 1 }, + { MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED, MAV_RESULT_FAILED, false, Vehicle::MavCmdResultCommandResultOnly, 1 }, + { MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED, MAV_RESULT_ACCEPTED, false, Vehicle::MavCmdResultCommandResultOnly, 2 }, + { MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED, MAV_RESULT_FAILED, false, Vehicle::MavCmdResultCommandResultOnly, 2 }, + { MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, MAV_RESULT_FAILED, false, Vehicle::MavCmdResultFailureNoResponseToCommand, Vehicle::_mavCommandMaxRetryCount }, + { MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY, MAV_RESULT_FAILED, false, Vehicle::MavCmdResultFailureNoResponseToCommand, 1 }, + { MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED, MAV_RESULT_ACCEPTED, true, Vehicle::MavCmdResultCommandResultOnly, 1 }, + { MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED, MAV_RESULT_FAILED, true, Vehicle::MavCmdResultCommandResultOnly, 1 }, + { MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK, MAV_RESULT_FAILED, true, Vehicle::MavCmdResultFailureNoResponseToCommand, 1 }, }; -bool SendMavCommandWithHandlerTest::_handlerCalled = false; +bool SendMavCommandWithHandlerTest::_resultHandlerCalled = false; +bool SendMavCommandWithHandlerTest::_progressHandlerCalled = false; -void SendMavCommandWithHandlerTest::_mavCmdResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, Vehicle::MavCmdResultFailureCode_t failureCode) +void SendMavCommandWithHandlerTest::_mavCmdResultHandler(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode) { TestCase_t* testCase = static_cast(resultHandlerData); - _handlerCalled = true; + _resultHandlerCalled = true; - QCOMPARE(compId, MAV_COMP_ID_AUTOPILOT1); - QCOMPARE(testCase->expectedCommandResult, commandResult); - QCOMPARE(testCase->progress, progress); + QCOMPARE(MAV_COMP_ID_AUTOPILOT1, compId); + QCOMPARE(testCase->expectedCommandResult, ack.result); QCOMPARE(testCase->expectedFailureCode, failureCode); } +void SendMavCommandWithHandlerTest::_mavCmdProgressHandler(void* progressHandlerData, int compId, const mavlink_command_ack_t& ack) +{ + TestCase_t* testCase = static_cast(progressHandlerData); + MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); + Vehicle* vehicle = vehicleMgr->activeVehicle(); + + _progressHandlerCalled = true; + + QCOMPARE(MAV_COMP_ID_AUTOPILOT1, compId); + QCOMPARE(MAV_RESULT_IN_PROGRESS, ack.result); + QCOMPARE(1, ack.progress); + + // Command should still be in list + QVERIFY(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, testCase->command) != -1); +} + void SendMavCommandWithHandlerTest::_testCaseWorker(TestCase_t& testCase) { _connectMockLinkNoInitialConnectSequence(); @@ -42,12 +61,25 @@ void SendMavCommandWithHandlerTest::_testCaseWorker(TestCase_t& testCase) MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); Vehicle* vehicle = vehicleMgr->activeVehicle(); - _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); + Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {}; + handlerInfo.resultHandler = _mavCmdResultHandler; + handlerInfo.resultHandlerData = &testCase; + handlerInfo.progressHandler = _mavCmdProgressHandler; + handlerInfo.progressHandlerData = &testCase; + + _resultHandlerCalled = false; + _progressHandlerCalled = false; + + _mockLink->clearReceivedMavCommandCounts(); + vehicle->sendMavCommandWithHandler(&handlerInfo, MAV_COMP_ID_AUTOPILOT1, testCase.command); + + if (testCase.expectInProgressResult) { + QVERIFY(QTest::qWaitFor([&]() { return _progressHandlerCalled; }, 10000)); + } + + QVERIFY(QTest::qWaitFor([&]() { return _resultHandlerCalled; }, 10000)); + QCOMPARE(_mockLink->receivedMavCommandCount(testCase.command), testCase.expectedSendCount); + QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, testCase.command), -1); _disconnectMockLink(); } @@ -72,27 +104,35 @@ void SendMavCommandWithHandlerTest::_duplicateCommand(void) MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); Vehicle* vehicle = vehicleMgr->activeVehicle(); - _handlerCalled = false; - _mockLink->clearSendMavCommandCounts(); + Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {}; + handlerInfo.resultHandler = _mavCmdResultHandler; + handlerInfo.resultHandlerData = &testCase; + handlerInfo.progressHandler = _mavCmdProgressHandler; + handlerInfo.progressHandlerData = &testCase; + + _resultHandlerCalled = false; + _progressHandlerCalled = false; + + _mockLink->clearReceivedMavCommandCounts(); 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); + QVERIFY(QTest::qWaitFor([&]() { return _mockLink->receivedMavCommandCount(testCase.command) == 1; }, 10)); + QVERIFY(!_resultHandlerCalled); + QVERIFY(!_progressHandlerCalled); + vehicle->sendMavCommandWithHandler(&handlerInfo, MAV_COMP_ID_AUTOPILOT1, testCase.command); // Duplicate command response should happen immediately - QVERIFY(_handlerCalled); + QVERIFY(_resultHandlerCalled); QVERIFY(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, testCase.command) != -1); - QCOMPARE(_mockLink->sendMavCommandCount(testCase.command), 1); + QCOMPARE(_mockLink->receivedMavCommandCount(testCase.command), 1); } -void SendMavCommandWithHandlerTest::_compIdAllMavCmdResultHandler(void* /*resultHandlerData*/, int compId, MAV_RESULT commandResult, uint8_t progress, Vehicle::MavCmdResultFailureCode_t failureCode) +void SendMavCommandWithHandlerTest::_compIdAllFailureMavCmdResultHandler(void* /*resultHandlerData*/, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode) { - _handlerCalled = true; + _resultHandlerCalled = true; - QCOMPARE(compId, MAV_COMP_ID_ALL); - QCOMPARE(commandResult, MAV_RESULT_FAILED); - QCOMPARE(progress, 0); - QCOMPARE(failureCode, Vehicle::MavCmdResultCommandResultOnly); + QCOMPARE(compId, MAV_COMP_ID_ALL); + QCOMPARE(ack.result, MAV_RESULT_FAILED); + QCOMPARE(failureCode, Vehicle::MavCmdResultCommandResultOnly); } void SendMavCommandWithHandlerTest::_compIdAllFailure(void) @@ -106,12 +146,17 @@ void SendMavCommandWithHandlerTest::_compIdAllFailure(void) 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); + Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {}; + handlerInfo.resultHandler = _compIdAllFailureMavCmdResultHandler; + + _resultHandlerCalled = false; + + _mockLink->clearReceivedMavCommandCounts(); + vehicle->sendMavCommandWithHandler(&handlerInfo, MAV_COMP_ID_ALL, testCase.command); + + QCOMPARE(_resultHandlerCalled, true); QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_ALL, testCase.command), -1); - QCOMPARE(_mockLink->sendMavCommandCount(testCase.command), testCase.expectedSendCount); + QCOMPARE(_mockLink->receivedMavCommandCount(testCase.command), testCase.expectedSendCount); _disconnectMockLink(); } diff --git a/src/Vehicle/SendMavCommandWithHandlerTest.h b/src/Vehicle/SendMavCommandWithHandlerTest.h index 4f70bd0..1d97c5e 100644 --- a/src/Vehicle/SendMavCommandWithHandlerTest.h +++ b/src/Vehicle/SendMavCommandWithHandlerTest.h @@ -25,17 +25,19 @@ private: typedef struct { MAV_CMD command; MAV_RESULT expectedCommandResult; - uint8_t progress; + bool expectInProgressResult; Vehicle::MavCmdResultFailureCode_t expectedFailureCode; int expectedSendCount; } TestCase_t; void _testCaseWorker(TestCase_t& testCase); - static void _mavCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, Vehicle::MavCmdResultFailureCode_t failureCode); - static void _compIdAllMavCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, Vehicle::MavCmdResultFailureCode_t failureCode); + static void _mavCmdResultHandler (void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode); + static void _mavCmdProgressHandler (void* progressHandlerData, int compId, const mavlink_command_ack_t& ack); + static void _compIdAllFailureMavCmdResultHandler(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode); - static bool _handlerCalled; + static bool _resultHandlerCalled; + static bool _progressHandlerCalled; static TestCase_t _rgTestCases[]; }; diff --git a/src/Vehicle/SendMavCommandWithSignallingTest.cc b/src/Vehicle/SendMavCommandWithSignallingTest.cc index a5b0139..ff1c26e 100644 --- a/src/Vehicle/SendMavCommandWithSignallingTest.cc +++ b/src/Vehicle/SendMavCommandWithSignallingTest.cc @@ -19,6 +19,9 @@ SendMavCommandWithSignallingTest::TestCase_t SendMavCommandWithSignallingTest::_ { 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 }, + { MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED, MAV_RESULT_ACCEPTED, Vehicle::MavCmdResultCommandResultOnly, 1 }, + { MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED, MAV_RESULT_FAILED, Vehicle::MavCmdResultCommandResultOnly, 1 }, + { MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK, MAV_RESULT_FAILED, Vehicle::MavCmdResultFailureNoResponseToCommand, 1 }, }; void SendMavCommandWithSignallingTest::_testCaseWorker(TestCase_t& testCase) @@ -29,7 +32,7 @@ void SendMavCommandWithSignallingTest::_testCaseWorker(TestCase_t& testCase) Vehicle* vehicle = vehicleMgr->activeVehicle(); QSignalSpy spyResult(vehicle, &Vehicle::mavCommandResult); - _mockLink->clearSendMavCommandCounts(); + _mockLink->clearReceivedMavCommandCounts(); vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1, testCase.command, true /* showError */); @@ -42,7 +45,7 @@ void SendMavCommandWithSignallingTest::_testCaseWorker(TestCase_t& testCase) 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); + QCOMPARE(_mockLink->receivedMavCommandCount(testCase.command), testCase.expectedSendCount); _disconnectMockLink(); } @@ -64,7 +67,7 @@ void SendMavCommandWithSignallingTest::_duplicateCommand(void) Vehicle* vehicle = vehicleMgr->activeVehicle(); 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)); + QVERIFY(QTest::qWaitFor([&]() { return _mockLink->receivedMavCommandCount(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 */); @@ -76,6 +79,6 @@ void SendMavCommandWithSignallingTest::_duplicateCommand(void) 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); + QCOMPARE(_mockLink->receivedMavCommandCount(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/Vehicle.cc b/src/Vehicle/Vehicle.cc index a82fa79..5962b4b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2960,8 +2960,7 @@ void Vehicle::sendMavCommand(int compId, MAV_CMD command, bool showError, float { _sendMavCommandWorker(false, // commandInt showError, - nullptr, // resultHandler - nullptr, // resultHandlerData + nullptr, // no handlers compId, command, MAV_FRAME_GLOBAL, @@ -2982,12 +2981,11 @@ void Vehicle::sendCommand(int compId, int command, bool showError, double param1 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) +void Vehicle::sendMavCommandWithHandler(const MavCmdAckHandlerInfo_t* ackHandlerInfo, int compId, MAV_CMD command, float param1, float param2, float param3, float param4, float param5, float param6, float param7) { _sendMavCommandWorker(false, // commandInt false, // showError - resultHandler, - resultHandlerData, + ackHandlerInfo, compId, command, MAV_FRAME_GLOBAL, @@ -2998,20 +2996,18 @@ void Vehicle::sendMavCommandInt(int compId, MAV_CMD command, MAV_FRAME frame, bo { _sendMavCommandWorker(true, // commandInt showError, - nullptr, // resultHandler - nullptr, // resultHandlerData + nullptr, // no handlers compId, command, frame, param1, param2, param3, param4, param5, param6, param7); } -void Vehicle::sendMavCommandIntWithHandler(MavCmdResultHandler resultHandler, void *resultHandlerData, int compId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, double param5, double param6, float param7) +void Vehicle::sendMavCommandIntWithHandler(const MavCmdAckHandlerInfo_t* ackHandlerInfo, int compId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, double param5, double param6, float param7) { _sendMavCommandWorker(true, // commandInt false, // showError - resultHandler, - resultHandlerData, + ackHandlerInfo, compId, command, frame, @@ -3080,19 +3076,30 @@ bool Vehicle::_commandCanBeDuplicated(MAV_CMD command) } } -void Vehicle::_sendMavCommandWorker(bool commandInt, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int targetCompId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, double param5, double param6, float param7) +void Vehicle::_sendMavCommandWorker( + bool commandInt, + bool showError, + const MavCmdAckHandlerInfo_t* ackHandlerInfo, + int targetCompId, + MAV_CMD command, + MAV_FRAME frame, + float param1, float param2, float param3, float param4, double param5, double param6, float param7) { + // We can't send commands to compIdAll using this method. The reason being that we would get responses back possibly from multiple components + // which this code can't handle. + // We also can't send the majority of commands again if we are already waiting for a response from that same command. If we did that we would not be able to discern + // which ack was associated with which command. if ((targetCompId == MAV_COMP_ID_ALL) || (isMavCommandPending(targetCompId, command) && !_commandCanBeDuplicated(command))) { 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, 0, failureCode); + if (ackHandlerInfo && ackHandlerInfo->resultHandler) { + mavlink_command_ack_t ack = {}; + ack.result = MAV_RESULT_FAILED; + (*ackHandlerInfo->resultHandler)(ackHandlerInfo->resultHandlerData, targetCompId, ack, failureCode); } else { emit mavCommandResult(_id, targetCompId, command, MAV_RESULT_FAILED, failureCode); } @@ -3116,8 +3123,10 @@ void Vehicle::_sendMavCommandWorker(bool commandInt, bool showError, MavCmdResul entry.command = command; entry.frame = frame; entry.showError = showError; - entry.resultHandler = resultHandler; - entry.resultHandlerData = resultHandlerData; + entry.ackHandlerInfo = {}; + if (ackHandlerInfo) { + entry.ackHandlerInfo = *ackHandlerInfo; + } entry.rgParam1 = param1; entry.rgParam2 = param2; entry.rgParam3 = param3; @@ -3142,8 +3151,10 @@ void Vehicle::_sendMavCommandFromList(int index) if (++_mavCommandList[index].tryCount > commandEntry.maxTries) { qCDebug(VehicleLog) << "_sendMavCommandFromList giving up after max retries" << rawCommandName; _mavCommandList.removeAt(index); - if (commandEntry.resultHandler) { - (*commandEntry.resultHandler)(commandEntry.resultHandlerData, commandEntry.targetCompId, MAV_RESULT_FAILED, 0, MavCmdResultFailureNoResponseToCommand); + if (commandEntry.ackHandlerInfo.resultHandler) { + mavlink_command_ack_t ack = {}; + ack.result = MAV_RESULT_FAILED; + (*commandEntry.ackHandlerInfo.resultHandler)(commandEntry.ackHandlerInfo.resultHandlerData, commandEntry.targetCompId, ack, MavCmdResultFailureNoResponseToCommand); } else { emit mavCommandResult(_id, commandEntry.targetCompId, commandEntry.command, MAV_RESULT_FAILED, MavCmdResultFailureNoResponseToCommand); } @@ -3264,12 +3275,21 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) #endif int entryIndex = _findMavCommandListEntryIndex(message.compid, static_cast(ack.command)); - bool commandInList = false; if (entryIndex != -1) { - MavCommandListEntry_t commandEntry = _mavCommandList.takeAt(entryIndex); - if (commandEntry.command == ack.command) { - if (commandEntry.resultHandler) { - (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast(ack.result), ack.progress, MavCmdResultCommandResultOnly); + if (ack.result == MAV_RESULT_IN_PROGRESS) { + MavCommandListEntry_t commandEntry = _mavCommandList.at(entryIndex); // Command has not completed yet, don't remove + + commandEntry.maxTries = 1; // Vehicle responsed to command so don't retry + commandEntry.elapsedTimer.restart(); // We've heard from vehicle, restart elapsed timer for no ack received timeout + + if (commandEntry.ackHandlerInfo.progressHandler) { + (*commandEntry.ackHandlerInfo.progressHandler)(commandEntry.ackHandlerInfo.progressHandlerData, message.compid, ack); + } + } else { + MavCommandListEntry_t commandEntry = _mavCommandList.takeAt(entryIndex); + + if (commandEntry.ackHandlerInfo.resultHandler) { + (*commandEntry.ackHandlerInfo.resultHandler)(commandEntry.ackHandlerInfo.resultHandlerData, message.compid, ack, MavCmdResultCommandResultOnly); } else { if (commandEntry.showError) { switch (ack.result) { @@ -3292,11 +3312,8 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) } emit mavCommandResult(_id, message.compid, ack.command, ack.result, MavCmdResultCommandResultOnly); } - commandInList = true; } - } - - if (!commandInList) { + } else { qCDebug(VehicleLog) << "_handleCommandAck Ack not in list" << rawCommandName; } @@ -3360,10 +3377,13 @@ void Vehicle::requestMessage(RequestMessageResultHandler resultHandler, void* re _waitForMavlinkMessage(_requestMessageWaitForMessageResultHandler, pInfo, pInfo->msgId, 1000); + Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {}; + handlerInfo.resultHandler = _requestMessageCmdResultHandler; + handlerInfo.resultHandlerData = pInfo; + _sendMavCommandWorker(false, // commandInt false, // showError - _requestMessageCmdResultHandler, - pInfo, // resultHandlerData + &handlerInfo, compId, MAV_CMD_REQUEST_MESSAGE, MAV_FRAME_GLOBAL, @@ -3371,13 +3391,13 @@ void Vehicle::requestMessage(RequestMessageResultHandler resultHandler, void* re param1, param2, param3, param4, param5, 0); } -void Vehicle::_requestMessageCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, uint8_t progress, MavCmdResultFailureCode_t failureCode) +void Vehicle::_requestMessageCmdResultHandler(void* resultHandlerData, int /*compId*/, const mavlink_command_ack_t& ack, MavCmdResultFailureCode_t failureCode) { RequestMessageInfo_t* pInfo = static_cast(resultHandlerData); Vehicle* vehicle = pInfo->vehicle; pInfo->commandAckReceived = true; - if (result != MAV_RESULT_ACCEPTED) { + if (ack.result != MAV_RESULT_ACCEPTED) { mavlink_message_t message; RequestMessageResultHandlerFailureCode_t requestMessageFailureCode; @@ -3394,12 +3414,12 @@ void Vehicle::_requestMessageCmdResultHandler(void* resultHandlerData, int /*com } vehicle->_waitForMavlinkMessageClear(); - (*pInfo->resultHandler)(pInfo->resultHandlerData, result, requestMessageFailureCode, message); + (*pInfo->resultHandler)(pInfo->resultHandlerData, static_cast(ack.result), requestMessageFailureCode, message); return; } if (pInfo->messageReceived) { - (*pInfo->resultHandler)(pInfo->resultHandlerData, result, RequestMessageNoFailure, pInfo->message); + (*pInfo->resultHandler)(pInfo->resultHandlerData, static_cast(ack.result), RequestMessageNoFailure, pInfo->message); delete pInfo; } else { vehicle->_waitForMavlinkMessageTimeoutActive = true; @@ -3468,16 +3488,14 @@ QString Vehicle::firmwareVersionTypeString() const } } -void Vehicle::_rebootCommandResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT commandResult, uint8_t progress, MavCmdResultFailureCode_t failureCode) +void Vehicle::_rebootCommandResultHandler(void* resultHandlerData, int /*compId*/, const mavlink_command_ack_t& ack, MavCmdResultFailureCode_t failureCode) { - Q_UNUSED(progress) - Vehicle* vehicle = static_cast(resultHandlerData); - if (commandResult != MAV_RESULT_ACCEPTED) { + if (ack.result != MAV_RESULT_ACCEPTED) { switch (failureCode) { case MavCmdResultCommandResultOnly: - qCDebug(VehicleLog) << QStringLiteral("MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN error(%1)").arg(commandResult); + qCDebug(VehicleLog) << QStringLiteral("MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN error(%1)").arg(ack.result); break; case MavCmdResultFailureNoResponseToCommand: qCDebug(VehicleLog) << "MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN failed: no response from vehicle"; @@ -3494,7 +3512,11 @@ void Vehicle::_rebootCommandResultHandler(void* resultHandlerData, int /*compId* void Vehicle::rebootVehicle() { - sendMavCommandWithHandler(_rebootCommandResultHandler, this, _defaultComponentId, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1); + Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {}; + handlerInfo.resultHandler = _rebootCommandResultHandler; + handlerInfo.resultHandlerData = this; + + sendMavCommandWithHandler(&handlerInfo, _defaultComponentId, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1); } void Vehicle::startCalibration(Vehicle::CalibrationType calType) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 1eb292f..5a61c78 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -763,21 +763,38 @@ public: MavCmdResultFailureDuplicateCommand, ///< Unable 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 failureCode Failure reason - typedef void (*MavCmdResultHandler)(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, MavCmdResultFailureCode_t failureCode); + /// Callback for sendMavCommandWithHandler which handles MAV_RESULT_IN_PROGRESS acks + /// @param progressHandlerData Opaque data passed in to sendMavCommand call + /// @param ack Received COMMAND_ACK + typedef void (*MavCmdProgressHandler)(void* progressHandlerData, int compId, const mavlink_command_ack_t& ack); + + /// Callback for sendMavCommandWithHandler which handles all acks which are not MAV_RESULT_IN_PROGRESS + /// @param resultHandlerData Opaque data passed in to sendMavCommand call + /// @param ack Received COMMAND_ACK + /// @param failureCode Failure reason. If not MavCmdResultCommandResultOnly only ack.result == MAV_RESULT_FAILED is valid. + typedef void (*MavCmdResultHandler)(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, MavCmdResultFailureCode_t failureCode); + + // Callback info for sendMavCommandWithHandler + typedef struct MavCmdAckHandlerInfo_s { + MavCmdResultHandler resultHandler; ///> nullptr for no handler + void* resultHandlerData; + MavCmdProgressHandler progressHandler; + void* progressHandlerData; ///> nullptr for no handler + } MavCmdAckHandlerInfo_t; /// Sends the command and calls the callback with the result - /// @param resultHandler Callback for result, nullptr for no callback - /// @param resultHandleData Opaque data passed through callback - void sendMavCommandWithHandler(MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); + void sendMavCommandWithHandler( + const MavCmdAckHandlerInfo_t* ackHandlerInfo, ///> nullptr to signale no handlers + int compId, MAV_CMD command, + float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); /// Sends the command and calls the callback with the result /// @param resultHandler Callback for result, nullptr for no callback /// @param resultHandleData Opaque data passed through callback - void sendMavCommandIntWithHandler(MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, MAV_FRAME frame, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, double param5 = 0.0f, double param6 = 0.0f, float param7 = 0.0f); + void sendMavCommandIntWithHandler( + const MavCmdAckHandlerInfo_t* ackHandlerInfo, ///> nullptr to signale no handlers + int compId, MAV_CMD command, MAV_FRAME frame, + float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, double param5 = 0.0f, double param6 = 0.0f, float param7 = 0.0f); typedef enum { RequestMessageNoFailure, @@ -1095,7 +1112,7 @@ private: EventHandler& _eventHandler (uint8_t compid); bool setFlightModeCustom (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode); - static void _rebootCommandResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, MavCmdResultFailureCode_t failureCode); + static void _rebootCommandResultHandler(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, MavCmdResultFailureCode_t failureCode); // This is called after we get terrain data triggered from a doSetHome() void _doSetHomeTerrainReceived (bool success, QList heights); @@ -1309,28 +1326,27 @@ private: mavlink_message_t message; } RequestMessageInfo_t; - static void _requestMessageCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT result, uint8_t progress, MavCmdResultFailureCode_t failureCode); + static void _requestMessageCmdResultHandler (void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, MavCmdResultFailureCode_t failureCode); static void _requestMessageWaitForMessageResultHandler (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message); typedef struct MavCommandListEntry { - int targetCompId = MAV_COMP_ID_AUTOPILOT1; - bool useCommandInt = false; - MAV_CMD command; - MAV_FRAME frame; - float rgParam1 = 0; - float rgParam2 = 0; - float rgParam3 = 0; - float rgParam4 = 0; - double rgParam5 = 0; - double rgParam6 = 0; - float rgParam7 = 0; - bool showError = true; - MavCmdResultHandler resultHandler; - void* resultHandlerData = nullptr; - int maxTries = _mavCommandMaxRetryCount; - int tryCount = 0; - QElapsedTimer elapsedTimer; - int ackTimeoutMSecs = _mavCommandAckTimeoutMSecs; + int targetCompId = MAV_COMP_ID_AUTOPILOT1; + bool useCommandInt = false; + MAV_CMD command; + MAV_FRAME frame; + float rgParam1 = 0; + float rgParam2 = 0; + float rgParam3 = 0; + float rgParam4 = 0; + double rgParam5 = 0; + double rgParam6 = 0; + float rgParam7 = 0; + bool showError = true; + MavCmdAckHandlerInfo_t ackHandlerInfo; + int maxTries = _mavCommandMaxRetryCount; + int tryCount = 0; + QElapsedTimer elapsedTimer; + int ackTimeoutMSecs = _mavCommandAckTimeoutMSecs; } MavCommandListEntry_t; QList _mavCommandList; @@ -1340,7 +1356,11 @@ private: static const int _mavCommandAckTimeoutMSecs = 3000; static const int _mavCommandAckTimeoutMSecsHighLatency = 120000; - void _sendMavCommandWorker (bool commandInt, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, double param5, double param6, float param7); + void _sendMavCommandWorker ( + bool commandInt, bool showError, + const MavCmdAckHandlerInfo_t* ackHandlerInfo, ///> nullptr to signale no handlers + int compId, MAV_CMD command, MAV_FRAME frame, + float param1, float param2, float param3, float param4, double param5, double param6, float param7); void _sendMavCommandFromList(int index); int _findMavCommandListEntryIndex(int targetCompId, MAV_CMD command); bool _sendMavCommandShouldRetry(MAV_CMD command); diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index bd264e3..025662f 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -55,6 +55,9 @@ 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; +constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED; +constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED; +constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK; // The LinkManager is only forward declared in the header, so a static_assert is here instead to ensure we update if the value changes. static_assert(LinkManager::invalidMavlinkChannel() == std::numeric_limits::max(), "update MockLink::_mavlinkAuxChannel"); @@ -1052,6 +1055,53 @@ void MockLink::_handleFTP(const mavlink_message_t& msg) _mockLinkFTP->mavlinkMessageReceived(msg); } +void MockLink::_handleInProgressCommandLong(const mavlink_command_long_t& request) +{ + uint8_t commandResult = MAV_RESULT_UNSUPPORTED; + + switch (request.command) { + case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED: + // Test command which sends in progress messages and then acceptance ack + commandResult = MAV_RESULT_ACCEPTED; + break; + case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED: + // Test command which sends in progress messages and then failure ack + commandResult = MAV_RESULT_FAILED; + break; + case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK: + // Test command which sends in progress messages and then never sends final result ack + break; + } + + mavlink_message_t commandAck; + + mavlink_msg_command_ack_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &commandAck, + request.command, + MAV_RESULT_IN_PROGRESS, + 1, // progress + 0, // result_param2 + 0, // target_system + 0); // target_component + respondWithMavlinkMessage(commandAck); + + if (request.command != MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK) { + mavlink_msg_command_ack_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &commandAck, + request.command, + commandResult, + 0, // progress + 0, // result_param2 + 0, // target_system + 0); // target_component + respondWithMavlinkMessage(commandAck); + } +} + void MockLink::_handleCommandLong(const mavlink_message_t& msg) { static bool firstCmdUser3 = true; @@ -1063,7 +1113,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) mavlink_msg_command_long_decode(&msg, &request); - _sendMavCommandCountMap[static_cast(request.command)]++; + _receivedMavCommandCountMap[static_cast(request.command)]++; switch (request.command) { case MAV_CMD_COMPONENT_ARM_DISARM: @@ -1111,7 +1161,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) commandResult = MAV_RESULT_FAILED; break; case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED: - // Test command which returns MAV_RESULT_ACCEPTED on second attempt + // Test command which does not respond to first request and returns MAV_RESULT_ACCEPTED on second attempt if (firstCmdUser3) { firstCmdUser3 = false; return; @@ -1121,7 +1171,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) } break; case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED: - // Test command which returns MAV_RESULT_FAILED on second attempt + // Test command which does not respond to first request and returns MAV_RESULT_FAILED on second attempt if (firstCmdUser4) { firstCmdUser4 = false; return; @@ -1132,7 +1182,12 @@ 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 + // Test command which never responds + return; + case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED: + case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED: + case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK: + _handleInProgressCommandLong(request); return; } diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index d1b4e64..dca13e9 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -162,16 +162,19 @@ 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. By default all commands except for MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY should retry. + // Special commands for testing Vehicle::sendMavCommandWithHandler 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); + static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED = static_cast(MAV_CMD_USER_5 + 2); + static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED = static_cast(MAV_CMD_USER_5 + 3); + static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK = static_cast(MAV_CMD_USER_5 + 4); - void clearSendMavCommandCounts(void) { _sendMavCommandCountMap.clear(); } - int sendMavCommandCount(MAV_CMD command) { return _sendMavCommandCountMap[command]; } + void clearReceivedMavCommandCounts(void) { _receivedMavCommandCountMap.clear(); } + int receivedMavCommandCount(MAV_CMD command) { return _receivedMavCommandCountMap[command]; } typedef enum { FailRequestMessageNone, @@ -220,6 +223,7 @@ private: void _handleParamRequestRead (const mavlink_message_t& msg); void _handleFTP (const mavlink_message_t& msg); void _handleCommandLong (const mavlink_message_t& msg); + void _handleInProgressCommandLong (const mavlink_command_long_t& request); void _handleManualControl (const mavlink_message_t& msg); void _handlePreFlightCalibration (const mavlink_command_long_t& request); void _handleLogRequestList (const mavlink_message_t& msg); @@ -310,7 +314,7 @@ private: RequestMessageFailureMode_t _requestMessageFailureMode = FailRequestMessageNone; - QMap _sendMavCommandCountMap; + QMap _receivedMavCommandCountMap; QMap> _mapParamName2Value; QMap> _mapParamName2MavParamType;