zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
QGC4.4
Don Gagne 1 year ago committed by Don Gagne
parent
9b660f19fc
commit
570548a4d1
18 changed files with 360 additions and 182 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 16
      src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
  2. 17
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
  3. 12
      src/Vehicle/Actuators/ActuatorActions.cc
  4. 3
      src/Vehicle/Actuators/ActuatorActions.h
  5. 12
      src/Vehicle/Actuators/ActuatorTesting.cc
  6. 3
      src/Vehicle/Actuators/ActuatorTesting.h
  7. 12
      src/Vehicle/Actuators/MotorAssignment.cc
  8. 3
      src/Vehicle/Actuators/MotorAssignment.h
  9. 46
      src/Vehicle/Autotune.cpp
  10. 3
      src/Vehicle/Autotune.h
  11. 18
      src/Vehicle/RequestMessageTest.cc
  12. 117
      src/Vehicle/SendMavCommandWithHandlerTest.cc
  13. 10
      src/Vehicle/SendMavCommandWithHandlerTest.h
  14. 11
      src/Vehicle/SendMavCommandWithSignallingTest.cc
  15. 102
      src/Vehicle/Vehicle.cc
  16. 80
      src/Vehicle/Vehicle.h
  17. 63
      src/comm/MockLink.cc
  18. 12
      src/comm/MockLink.h

16
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
Unescape View File

@ -777,10 +777,8 @@ typedef struct {
Vehicle* vehicle; Vehicle* vehicle;
} MAV_CMD_DO_REPOSITION_HandlerData_t; } 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* data = (MAV_CMD_DO_REPOSITION_HandlerData_t*)resultHandlerData;
auto* vehicle = data->vehicle; auto* vehicle = data->vehicle;
auto* instanceData = qobject_cast<APMFirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData()); auto* instanceData = qobject_cast<APMFirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData());
@ -791,8 +789,8 @@ static void _MAV_CMD_DO_REPOSITION_ResultHandler(void* resultHandlerData, int /*
goto out; goto out;
} }
instanceData->MAV_CMD_DO_REPOSITION_supported = (commandResult == MAV_RESULT_ACCEPTED); instanceData->MAV_CMD_DO_REPOSITION_supported = (ack.result == MAV_RESULT_ACCEPTED);
instanceData->MAV_CMD_DO_REPOSITION_unsupported = (commandResult == MAV_RESULT_UNSUPPORTED); instanceData->MAV_CMD_DO_REPOSITION_unsupported = (ack.result == MAV_RESULT_UNSUPPORTED);
out: out:
delete data; delete data;
@ -818,9 +816,13 @@ void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord
auto* result_handler_data = new MAV_CMD_DO_REPOSITION_HandlerData_t{ auto* result_handler_data = new MAV_CMD_DO_REPOSITION_HandlerData_t{
vehicle vehicle
}; };
Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {};
handlerInfo.resultHandler = _MAV_CMD_DO_REPOSITION_ResultHandler;
handlerInfo.resultHandlerData = result_handler_data;
vehicle->sendMavCommandIntWithHandler( vehicle->sendMavCommandIntWithHandler(
_MAV_CMD_DO_REPOSITION_ResultHandler, &handlerInfo,
result_handler_data,
vehicle->defaultComponentId(), vehicle->defaultComponentId(),
MAV_CMD_DO_REPOSITION, MAV_CMD_DO_REPOSITION,
MAV_FRAME_GLOBAL, MAV_FRAME_GLOBAL,

17
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
Unescape View File

@ -494,14 +494,12 @@ typedef struct {
double newAMSLAlt; double newAMSLAlt;
} PauseVehicleThenChangeAltData_t; } 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 (ack.result != MAV_RESULT_ACCEPTED) {
if (commandResult != MAV_RESULT_ACCEPTED) {
switch (failureCode) { switch (failureCode) {
case Vehicle::MavCmdResultCommandResultOnly: 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; break;
case Vehicle::MavCmdResultFailureNoResponseToCommand: case Vehicle::MavCmdResultFailureNoResponseToCommand:
qDebug() << "MAV_CMD_DO_REPOSITION no response from vehicle"; qDebug() << "MAV_CMD_DO_REPOSITION no response from vehicle";
@ -513,7 +511,7 @@ static void _pauseVehicleThenChangeAltResultHandler(void* resultHandlerData, int
} }
PauseVehicleThenChangeAltData_t* pData = static_cast<PauseVehicleThenChangeAltData_t*>(resultHandlerData); PauseVehicleThenChangeAltData_t* pData = static_cast<PauseVehicleThenChangeAltData_t*>(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) void PX4FirmwarePlugin::_changeAltAfterPause(void* resultHandlerData, bool pauseSucceeded)
@ -557,9 +555,12 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
resultData->newAMSLAlt = vehicle->homePosition().altitude() + newAltRel; resultData->newAMSLAlt = vehicle->homePosition().altitude() + newAltRel;
if (pauseVehicle) { if (pauseVehicle) {
Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {};
handlerInfo.resultHandler = _pauseVehicleThenChangeAltResultHandler;
handlerInfo.resultHandlerData = resultData;
vehicle->sendMavCommandWithHandler( vehicle->sendMavCommandWithHandler(
_pauseVehicleThenChangeAltResultHandler, &handlerInfo,
resultData,
vehicle->defaultComponentId(), vehicle->defaultComponentId(),
MAV_CMD_DO_REPOSITION, MAV_CMD_DO_REPOSITION,
-1.0f, // Don't change groundspeed -1.0f, // Don't change groundspeed

12
src/Vehicle/Actuators/ActuatorActions.cc
Unescape View File

@ -39,11 +39,10 @@ void Action::trigger()
sendMavlinkRequest(); sendMavlinkRequest();
} }
void Action::ackHandlerEntry(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, void Action::ackHandlerEntry(void* resultHandlerData, int /*compId*/, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode)
Vehicle::MavCmdResultFailureCode_t failureCode)
{ {
Action* action = (Action*)resultHandlerData; Action* action = (Action*)resultHandlerData;
action->ackHandler(commandResult, failureCode); action->ackHandler(static_cast<MAV_RESULT>(ack.result), failureCode);
} }
void Action::ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t 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; qCDebug(ActuatorsConfigLog) << "Sending actuator action, function:" << _outputFunction << "type:" << (int)_type;
Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {};
handlerInfo.resultHandler = ackHandlerEntry;
handlerInfo.resultHandlerData = this;
_vehicle->sendMavCommandWithHandler( _vehicle->sendMavCommandWithHandler(
ackHandlerEntry, // Ack callback &handlerInfo,
this, // Ack callback data
MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot
MAV_CMD_CONFIGURE_ACTUATOR, // the mavlink command MAV_CMD_CONFIGURE_ACTUATOR, // the mavlink command
(int)_type, // action type (int)_type, // action type

3
src/Vehicle/Actuators/ActuatorActions.h
Unescape View File

@ -48,8 +48,7 @@ public:
Q_INVOKABLE void trigger(); Q_INVOKABLE void trigger();
private: private:
static void ackHandlerEntry(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, static void ackHandlerEntry(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode);
Vehicle::MavCmdResultFailureCode_t failureCode);
void ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode); void ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode);
void sendMavlinkRequest(); void sendMavlinkRequest();

