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
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 16
      src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
  2. 19
      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 { @@ -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<APMFirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData());
@ -791,8 +789,8 @@ static void _MAV_CMD_DO_REPOSITION_ResultHandler(void* resultHandlerData, int /* @@ -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 @@ -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,

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

@ -494,14 +494,12 @@ typedef struct { @@ -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 @@ -513,7 +511,7 @@ static void _pauseVehicleThenChangeAltResultHandler(void* resultHandlerData, int
}
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)
@ -557,9 +555,12 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu @@ -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 @@ -736,4 +737,4 @@ bool PX4FirmwarePlugin::hasGripper(const Vehicle* vehicle) const
return _hasGripper;
}
return false;
}
}

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

@ -39,11 +39,10 @@ void Action::trigger() @@ -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<MAV_RESULT>(ack.result), failureCode);
}
void Action::ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode)
@ -58,9 +57,12 @@ void Action::sendMavlinkRequest() @@ -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

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

@ -48,8 +48,7 @@ public: @@ -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();

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

@ -120,11 +120,10 @@ void ActuatorTest::setActive(bool active) @@ -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<MAV_RESULT>(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) @@ -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

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

@ -110,8 +110,7 @@ private: @@ -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);

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

@ -197,11 +197,10 @@ void MotorAssignment::spinTimeout() @@ -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<MAV_RESULT>(ack.result), failureCode);
}
void MotorAssignment::ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode)
@ -217,9 +216,12 @@ void MotorAssignment::sendMavlinkRequest(int function, float value) @@ -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

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

@ -62,8 +62,7 @@ private: @@ -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);

46
src/Vehicle/Autotune.cpp
Unescape View File

@ -41,7 +41,7 @@ void Autotune::autotuneRequest() @@ -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 @@ -49,23 +49,38 @@ void Autotune::ackHandler(void* resultHandlerData, int compId, MAV_RESULT comman
auto * autotune = static_cast<Autotune *>(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<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()
@ -162,9 +177,14 @@ void Autotune::stopTimers() @@ -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

3
src/Vehicle/Autotune.h
Unescape View File

@ -28,7 +28,8 @@ public: @@ -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; }

18
src/Vehicle/RequestMessageTest.cc
Unescape View File

@ -38,21 +38,21 @@ void RequestMessageTest::_testCaseWorker(TestCase_t& testCase) @@ -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) @@ -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) @@ -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();
}

117
src/Vehicle/SendMavCommandWithHandlerTest.cc
Unescape View File

@ -13,28 +13,47 @@ @@ -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<TestCase_t*>(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<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)
{
_connectMockLinkNoInitialConnectSequence();
@ -42,12 +61,25 @@ void SendMavCommandWithHandlerTest::_testCaseWorker(TestCase_t& testCase) @@ -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) @@ -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) @@ -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();
}

10
src/Vehicle/SendMavCommandWithHandlerTest.h
Unescape View File

@ -25,17 +25,19 @@ private: @@ -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[];
};

11
src/Vehicle/SendMavCommandWithSignallingTest.cc
Unescape View File

@ -19,6 +19,9 @@ SendMavCommandWithSignallingTest::TestCase_t SendMavCommandWithSignallingTest::_ @@ -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) @@ -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) @@ -42,7 +45,7 @@ void SendMavCommandWithSignallingTest::_testCaseWorker(TestCase_t& testCase)
QCOMPARE(arguments.at(3).toInt(), testCase.expectedCommandResult);
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(_mockLink->sendMavCommandCount(testCase.command), testCase.expectedSendCount);
QCOMPARE(_mockLink->receivedMavCommandCount(testCase.command), testCase.expectedSendCount);
_disconnectMockLink();
}
@ -64,7 +67,7 @@ void SendMavCommandWithSignallingTest::_duplicateCommand(void) @@ -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) @@ -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::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);
}

102
src/Vehicle/Vehicle.cc
Unescape View File

@ -2960,8 +2960,7 @@ void Vehicle::sendMavCommand(int compId, MAV_CMD command, bool showError, float @@ -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 @@ -2982,12 +2981,11 @@ void Vehicle::sendCommand(int compId, int command, bool showError, double param1
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
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 @@ -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) @@ -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 @@ -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) @@ -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) @@ -3264,12 +3275,21 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
#endif
int entryIndex = _findMavCommandListEntryIndex(message.compid, static_cast<MAV_CMD>(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<MAV_RESULT>(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) @@ -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 @@ -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 @@ -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<RequestMessageInfo_t*>(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 @@ -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<MAV_RESULT>(ack.result), requestMessageFailureCode, message);
return;
}
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;
} else {
vehicle->_waitForMavlinkMessageTimeoutActive = true;
@ -3468,16 +3488,14 @@ QString Vehicle::firmwareVersionTypeString() const @@ -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);
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* @@ -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)

80
src/Vehicle/Vehicle.h
Unescape View File

@ -763,21 +763,38 @@ public: @@ -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: @@ -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<double> heights);
@ -1309,28 +1326,27 @@ private: @@ -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<MavCommandListEntry_t> _mavCommandList;
@ -1340,7 +1356,11 @@ private: @@ -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);

63
src/comm/MockLink.cc
Unescape View File

@ -55,6 +55,9 @@ constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED; @@ -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<uint8_t>::max(), "update MockLink::_mavlinkAuxChannel");
@ -1052,6 +1055,53 @@ void MockLink::_handleFTP(const mavlink_message_t& msg) @@ -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) @@ -1063,7 +1113,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
mavlink_msg_command_long_decode(&msg, &request);
_sendMavCommandCountMap[static_cast<MAV_CMD>(request.command)]++;
_receivedMavCommandCountMap[static_cast<MAV_CMD>(request.command)]++;
switch (request.command) {
case MAV_CMD_COMPONENT_ARM_DISARM:
@ -1111,7 +1161,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) @@ -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) @@ -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) @@ -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;
}

12
src/comm/MockLink.h
Unescape View File

@ -162,16 +162,19 @@ public: @@ -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>(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(); }
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: @@ -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: @@ -310,7 +314,7 @@ private:
RequestMessageFailureMode_t _requestMessageFailureMode = FailRequestMessageNone;
QMap<MAV_CMD, int> _sendMavCommandCountMap;
QMap<MAV_CMD, int> _receivedMavCommandCountMap;
QMap<int, QMap<QString, QVariant>> _mapParamName2Value;
QMap<int, QMap<QString, MAV_PARAM_TYPE>> _mapParamName2MavParamType;

Write Preview
Loading…
Cancel
Save