Skip to content

Q
qgroundcontrol
Unverified Commit 15cc3b70 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Vehicle::sendMavCommand: send command immediately, retries only for limited set of messages

parent 455ac1ed
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 536 additions and 591 deletions
qgroundcontrol.pro
View file @ 15cc3b70
......@@ -495,7 +495,6 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src/qgcunittest/MultiSignalSpyV2.h \
src/qgcunittest/UnitTest.h \
src/Vehicle/FTPManagerTest.h \
src/Vehicle/InitialConnectTest.h \
src/Vehicle/RequestMessageTest.h \
src/Vehicle/SendMavCommandWithHandlerTest.h \
src/Vehicle/SendMavCommandWithSignallingTest.h \
......@@ -543,7 +542,6 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src/qgcunittest/UnitTest.cc \
src/qgcunittest/UnitTestList.cc \
src/Vehicle/FTPManagerTest.cc \
src/Vehicle/InitialConnectTest.cc \
src/Vehicle/RequestMessageTest.cc \
src/Vehicle/SendMavCommandWithHandlerTest.cc \
src/Vehicle/SendMavCommandWithSignallingTest.cc \
......
src/Vehicle/InitialConnectStateMachine.cc
View file @ 15cc3b70
......@@ -72,17 +72,24 @@ void InitialConnectStateMachine::_stateRequestCapabilities(StateMachine* stateMa
}
}
void InitialConnectStateMachine::_capabilitiesCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, bool noResponsefromVehicle)
void InitialConnectStateMachine::_capabilitiesCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, Vehicle::MavCmdResultFailureCode_t failureCode)
{
InitialConnectStateMachine* connectMachine = static_cast<InitialConnectStateMachine*>(resultHandlerData);
Vehicle* vehicle = connectMachine->_vehicle;
if (result != MAV_RESULT_ACCEPTED) {
if (noResponsefromVehicle) {
qCDebug(InitialConnectStateMachineLog) << "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES no response from vehicle";
} else {
switch (failureCode) {
case Vehicle::MavCmdResultCommandResultOnly:
qCDebug(InitialConnectStateMachineLog) << QStringLiteral("MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES error(%1)").arg(result);
break;
case Vehicle::MavCmdResultFailureNoResponseToCommand:
qCDebug(InitialConnectStateMachineLog) << "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES no response from vehicle";
break;
case Vehicle::MavCmdResultFailureDuplicateCommand:
qCDebug(InitialConnectStateMachineLog) << "Internal Error: MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES could not be sent due to duplicate command";
break;
}
qCDebug(InitialConnectStateMachineLog) << "Setting no capabilities";
vehicle->_setCapabilities(0);
vehicle->_waitForMavlinkMessageClear();
......@@ -170,16 +177,22 @@ void InitialConnectStateMachine::_stateRequestProtocolVersion(StateMachine* stat
}
}
void InitialConnectStateMachine::_protocolVersionCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, bool noResponsefromVehicle)
void InitialConnectStateMachine::_protocolVersionCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, Vehicle::MavCmdResultFailureCode_t failureCode)
{
if (result != MAV_RESULT_ACCEPTED) {
InitialConnectStateMachine* connectMachine = static_cast<InitialConnectStateMachine*>(resultHandlerData);
Vehicle* vehicle = connectMachine->_vehicle;
if (noResponsefromVehicle) {
qCDebug(InitialConnectStateMachineLog) << "MAV_CMD_REQUEST_PROTOCOL_VERSION no response from vehicle";
} else {
switch (failureCode) {
case Vehicle::MavCmdResultCommandResultOnly:
qCDebug(InitialConnectStateMachineLog) << QStringLiteral("MAV_CMD_REQUEST_PROTOCOL_VERSION error(%1)").arg(result);
break;
case Vehicle::MavCmdResultFailureNoResponseToCommand:
qCDebug(InitialConnectStateMachineLog) << "MAV_CMD_REQUEST_PROTOCOL_VERSION no response from vehicle";
break;
case Vehicle::MavCmdResultFailureDuplicateCommand:
qCDebug(InitialConnectStateMachineLog) << "Internal Error: MAV_CMD_REQUEST_PROTOCOL_VERSION could not be sent due to duplicate command";
break;
}
// _mavlinkProtocolRequestMaxProtoVersion stays at 0 to indicate unknown
......
src/Vehicle/InitialConnectStateMachine.h
View file @ 15cc3b70
......@@ -12,6 +12,7 @@
#include "StateMachine.h"
#include "QGCMAVLink.h"
#include "QGCLoggingCategory.h"
#include "Vehicle.h"
Q_DECLARE_LOGGING_CATEGORY(InitialConnectStateMachineLog)
......@@ -38,8 +39,8 @@ private:
static void _stateRequestRallyPoints (StateMachine* stateMachine);
static void _stateSignalInitialConnectComplete (StateMachine* stateMachine);
static void _capabilitiesCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT result, bool noResponsefromVehicle);
static void _protocolVersionCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT result, bool noResponsefromVehicle);
static void _capabilitiesCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT result, Vehicle::MavCmdResultFailureCode_t failureCode);
static void _protocolVersionCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT result, Vehicle::MavCmdResultFailureCode_t failureCode);
static void _waitForAutopilotVersionResultHandler (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message);
static void _waitForProtocolVersionResultHandler (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message);
......
src/Vehicle/InitialConnectTest.cc deleted 100644 → 0
View file @ 455ac1ed
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "InitialConnectTest.h"
#include "MultiVehicleManager.h"
#include "QGCApplication.h"
#include "MockLink.h"
InitialConnectTest::TestCase_t InitialConnectTest::_rgTestCases[] = {
{ MockLink::FailRequestMessageNone, MAV_RESULT_ACCEPTED, Vehicle::RequestMessageNoFailure, false },
{ MockLink::FailRequestMessageCommandAcceptedMsgNotSent, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureMessageNotReceived, false },
{ MockLink::FailRequestMessageCommandUnsupported, MAV_RESULT_UNSUPPORTED, Vehicle::RequestMessageFailureCommandError, false },
{ MockLink::FailRequestMessageCommandNoResponse, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureCommandNotAcked, false },
//{ MockLink::FailRequestMessageCommandAcceptedSecondAttempMsgSent, MAV_RESULT_FAILED, Vehicle::RequestMessageNoFailure, false },
};
void InitialConnectTest::_requestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message)
{
TestCase_t* testCase = static_cast<TestCase_t*>(resultHandlerData);
testCase->resultHandlerCalled = true;
QCOMPARE((int)testCase->expectedCommandResult, (int)commandResult);
QCOMPARE((int)testCase->expectedFailureCode, (int)failureCode);
if (testCase->expectedFailureCode == Vehicle::RequestMessageNoFailure) {
QVERIFY(message.msgid == MAVLINK_MSG_ID_DEBUG);
}
}
void InitialConnectTest::_testCaseWorker(TestCase_t& testCase)
{
_connectMockLink();
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle();
_mockLink->setRequestMessageFailureMode(testCase.failureMode);
vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_ALL, MAVLINK_MSG_ID_DEBUG);
for (int i=0; i<100; i++) {
QTest::qWait(100);
if (testCase.resultHandlerCalled) {
break;
}
}
QVERIFY(testCase.resultHandlerCalled);
_disconnectMockLink();
}
void InitialConnectTest::_test(void)
{
_connectMockLink();
//MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
//Vehicle* vehicle = vehicleMgr->activeVehicle();
_disconnectMockLink();
}
void InitialConnectTest::_performTestCases(void)
{
int index = 0;
for (TestCase_t& testCase: _rgTestCases) {
qDebug() << "Testing case" << index++;
_testCaseWorker(testCase);
}
}
src/Vehicle/InitialConnectTest.h deleted 100644 → 0
View file @ 455ac1ed
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once
#include "UnitTest.h"
#include "MockLink.h"
#include "Vehicle.h"
class InitialConnectTest : public UnitTest
{
Q_OBJECT
signals:
void resultHandlerCalled(void);
private slots:
void _test(void);
private:
void _performTestCases(void);
typedef struct {
MockLink::RequestMessageFailureMode_t failureMode;
MAV_RESULT expectedCommandResult;
Vehicle::RequestMessageResultHandlerFailureCode_t expectedFailureCode;
bool resultHandlerCalled;
} TestCase_t;
void _testCaseWorker(TestCase_t& testCase);
static void _requestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message);
bool _resultHandlerCalled;
static TestCase_t _rgTestCases[];
};
src/Vehicle/RequestMessageTest.cc
View file @ 15cc3b70
......@@ -13,11 +13,10 @@
#include "MockLink.h"
RequestMessageTest::TestCase_t RequestMessageTest::_rgTestCases[] = {
{ MockLink::FailRequestMessageNone, MAV_RESULT_ACCEPTED, Vehicle::RequestMessageNoFailure, false },
{ MockLink::FailRequestMessageCommandAcceptedMsgNotSent, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureMessageNotReceived, false },
{ MockLink::FailRequestMessageCommandUnsupported, MAV_RESULT_UNSUPPORTED, Vehicle::RequestMessageFailureCommandError, false },
{ MockLink::FailRequestMessageCommandNoResponse, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureCommandNotAcked, false },
//{ MockLink::FailRequestMessageCommandAcceptedSecondAttempMsgSent, MAV_RESULT_FAILED, Vehicle::RequestMessageNoFailure, false },
// { MockLink::FailRequestMessageNone, MAV_RESULT_ACCEPTED, Vehicle::RequestMessageNoFailure, 1, false },
{ MockLink::FailRequestMessageCommandAcceptedMsgNotSent, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureMessageNotReceived, 1, false },
{ MockLink::FailRequestMessageCommandUnsupported, MAV_RESULT_UNSUPPORTED, Vehicle::RequestMessageFailureCommandError, 1, false },
{ MockLink::FailRequestMessageCommandNoResponse, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureCommandNotAcked, Vehicle::_mavCommandMaxRetryCount, false },
};
void RequestMessageTest::_requestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message)
......@@ -39,15 +38,13 @@ void RequestMessageTest::_testCaseWorker(TestCase_t& testCase)
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle();
_mockLink->clearSendMavCommandCounts();
_mockLink->setRequestMessageFailureMode(testCase.failureMode);
vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_ALL, MAVLINK_MSG_ID_DEBUG);
for (int i=0; i<100; i++) {
QTest::qWait(100);
if (testCase.resultHandlerCalled) {
break;
}
}
QVERIFY(testCase.resultHandlerCalled);
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);
_disconnectMockLink();
}
......@@ -60,3 +57,59 @@ void RequestMessageTest::_performTestCases(void)
_testCaseWorker(testCase);
}
}
void RequestMessageTest::_duplicateCommand(void)
{
_connectMockLinkNoInitialConnectSequence();
RequestMessageTest::TestCase_t testCase = {
MockLink::FailRequestMessageCommandNoResponse, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureDuplicateCommand, 1, false
};
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle();
_mockLink->clearSendMavCommandCounts();
_mockLink->setRequestMessageFailureMode(testCase.failureMode);
vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG);
QVERIFY(QTest::qWaitFor([&]() { return _mockLink->sendMavCommandCount(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);
QVERIFY(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE) != -1);
}
void RequestMessageTest::_compIdAllRequestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& /*message*/)
{
TestCase_t* testCase = static_cast<TestCase_t*>(resultHandlerData);
testCase->resultHandlerCalled = true;
QCOMPARE((int)testCase->expectedCommandResult, (int)commandResult);
QCOMPARE((int)testCase->expectedFailureCode, (int)failureCode);
}
void RequestMessageTest::_compIdAllFailure(void)
{
_connectMockLinkNoInitialConnectSequence();
RequestMessageTest::TestCase_t testCase = {
MockLink::FailRequestMessageCommandNoResponse, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureCommandError, 0, false
};
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle();
_mockLink->clearSendMavCommandCounts();
_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);
_disconnectMockLink();
}
src/Vehicle/RequestMessageTest.h
View file @ 15cc3b70
......@@ -22,18 +22,22 @@ signals:
private slots:
void _performTestCases(void);
void _compIdAllFailure(void);
void _duplicateCommand(void);
private:
typedef struct {
MockLink::RequestMessageFailureMode_t failureMode;
MAV_RESULT expectedCommandResult;
Vehicle::RequestMessageResultHandlerFailureCode_t expectedFailureCode;
int expectedSendCount;
bool resultHandlerCalled;
} TestCase_t;
void _testCaseWorker(TestCase_t& testCase);
static void _requestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message);
static void _requestMessageResultHandler (void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message);
static void _compIdAllRequestMessageResultHandler (void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message);
bool _resultHandlerCalled;
......
src/Vehicle/SendMavCommandWithHandlerTest.cc
View file @ 15cc3b70
......@@ -13,20 +13,25 @@
#include "MockLink.h"
SendMavCommandWithHandlerTest::TestCase_t SendMavCommandWithHandlerTest::_rgTestCases[] = {
{ MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, MAV_RESULT_ACCEPTED, false },
{ MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED, MAV_RESULT_ACCEPTED, false },
{ MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED, MAV_RESULT_ACCEPTED, false },
{ MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED, MAV_RESULT_ACCEPTED, false },
{ MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, MAV_RESULT_ACCEPTED, false },
{ MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, MAV_RESULT_ACCEPTED, Vehicle::MavCmdResultCommandResultOnly, 1 },
{ MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED, MAV_RESULT_FAILED, Vehicle::MavCmdResultCommandResultOnly, 1 },
{ MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED, MAV_RESULT_ACCEPTED, 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_NO_RETRY, MAV_RESULT_FAILED, Vehicle::MavCmdResultFailureNoResponseToCommand, 1 },
};
void SendMavCommandWithHandlerTest::_mavCmdResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle)
bool SendMavCommandWithHandlerTest::_handlerCalled = false;
void SendMavCommandWithHandlerTest::_mavCmdResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode)
{
TestCase_t* testCase = static_cast<TestCase_t*>(resultHandlerData);
QVERIFY(compId == MAV_COMP_ID_AUTOPILOT1);
QCOMPARE(testCase->expectedCommandResult, commandResult);
QCOMPARE(testCase->expectedNoResponseFromVehicle, noResponsefromVehicle);
_handlerCalled = true;
QCOMPARE(compId, MAV_COMP_ID_AUTOPILOT1);
QCOMPARE(testCase->expectedCommandResult, commandResult);
QCOMPARE(testCase->expectedFailureCode, failureCode);
}
void SendMavCommandWithHandlerTest::_testCaseWorker(TestCase_t& testCase)
......@@ -36,7 +41,12 @@ void SendMavCommandWithHandlerTest::_testCaseWorker(TestCase_t& testCase)
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle();
vehicle->sendMavCommandWithHandler(_mavCmdResultHandler, &testCase, MAV_COMP_ID_ALL, testCase.command);
_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);
_disconnectMockLink();
}
......@@ -49,3 +59,57 @@ void SendMavCommandWithHandlerTest::_performTestCases(void)
_testCaseWorker(testCase);
}
}
void SendMavCommandWithHandlerTest::_duplicateCommand(void)
{
_connectMockLinkNoInitialConnectSequence();
SendMavCommandWithHandlerTest::TestCase_t testCase = {
MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, MAV_RESULT_FAILED, Vehicle::MavCmdResultFailureDuplicateCommand, 1
};
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle();
_handlerCalled = false;
_mockLink->clearSendMavCommandCounts();
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);
// Duplicate command response should happen immediately
QVERIFY(_handlerCalled);
QVERIFY(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, testCase.command) != -1);
QCOMPARE(_mockLink->sendMavCommandCount(testCase.command), 1);
}
void SendMavCommandWithHandlerTest::_compIdAllMavCmdResultHandler(void* /*resultHandlerData*/, int compId, MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode)
{
_handlerCalled = true;
QCOMPARE(compId, MAV_COMP_ID_ALL);
QCOMPARE(commandResult, MAV_RESULT_FAILED);
QCOMPARE(failureCode, Vehicle::MavCmdResultCommandResultOnly);
}
void SendMavCommandWithHandlerTest::_compIdAllFailure(void)
{
_connectMockLinkNoInitialConnectSequence();
SendMavCommandWithHandlerTest::TestCase_t testCase = {
MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, MAV_RESULT_FAILED, Vehicle::MavCmdResultFailureDuplicateCommand, 0
};
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);
QCOMPARE(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_ALL, testCase.command), -1);
QCOMPARE(_mockLink->sendMavCommandCount(testCase.command), testCase.expectedSendCount);
_disconnectMockLink();
}
src/Vehicle/SendMavCommandWithHandlerTest.h
View file @ 15cc3b70
......@@ -10,6 +10,7 @@
#pragma once
#include "UnitTest.h"
#include "Vehicle.h"
class SendMavCommandWithHandlerTest : public UnitTest
{
......@@ -17,17 +18,23 @@ class SendMavCommandWithHandlerTest : public UnitTest
private slots:
void _performTestCases(void);
void _compIdAllFailure(void);
void _duplicateCommand(void);
private:
typedef struct {
MAV_CMD command;
MAV_RESULT expectedCommandResult;
bool expectedNoResponseFromVehicle;
MAV_CMD command;
MAV_RESULT expectedCommandResult;
Vehicle::MavCmdResultFailureCode_t expectedFailureCode;
int expectedSendCount;
} TestCase_t;
void _testCaseWorker(TestCase_t& testCase);
static void _mavCmdResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle);
static void _mavCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode);
static void _compIdAllMavCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode);
static bool _handlerCalled;
static TestCase_t _rgTestCases[];
};
src/Vehicle/SendMavCommandWithSignallingTest.h
View file @ 15cc3b70
......@@ -10,19 +10,25 @@
#pragma once
#include "UnitTest.h"
#include "Vehicle.h"
class SendMavCommandWithSignallingTest : public UnitTest
{
Q_OBJECT
private slots:
void _noFailure (void);
void _failureShowError (void);
void _failureNoShowError (void);
void _noFailureAfterRetry (void);
void _failureAfterRetry (void);
void _failureAfterNoReponse (void);
void _unexpectedAck (void);
void _performTestCases(void);
void _duplicateCommand(void);
private:
typedef struct {
MAV_CMD command;
MAV_RESULT expectedCommandResult;
Vehicle::MavCmdResultFailureCode_t expectedFailureCode;
int expectedSendCount;
} TestCase_t;
void _testCaseWorker(TestCase_t& testCase);
static TestCase_t _rgTestCases[];
};
src/Vehicle/Vehicle.cc
View file @ 15cc3b70
This diff is collapsed.
src/Vehicle/Vehicle.h
View file @ 15cc3b70
This diff is collapsed.
src/comm/MockLink.cc
View file @ 15cc3b70
......@@ -48,6 +48,13 @@ const char* MockConfiguration::_sendStatusTextKey = "SendStatusText";
const char* MockConfiguration::_incrementVehicleIdKey = "IncrementVehicleId";
const char* MockConfiguration::_failureModeKey = "FailureMode";
constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED;
constexpr MAV_CMD MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED;
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;
MockLink::MockLink(SharedLinkConfigurationPtr& config)
: LinkInterface (config)
, _missionItemHandler (this, qgcApp()->toolbox()->mavlinkProtocol())
......@@ -985,6 +992,8 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
mavlink_msg_command_long_decode(&msg, &request);
_sendMavCommandCountMap[static_cast<MAV_CMD>(request.command)]++;
switch (request.command) {
case MAV_CMD_COMPONENT_ARM_DISARM:
if (request.param1 == 0.0f) {
......@@ -1051,9 +1060,9 @@ 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
return;
break;
}
mavlink_message_t commandAck;
......
src/comm/MockLink.h
View file @ 15cc3b70
......@@ -153,12 +153,16 @@ 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
static const MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED = MAV_CMD_USER_1;
static const MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED = MAV_CMD_USER_2;
static const MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED = MAV_CMD_USER_3;
static const MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED = MAV_CMD_USER_4;
static const MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE = MAV_CMD_USER_5;
// Special commands for testing COMMAND_LONG handlers. By default all commands except for MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY should retry.
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);
void clearSendMavCommandCounts(void) { _sendMavCommandCountMap.clear(); }
int sendMavCommandCount(MAV_CMD command) { return _sendMavCommandCountMap[command]; }
// Special message ids for testing requestMessage support
typedef enum {
......@@ -237,7 +241,7 @@ private:
int _mavlinkChannel;
uint8_t _vehicleSystemId;
uint8_t _vehicleComponentId;
uint8_t _vehicleComponentId = MAV_COMP_ID_AUTOPILOT1;
bool _inNSH;
bool _mavlinkStarted;
......@@ -287,6 +291,7 @@ private:
RequestMessageFailureMode_t _requestMessageFailureMode = FailRequestMessageNone;
QMap<MAV_CMD, int> _sendMavCommandCountMap;
QMap<int, QMap<QString, QVariant>> _mapParamName2Value;
QMap<int, QMap<QString, MAV_PARAM_TYPE>> _mapParamName2MavParamType;
......
src/main.cc
View file @ 15cc3b70
......@@ -289,6 +289,8 @@ int main(int argc, char *argv[])
#endif
#endif
qRegisterMetaType<Vehicle::MavCmdResultFailureCode_t>("Vehicle::MavCmdResultFailureCode_t");
// We statically link our own QtLocation plugin
#ifdef Q_OS_WIN
......
src/qgcunittest/UnitTestList.cc
View file @ 15cc3b70
......@@ -44,7 +44,6 @@
#include "CameraCalcTest.h"
#include "FWLandingPatternTest.h"
#include "RequestMessageTest.h"
#include "InitialConnectTest.h"
#include "FTPManagerTest.h"
#include "MissionCommandTreeEditorTest.h"
#include "VehicleLinkManagerTest.h"
......@@ -60,7 +59,6 @@ UT_REGISTER_TEST(SendMavCommandWithSignallingTest)
UT_REGISTER_TEST(SendMavCommandWithHandlerTest)
UT_REGISTER_TEST(RequestMessageTest)
UT_REGISTER_TEST(FTPManagerTest)
UT_REGISTER_TEST(InitialConnectTest)
UT_REGISTER_TEST(MissionItemTest)
UT_REGISTER_TEST(SimpleMissionItemTest)
UT_REGISTER_TEST(MissionControllerTest)
......
src/uas/UAS.cc
View file @ 15cc3b70
......@@ -67,7 +67,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
imagePackets(0), // We must initialize to 0, otherwise extended data packets maybe incorrectly thought to be images
blockHomePositionChanges(false),
receivedMode(false),
// Note variances calculated from flight case from this log: http://dash.oznet.ch/view/MRjW8NUNYQSuSZkbn8dEjY
// TODO: calibrate stand-still pixhawk variances
......@@ -144,93 +143,8 @@ void UAS::receiveMessage(mavlink_message_t message)
// and we already got one attitude packet
if (message.sysid == uasId && (!attitudeStamped || lastAttitude != 0 || message.msgid == MAVLINK_MSG_ID_ATTITUDE))
{
bool multiComponentSourceDetected = false;
bool wrongComponent = false;
switch (message.compid)
{
case MAV_COMP_ID_IMU_2:
// Prefer IMU 2 over IMU 1 (FIXME)
componentID[message.msgid] = MAV_COMP_ID_IMU_2;
break;
default:
// Do nothing
break;
}
// Store component ID
if (!componentID.contains(message.msgid))
{
// Prefer the first component
componentID[message.msgid] = message.compid;
componentMulti[message.msgid] = false;
}
else
{
// Got this message already
if (componentID[message.msgid] != message.compid)
{
componentMulti[message.msgid] = true;
wrongComponent = true;
}
}
if (componentMulti[message.msgid] == true) {
multiComponentSourceDetected = true;
}
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
if (multiComponentSourceDetected && wrongComponent)
{
break;
}
mavlink_heartbeat_t state;
mavlink_msg_heartbeat_decode(&message, &state);
// Send the base_mode and system_status values to the plotter. This uses the ground time
// so the Ground Time checkbox must be ticked for these values to display
quint64 time = getUnixTime();
QString name = QString("M%1:HEARTBEAT.%2").arg(message.sysid);
emit valueChanged(uasId, name.arg("base_mode"), "bits", state.base_mode, time);
emit valueChanged(uasId, name.arg("custom_mode"), "bits", state.custom_mode, time);
emit valueChanged(uasId, name.arg("system_status"), "-", state.system_status, time);
// We got the mode
receivedMode = true;
}
break;
case MAVLINK_MSG_ID_SYS_STATUS:
{
if (multiComponentSourceDetected && wrongComponent)
{
break;
}
mavlink_sys_status_t state;
mavlink_msg_sys_status_decode(&message, &state);
// Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present.
quint64 time = getUnixTime();
QString name = QString("M%1:SYS_STATUS.%2").arg(message.sysid);
emit valueChanged(uasId, name.arg("sensors_enabled"), "bits", state.onboard_control_sensors_enabled, time);
emit valueChanged(uasId, name.arg("sensors_health"), "bits", state.onboard_control_sensors_health, time);
emit valueChanged(uasId, name.arg("errors_comm"), "-", state.errors_comm, time);
emit valueChanged(uasId, name.arg("errors_count1"), "-", state.errors_count1, time);
emit valueChanged(uasId, name.arg("errors_count2"), "-", state.errors_count2, time);
emit valueChanged(uasId, name.arg("errors_count3"), "-", state.errors_count3, time);
emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time);
// Process CPU load.
emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time);