12
src/Vehicle/Actuators/ActuatorTesting.cc
Unescape View File

@ -120,11 +120,10 @@ void ActuatorTest::setActive(bool active)
_active = active; _active = active;
} }
void ActuatorTest::ackHandlerEntry(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, void ActuatorTest::ackHandlerEntry(void* resultHandlerData, int /*compId*/, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode)
Vehicle::MavCmdResultFailureCode_t failureCode)
{ {
ActuatorTest* actuatorTest = (ActuatorTest*)resultHandlerData; ActuatorTest* actuatorTest = (ActuatorTest*)resultHandlerData;
actuatorTest->ackHandler(commandResult, failureCode); actuatorTest->ackHandler(static_cast<MAV_RESULT>(ack.result), failureCode);
} }
void ActuatorTest::ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t 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 // TODO: consider using a lower command timeout
Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {};
handlerInfo.resultHandler = ackHandlerEntry;
handlerInfo.resultHandlerData = this;
_vehicle->sendMavCommandWithHandler( _vehicle->sendMavCommandWithHandler(
ackHandlerEntry, // Ack callback &handlerInfo,
this, // Ack callback data
MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot
MAV_CMD_ACTUATOR_TEST, // the mavlink command MAV_CMD_ACTUATOR_TEST, // the mavlink command
value, // value value, // value

3
src/Vehicle/Actuators/ActuatorTesting.h
Unescape View File

@ -110,8 +110,7 @@ private:
void resetStates(); void resetStates();
static void ackHandlerEntry(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, static void ackHandlerEntry(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode);
Vehicle::MavCmdResultFailureCode_t failureCode);
void ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode); void ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode);
void sendMavlinkRequest(int function, float value, float timeout); void sendMavlinkRequest(int function, float value, float timeout);

12
src/Vehicle/Actuators/MotorAssignment.cc
Unescape View File

@ -197,11 +197,10 @@ void MotorAssignment::spinTimeout()
spinCurrentMotor(); spinCurrentMotor();
} }
void MotorAssignment::ackHandlerEntry(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, void MotorAssignment::ackHandlerEntry(void* resultHandlerData, int /*compId*/, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode)
Vehicle::MavCmdResultFailureCode_t failureCode)
{ {
MotorAssignment* motorAssignment = (MotorAssignment*)resultHandlerData; MotorAssignment* motorAssignment = (MotorAssignment*)resultHandlerData;
motorAssignment->ackHandler(commandResult, failureCode); motorAssignment->ackHandler(static_cast<MAV_RESULT>(ack.result), failureCode);
} }
void MotorAssignment::ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t 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; qCDebug(ActuatorsConfigLog) << "Sending actuator test function:" << function << "value:" << value;
Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {};
handlerInfo.resultHandler = ackHandlerEntry;
handlerInfo.resultHandlerData = this;
_vehicle->sendMavCommandWithHandler( _vehicle->sendMavCommandWithHandler(
ackHandlerEntry, // Ack callback &handlerInfo,
this, // Ack callback data
MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot
MAV_CMD_ACTUATOR_TEST, // the mavlink command MAV_CMD_ACTUATOR_TEST, // the mavlink command
value, // value value, // value

3
src/Vehicle/Actuators/MotorAssignment.h
Unescape View File

@ -62,8 +62,7 @@ private:
static constexpr int _spinTimeoutDefaultSec = 1000; static constexpr int _spinTimeoutDefaultSec = 1000;
static constexpr int _spinTimeoutHighSec = 3000; ///< wait a bit longer after assigning motors, so ESCs can initialize 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, static void ackHandlerEntry(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode);
Vehicle::MavCmdResultFailureCode_t failureCode);
void ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode); void ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode);
void sendMavlinkRequest(int function, float value); void sendMavlinkRequest(int function, float value);

