Skip to content

Commit

Permalink
Fix up support for MAV_RESULT_IN_PROGRESS
Browse files Browse the repository at this point in the history 
* Update unit tests to test for in progress
* Split result handler into regular ack and in progress ack callbacks
  • Loading branch information
@DonLakeFlyer
DonLakeFlyer committed Nov 6, 2023
1 parent f79b466 commit fa0a445
Show file tree
Hide file tree
Showing 18 changed files with 360 additions and 182 deletions.
16 changes: 9 additions & 7 deletions src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand All @@ -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;
Expand All @@ -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,
Expand Down
19 changes: 10 additions & 9 deletions src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand All @@ -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)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -736,4 +737,4 @@ bool PX4FirmwarePlugin::hasGripper(const Vehicle* vehicle) const
return _hasGripper;
}
return false;
}
}
12 changes: 7 additions & 5 deletions src/Vehicle/Actuators/ActuatorActions.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand Down
3 changes: 1 addition & 2 deletions src/Vehicle/Actuators/ActuatorActions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,8 +48,7 @@ class Action : public QObject
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();

Expand Down
12 changes: 7 additions & 5 deletions src/Vehicle/Actuators/ActuatorTesting.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down
3 changes: 1 addition & 2 deletions src/Vehicle/Actuators/ActuatorTesting.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,8 +110,7 @@ class ActuatorTest : public QObject

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);

Expand Down
12 changes: 7 additions & 5 deletions src/Vehicle/Actuators/MotorAssignment.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand Down
3 changes: 1 addition & 2 deletions src/Vehicle/Actuators/MotorAssignment.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,8 +62,7 @@ private slots:
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);

Expand Down
46 changes: 33 additions & 13 deletions src/Vehicle/Autotune.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,31 +41,46 @@ 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);

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()
Expand Down Expand Up @@ -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
Expand Down
3 changes: 2 additions & 1 deletion src/Vehicle/Autotune.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,8 @@ class Autotune : public QObject

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; }
Expand Down
18 changes: 9 additions & 9 deletions src/Vehicle/RequestMessageTest.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand All @@ -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));

Expand Down Expand Up @@ -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();
}
Loading

0 comments on commit fa0a445

Please sign in to comment.