Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
15cc3b70
Unverified
Commit
15cc3b70
authored
Oct 09, 2020
by
Don Gagne
Committed by
GitHub
Oct 09, 2020
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Vehicle::sendMavCommand: send command immediately, retries only for limited set of messages
parent
455ac1ed
Changes
20
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
20 changed files
with
536 additions
and
591 deletions
+536
-591
qgroundcontrol.pro
qgroundcontrol.pro
+0
-2
InitialConnectStateMachine.cc
src/Vehicle/InitialConnectStateMachine.cc
+21
-8
InitialConnectStateMachine.h
src/Vehicle/InitialConnectStateMachine.h
+3
-2
InitialConnectTest.cc
src/Vehicle/InitialConnectTest.cc
+0
-72
InitialConnectTest.h
src/Vehicle/InitialConnectTest.h
+0
-42
RequestMessageTest.cc
src/Vehicle/RequestMessageTest.cc
+66
-13
RequestMessageTest.h
src/Vehicle/RequestMessageTest.h
+5
-1
SendMavCommandWithHandlerTest.cc
src/Vehicle/SendMavCommandWithHandlerTest.cc
+74
-10
SendMavCommandWithHandlerTest.h
src/Vehicle/SendMavCommandWithHandlerTest.h
+11
-4
SendMavCommandWithSignallingTest.cc
src/Vehicle/SendMavCommandWithSignallingTest.cc
+43
-128
SendMavCommandWithSignallingTest.h
src/Vehicle/SendMavCommandWithSignallingTest.h
+13
-7
Vehicle.cc
src/Vehicle/Vehicle.cc
+216
-124
Vehicle.h
src/Vehicle/Vehicle.h
+60
-50
MockLink.cc
src/comm/MockLink.cc
+10
-1
MockLink.h
src/comm/MockLink.h
+12
-7
main.cc
src/main.cc
+2
-0
UnitTestList.cc
src/qgcunittest/UnitTestList.cc
+0
-2
UAS.cc
src/uas/UAS.cc
+0
-100
UAS.h
src/uas/UAS.h
+0
-4
UASInterface.h
src/uas/UASInterface.h
+0
-14
No files found.
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
noResponsefromVehicl
e
)
void
InitialConnectStateMachine
::
_capabilitiesCmdResultHandler
(
void
*
resultHandlerData
,
int
/*compId*/
,
MAV_RESULT
result
,
Vehicle
::
MavCmdResultFailureCode_t
failureCod
e
)
{
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
noResponsefromVehicl
e
)
void
InitialConnectStateMachine
::
_protocolVersionCmdResultHandler
(
void
*
resultHandlerData
,
int
/*compId*/
,
MAV_RESULT
result
,
Vehicle
::
MavCmdResultFailureCode_t
failureCod
e
)
{
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
noResponsefromVehicl
e
);
static
void
_protocolVersionCmdResultHandler
(
void
*
resultHandlerData
,
int
compId
,
MAV_RESULT
result
,
bool
noResponsefromVehicl
e
);
static
void
_capabilitiesCmdResultHandler
(
void
*
resultHandlerData
,
int
compId
,
MAV_RESULT
result
,
Vehicle
::
MavCmdResultFailureCode_t
failureCod
e
);
static
void
_protocolVersionCmdResultHandler
(
void
*
resultHandlerData
,
int
compId
,
MAV_RESULT
result
,
Vehicle
::
MavCmdResultFailureCode_t
failureCod
e
);
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.cc
View file @
15cc3b70
This diff is collapsed.
Click to expand it.
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.
Click to expand it.
src/Vehicle/Vehicle.h
View file @
15cc3b70
This diff is collapsed.
Click to expand it.
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.0
f
)
{
...
...
@@ -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.0
f
,
time
);