46
src/Vehicle/Autotune.cpp
Unescape View File

@ -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(compId);
Q_UNUSED(failureCode); Q_UNUSED(failureCode);
@ -49,23 +49,38 @@ void Autotune::ackHandler(void* resultHandlerData, int compId, MAV_RESULT comman
auto * autotune = static_cast<Autotune *>(resultHandlerData); auto * autotune = static_cast<Autotune *>(resultHandlerData);
if (autotune->_autotuneInProgress) { if (autotune->_autotuneInProgress) {
if ((commandResult == MAV_RESULT_IN_PROGRESS) || (commandResult == MAV_RESULT_ACCEPTED)) { if (failureCode == Vehicle::MavCmdResultCommandResultOnly) {
autotune->handleAckStatus(progress); if ((ack.result == MAV_RESULT_IN_PROGRESS) || (ack.result == MAV_RESULT_ACCEPTED)) {
} autotune->handleAckStatus(ack.progress);
else if (commandResult == MAV_RESULT_FAILED) { }
else if (ack.result == MAV_RESULT_FAILED) {
autotune->handleAckFailure();
}
else {
autotune->handleAckError(ack.result);
}
} else {
autotune->handleAckFailure(); autotune->handleAckFailure();
} }
else {
autotune->handleAckError(commandResult);
}
emit autotune->autotuneChanged(); emit autotune->autotuneChanged();
} } else {
else {
qWarning() << "Ack received for a command different from MAV_CMD_DO_AUTOTUNE_ENABLE ot wrong UI state."; 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<Autotune *>(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() bool Autotune::autotuneEnabled()
@ -162,9 +177,14 @@ void Autotune::stopTimers()
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void Autotune::sendMavlinkRequest() void Autotune::sendMavlinkRequest()
{ {
Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {};
handlerInfo.resultHandler = ackHandler;
handlerInfo.resultHandlerData = this;
handlerInfo.progressHandler = progressHandler;
handlerInfo.progressHandlerData = this;
_vehicle->sendMavCommandWithHandler( _vehicle->sendMavCommandWithHandler(
ackHandler, // Ack callback &handlerInfo,
this, // Ack callback data
MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot
MAV_CMD_DO_AUTOTUNE_ENABLE, // the mavlink command MAV_CMD_DO_AUTOTUNE_ENABLE, // the mavlink command
1, // request autotune 1, // request autotune

3
src/Vehicle/Autotune.h
Unescape View File

@ -28,7 +28,8 @@ public:
Q_INVOKABLE void autotuneRequest (); 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 autotuneEnabled ();
bool autotuneInProgress () { return _autotuneInProgress; } bool autotuneInProgress () { return _autotuneInProgress; }

18
src/Vehicle/RequestMessageTest.cc
Unescape View File

@ -38,21 +38,21 @@ void RequestMessageTest::_testCaseWorker(TestCase_t& testCase)
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle(); Vehicle* vehicle = vehicleMgr->activeVehicle();
_mockLink->clearSendMavCommandCounts(); _mockLink->clearReceivedMavCommandCounts();
_mockLink->setRequestMessageFailureMode(testCase.failureMode); _mockLink->setRequestMessageFailureMode(testCase.failureMode);
vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG); vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG);
QVERIFY(QTest::qWaitFor([&]() { return testCase.resultHandlerCalled; }, 10000)); QVERIFY(QTest::qWaitFor([&]() { return testCase.resultHandlerCalled; }, 10000));
QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE), -1); 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 // We should be able to do it twice in a row without any duplicate command problems
testCase.resultHandlerCalled = false; testCase.resultHandlerCalled = false;
_mockLink->clearSendMavCommandCounts(); _mockLink->clearReceivedMavCommandCounts();
vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG); vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG);
QVERIFY(QTest::qWaitFor([&]() { return testCase.resultHandlerCalled; }, 10000)); QVERIFY(QTest::qWaitFor([&]() { return testCase.resultHandlerCalled; }, 10000));
QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE), -1); 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(); _disconnectMockLink();
} }
@ -77,19 +77,19 @@ void RequestMessageTest::_duplicateCommand(void)
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle(); Vehicle* vehicle = vehicleMgr->activeVehicle();
_mockLink->clearSendMavCommandCounts(); _mockLink->clearReceivedMavCommandCounts();
_mockLink->setRequestMessageFailureMode(testCase.failureMode); _mockLink->setRequestMessageFailureMode(testCase.failureMode);
QVERIFY(false == vehicle->isMavCommandPending(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE)); QVERIFY(false == vehicle->isMavCommandPending(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE));
vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG); 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); QCOMPARE(testCase.resultHandlerCalled, false);
vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG); vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG);
// Duplicate command returns immediately // Duplicate command returns immediately
QCOMPARE(testCase.resultHandlerCalled, true); 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(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE) != -1);
QVERIFY(true == vehicle->isMavCommandPending(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE)); 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(); MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle(); Vehicle* vehicle = vehicleMgr->activeVehicle();
_mockLink->clearSendMavCommandCounts(); _mockLink->clearReceivedMavCommandCounts();
_mockLink->setRequestMessageFailureMode(testCase.failureMode); _mockLink->setRequestMessageFailureMode(testCase.failureMode);
vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_ALL, MAVLINK_MSG_ID_DEBUG); vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_ALL, MAVLINK_MSG_ID_DEBUG);
QCOMPARE(testCase.resultHandlerCalled, true); QCOMPARE(testCase.resultHandlerCalled, true);
QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_ALL, MAV_CMD_REQUEST_MESSAGE), -1); 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(); _disconnectMockLink();
} }

117
src/Vehicle/SendMavCommandWithHandlerTest.cc
Unescape View File

@ -13,28 +13,47 @@
#include "MockLink.h" #include "MockLink.h"
SendMavCommandWithHandlerTest::TestCase_t SendMavCommandWithHandlerTest::_rgTestCases[] = { 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_ACCEPTED, MAV_RESULT_ACCEPTED, false, Vehicle::MavCmdResultCommandResultOnly, 1 },
{ MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED, MAV_RESULT_FAILED, 0, 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, 0, Vehicle::MavCmdResultCommandResultOnly, 2 }, { 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, 0, 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, 0, Vehicle::MavCmdResultFailureNoResponseToCommand, Vehicle::_mavCommandMaxRetryCount }, { 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, 0, Vehicle::MavCmdResultFailureNoResponseToCommand, 1 }, { 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<TestCase_t*>(resultHandlerData); TestCase_t* testCase = static_cast<TestCase_t*>(resultHandlerData);
_handlerCalled = true; _resultHandlerCalled = true;
QCOMPARE(compId, MAV_COMP_ID_AUTOPILOT1); QCOMPARE(MAV_COMP_ID_AUTOPILOT1, compId);
QCOMPARE(testCase->expectedCommandResult, commandResult); QCOMPARE(testCase->expectedCommandResult, ack.result);
QCOMPARE(testCase->progress, progress);
QCOMPARE(testCase->expectedFailureCode, failureCode); QCOMPARE(testCase->expectedFailureCode, failureCode);
} }
void SendMavCommandWithHandlerTest::_mavCmdProgressHandler(void* progressHandlerData, int compId, const mavlink_command_ack_t& ack)
{
TestCase_t* testCase = static_cast<TestCase_t*>(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) void SendMavCommandWithHandlerTest::_testCaseWorker(TestCase_t& testCase)
{ {
_connectMockLinkNoInitialConnectSequence(); _connectMockLinkNoInitialConnectSequence();
@ -42,12 +61,25 @@ void SendMavCommandWithHandlerTest::_testCaseWorker(TestCase_t& testCase)
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle(); Vehicle* vehicle = vehicleMgr->activeVehicle();
_handlerCalled = false; Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {};
_mockLink->clearSendMavCommandCounts(); handlerInfo.resultHandler = _mavCmdResultHandler;
vehicle->sendMavCommandWithHandler(_mavCmdResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, testCase.command); handlerInfo.resultHandlerData = &testCase;
QVERIFY(QTest::qWaitFor([&]() { return _handlerCalled; }, 10000)); handlerInfo.progressHandler = _mavCmdProgressHandler;
QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, testCase.command), -1); handlerInfo.progressHandlerData = &testCase;
QCOMPARE(_mockLink->sendMavCommandCount(testCase.command), testCase.expectedSendCount);
_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(); _disconnectMockLink();
} }
@ -72,27 +104,35 @@ void SendMavCommandWithHandlerTest::_duplicateCommand(void)
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle(); Vehicle* vehicle = vehicleMgr->activeVehicle();
_handlerCalled = false; Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {};
_mockLink->clearSendMavCommandCounts(); 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 */); vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1, testCase.command, true /* showError */);
QVERIFY(QTest::qWaitFor([&]() { return _mockLink->sendMavCommandCount(testCase.command) == 1; }, 10)); QVERIFY(QTest::qWaitFor([&]() { return _mockLink->receivedMavCommandCount(testCase.command) == 1; }, 10));
QVERIFY(!_handlerCalled); QVERIFY(!_resultHandlerCalled);
vehicle->sendMavCommandWithHandler(_mavCmdResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, testCase.command); QVERIFY(!_progressHandlerCalled);
vehicle->sendMavCommandWithHandler(&handlerInfo, MAV_COMP_ID_AUTOPILOT1, testCase.command);
// Duplicate command response should happen immediately // Duplicate command response should happen immediately
QVERIFY(_handlerCalled); QVERIFY(_resultHandlerCalled);
QVERIFY(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, testCase.command) != -1); 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(compId, MAV_COMP_ID_ALL);
QCOMPARE(commandResult, MAV_RESULT_FAILED); QCOMPARE(ack.result, MAV_RESULT_FAILED);
QCOMPARE(progress, 0); QCOMPARE(failureCode, Vehicle::MavCmdResultCommandResultOnly);
QCOMPARE(failureCode, Vehicle::MavCmdResultCommandResultOnly);
} }
void SendMavCommandWithHandlerTest::_compIdAllFailure(void) void SendMavCommandWithHandlerTest::_compIdAllFailure(void)
@ -106,12 +146,17 @@ void SendMavCommandWithHandlerTest::_compIdAllFailure(void)
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle(); Vehicle* vehicle = vehicleMgr->activeVehicle();
_handlerCalled = false; Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {};
_mockLink->clearSendMavCommandCounts(); handlerInfo.resultHandler = _compIdAllFailureMavCmdResultHandler;
vehicle->sendMavCommandWithHandler(_compIdAllMavCmdResultHandler, nullptr, MAV_COMP_ID_ALL, testCase.command);
QCOMPARE(_handlerCalled, true); _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(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_ALL, testCase.command), -1);
QCOMPARE(_mockLink->sendMavCommandCount(testCase.command), testCase.expectedSendCount); QCOMPARE(_mockLink->receivedMavCommandCount(testCase.command), testCase.expectedSendCount);
_disconnectMockLink(); _disconnectMockLink();
} }

10
src/Vehicle/SendMavCommandWithHandlerTest.h
Unescape View File

@ -25,17 +25,19 @@ private:
typedef struct { typedef struct {
MAV_CMD command; MAV_CMD command;
MAV_RESULT expectedCommandResult; MAV_RESULT expectedCommandResult;
uint8_t progress; bool expectInProgressResult;
Vehicle::MavCmdResultFailureCode_t expectedFailureCode; Vehicle::MavCmdResultFailureCode_t expectedFailureCode;
int expectedSendCount; int expectedSendCount;
} TestCase_t; } TestCase_t;
void _testCaseWorker(TestCase_t& testCase); void _testCaseWorker(TestCase_t& testCase);
static void _mavCmdResultHandler (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 _compIdAllMavCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, 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[]; static TestCase_t _rgTestCases[];
}; };

11
src/Vehicle/SendMavCommandWithSignallingTest.cc
Unescape View File

@ -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_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, 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_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) void SendMavCommandWithSignallingTest::_testCaseWorker(TestCase_t& testCase)
@ -29,7 +32,7 @@ void SendMavCommandWithSignallingTest::_testCaseWorker(TestCase_t& testCase)
Vehicle* vehicle = vehicleMgr->activeVehicle(); Vehicle* vehicle = vehicleMgr->activeVehicle();
QSignalSpy spyResult(vehicle, &Vehicle::mavCommandResult); QSignalSpy spyResult(vehicle, &Vehicle::mavCommandResult);
_mockLink->clearSendMavCommandCounts(); _mockLink->clearReceivedMavCommandCounts();
vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1, testCase.command, true /* showError */); 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(3).toInt(), testCase.expectedCommandResult);
QCOMPARE(arguments.at(4).value<Vehicle::MavCmdResultFailureCode_t>(), testCase.expectedFailureCode); QCOMPARE(arguments.at(4).value<Vehicle::MavCmdResultFailureCode_t>(), testCase.expectedFailureCode);
QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED), -1); 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(); _disconnectMockLink();
} }
@ -64,7 +67,7 @@ void SendMavCommandWithSignallingTest::_duplicateCommand(void)
Vehicle* vehicle = vehicleMgr->activeVehicle(); Vehicle* vehicle = vehicleMgr->activeVehicle();
vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1, MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, true /* showError */); 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); QSignalSpy spyResult(vehicle, &Vehicle::mavCommandResult);
vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1, MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, true /* showError */); 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(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE);
QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED); QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
QCOMPARE(arguments.at(4).value<Vehicle::MavCmdResultFailureCode_t>(), Vehicle::MavCmdResultFailureDuplicateCommand); QCOMPARE(arguments.at(4).value<Vehicle::MavCmdResultFailureCode_t>(), 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); QVERIFY(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE) != -1);
} }

102
src/Vehicle/Vehicle.cc
Unescape View File

@ -2960,8 +2960,7 @@ void Vehicle::sendMavCommand(int compId, MAV_CMD command, bool showError, float
{ {
_sendMavCommandWorker(false, // commandInt _sendMavCommandWorker(false, // commandInt
showError, showError,
nullptr, // resultHandler nullptr, // no handlers
nullptr, // resultHandlerData
compId, compId,
command, command,
MAV_FRAME_GLOBAL, MAV_FRAME_GLOBAL,
@ -2982,12 +2981,11 @@ void Vehicle::sendCommand(int compId, int command, bool showError, double param1
static_cast<float>(param7)); static_cast<float>(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 _sendMavCommandWorker(false, // commandInt
false, // showError false, // showError
resultHandler, ackHandlerInfo,
resultHandlerData,
compId, compId,
command, command,
MAV_FRAME_GLOBAL, MAV_FRAME_GLOBAL,
@ -2998,20 +2996,18 @@ void Vehicle::sendMavCommandInt(int compId, MAV_CMD command, MAV_FRAME frame, bo
{ {
_sendMavCommandWorker(true, // commandInt _sendMavCommandWorker(true, // commandInt
showError, showError,
nullptr, // resultHandler nullptr, // no handlers
nullptr, // resultHandlerData
compId, compId,
command, command,
frame, frame,
param1, param2, param3, param4, param5, param6, param7); 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 _sendMavCommandWorker(true, // commandInt
false, // showError false, // showError
resultHandler, ackHandlerInfo,
resultHandlerData,
compId, compId,
command, command,
frame, 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))) { if ((targetCompId == MAV_COMP_ID_ALL) || (isMavCommandPending(targetCompId, command) && !_commandCanBeDuplicated(command))) {
bool compIdAll = targetCompId == MAV_COMP_ID_ALL; bool compIdAll = targetCompId == MAV_COMP_ID_ALL;
QString rawCommandName = _toolbox->missionCommandTree()->rawName(command); QString rawCommandName = _toolbox->missionCommandTree()->rawName(command);
qCDebug(VehicleLog) << QStringLiteral("_sendMavCommandWorker failing %1").arg(compIdAll ? "MAV_COMP_ID_ALL not supportded" : "duplicate command") << rawCommandName; 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; MavCmdResultFailureCode_t failureCode = compIdAll ? MavCmdResultCommandResultOnly : MavCmdResultFailureDuplicateCommand;
if (resultHandler) { if (ackHandlerInfo && ackHandlerInfo->resultHandler) {
(*resultHandler)(resultHandlerData, targetCompId, MAV_RESULT_FAILED, 0, failureCode); mavlink_command_ack_t ack = {};
ack.result = MAV_RESULT_FAILED;
(*ackHandlerInfo->resultHandler)(ackHandlerInfo->resultHandlerData, targetCompId, ack, failureCode);
} else { } else {
emit mavCommandResult(_id, targetCompId, command, MAV_RESULT_FAILED, failureCode); 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.command = command;
entry.frame = frame; entry.frame = frame;
entry.showError = showError; entry.showError = showError;
entry.resultHandler = resultHandler; entry.ackHandlerInfo = {};
entry.resultHandlerData = resultHandlerData; if (ackHandlerInfo) {
entry.ackHandlerInfo = *ackHandlerInfo;
}
entry.rgParam1 = param1; entry.rgParam1 = param1;
entry.rgParam2 = param2; entry.rgParam2 = param2;
entry.rgParam3 = param3; entry.rgParam3 = param3;
@ -3142,8 +3151,10 @@ void Vehicle::_sendMavCommandFromList(int index)
if (++_mavCommandList[index].tryCount > commandEntry.maxTries) { if (++_mavCommandList[index].tryCount > commandEntry.maxTries) {
qCDebug(VehicleLog) << "_sendMavCommandFromList giving up after max retries" << rawCommandName; qCDebug(VehicleLog) << "_sendMavCommandFromList giving up after max retries" << rawCommandName;
_mavCommandList.removeAt(index); _mavCommandList.removeAt(index);
if (commandEntry.resultHandler) { if (commandEntry.ackHandlerInfo.resultHandler) {
(*commandEntry.resultHandler)(commandEntry.resultHandlerData, commandEntry.targetCompId, MAV_RESULT_FAILED, 0, MavCmdResultFailureNoResponseToCommand); mavlink_command_ack_t ack = {};
ack.result = MAV_RESULT_FAILED;
(*commandEntry.ackHandlerInfo.resultHandler)(commandEntry.ackHandlerInfo.resultHandlerData, commandEntry.targetCompId, ack, MavCmdResultFailureNoResponseToCommand);
} else { } else {
emit mavCommandResult(_id, commandEntry.targetCompId, commandEntry.command, MAV_RESULT_FAILED, MavCmdResultFailureNoResponseToCommand); emit mavCommandResult(_id, commandEntry.targetCompId, commandEntry.command, MAV_RESULT_FAILED, MavCmdResultFailureNoResponseToCommand);
} }
@ -3264,12 +3275,21 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
#endif #endif
int entryIndex = _findMavCommandListEntryIndex(message.compid, static_cast<MAV_CMD>(ack.command)); int entryIndex = _findMavCommandListEntryIndex(message.compid, static_cast<MAV_CMD>(ack.command));
bool commandInList = false;
if (entryIndex != -1) { if (entryIndex != -1) {
MavCommandListEntry_t commandEntry = _mavCommandList.takeAt(entryIndex); if (ack.result == MAV_RESULT_IN_PROGRESS) {
if (commandEntry.command == ack.command) { MavCommandListEntry_t commandEntry = _mavCommandList.at(entryIndex); // Command has not completed yet, don't remove
if (commandEntry.resultHandler) {
(*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast<MAV_RESULT>(ack.result), ack.progress, MavCmdResultCommandResultOnly); 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 { } else {
if (commandEntry.showError) { if (commandEntry.showError) {
switch (ack.result) { switch (ack.result) {
@ -3292,11 +3312,8 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
} }
emit mavCommandResult(_id, message.compid, ack.command, ack.result, MavCmdResultCommandResultOnly); emit mavCommandResult(_id, message.compid, ack.command, ack.result, MavCmdResultCommandResultOnly);
} }
commandInList = true;
} }
} } else {
if (!commandInList) {
qCDebug(VehicleLog) << "_handleCommandAck Ack not in list" << rawCommandName; 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); _waitForMavlinkMessage(_requestMessageWaitForMessageResultHandler, pInfo, pInfo->msgId, 1000);
Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {};
handlerInfo.resultHandler = _requestMessageCmdResultHandler;
handlerInfo.resultHandlerData = pInfo;
_sendMavCommandWorker(false, // commandInt _sendMavCommandWorker(false, // commandInt
false, // showError false, // showError
_requestMessageCmdResultHandler, &handlerInfo,
pInfo, // resultHandlerData
compId, compId,
MAV_CMD_REQUEST_MESSAGE, MAV_CMD_REQUEST_MESSAGE,
MAV_FRAME_GLOBAL, MAV_FRAME_GLOBAL,
@ -3371,13 +3391,13 @@ void Vehicle::requestMessage(RequestMessageResultHandler resultHandler, void* re
param1, param2, param3, param4, param5, 0); 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<RequestMessageInfo_t*>(resultHandlerData); RequestMessageInfo_t* pInfo = static_cast<RequestMessageInfo_t*>(resultHandlerData);
Vehicle* vehicle = pInfo->vehicle; Vehicle* vehicle = pInfo->vehicle;
pInfo->commandAckReceived = true; pInfo->commandAckReceived = true;
if (result != MAV_RESULT_ACCEPTED) { if (ack.result != MAV_RESULT_ACCEPTED) {
mavlink_message_t message; mavlink_message_t message;
RequestMessageResultHandlerFailureCode_t requestMessageFailureCode; RequestMessageResultHandlerFailureCode_t requestMessageFailureCode;
@ -3394,12 +3414,12 @@ void Vehicle::_requestMessageCmdResultHandler(void* resultHandlerData, int /*com
} }
vehicle->_waitForMavlinkMessageClear(); vehicle->_waitForMavlinkMessageClear();
(*pInfo->resultHandler)(pInfo->resultHandlerData, result, requestMessageFailureCode, message); (*pInfo->resultHandler)(pInfo->resultHandlerData, static_cast<MAV_RESULT>(ack.result), requestMessageFailureCode, message);
return; return;
} }
if (pInfo->messageReceived) { if (pInfo->messageReceived) {
(*pInfo->resultHandler)(pInfo->resultHandlerData, result, RequestMessageNoFailure, pInfo->message); (*pInfo->resultHandler)(pInfo->resultHandlerData, static_cast<MAV_RESULT>(ack.result), RequestMessageNoFailure, pInfo->message);
delete pInfo; delete pInfo;
} else { } else {
vehicle->_waitForMavlinkMessageTimeoutActive = true; 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<Vehicle*>(resultHandlerData); Vehicle* vehicle = static_cast<Vehicle*>(resultHandlerData);
if (commandResult != MAV_RESULT_ACCEPTED) { if (ack.result != MAV_RESULT_ACCEPTED) {
switch (failureCode) { switch (failureCode) {
case MavCmdResultCommandResultOnly: 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; break;
case MavCmdResultFailureNoResponseToCommand: case MavCmdResultFailureNoResponseToCommand:
qCDebug(VehicleLog) << "MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN failed: no response from vehicle"; 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() 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) void Vehicle::startCalibration(Vehicle::CalibrationType calType)

80
src/Vehicle/Vehicle.h
Unescape View File

@ -763,21 +763,38 @@ public:
MavCmdResultFailureDuplicateCommand, ///< Unable to send command since duplicate is already being waited on for response MavCmdResultFailureDuplicateCommand, ///< Unable to send command since duplicate is already being waited on for response
} MavCmdResultFailureCode_t; } MavCmdResultFailureCode_t;
/// Callback for sendMavCommandWithHandler /// Callback for sendMavCommandWithHandler which handles MAV_RESULT_IN_PROGRESS acks
/// @param resultHandleData Opaque data passed in to sendMavCommand call /// @param progressHandlerData Opaque data passed in to sendMavCommand call
/// @param commandResult Ack result for command send /// @param ack Received COMMAND_ACK
/// @param failureCode Failure reason typedef void (*MavCmdProgressHandler)(void* progressHandlerData, int compId, const mavlink_command_ack_t& ack);
typedef void (*MavCmdResultHandler)(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, MavCmdResultFailureCode_t failureCode);
/// 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 /// Sends the command and calls the callback with the result
/// @param resultHandler Callback for result, nullptr for no callback void sendMavCommandWithHandler(
/// @param resultHandleData Opaque data passed through callback const MavCmdAckHandlerInfo_t* ackHandlerInfo, ///> nullptr to signale no handlers
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); 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 /// Sends the command and calls the callback with the result
/// @param resultHandler Callback for result, nullptr for no callback /// @param resultHandler Callback for result, nullptr for no callback
/// @param resultHandleData Opaque data passed through 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 { typedef enum {
RequestMessageNoFailure, RequestMessageNoFailure,
@ -1095,7 +1112,7 @@ private:
EventHandler& _eventHandler (uint8_t compid); EventHandler& _eventHandler (uint8_t compid);
bool setFlightModeCustom (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode); 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() // This is called after we get terrain data triggered from a doSetHome()
void _doSetHomeTerrainReceived (bool success, QList<double> heights); void _doSetHomeTerrainReceived (bool success, QList<double> heights);
@ -1309,28 +1326,27 @@ private:
mavlink_message_t message; mavlink_message_t message;
} RequestMessageInfo_t; } 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); static void _requestMessageWaitForMessageResultHandler (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message);
typedef struct MavCommandListEntry { typedef struct MavCommandListEntry {
int targetCompId = MAV_COMP_ID_AUTOPILOT1; int targetCompId = MAV_COMP_ID_AUTOPILOT1;
bool useCommandInt = false; bool useCommandInt = false;
MAV_CMD command; MAV_CMD command;
MAV_FRAME frame; MAV_FRAME frame;
float rgParam1 = 0; float rgParam1 = 0;
float rgParam2 = 0; float rgParam2 = 0;
float rgParam3 = 0; float rgParam3 = 0;
float rgParam4 = 0; float rgParam4 = 0;
double rgParam5 = 0; double rgParam5 = 0;
double rgParam6 = 0; double rgParam6 = 0;
float rgParam7 = 0; float rgParam7 = 0;
bool showError = true; bool showError = true;
MavCmdResultHandler resultHandler; MavCmdAckHandlerInfo_t ackHandlerInfo;
void* resultHandlerData = nullptr; int maxTries = _mavCommandMaxRetryCount;
int maxTries = _mavCommandMaxRetryCount; int tryCount = 0;
int tryCount = 0; QElapsedTimer elapsedTimer;
QElapsedTimer elapsedTimer; int ackTimeoutMSecs = _mavCommandAckTimeoutMSecs;
int ackTimeoutMSecs = _mavCommandAckTimeoutMSecs;
} MavCommandListEntry_t; } MavCommandListEntry_t;
QList<MavCommandListEntry_t> _mavCommandList; QList<MavCommandListEntry_t> _mavCommandList;
@ -1340,7 +1356,11 @@ private:
static const int _mavCommandAckTimeoutMSecs = 3000; static const int _mavCommandAckTimeoutMSecs = 3000;
static const int _mavCommandAckTimeoutMSecsHighLatency = 120000; 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); void _sendMavCommandFromList(int index);
int _findMavCommandListEntryIndex(int targetCompId, MAV_CMD command); int _findMavCommandListEntryIndex(int targetCompId, MAV_CMD command);
bool _sendMavCommandShouldRetry(MAV_CMD command); bool _sendMavCommandShouldRetry(MAV_CMD command);

63
src/comm/MockLink.cc
Unescape View File

@ -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_SECOND_ATTEMPT_RESULT_FAILED;
constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE; 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_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. // 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<uint8_t>::max(), "update MockLink::_mavlinkAuxChannel"); static_assert(LinkManager::invalidMavlinkChannel() == std::numeric_limits<uint8_t>::max(), "update MockLink::_mavlinkAuxChannel");
@ -1052,6 +1055,53 @@ void MockLink::_handleFTP(const mavlink_message_t& msg)
_mockLinkFTP->mavlinkMessageReceived(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) void MockLink::_handleCommandLong(const mavlink_message_t& msg)
{ {
static bool firstCmdUser3 = true; static bool firstCmdUser3 = true;
@ -1063,7 +1113,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
mavlink_msg_command_long_decode(&msg, &request); mavlink_msg_command_long_decode(&msg, &request);
_sendMavCommandCountMap[static_cast<MAV_CMD>(request.command)]++; _receivedMavCommandCountMap[static_cast<MAV_CMD>(request.command)]++;
switch (request.command) { switch (request.command) {
case MAV_CMD_COMPONENT_ARM_DISARM: case MAV_CMD_COMPONENT_ARM_DISARM:
@ -1111,7 +1161,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
commandResult = MAV_RESULT_FAILED; commandResult = MAV_RESULT_FAILED;
break; break;
case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED: 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) { if (firstCmdUser3) {
firstCmdUser3 = false; firstCmdUser3 = false;
return; return;
@ -1121,7 +1171,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
} }
break; break;
case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED: 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) { if (firstCmdUser4) {
firstCmdUser4 = false; firstCmdUser4 = false;
return; return;
@ -1132,7 +1182,12 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
break; break;
case MAV_CMD_MOCKLINK_NO_RESPONSE: case MAV_CMD_MOCKLINK_NO_RESPONSE:
case MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY: 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; return;
} }

12
src/comm/MockLink.h
Unescape View File

@ -162,16 +162,19 @@ public:
static MockLink* startAPMArduSubMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); static MockLink* startAPMArduSubMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
static MockLink* startAPMArduRoverMockLink (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_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_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_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_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 = MAV_CMD_USER_5;
static constexpr MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY = static_cast<MAV_CMD>(MAV_CMD_USER_5 + 1); static constexpr MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY = static_cast<MAV_CMD>(MAV_CMD_USER_5 + 1);
static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED = static_cast<MAV_CMD>(MAV_CMD_USER_5 + 2);
static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED = static_cast<MAV_CMD>(MAV_CMD_USER_5 + 3);
static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK = static_cast<MAV_CMD>(MAV_CMD_USER_5 + 4);
void clearSendMavCommandCounts(void) { _sendMavCommandCountMap.clear(); } void clearReceivedMavCommandCounts(void) { _receivedMavCommandCountMap.clear(); }
int sendMavCommandCount(MAV_CMD command) { return _sendMavCommandCountMap[command]; } int receivedMavCommandCount(MAV_CMD command) { return _receivedMavCommandCountMap[command]; }
typedef enum { typedef enum {
FailRequestMessageNone, FailRequestMessageNone,
@ -220,6 +223,7 @@ private:
void _handleParamRequestRead (const mavlink_message_t& msg); void _handleParamRequestRead (const mavlink_message_t& msg);
void _handleFTP (const mavlink_message_t& msg); void _handleFTP (const mavlink_message_t& msg);
void _handleCommandLong (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 _handleManualControl (const mavlink_message_t& msg);
void _handlePreFlightCalibration (const mavlink_command_long_t& request); void _handlePreFlightCalibration (const mavlink_command_long_t& request);
void _handleLogRequestList (const mavlink_message_t& msg); void _handleLogRequestList (const mavlink_message_t& msg);
@ -310,7 +314,7 @@ private:
RequestMessageFailureMode_t _requestMessageFailureMode = FailRequestMessageNone; RequestMessageFailureMode_t _requestMessageFailureMode = FailRequestMessageNone;
QMap<MAV_CMD, int> _sendMavCommandCountMap; QMap<MAV_CMD, int> _receivedMavCommandCountMap;
QMap<int, QMap<QString, QVariant>> _mapParamName2Value; QMap<int, QMap<QString, QVariant>> _mapParamName2Value;
QMap<int, QMap<QString, MAV_PARAM_TYPE>> _mapParamName2MavParamType; QMap<int, QMap<QString, MAV_PARAM_TYPE>> _mapParamName2MavParamType;

Write Preview
Loading…
Cancel
Save