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
36d57743
Commit
36d57743
authored
Feb 23, 2016
by
Don Gagne
Browse files
Options
Browse Files
Download
Plain Diff
Merge pull request #2871 from DonLakeFlyer/SimpleComplex
Rearchitecture of Simple/Complex mission item concept
parents
28625888
10c7e170
Changes
37
Hide whitespace changes
Inline
Side-by-side
Showing
37 changed files
with
2532 additions
and
1342 deletions
+2532
-1342
qgroundcontrol.pro
qgroundcontrol.pro
+4
-0
MavCmdInfoCommon.json
src/FirmwarePlugin/APM/MavCmdInfoCommon.json
+8
-2
MavCmdInfoMultiRotor.json
src/FirmwarePlugin/APM/MavCmdInfoMultiRotor.json
+7
-1
FlightDisplayViewMap.qml
src/FlightDisplay/FlightDisplayViewMap.qml
+1
-1
MissionItemIndicator.qml
src/FlightMap/MapItems/MissionItemIndicator.qml
+5
-2
JsonHelper.cc
src/JsonHelper.cc
+8
-1
MissionEditor.qml
src/MissionEditor/MissionEditor.qml
+30
-30
MissionItemStatus.qml
src/MissionEditor/MissionItemStatus.qml
+1
-1
ComplexMissionItem.cc
src/MissionManager/ComplexMissionItem.cc
+34
-28
ComplexMissionItem.h
src/MissionManager/ComplexMissionItem.h
+29
-23
MavCmdInfoCommon.json
src/MissionManager/MavCmdInfoCommon.json
+2
-2
MissionCommandList.h
src/MissionManager/MissionCommandList.h
+2
-2
MissionController.cc
src/MissionManager/MissionController.cc
+278
-177
MissionController.h
src/MissionManager/MissionController.h
+24
-24
MissionControllerManagerTest.cc
src/MissionManager/MissionControllerManagerTest.cc
+1
-1
MissionControllerTest.cc
src/MissionManager/MissionControllerTest.cc
+19
-18
MissionControllerTest.h
src/MissionManager/MissionControllerTest.h
+4
-3
MissionItem.cc
src/MissionManager/MissionItem.cc
+11
-551
MissionItem.h
src/MissionManager/MissionItem.h
+46
-187
MissionItemTest.cc
src/MissionManager/MissionItemTest.cc
+627
-140
MissionItemTest.h
src/MissionManager/MissionItemTest.h
+7
-38
MissionManager.cc
src/MissionManager/MissionManager.cc
+12
-24
MissionManager.h
src/MissionManager/MissionManager.h
+5
-17
MissionManagerTest.cc
src/MissionManager/MissionManagerTest.cc
+12
-16
SimpleMissionItem.cc
src/MissionManager/SimpleMissionItem.cc
+544
-18
SimpleMissionItem.h
src/MissionManager/SimpleMissionItem.h
+132
-20
SimpleMissionItemTest.cc
src/MissionManager/SimpleMissionItemTest.cc
+293
-0
SimpleMissionItemTest.h
src/MissionManager/SimpleMissionItemTest.h
+76
-0
VisualMissionItem.cc
src/MissionManager/VisualMissionItem.cc
+116
-0
VisualMissionItem.h
src/MissionManager/VisualMissionItem.h
+173
-0
MissionItemEditor.qml
src/QmlControls/MissionItemEditor.qml
+6
-6
QmlObjectListModel.cc
src/QmlControls/QmlObjectListModel.cc
+8
-0
QmlObjectListModel.h
src/QmlControls/QmlObjectListModel.h
+3
-0
Vehicle.cc
src/Vehicle/Vehicle.cc
+0
-5
Vehicle.h
src/Vehicle/Vehicle.h
+0
-3
MultiSignalSpy.cc
src/qgcunittest/MultiSignalSpy.cc
+2
-1
UnitTestList.cc
src/qgcunittest/UnitTestList.cc
+2
-0
No files found.
qgroundcontrol.pro
View file @
36d57743
...
@@ -263,6 +263,7 @@ HEADERS += \
...
@@ -263,6 +263,7 @@ HEADERS += \
src
/
MissionManager
/
MissionManager
.
h
\
src
/
MissionManager
/
MissionManager
.
h
\
src
/
MissionManager
/
ComplexMissionItem
.
h
\
src
/
MissionManager
/
ComplexMissionItem
.
h
\
src
/
MissionManager
/
SimpleMissionItem
.
h
\
src
/
MissionManager
/
SimpleMissionItem
.
h
\
src
/
MissionManager
/
VisualMissionItem
.
h
\
src
/
QGC
.
h
\
src
/
QGC
.
h
\
src
/
QGCApplication
.
h
\
src
/
QGCApplication
.
h
\
src
/
QGCComboBox
.
h
\
src
/
QGCComboBox
.
h
\
...
@@ -392,6 +393,7 @@ SOURCES += \
...
@@ -392,6 +393,7 @@ SOURCES += \
src
/
MissionManager
/
MissionManager
.
cc
\
src
/
MissionManager
/
MissionManager
.
cc
\
src
/
MissionManager
/
ComplexMissionItem
.
cc
\
src
/
MissionManager
/
ComplexMissionItem
.
cc
\
src
/
MissionManager
/
SimpleMissionItem
.
cc
\
src
/
MissionManager
/
SimpleMissionItem
.
cc
\
src
/
MissionManager
/
VisualMissionItem
.
cc
\
src
/
QGC
.
cc
\
src
/
QGC
.
cc
\
src
/
QGCApplication
.
cc
\
src
/
QGCApplication
.
cc
\
src
/
QGCComboBox
.
cc
\
src
/
QGCComboBox
.
cc
\
...
@@ -508,6 +510,7 @@ HEADERS += \
...
@@ -508,6 +510,7 @@ HEADERS += \
src
/
MissionManager
/
MissionControllerManagerTest
.
h
\
src
/
MissionManager
/
MissionControllerManagerTest
.
h
\
src
/
MissionManager
/
MissionItemTest
.
h
\
src
/
MissionManager
/
MissionItemTest
.
h
\
src
/
MissionManager
/
MissionManagerTest
.
h
\
src
/
MissionManager
/
MissionManagerTest
.
h
\
src
/
MissionManager
/
SimpleMissionItemTest
.
h
\
src
/
qgcunittest
/
GeoTest
.
h
\
src
/
qgcunittest
/
GeoTest
.
h
\
src
/
qgcunittest
/
FileDialogTest
.
h
\
src
/
qgcunittest
/
FileDialogTest
.
h
\
src
/
qgcunittest
/
FileManagerTest
.
h
\
src
/
qgcunittest
/
FileManagerTest
.
h
\
...
@@ -531,6 +534,7 @@ SOURCES += \
...
@@ -531,6 +534,7 @@ SOURCES += \
src
/
MissionManager
/
MissionControllerManagerTest
.
cc
\
src
/
MissionManager
/
MissionControllerManagerTest
.
cc
\
src
/
MissionManager
/
MissionItemTest
.
cc
\
src
/
MissionManager
/
MissionItemTest
.
cc
\
src
/
MissionManager
/
MissionManagerTest
.
cc
\
src
/
MissionManager
/
MissionManagerTest
.
cc
\
src
/
MissionManager
/
SimpleMissionItemTest
.
cc
\
src
/
qgcunittest
/
GeoTest
.
cc
\
src
/
qgcunittest
/
GeoTest
.
cc
\
src
/
qgcunittest
/
FileDialogTest
.
cc
\
src
/
qgcunittest
/
FileDialogTest
.
cc
\
src
/
qgcunittest
/
FileManagerTest
.
cc
\
src
/
qgcunittest
/
FileManagerTest
.
cc
\
...
...
src/FirmwarePlugin/APM/MavCmdInfoCommon.json
View file @
36d57743
...
@@ -12,8 +12,14 @@
...
@@ -12,8 +12,14 @@
"category"
:
"Basic"
,
"category"
:
"Basic"
,
"param1"
:
{
"param1"
:
{
"label"
:
"Pitch:"
,
"label"
:
"Pitch:"
,
"units"
:
"radians"
,
"units"
:
"degrees"
,
"default"
:
0.26179939
,
"default"
:
15
,
"decimalPlaces"
:
2
},
"param7"
:
{
"label"
:
"Altitude:"
,
"units"
:
"meters"
,
"default"
:
25.0
,
"decimalPlaces"
:
2
"decimalPlaces"
:
2
}
}
},
},
...
...
src/FirmwarePlugin/APM/MavCmdInfoMultiRotor.json
View file @
36d57743
...
@@ -9,7 +9,13 @@
...
@@ -9,7 +9,13 @@
"description"
:
"Take off from the ground."
,
"description"
:
"Take off from the ground."
,
"specifiesCoordinate"
:
false
,
"specifiesCoordinate"
:
false
,
"friendlyEdit"
:
true
,
"friendlyEdit"
:
true
,
"category"
:
"Basic"
"category"
:
"Basic"
,
"param7"
:
{
"label"
:
"Altitude:"
,
"units"
:
"meters"
,
"default"
:
25.0
,
"decimalPlaces"
:
2
}
},
},
{
{
"id"
:
17
,
"id"
:
17
,
...
...
src/FlightDisplay/FlightDisplayViewMap.qml
View file @
36d57743
...
@@ -89,7 +89,7 @@ FlightMap {
...
@@ -89,7 +89,7 @@ FlightMap {
// Add the mission items to the map
// Add the mission items to the map
MissionItemView
{
MissionItemView
{
model
:
_mainIsMap
?
_missionController
.
mission
Items
:
0
model
:
_mainIsMap
?
_missionController
.
visual
Items
:
0
}
}
// Add lines between waypoints
// Add lines between waypoints
...
...
src/FlightMap/MapItems/MissionItemIndicator.qml
View file @
36d57743
...
@@ -42,8 +42,11 @@ MapQuickItem {
...
@@ -42,8 +42,11 @@ MapQuickItem {
sourceItem
:
sourceItem
:
MissionItemIndexLabel
{
MissionItemIndexLabel
{
id
:
_label
id
:
_label
isCurrentItem
:
missionItem
.
isCurrentItem
isCurrentItem
:
_
isCurrentItem
label
:
missionItem
.
sequenceNumber
==
0
?
"
H
"
:
missionItem
.
sequenceNumber
label
:
_
sequenceNumber
==
0
?
"
H
"
:
missionItem
.
sequenceNumber
onClicked
:
_item
.
clicked
()
onClicked
:
_item
.
clicked
()
property
bool
_isCurrentItem
:
missionItem
?
missionItem
.
isCurrentItem
:
false
property
bool
_sequenceNumber
:
missionItem
?
missionItem
.
sequenceNumber
:
0
}
}
}
}
src/JsonHelper.cc
View file @
36d57743
...
@@ -58,10 +58,17 @@ bool JsonHelper::toQGeoCoordinate(const QJsonValue& jsonValue, QGeoCoordinate& c
...
@@ -58,10 +58,17 @@ bool JsonHelper::toQGeoCoordinate(const QJsonValue& jsonValue, QGeoCoordinate& c
QJsonArray
coordinateArray
=
jsonValue
.
toArray
();
QJsonArray
coordinateArray
=
jsonValue
.
toArray
();
int
requiredCount
=
altitudeRequired
?
3
:
2
;
int
requiredCount
=
altitudeRequired
?
3
:
2
;
if
(
coordinateArray
.
count
()
!=
requiredCount
)
{
if
(
coordinateArray
.
count
()
!=
requiredCount
)
{
errorString
=
QString
(
"
Json array must contains
%1 values"
).
arg
(
requiredCount
);
errorString
=
QString
(
"
Coordinate array must contain
%1 values"
).
arg
(
requiredCount
);
return
false
;
return
false
;
}
}
foreach
(
const
QJsonValue
&
jsonValue
,
coordinateArray
)
{
if
(
jsonValue
.
type
()
!=
QJsonValue
::
Double
)
{
errorString
=
QString
(
"Coordinate array may only contain double values, found: %1"
).
arg
(
jsonValue
.
type
());
return
false
;
}
}
coordinate
=
QGeoCoordinate
(
coordinateArray
[
0
].
toDouble
(),
coordinateArray
[
1
].
toDouble
());
coordinate
=
QGeoCoordinate
(
coordinateArray
[
0
].
toDouble
(),
coordinateArray
[
1
].
toDouble
());
if
(
altitudeRequired
)
{
if
(
altitudeRequired
)
{
coordinate
.
setAltitude
(
coordinateArray
[
2
].
toDouble
());
coordinate
.
setAltitude
(
coordinateArray
[
2
].
toDouble
());
...
...
src/MissionEditor/MissionEditor.qml
View file @
36d57743
...
@@ -40,7 +40,7 @@ import QGroundControl.Controllers 1.0
...
@@ -40,7 +40,7 @@ import QGroundControl.Controllers 1.0
QGCView
{
QGCView
{
id
:
_root
id
:
_root
property
bool
syncNeeded
:
controller
.
mission
Items
.
dirty
// Unsaved changes, visible to parent container
property
bool
syncNeeded
:
controller
.
visual
Items
.
dirty
// Unsaved changes, visible to parent container
viewPanel
:
panel
viewPanel
:
panel
topDialogMargin
:
height
-
mainWindow
.
availableHeight
topDialogMargin
:
height
-
mainWindow
.
availableHeight
...
@@ -60,13 +60,11 @@ QGCView {
...
@@ -60,13 +60,11 @@ QGCView {
readonly
property
int
_addMissionItemsButtonAutoOffTimeout
:
10000
readonly
property
int
_addMissionItemsButtonAutoOffTimeout
:
10000
readonly
property
var
_defaultVehicleCoordinate
:
QtPositioning
.
coordinate
(
37.803784
,
-
122.462276
)
readonly
property
var
_defaultVehicleCoordinate
:
QtPositioning
.
coordinate
(
37.803784
,
-
122.462276
)
property
var
_
missionItems
:
controller
.
mission
Items
property
var
_
visualItems
:
controller
.
visual
Items
property
var
_currentMissionItem
property
var
_currentMissionItem
property
bool
_firstVehiclePosition
:
true
property
bool
_firstVehiclePosition
:
true
property
var
activeVehiclePosition
:
_activeVehicle
?
_activeVehicle
.
coordinate
:
QtPositioning
.
coordinate
()
property
var
activeVehiclePosition
:
_activeVehicle
?
_activeVehicle
.
coordinate
:
QtPositioning
.
coordinate
()
property
bool
_syncInProgress
:
_activeVehicle
?
_activeVehicle
.
missionManager
.
inProgress
:
false
onActiveVehiclePositionChanged
:
updateMapToVehiclePosition
()
onActiveVehiclePositionChanged
:
updateMapToVehiclePosition
()
Connections
{
Connections
{
...
@@ -119,19 +117,19 @@ QGCView {
...
@@ -119,19 +117,19 @@ QGCView {
/// Fix the map viewport to the current mission items.
/// Fix the map viewport to the current mission items.
function
fitViewportToMissionItems
()
{
function
fitViewportToMissionItems
()
{
if
(
_
mission
Items
.
count
==
1
)
{
if
(
_
visual
Items
.
count
==
1
)
{
editorMap
.
center
=
_
mission
Items
.
get
(
0
).
coordinate
editorMap
.
center
=
_
visual
Items
.
get
(
0
).
coordinate
}
else
{
}
else
{
var
missionItem
=
_
mission
Items
.
get
(
0
)
var
missionItem
=
_
visual
Items
.
get
(
0
)
var
north
=
normalizeLat
(
missionItem
.
coordinate
.
latitude
)
var
north
=
normalizeLat
(
missionItem
.
coordinate
.
latitude
)
var
south
=
north
var
south
=
north
var
east
=
normalizeLon
(
missionItem
.
coordinate
.
longitude
)
var
east
=
normalizeLon
(
missionItem
.
coordinate
.
longitude
)
var
west
=
east
var
west
=
east
for
(
var
i
=
1
;
i
<
_
mission
Items
.
count
;
i
++
)
{
for
(
var
i
=
1
;
i
<
_
visual
Items
.
count
;
i
++
)
{
missionItem
=
_
mission
Items
.
get
(
i
)
missionItem
=
_
visual
Items
.
get
(
i
)
if
(
missionItem
.
specifiesCoordinate
&&
!
missionItem
.
s
tandaloneCoordinate
)
{
if
(
missionItem
.
specifiesCoordinate
&&
!
missionItem
.
isS
tandaloneCoordinate
)
{
var
lat
=
normalizeLat
(
missionItem
.
coordinate
.
latitude
)
var
lat
=
normalizeLat
(
missionItem
.
coordinate
.
latitude
)
var
lon
=
normalizeLon
(
missionItem
.
coordinate
.
longitude
)
var
lon
=
normalizeLon
(
missionItem
.
coordinate
.
longitude
)
...
@@ -161,7 +159,7 @@ QGCView {
...
@@ -161,7 +159,7 @@ QGCView {
onAutoSyncChanged: QGroundControl.flightMapSettings.saveMapSetting(editorMap.mapName, _autoSyncKey, autoSync)
onAutoSyncChanged: QGroundControl.flightMapSettings.saveMapSetting(editorMap.mapName, _autoSyncKey, autoSync)
*/
*/
on
Mission
ItemsChanged
:
itemDragger
.
clearItem
()
on
Visual
ItemsChanged
:
itemDragger
.
clearItem
()
onNewItemsFromVehicle
:
fitViewportToMissionItems
()
onNewItemsFromVehicle
:
fitViewportToMissionItems
()
}
}
...
@@ -177,12 +175,12 @@ QGCView {
...
@@ -177,12 +175,12 @@ QGCView {
function
setCurrentItem
(
index
)
{
function
setCurrentItem
(
index
)
{
_currentMissionItem
=
undefined
_currentMissionItem
=
undefined
for
(
var
i
=
0
;
i
<
_
mission
Items
.
count
;
i
++
)
{
for
(
var
i
=
0
;
i
<
_
visual
Items
.
count
;
i
++
)
{
if
(
i
==
index
)
{
if
(
i
==
index
)
{
_currentMissionItem
=
_
mission
Items
.
get
(
i
)
_currentMissionItem
=
_
visual
Items
.
get
(
i
)
_currentMissionItem
.
isCurrentItem
=
true
_currentMissionItem
.
isCurrentItem
=
true
}
else
{
}
else
{
_
mission
Items
.
get
(
i
).
isCurrentItem
=
false
_
visual
Items
.
get
(
i
).
isCurrentItem
=
false
}
}
}
}
}
}
...
@@ -266,7 +264,7 @@ QGCView {
...
@@ -266,7 +264,7 @@ QGCView {
QGCComboBox
{
QGCComboBox
{
id
:
toCombo
id
:
toCombo
model
:
_
mission
Items
.
count
model
:
_
visual
Items
.
count
currentIndex
:
_moveDialogMissionItemIndex
currentIndex
:
_moveDialogMissionItemIndex
}
}
}
}
...
@@ -308,7 +306,7 @@ QGCView {
...
@@ -308,7 +306,7 @@ QGCView {
coordinate
.
longitude
=
coordinate
.
longitude
.
toFixed
(
_decimalPlaces
)
coordinate
.
longitude
=
coordinate
.
longitude
.
toFixed
(
_decimalPlaces
)
coordinate
.
altitude
=
coordinate
.
altitude
.
toFixed
(
_decimalPlaces
)
coordinate
.
altitude
=
coordinate
.
altitude
.
toFixed
(
_decimalPlaces
)
if
(
addMissionItemsButton
.
checked
)
{
if
(
addMissionItemsButton
.
checked
)
{
var
index
=
controller
.
insertSimpleMissionItem
(
coordinate
,
controller
.
mission
Items
.
count
)
var
index
=
controller
.
insertSimpleMissionItem
(
coordinate
,
controller
.
visual
Items
.
count
)
setCurrentItem
(
index
)
setCurrentItem
(
index
)
}
else
{
}
else
{
editorMap
.
mapClicked
(
coordinate
)
editorMap
.
mapClicked
(
coordinate
)
...
@@ -368,7 +366,7 @@ QGCView {
...
@@ -368,7 +366,7 @@ QGCView {
// Add the simple mission items to the map
// Add the simple mission items to the map
MapItemView
{
MapItemView
{
model
:
controller
.
mission
Items
model
:
controller
.
visual
Items
delegate
:
missionItemComponent
delegate
:
missionItemComponent
}
}
...
@@ -399,8 +397,8 @@ QGCView {
...
@@ -399,8 +397,8 @@ QGCView {
Connections
{
Connections
{
target
:
object
target
:
object
onIsCurrentItemChanged
:
updateItemIndicator
()
onIsCurrentItemChanged
:
updateItemIndicator
()
on
CommandChanged
:
updateItemIndicator
()
on
SpecifiesCoordinateChanged
:
updateItemIndicator
()
}
}
// These are the non-coordinate child mission items attached to this item
// These are the non-coordinate child mission items attached to this item
...
@@ -425,7 +423,7 @@ QGCView {
...
@@ -425,7 +423,7 @@ QGCView {
// Add the complex mission items to the map
// Add the complex mission items to the map
MapItemView
{
MapItemView
{
model
:
controller
.
complex
Mission
Items
model
:
controller
.
complex
Visual
Items
delegate
:
polygonItemComponent
delegate
:
polygonItemComponent
}
}
...
@@ -480,10 +478,10 @@ QGCView {
...
@@ -480,10 +478,10 @@ QGCView {
anchors.left
:
parent
.
left
anchors.left
:
parent
.
left
anchors.right
:
parent
.
right
anchors.right
:
parent
.
right
anchors.top
:
parent
.
top
anchors.top
:
parent
.
top
height
:
Math
.
min
(
contentHeight
,
parent
.
height
)
height
:
parent
.
height
spacing
:
_margin
/
2
spacing
:
_margin
/
2
orientation
:
ListView
.
Vertical
orientation
:
ListView
.
Vertical
model
:
controller
.
mission
Items
model
:
controller
.
visual
Items
cacheBuffer
:
height
*
2
cacheBuffer
:
height
*
2
delegate
:
MissionItemEditor
{
delegate
:
MissionItemEditor
{
...
@@ -504,7 +502,7 @@ QGCView {
...
@@ -504,7 +502,7 @@ QGCView {
setCurrentItem
(
i
)
setCurrentItem
(
i
)
}
}
onMoveHomeToMapCenter
:
controller
.
mission
Items
.
get
(
0
).
coordinate
=
editorMap
.
center
onMoveHomeToMapCenter
:
controller
.
visual
Items
.
get
(
0
).
coordinate
=
editorMap
.
center
}
}
}
// ListView
}
// ListView
}
// Item - Mission Item editor
}
// Item - Mission Item editor
...
@@ -551,7 +549,7 @@ QGCView {
...
@@ -551,7 +549,7 @@ QGCView {
coordinate
.
latitude
=
coordinate
.
latitude
.
toFixed
(
_decimalPlaces
)
coordinate
.
latitude
=
coordinate
.
latitude
.
toFixed
(
_decimalPlaces
)
coordinate
.
longitude
=
coordinate
.
longitude
.
toFixed
(
_decimalPlaces
)
coordinate
.
longitude
=
coordinate
.
longitude
.
toFixed
(
_decimalPlaces
)
coordinate
.
altitude
=
coordinate
.
altitude
.
toFixed
(
_decimalPlaces
)
coordinate
.
altitude
=
coordinate
.
altitude
.
toFixed
(
_decimalPlaces
)
var
index
=
controller
.
insertComplexMissionItem
(
coordinate
,
controller
.
mission
Items
.
count
)
var
index
=
controller
.
insertComplexMissionItem
(
coordinate
,
controller
.
visual
Items
.
count
)
setCurrentItem
(
index
)
setCurrentItem
(
index
)
checked
=
false
checked
=
false
}
}
...
@@ -565,8 +563,8 @@ QGCView {
...
@@ -565,8 +563,8 @@ QGCView {
exclusiveGroup
:
_dropButtonsExclusiveGroup
exclusiveGroup
:
_dropButtonsExclusiveGroup
z
:
QGroundControl
.
zOrderWidgets
z
:
QGroundControl
.
zOrderWidgets
dropDownComponent
:
syncDropDownComponent
dropDownComponent
:
syncDropDownComponent
enabled
:
!
_
syncInProgress
enabled
:
!
controller
.
syncInProgress
rotateImage
:
_
syncInProgress
rotateImage
:
controller
.
syncInProgress
}
}
DropButton
{
DropButton
{
...
@@ -589,7 +587,7 @@ QGCView {
...
@@ -589,7 +587,7 @@ QGCView {
onClicked
:
{
onClicked
:
{
centerMapButton
.
hideDropDown
()
centerMapButton
.
hideDropDown
()
editorMap
.
center
=
controller
.
mission
Items
.
get
(
0
).
coordinate
editorMap
.
center
=
controller
.
visual
Items
.
get
(
0
).
coordinate
}
}
}
}
...
@@ -688,7 +686,7 @@ QGCView {
...
@@ -688,7 +686,7 @@ QGCView {
anchors.bottom
:
parent
.
bottom
anchors.bottom
:
parent
.
bottom
z
:
QGroundControl
.
zOrderTopMost
z
:
QGroundControl
.
zOrderTopMost
currentMissionItem
:
_currentMissionItem
currentMissionItem
:
_currentMissionItem
missionItems
:
controller
.
mission
Items
missionItems
:
controller
.
visual
Items
expandedWidth
:
missionItemEditor
.
x
-
(
ScreenTools
.
defaultFontPixelWidth
*
2
)
expandedWidth
:
missionItemEditor
.
x
-
(
ScreenTools
.
defaultFontPixelWidth
*
2
)
}
}
}
// FlightMap
}
// FlightMap
...
@@ -758,7 +756,7 @@ QGCView {
...
@@ -758,7 +756,7 @@ QGCView {
QGCButton
{
QGCButton
{
text
:
"
Send to vehicle
"
text
:
"
Send to vehicle
"
enabled
:
_activeVehicle
&&
!
_activeVehicle
.
missionManager
.
i
nProgress
enabled
:
!
controller
.
syncI
nProgress
onClicked
:
{
onClicked
:
{
syncButton
.
hideDropDown
()
syncButton
.
hideDropDown
()
...
@@ -768,7 +766,7 @@ QGCView {
...
@@ -768,7 +766,7 @@ QGCView {
QGCButton
{
QGCButton
{
text
:
"
Load from vehicle
"
text
:
"
Load from vehicle
"
enabled
:
_activeVehicle
&&
!
_activeVehicle
.
missionManager
.
i
nProgress
enabled
:
!
controller
.
syncI
nProgress
onClicked
:
{
onClicked
:
{
syncButton
.
hideDropDown
()
syncButton
.
hideDropDown
()
...
@@ -786,6 +784,7 @@ QGCView {
...
@@ -786,6 +784,7 @@ QGCView {
QGCButton
{
QGCButton
{
text
:
"
Save to file...
"
text
:
"
Save to file...
"
enabled
:
!
controller
.
syncInProgress
onClicked
:
{
onClicked
:
{
syncButton
.
hideDropDown
()
syncButton
.
hideDropDown
()
...
@@ -795,6 +794,7 @@ QGCView {
...
@@ -795,6 +794,7 @@ QGCView {
QGCButton
{
QGCButton
{
text
:
"
Load from file...
"
text
:
"
Load from file...
"
enabled
:
!
controller
.
syncInProgress
onClicked
:
{
onClicked
:
{
syncButton
.
hideDropDown
()
syncButton
.
hideDropDown
()
...
...
src/MissionEditor/MissionItemStatus.qml
View file @
36d57743
...
@@ -114,7 +114,7 @@ Rectangle {
...
@@ -114,7 +114,7 @@ Rectangle {
Item
{
Item
{
height
:
graphRow
.
height
height
:
graphRow
.
height
width
:
ScreenTools
.
smallFontPixelWidth
*
2
width
:
ScreenTools
.
smallFontPixelWidth
*
2
visible
:
object
.
specifiesCoordinate
&&
!
object
.
s
tandaloneCoordinate
visible
:
object
.
specifiesCoordinate
&&
!
object
.
isS
tandaloneCoordinate
property
real
availableHeight
:
height
-
ScreenTools
.
smallFontPixelHeight
-
indicator
.
height
property
real
availableHeight
:
height
-
ScreenTools
.
smallFontPixelHeight
-
indicator
.
height
...
...
src/MissionManager/ComplexMissionItem.cc
View file @
36d57743
...
@@ -23,43 +23,19 @@ This file is part of the QGROUNDCONTROL project
...
@@ -23,43 +23,19 @@ This file is part of the QGROUNDCONTROL project
#include "ComplexMissionItem.h"
#include "ComplexMissionItem.h"
ComplexMissionItem
::
ComplexMissionItem
(
Vehicle
*
vehicle
,
QObject
*
parent
)
ComplexMissionItem
::
ComplexMissionItem
(
Vehicle
*
vehicle
,
QObject
*
parent
)
:
MissionItem
(
vehicle
,
parent
)
:
VisualMissionItem
(
vehicle
,
parent
)
{
,
_dirty
(
false
)
}
ComplexMissionItem
::
ComplexMissionItem
(
Vehicle
*
vehicle
,
int
sequenceNumber
,
MAV_CMD
command
,
MAV_FRAME
frame
,
double
param1
,
double
param2
,
double
param3
,
double
param4
,
double
param5
,
double
param6
,
double
param7
,
bool
autoContinue
,
bool
isCurrentItem
,
QObject
*
parent
)
:
MissionItem
(
vehicle
,
sequenceNumber
,
command
,
frame
,
param1
,
param2
,
param3
,
param4
,
param5
,
param6
,
param7
,
autoContinue
,
isCurrentItem
,
parent
)
{
{
}
}
ComplexMissionItem
::
ComplexMissionItem
(
const
ComplexMissionItem
&
other
,
QObject
*
parent
)
ComplexMissionItem
::
ComplexMissionItem
(
const
ComplexMissionItem
&
other
,
QObject
*
parent
)
:
MissionItem
(
other
,
parent
)
:
VisualMissionItem
(
other
,
parent
)
,
_dirty
(
false
)
{
{
}
}
const
ComplexMissionItem
&
ComplexMissionItem
::
operator
=
(
const
ComplexMissionItem
&
other
)
{
static_cast
<
MissionItem
&>
(
*
this
)
=
other
;
return
*
this
;
}
QVariantList
ComplexMissionItem
::
polygonPath
(
void
)
QVariantList
ComplexMissionItem
::
polygonPath
(
void
)
{
{
return
_polygonPath
;
return
_polygonPath
;
...
@@ -85,3 +61,33 @@ void ComplexMissionItem::addPolygonCoordinate(const QGeoCoordinate coordinate)
...
@@ -85,3 +61,33 @@ void ComplexMissionItem::addPolygonCoordinate(const QGeoCoordinate coordinate)
_polygonPath
<<
QVariant
::
fromValue
(
coordinate
);
_polygonPath
<<
QVariant
::
fromValue
(
coordinate
);
emit
polygonPathChanged
();
emit
polygonPathChanged
();
}
}
int
ComplexMissionItem
::
nextSequenceNumber
(
void
)
const
{
return
_sequenceNumber
+
_missionItems
.
count
();
}
void
ComplexMissionItem
::
setCoordinate
(
const
QGeoCoordinate
&
coordinate
)
{
if
(
_coordinate
!=
coordinate
)
{
_coordinate
=
coordinate
;
emit
coordinateChanged
(
_coordinate
);
}
}
void
ComplexMissionItem
::
setDirty
(
bool
dirty
)
{
if
(
_dirty
!=
dirty
)
{
_dirty
=
dirty
;
emit
dirtyChanged
(
_dirty
);
}
}
bool
ComplexMissionItem
::
save
(
QJsonObject
&
missionObject
,
QJsonArray
&
missionItems
,
QString
&
errorString
)
{
Q_UNUSED
(
missionObject
);
Q_UNUSED
(
missionItems
);
errorString
=
"Complex save NYI"
;
return
false
;
}
src/MissionManager/ComplexMissionItem.h
View file @
36d57743
...
@@ -24,34 +24,17 @@
...
@@ -24,34 +24,17 @@
#ifndef ComplexMissionItem_H
#ifndef ComplexMissionItem_H
#define ComplexMissionItem_H
#define ComplexMissionItem_H
#include "VisualMissionItem.h"
#include "MissionItem.h"
#include "MissionItem.h"
class
ComplexMissionItem
:
public
MissionItem
class
ComplexMissionItem
:
public
Visual
MissionItem
{
{
Q_OBJECT
Q_OBJECT
public:
public:
ComplexMissionItem
(
Vehicle
*
vehicle
,
QObject
*
parent
=
NULL
);
ComplexMissionItem
(
Vehicle
*
vehicle
,
QObject
*
parent
=
NULL
);
ComplexMissionItem
(
Vehicle
*
vehicle
,
int
sequenceNumber
,
MAV_CMD
command
,
MAV_FRAME
frame
,
double
param1
,
double
param2
,
double
param3
,
double
param4
,
double
param5
,
double
param6
,
double
param7
,
bool
autoContinue
,
bool
isCurrentItem
,
QObject
*
parent
=
NULL
);
ComplexMissionItem
(
const
ComplexMissionItem
&
other
,
QObject
*
parent
=
NULL
);
ComplexMissionItem
(
const
ComplexMissionItem
&
other
,
QObject
*
parent
=
NULL
);
const
ComplexMissionItem
&
operator
=
(
const
ComplexMissionItem
&
other
);
Q_PROPERTY
(
QVariantList
polygonPath
READ
polygonPath
NOTIFY
polygonPathChanged
)
Q_PROPERTY
(
QVariantList
polygonPath
READ
polygonPath
NOTIFY
polygonPathChanged
)
Q_INVOKABLE
void
clearPolygon
(
void
);
Q_INVOKABLE
void
clearPolygon
(
void
);
...
@@ -59,15 +42,38 @@ public:
...
@@ -59,15 +42,38 @@ public:
QVariantList
polygonPath
(
void
);
QVariantList
polygonPath
(
void
);
// Overrides from MissionItem base class
const
QList
<
MissionItem
*>&
missionItems
(
void
)
{
return
_missionItems
;
}
bool
simpleItem
(
void
)
const
final
{
return
false
;
}
QGeoCoordinate
exitCoordinate
(
void
)
const
final
{
return
coordinate
();
}
/// @return The next sequence number to use after this item. Takes into account child items of the complex item
int
nextSequenceNumber
(
void
)
const
;
// Overrides from VisualMissionItem
bool
dirty
(
void
)
const
final
{
return
_dirty
;
}
bool
isSimpleItem
(
void
)
const
final
{
return
false
;
}
bool
isStandaloneCoordinate
(
void
)
const
final
{
return
false
;
}
bool
specifiesCoordinate
(
void
)
const
final
{
return
true
;
}
QString
commandDescription
(
void
)
const
final
{
return
"Survey"
;
}
QString
commandName
(
void
)
const
final
{
return
"Survey"
;
}
QGeoCoordinate
coordinate
(
void
)
const
final
{
return
_coordinate
;
}
QGeoCoordinate
exitCoordinate
(
void
)
const
final
{
return
_coordinate
;
}
bool
coordinateHasRelativeAltitude
(
void
)
const
final
{
return
true
;
}
bool
exitCoordinateHasRelativeAltitude
(
void
)
const
final
{
return
true
;
}
bool
exitCoordinateSameAsEntry
(
void
)
const
final
{
return
false
;
}
void
setDirty
(
bool
dirty
)
final
;
void
setCoordinate
(
const
QGeoCoordinate
&
coordinate
);
bool
save
(
QJsonObject
&
missionObject
,
QJsonArray
&
missionItems
,
QString
&
errorString
)
final
;
signals:
signals:
void
polygonPathChanged
(
void
);
void
polygonPathChanged
(
void
);
private:
private:
QVariantList
_polygonPath
;
bool
_dirty
;
QVariantList
_polygonPath
;
QList
<
MissionItem
*>
_missionItems
;
QGeoCoordinate
_coordinate
;
};
};
#endif
#endif
src/MissionManager/MavCmdInfoCommon.json
View file @
36d57743
...
@@ -138,8 +138,8 @@
...
@@ -138,8 +138,8 @@
"category"
:
"Basic"
,
"category"
:
"Basic"
,
"param1"
:
{
"param1"
:
{
"label"
:
"Pitch:"
,
"label"
:
"Pitch:"
,
"units"
:
"
radian
s"
,
"units"
:
"
degree
s"
,
"default"
:
0.26179939
,
"default"
:
15
,
"decimalPlaces"
:
2
"decimalPlaces"
:
2
},
},
"param4"
:
{
"param4"
:
{
...
...
src/MissionManager/MissionCommandList.h
View file @
36d57743
...
@@ -95,7 +95,7 @@ public:
...
@@ -95,7 +95,7 @@ public:
Q_PROPERTY
(
bool
friendlyEdit
READ
friendlyEdit
CONSTANT
)
Q_PROPERTY
(
bool
friendlyEdit
READ
friendlyEdit
CONSTANT
)
Q_PROPERTY
(
QString
friendlyName
READ
friendlyName
CONSTANT
)
Q_PROPERTY
(
QString
friendlyName
READ
friendlyName
CONSTANT
)
Q_PROPERTY
(
QString
rawName
READ
rawName
CONSTANT
)
Q_PROPERTY
(
QString
rawName
READ
rawName
CONSTANT
)
Q_PROPERTY
(
bool
standaloneCoordinate
READ
s
tandaloneCoordinate
CONSTANT
)
Q_PROPERTY
(
bool
isStandaloneCoordinate
READ
isS
tandaloneCoordinate
CONSTANT
)
Q_PROPERTY
(
bool
specifiesCoordinate
READ
specifiesCoordinate
CONSTANT
)
Q_PROPERTY
(
bool
specifiesCoordinate
READ
specifiesCoordinate
CONSTANT
)
MavlinkQmlSingleton
::
Qml_MAV_CMD
qmlCommand
(
void
)
const
{
return
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
)
_command
;
}
MavlinkQmlSingleton
::
Qml_MAV_CMD
qmlCommand
(
void
)
const
{
return
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
)
_command
;
}
...
@@ -106,7 +106,7 @@ public:
...
@@ -106,7 +106,7 @@ public:
bool
friendlyEdit
(
void
)
const
{
return
_friendlyEdit
;
}
bool
friendlyEdit
(
void
)
const
{
return
_friendlyEdit
;
}
QString
friendlyName
(
void
)
const
{
return
_friendlyName
;
}
QString
friendlyName
(
void
)
const
{
return
_friendlyName
;
}
QString
rawName
(
void
)
const
{
return
_rawName
;
}
QString
rawName
(
void
)
const
{
return
_rawName
;
}
bool
s
tandaloneCoordinate
(
void
)
const
{
return
_standaloneCoordinate
;
}
bool
isS
tandaloneCoordinate
(
void
)
const
{
return
_standaloneCoordinate
;
}
bool
specifiesCoordinate
(
void
)
const
{
return
_specifiesCoordinate
;
}
bool
specifiesCoordinate
(
void
)
const
{
return
_specifiesCoordinate
;
}
const
QMap
<
int
,
MavCmdParamInfo
*>&
paramInfoMap
(
void
)
const
{
return
_paramInfoMap
;
}
const
QMap
<
int
,
MavCmdParamInfo
*>&
paramInfoMap
(
void
)
const
{
return
_paramInfoMap
;
}
...
...
src/MissionManager/MissionController.cc
View file @
36d57743
...
@@ -46,8 +46,8 @@ const char* MissionController::_jsonPlannedHomePositionKey = "plannedHomePosi
...
@@ -46,8 +46,8 @@ const char* MissionController::_jsonPlannedHomePositionKey = "plannedHomePosi
MissionController
::
MissionController
(
QObject
*
parent
)
MissionController
::
MissionController
(
QObject
*
parent
)
:
QObject
(
parent
)
:
QObject
(
parent
)
,
_editMode
(
false
)
,
_editMode
(
false
)
,
_
mission
Items
(
NULL
)
,
_
visual
Items
(
NULL
)
,
_complex
Mission
Items
(
NULL
)
,
_complexItems
(
NULL
)
,
_activeVehicle
(
NULL
)
,
_activeVehicle
(
NULL
)
,
_autoSync
(
false
)
,
_autoSync
(
false
)
,
_firstItemsFromVehicle
(
false
)
,
_firstItemsFromVehicle
(
false
)
...
@@ -74,9 +74,9 @@ void MissionController::start(bool editMode)
...
@@ -74,9 +74,9 @@ void MissionController::start(bool editMode)
_activeVehicleChanged
(
multiVehicleMgr
->
activeVehicle
());
_activeVehicleChanged
(
multiVehicleMgr
->
activeVehicle
());
// We start with an empty mission
// We start with an empty mission
_
mission
Items
=
new
QmlObjectListModel
(
this
);
_
visual
Items
=
new
QmlObjectListModel
(
this
);
_addPlannedHomePosition
(
_
mission
Items
,
false
/* addToCenter */
);
_addPlannedHomePosition
(
_
visual
Items
,
false
/* addToCenter */
);
_initAll
Mission
Items
();
_initAll
Visual
Items
();
}
}
// Called when new mission items have completed downloading from Vehicle
// Called when new mission items have completed downloading from Vehicle
...
@@ -84,25 +84,33 @@ void MissionController::_newMissionItemsAvailableFromVehicle(void)
...
@@ -84,25 +84,33 @@ void MissionController::_newMissionItemsAvailableFromVehicle(void)
{
{
qCDebug
(
MissionControllerLog
)
<<
"_newMissionItemsAvailableFromVehicle"
;
qCDebug
(
MissionControllerLog
)
<<
"_newMissionItemsAvailableFromVehicle"
;
if
(
!
_editMode
||
_missionItemsRequested
||
_
mission
Items
->
count
()
==
1
)
{
if
(
!
_editMode
||
_missionItemsRequested
||
_
visual
Items
->
count
()
==
1
)
{
// Fly Mode:
// Fly Mode:
// - Always accepts new items fromthe vehicle so Fly view is kept up to date
// - Always accepts new items fromthe vehicle so Fly view is kept up to date
// Edit Mode:
// Edit Mode:
// - Either a load from vehicle was manually requested or
// - Either a load from vehicle was manually requested or
// - The initial automatic load from a vehicle completed and the current editor it empty
// - The initial automatic load from a vehicle completed and the current editor it empty
_deinitAllMissionItems
();
_missionItems
->
deleteLater
();
_missionItems
=
_activeVehicle
->
missionManager
()
->
copyMissionItems
(
);
QmlObjectListModel
*
newControllerMissionItems
=
new
QmlObjectListModel
(
this
);
qCDebug
(
MissionControllerLog
)
<<
"loading from vehicle count"
<<
_missionItems
->
count
();
const
QList
<
MissionItem
*>&
newMissionItems
=
_activeVehicle
->
missionManager
()
->
missionItems
();
if
(
!
_activeVehicle
->
firmwarePlugin
()
->
sendHomePositionToVehicle
()
||
_missionItems
->
count
()
==
0
)
{
qCDebug
(
MissionControllerLog
)
<<
"loading from vehicle: count"
<<
_visualItems
->
count
();
_addPlannedHomePosition
(
_missionItems
,
true
/* addToCenter */
);
foreach
(
const
MissionItem
*
missionItem
,
newMissionItems
)
{
newControllerMissionItems
->
append
(
new
SimpleMissionItem
(
_activeVehicle
,
*
missionItem
,
this
));
}
_deinitAllVisualItems
();
_visualItems
->
deleteListAndContents
();
_visualItems
=
newControllerMissionItems
;
if
(
!
_activeVehicle
->
firmwarePlugin
()
->
sendHomePositionToVehicle
()
||
_visualItems
->
count
()
==
0
)
{
_addPlannedHomePosition
(
_visualItems
,
true
/* addToCenter */
);
}
}
_missionItemsRequested
=
false
;
_missionItemsRequested
=
false
;
_initAll
Mission
Items
();
_initAll
Visual
Items
();
emit
newItemsFromVehicle
();
emit
newItemsFromVehicle
();
}
}
}
}
...
@@ -119,22 +127,39 @@ void MissionController::getMissionItems(void)
...
@@ -119,22 +127,39 @@ void MissionController::getMissionItems(void)
void
MissionController
::
sendMissionItems
(
void
)
void
MissionController
::
sendMissionItems
(
void
)
{
{
Vehicle
*
activeVehicle
=
qgcApp
()
->
toolbox
()
->
multiVehicleManager
()
->
activeVehicle
();
if
(
_activeVehicle
)
{
// Convert to MissionItems so we can send to vehicle
if
(
activeVehicle
)
{
QList
<
MissionItem
*>
missionItems
;
activeVehicle
->
missionManager
()
->
writeMissionItems
(
*
_missionItems
);
_missionItems
->
setDirty
(
false
);
int
sequenceNumber
=
0
;
for
(
int
i
=
0
;
i
<
_visualItems
->
count
();
i
++
)
{
VisualMissionItem
*
visualItem
=
qobject_cast
<
VisualMissionItem
*>
(
_visualItems
->
get
(
i
));
if
(
visualItem
->
isSimpleItem
())
{
MissionItem
&
missionItem
=
qobject_cast
<
SimpleMissionItem
*>
(
visualItem
)
->
missionItem
();
missionItem
.
setSequenceNumber
(
sequenceNumber
++
);
missionItems
.
append
(
&
missionItem
);
}
else
{
foreach
(
MissionItem
*
missionItem
,
qobject_cast
<
ComplexMissionItem
*>
(
visualItem
)
->
missionItems
())
{
missionItem
->
setSequenceNumber
(
sequenceNumber
++
);
missionItems
.
append
(
missionItem
);
}
}
}
_activeVehicle
->
missionManager
()
->
writeMissionItems
(
missionItems
);
_visualItems
->
setDirty
(
false
);
}
}
}
}
int
MissionController
::
insertSimpleMissionItem
(
QGeoCoordinate
coordinate
,
int
i
)
int
MissionController
::
insertSimpleMissionItem
(
QGeoCoordinate
coordinate
,
int
i
)
{
{
MissionItem
*
newItem
=
new
SimpleMissionItem
(
_activeVehicle
,
this
);
Simple
MissionItem
*
newItem
=
new
SimpleMissionItem
(
_activeVehicle
,
this
);
newItem
->
setSequenceNumber
(
_
mission
Items
->
count
());
newItem
->
setSequenceNumber
(
_
visual
Items
->
count
());
newItem
->
setCoordinate
(
coordinate
);
newItem
->
setCoordinate
(
coordinate
);
newItem
->
setCommand
(
MAV_CMD_NAV_WAYPOINT
);
newItem
->
setCommand
(
M
avlinkQmlSingleton
::
M
AV_CMD_NAV_WAYPOINT
);
_init
Mission
Item
(
newItem
);
_init
Visual
Item
(
newItem
);
if
(
_
mission
Items
->
count
()
==
1
)
{
if
(
_
visual
Items
->
count
()
==
1
)
{
newItem
->
setCommand
(
MavlinkQmlSingleton
::
MAV_CMD_NAV_TAKEOFF
);
newItem
->
setCommand
(
MavlinkQmlSingleton
::
MAV_CMD_NAV_TAKEOFF
);
}
}
newItem
->
setDefaultsForCommand
();
newItem
->
setDefaultsForCommand
();
...
@@ -142,64 +167,71 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
...
@@ -142,64 +167,71 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
double
lastValue
;
double
lastValue
;
if
(
_findLastAcceptanceRadius
(
&
lastValue
))
{
if
(
_findLastAcceptanceRadius
(
&
lastValue
))
{
newItem
->
setParam2
(
lastValue
);
newItem
->
missionItem
().
setParam2
(
lastValue
);
}
}
if
(
_findLastAltitude
(
&
lastValue
))
{
if
(
_findLastAltitude
(
&
lastValue
))
{
newItem
->
setParam7
(
lastValue
);
newItem
->
missionItem
().
setParam7
(
lastValue
);
}
}
}
}
_
mission
Items
->
insert
(
i
,
newItem
);
_
visual
Items
->
insert
(
i
,
newItem
);
_recalcAll
();
_recalcAll
();
return
_
mission
Items
->
count
()
-
1
;
return
_
visual
Items
->
count
()
-
1
;
}
}
int
MissionController
::
insertComplexMissionItem
(
QGeoCoordinate
coordinate
,
int
i
)
int
MissionController
::
insertComplexMissionItem
(
QGeoCoordinate
coordinate
,
int
i
)
{
{
ComplexMissionItem
*
newItem
=
new
ComplexMissionItem
(
_activeVehicle
,
this
);
ComplexMissionItem
*
newItem
=
new
ComplexMissionItem
(
_activeVehicle
,
this
);
newItem
->
setSequenceNumber
(
_
mission
Items
->
count
());
newItem
->
setSequenceNumber
(
_
visual
Items
->
count
());
newItem
->
setCoordinate
(
coordinate
);
newItem
->
setCoordinate
(
coordinate
);
newItem
->
setCommand
(
MAV_CMD_NAV_WAYPOINT
);
_initVisualItem
(
newItem
);
_initMissionItem
(
newItem
);
_
mission
Items
->
insert
(
i
,
newItem
);
_
visual
Items
->
insert
(
i
,
newItem
);
_complex
Mission
Items
->
append
(
newItem
);
_complexItems
->
append
(
newItem
);
_recalcAll
();
_recalcAll
();
return
_
mission
Items
->
count
()
-
1
;
return
_
visual
Items
->
count
()
-
1
;
}
}
void
MissionController
::
removeMissionItem
(
int
index
)
void
MissionController
::
removeMissionItem
(
int
index
)
{
{
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
_mission
Items
->
removeAt
(
index
));
VisualMissionItem
*
item
=
qobject_cast
<
VisualMissionItem
*>
(
_visual
Items
->
removeAt
(
index
));
_deinitMissionItem
(
item
);
_deinitVisualItem
(
item
);
if
(
!
item
->
isSimpleItem
())
{
ComplexMissionItem
*
complexItem
=
qobject_cast
<
ComplexMissionItem
*>
(
_complexItems
->
removeOne
(
item
));
if
(
complexItem
)
{
complexItem
->
deleteLater
();
}
else
{
qWarning
()
<<
"Complex item missing"
;
}
}
item
->
deleteLater
();
item
->
deleteLater
();
_recalcAll
();
_recalcAll
();
// Set the new current item
// Set the new current item
if
(
index
>=
_
mission
Items
->
count
())
{
if
(
index
>=
_
visual
Items
->
count
())
{
index
--
;
index
--
;
}
}
for
(
int
i
=
0
;
i
<
_
mission
Items
->
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_
visual
Items
->
count
();
i
++
)
{
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
_
mission
Items
->
get
(
i
));
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
_
visual
Items
->
get
(
i
));
item
->
setIsCurrentItem
(
i
==
index
);
item
->
setIsCurrentItem
(
i
==
index
);
}
}
_
mission
Items
->
setDirty
(
true
);
_
visual
Items
->
setDirty
(
true
);
}
}
void
MissionController
::
removeAllMissionItems
(
void
)
void
MissionController
::
removeAllMissionItems
(
void
)
{
{
if
(
_
mission
Items
)
{
if
(
_
visual
Items
)
{
QmlObjectListModel
*
oldItems
=
_missionItems
;
_deinitAllVisualItems
()
;
_
missionItems
=
new
QmlObjectListModel
(
this
);
_
visualItems
->
deleteListAndContents
(
);
_
addPlannedHomePosition
(
_missionItems
,
false
/* addToCenter */
);
_
visualItems
=
new
QmlObjectListModel
(
this
);
_
initAllMissionItems
(
);
_
addPlannedHomePosition
(
_visualItems
,
false
/* addToCenter */
);
oldItems
->
deleteLater
();
_initAllVisualItems
();
}
}
}
}
...
@@ -237,7 +269,7 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL
...
@@ -237,7 +269,7 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL
return
false
;
return
false
;
}
}
MissionItem
*
item
=
new
SimpleMissionItem
(
_activeVehicle
,
this
);
Simple
MissionItem
*
item
=
new
SimpleMissionItem
(
_activeVehicle
,
this
);
if
(
item
->
load
(
itemValue
.
toObject
(),
errorString
))
{
if
(
item
->
load
(
itemValue
.
toObject
(),
errorString
))
{
missionItems
->
append
(
item
);
missionItems
->
append
(
item
);
}
else
{
}
else
{
...
@@ -247,7 +279,7 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL
...
@@ -247,7 +279,7 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL
}
}
if
(
json
.
contains
(
_jsonPlannedHomePositionKey
))
{
if
(
json
.
contains
(
_jsonPlannedHomePositionKey
))
{
MissionItem
*
item
=
new
SimpleMissionItem
(
_activeVehicle
,
this
);
Simple
MissionItem
*
item
=
new
SimpleMissionItem
(
_activeVehicle
,
this
);
if
(
item
->
load
(
json
[
_jsonPlannedHomePositionKey
].
toObject
(),
errorString
))
{
if
(
item
->
load
(
json
[
_jsonPlannedHomePositionKey
].
toObject
(),
errorString
))
{
missionItems
->
insert
(
0
,
item
);
missionItems
->
insert
(
0
,
item
);
...
@@ -261,7 +293,7 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL
...
@@ -261,7 +293,7 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL
return
true
;
return
true
;
}
}
bool
MissionController
::
_loadTextMissionFile
(
QTextStream
&
stream
,
QmlObjectListModel
*
mission
Items
,
QString
&
errorString
)
bool
MissionController
::
_loadTextMissionFile
(
QTextStream
&
stream
,
QmlObjectListModel
*
visual
Items
,
QString
&
errorString
)
{
{
bool
addPlannedHomePosition
=
false
;
bool
addPlannedHomePosition
=
false
;
...
@@ -282,10 +314,10 @@ bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListM
...
@@ -282,10 +314,10 @@ bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListM
if
(
versionOk
)
{
if
(
versionOk
)
{
while
(
!
stream
.
atEnd
())
{
while
(
!
stream
.
atEnd
())
{
MissionItem
*
item
=
new
SimpleMissionItem
(
_activeVehicle
,
this
);
Simple
MissionItem
*
item
=
new
SimpleMissionItem
(
_activeVehicle
,
this
);
if
(
item
->
load
(
stream
))
{
if
(
item
->
load
(
stream
))
{
mission
Items
->
append
(
item
);
visual
Items
->
append
(
item
);
}
else
{
}
else
{
errorString
=
QStringLiteral
(
"The mission file is corrupted."
);
errorString
=
QStringLiteral
(
"The mission file is corrupted."
);
return
false
;
return
false
;
...
@@ -296,15 +328,14 @@ bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListM
...
@@ -296,15 +328,14 @@ bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListM
return
false
;
return
false
;
}
}
if
(
addPlannedHomePosition
||
mission
Items
->
count
()
==
0
)
{
if
(
addPlannedHomePosition
||
visual
Items
->
count
()
==
0
)
{
_addPlannedHomePosition
(
mission
Items
,
true
/* addToCenter */
);
_addPlannedHomePosition
(
visual
Items
,
true
/* addToCenter */
);
// Update sequence numbers in DO_JUMP commands to take into account added home position
// Update sequence numbers in DO_JUMP commands to take into account added home position in index 0
for
(
int
i
=
1
;
i
<
missionItems
->
count
();
i
++
)
{
for
(
int
i
=
1
;
i
<
visualItems
->
count
();
i
++
)
{
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
missionItems
->
get
(
i
));
SimpleMissionItem
*
item
=
qobject_cast
<
SimpleMissionItem
*>
(
visualItems
->
get
(
i
));
if
(
item
->
command
()
==
MavlinkQmlSingleton
::
MAV_CMD_DO_JUMP
)
{
if
(
item
&&
item
->
command
()
==
MavlinkQmlSingleton
::
MAV_CMD_DO_JUMP
)
{
// Home is in position 0
item
->
missionItem
().
setParam1
((
int
)
item
->
missionItem
().
param1
()
+
1
);
item
->
setParam1
((
int
)
item
->
param1
()
+
1
);
}
}
}
}
}
}
...
@@ -345,16 +376,16 @@ void MissionController::_loadMissionFromFile(const QString& filename)
...
@@ -345,16 +376,16 @@ void MissionController::_loadMissionFromFile(const QString& filename)
return
;
return
;
}
}
if
(
_
mission
Items
)
{
if
(
_
visual
Items
)
{
_deinitAll
Mission
Items
();
_deinitAll
Visual
Items
();
_
missionItems
->
deleteLater
();
_
visualItems
->
deleteListAndContents
();
}
}
_
mission
Items
=
newMissionItems
;
_
visual
Items
=
newMissionItems
;
if
(
_
mission
Items
->
count
()
==
0
)
{
if
(
_
visual
Items
->
count
()
==
0
)
{
_addPlannedHomePosition
(
_
mission
Items
,
true
/* addToCenter */
);
_addPlannedHomePosition
(
_
visual
Items
,
true
/* addToCenter */
);
}
}
_initAll
Mission
Items
();
_initAll
Visual
Items
();
}
}
void
MissionController
::
loadMissionFromFile
(
void
)
void
MissionController
::
loadMissionFromFile
(
void
)
...
@@ -387,7 +418,9 @@ void MissionController::_saveMissionToFile(const QString& filename)
...
@@ -387,7 +418,9 @@ void MissionController::_saveMissionToFile(const QString& filename)
if
(
!
file
.
open
(
QIODevice
::
WriteOnly
|
QIODevice
::
Text
))
{
if
(
!
file
.
open
(
QIODevice
::
WriteOnly
|
QIODevice
::
Text
))
{
qgcApp
()
->
showMessage
(
file
.
errorString
());
qgcApp
()
->
showMessage
(
file
.
errorString
());
}
else
{
}
else
{
QJsonObject
missionObject
;
QJsonObject
missionObject
;
// top level json object
QJsonArray
missionItemArray
;
// array of mission items
QString
errorString
;
missionObject
[
_jsonVersionKey
]
=
"1.0"
;
missionObject
[
_jsonVersionKey
]
=
"1.0"
;
missionObject
[
_jsonGroundStationKey
]
=
"QGroundControl"
;
missionObject
[
_jsonGroundStationKey
]
=
"QGroundControl"
;
...
@@ -404,23 +437,32 @@ void MissionController::_saveMissionToFile(const QString& filename)
...
@@ -404,23 +437,32 @@ void MissionController::_saveMissionToFile(const QString& filename)
}
}
missionObject
[
_jsonMavAutopilotKey
]
=
firmwareType
;
missionObject
[
_jsonMavAutopilotKey
]
=
firmwareType
;
// Save planned home position
QJsonObject
homePositionObject
;
QJsonObject
homePositionObject
;
qobject_cast
<
MissionItem
*>
(
_missionItems
->
get
(
0
))
->
save
(
homePositionObject
);
SimpleMissionItem
*
homeItem
=
qobject_cast
<
SimpleMissionItem
*>
(
_visualItems
->
get
(
0
));
missionObject
[
"plannedHomePosition"
]
=
homePositionObject
;
if
(
homeItem
)
{
homeItem
->
missionItem
().
save
(
homePositionObject
);
QJsonArray
itemArray
;
}
else
{
for
(
int
i
=
1
;
i
<
_missionItems
->
count
();
i
++
)
{
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Internal error: VisualMissionItem at index 0 not SimpleMissionItem"
));
QJsonObject
itemObject
;
return
;
qobject_cast
<
MissionItem
*>
(
_missionItems
->
get
(
i
))
->
save
(
itemObject
);
}
itemArray
.
append
(
itemObject
);
missionObject
[
_jsonPlannedHomePositionKey
]
=
homePositionObject
;
// Save the remainder of the visual items
for
(
int
i
=
1
;
i
<
_visualItems
->
count
();
i
++
)
{
VisualMissionItem
*
visualItem
=
qobject_cast
<
VisualMissionItem
*>
(
_visualItems
->
get
(
i
));
if
(
!
visualItem
->
save
(
missionObject
,
missionItemArray
,
errorString
))
{
qgcApp
()
->
showMessage
(
errorString
);
return
;
}
}
}
missionObject
[
"items"
]
=
i
temArray
;
missionObject
[
"items"
]
=
missionI
temArray
;
QJsonDocument
saveDoc
(
missionObject
);
QJsonDocument
saveDoc
(
missionObject
);
file
.
write
(
saveDoc
.
toJson
());
file
.
write
(
saveDoc
.
toJson
());
}
}
_
mission
Items
->
setDirty
(
false
);
_
visual
Items
->
setDirty
(
false
);
}
}
void
MissionController
::
saveMissionToFile
(
void
)
void
MissionController
::
saveMissionToFile
(
void
)
...
@@ -456,23 +498,23 @@ void MissionController::loadMobileMissionFromFile(const QString& filename)
...
@@ -456,23 +498,23 @@ void MissionController::loadMobileMissionFromFile(const QString& filename)
_loadMissionFromFile
(
docDirs
.
at
(
0
)
+
QDir
::
separator
()
+
filename
);
_loadMissionFromFile
(
docDirs
.
at
(
0
)
+
QDir
::
separator
()
+
filename
);
}
}
void
MissionController
::
_calcPrevWaypointValues
(
double
homeAlt
,
MissionItem
*
currentItem
,
MissionItem
*
prevItem
,
double
*
azimuth
,
double
*
distance
,
double
*
altDifference
)
void
MissionController
::
_calcPrevWaypointValues
(
double
homeAlt
,
VisualMissionItem
*
currentItem
,
Visual
MissionItem
*
prevItem
,
double
*
azimuth
,
double
*
distance
,
double
*
altDifference
)
{
{
QGeoCoordinate
currentCoord
=
currentItem
->
coordinate
();
QGeoCoordinate
currentCoord
=
currentItem
->
coordinate
();
QGeoCoordinate
prevCoord
=
prevItem
->
c
oordinate
();
QGeoCoordinate
prevCoord
=
prevItem
->
exitC
oordinate
();
bool
distanceOk
=
false
;
bool
distanceOk
=
false
;
// Convert to fixed altitudes
// Convert to fixed altitudes
qCDebug
(
MissionControllerLog
)
<<
homeAlt
qCDebug
(
MissionControllerLog
)
<<
homeAlt
<<
currentItem
->
r
elativeAltitude
()
<<
currentItem
->
coordinate
().
altitude
()
<<
currentItem
->
coordinateHasR
elativeAltitude
()
<<
currentItem
->
coordinate
().
altitude
()
<<
prevItem
->
relativeAltitude
()
<<
prevItem
->
c
oordinate
().
altitude
();
<<
prevItem
->
exitCoordinateHasRelativeAltitude
()
<<
prevItem
->
exitC
oordinate
().
altitude
();
distanceOk
=
true
;
distanceOk
=
true
;
if
(
currentItem
->
r
elativeAltitude
())
{
if
(
currentItem
->
coordinateHasR
elativeAltitude
())
{
currentCoord
.
setAltitude
(
homeAlt
+
currentCoord
.
altitude
());
currentCoord
.
setAltitude
(
homeAlt
+
currentCoord
.
altitude
());
}
}
if
(
prevItem
->
r
elativeAltitude
())
{
if
(
prevItem
->
exitCoordinateHasR
elativeAltitude
())
{
prevCoord
.
setAltitude
(
homeAlt
+
prevCoord
.
altitude
());
prevCoord
.
setAltitude
(
homeAlt
+
prevCoord
.
altitude
());
}
}
...
@@ -491,11 +533,17 @@ void MissionController::_calcPrevWaypointValues(double homeAlt, MissionItem* cur
...
@@ -491,11 +533,17 @@ void MissionController::_calcPrevWaypointValues(double homeAlt, MissionItem* cur
void
MissionController
::
_recalcWaypointLines
(
void
)
void
MissionController
::
_recalcWaypointLines
(
void
)
{
{
MissionItem
*
lastCoordinateItem
=
qobject_cast
<
MissionItem
*>
(
_missionItems
->
get
(
0
));
bool
firstCoordinateItem
=
true
;
MissionItem
*
homeItem
=
lastCoordinateItem
;
VisualMissionItem
*
lastCoordinateItem
=
qobject_cast
<
VisualMissionItem
*>
(
_visualItems
->
get
(
0
));
bool
firstCoordinateItem
=
true
;
bool
showHomePosition
=
homeItem
->
showHomePosition
();
SimpleMissionItem
*
homeItem
=
qobject_cast
<
SimpleMissionItem
*>
(
lastCoordinateItem
);
double
homeAlt
=
homeItem
->
coordinate
().
altitude
();
if
(
!
homeItem
)
{
qWarning
()
<<
"Home item is not SimpleMissionItem"
;
}
bool
showHomePosition
=
homeItem
->
showHomePosition
();
double
homeAlt
=
homeItem
->
coordinate
().
altitude
();
qCDebug
(
MissionControllerLog
)
<<
"_recalcWaypointLines"
;
qCDebug
(
MissionControllerLog
)
<<
"_recalcWaypointLines"
;
...
@@ -516,28 +564,42 @@ void MissionController::_recalcWaypointLines(void)
...
@@ -516,28 +564,42 @@ void MissionController::_recalcWaypointLines(void)
_waypointLines
.
clear
();
_waypointLines
.
clear
();
bool
linkBackToHome
=
false
;
bool
linkBackToHome
=
false
;
for
(
int
i
=
1
;
i
<
_
mission
Items
->
count
();
i
++
)
{
for
(
int
i
=
1
;
i
<
_
visual
Items
->
count
();
i
++
)
{
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
_mission
Items
->
get
(
i
));
VisualMissionItem
*
item
=
qobject_cast
<
VisualMissionItem
*>
(
_visual
Items
->
get
(
i
));
// Assume the worst
// Assume the worst
item
->
setAzimuth
(
0.0
);
item
->
setAzimuth
(
0.0
);
item
->
setDistance
(
-
1.0
);
item
->
setDistance
(
-
1.0
);
if
(
firstCoordinateItem
&&
item
->
command
()
==
MavlinkQmlSingleton
::
MAV_CMD_NAV_TAKEOFF
)
{
// If we still haven't found the first coordinate item and we hit a a takeoff command link back to home
if
(
firstCoordinateItem
&&
item
->
isSimpleItem
()
&&
qobject_cast
<
SimpleMissionItem
*>
(
item
)
->
command
()
==
MavlinkQmlSingleton
::
MAV_CMD_NAV_TAKEOFF
)
{
linkBackToHome
=
true
;
linkBackToHome
=
true
;
}
}
if
(
item
->
specifiesCoordinate
())
{
if
(
item
->
specifiesCoordinate
())
{
// Keep track of the min/max altitude for all waypoints so we can show altitudes as a percentage
double
absoluteAltitude
=
item
->
coordinate
().
altitude
();
double
absoluteAltitude
=
item
->
coordinate
().
altitude
();
if
(
item
->
r
elativeAltitude
())
{
if
(
item
->
coordinateHasR
elativeAltitude
())
{
absoluteAltitude
+=
homePositionAltitude
;
absoluteAltitude
+=
homePositionAltitude
;
}
}
minAltSeen
=
std
::
min
(
minAltSeen
,
absoluteAltitude
);
minAltSeen
=
std
::
min
(
minAltSeen
,
absoluteAltitude
);
maxAltSeen
=
std
::
max
(
maxAltSeen
,
absoluteAltitude
);
maxAltSeen
=
std
::
max
(
maxAltSeen
,
absoluteAltitude
);
if
(
!
item
->
standaloneCoordinate
())
{
if
(
!
item
->
exitCoordinateSameAsEntry
())
{
absoluteAltitude
=
item
->
exitCoordinate
().
altitude
();
if
(
item
->
exitCoordinateHasRelativeAltitude
())
{
absoluteAltitude
+=
homePositionAltitude
;
}
minAltSeen
=
std
::
min
(
minAltSeen
,
absoluteAltitude
);
maxAltSeen
=
std
::
max
(
maxAltSeen
,
absoluteAltitude
);
}
if
(
!
item
->
isStandaloneCoordinate
())
{
firstCoordinateItem
=
false
;
firstCoordinateItem
=
false
;
if
(
!
lastCoordinateItem
->
homePosition
()
||
(
showHomePosition
&&
linkBackToHome
))
{
if
(
lastCoordinateItem
!=
homeItem
||
(
showHomePosition
&&
linkBackToHome
))
{
double
azimuth
,
distance
,
altDifference
;
double
azimuth
,
distance
,
altDifference
;
// Subsequent coordinate items link to last coordinate item. If the last coordinate item
// Subsequent coordinate items link to last coordinate item. If the last coordinate item
...
@@ -555,12 +617,12 @@ void MissionController::_recalcWaypointLines(void)
...
@@ -555,12 +617,12 @@ void MissionController::_recalcWaypointLines(void)
// Walk the list again calculating altitude percentages
// Walk the list again calculating altitude percentages
double
altRange
=
maxAltSeen
-
minAltSeen
;
double
altRange
=
maxAltSeen
-
minAltSeen
;
for
(
int
i
=
0
;
i
<
_
mission
Items
->
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_
visual
Items
->
count
();
i
++
)
{
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
_mission
Items
->
get
(
i
));
VisualMissionItem
*
item
=
qobject_cast
<
VisualMissionItem
*>
(
_visual
Items
->
get
(
i
));
if
(
item
->
specifiesCoordinate
())
{
if
(
item
->
specifiesCoordinate
())
{
double
absoluteAltitude
=
item
->
coordinate
().
altitude
();
double
absoluteAltitude
=
item
->
coordinate
().
altitude
();
if
(
item
->
r
elativeAltitude
())
{
if
(
item
->
coordinateHasR
elativeAltitude
())
{
absoluteAltitude
+=
homePositionAltitude
;
absoluteAltitude
+=
homePositionAltitude
;
}
}
if
(
altRange
==
0.0
)
{
if
(
altRange
==
0.0
)
{
...
@@ -577,23 +639,34 @@ void MissionController::_recalcWaypointLines(void)
...
@@ -577,23 +639,34 @@ void MissionController::_recalcWaypointLines(void)
// This will update the sequence numbers to be sequential starting from 0
// This will update the sequence numbers to be sequential starting from 0
void
MissionController
::
_recalcSequence
(
void
)
void
MissionController
::
_recalcSequence
(
void
)
{
{
for
(
int
i
=
0
;
i
<
_missionItems
->
count
();
i
++
)
{
// Setup ascending sequence numbers for all visual items
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
_missionItems
->
get
(
i
));
int
sequenceNumber
=
0
;
for
(
int
i
=
0
;
i
<
_visualItems
->
count
();
i
++
)
{
VisualMissionItem
*
item
=
qobject_cast
<
VisualMissionItem
*>
(
_visualItems
->
get
(
i
));
item
->
setSequenceNumber
(
sequenceNumber
++
);
if
(
!
item
->
isSimpleItem
())
{
ComplexMissionItem
*
complexItem
=
qobject_cast
<
ComplexMissionItem
*>
(
item
);
// Setup ascending sequence numbers
if
(
complexItem
)
{
item
->
setSequenceNumber
(
i
);
sequenceNumber
=
complexItem
->
nextSequenceNumber
();
}
else
{
qWarning
()
<<
"isSimpleItem == false, yet not ComplexMissionItem"
;
}
}
}
}
}
}
// This will update the child item hierarchy
// This will update the child item hierarchy
void
MissionController
::
_recalcChildItems
(
void
)
void
MissionController
::
_recalcChildItems
(
void
)
{
{
MissionItem
*
currentParentItem
=
qobject_cast
<
MissionItem
*>
(
_mission
Items
->
get
(
0
));
VisualMissionItem
*
currentParentItem
=
qobject_cast
<
VisualMissionItem
*>
(
_visual
Items
->
get
(
0
));
currentParentItem
->
childItems
()
->
clear
();
currentParentItem
->
childItems
()
->
clear
();
for
(
int
i
=
1
;
i
<
_
mission
Items
->
count
();
i
++
)
{
for
(
int
i
=
1
;
i
<
_
visual
Items
->
count
();
i
++
)
{
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
_mission
Items
->
get
(
i
));
VisualMissionItem
*
item
=
qobject_cast
<
VisualMissionItem
*>
(
_visual
Items
->
get
(
i
));
// Set up non-coordinate item child hierarchy
// Set up non-coordinate item child hierarchy
if
(
item
->
specifiesCoordinate
())
{
if
(
item
->
specifiesCoordinate
())
{
...
@@ -613,15 +686,22 @@ void MissionController::_recalcAll(void)
...
@@ -613,15 +686,22 @@ void MissionController::_recalcAll(void)
}
}
/// Initializes a new set of mission items
/// Initializes a new set of mission items
void
MissionController
::
_initAll
Mission
Items
(
void
)
void
MissionController
::
_initAll
Visual
Items
(
void
)
{
{
MissionItem
*
homeItem
=
NULL
;
SimpleMissionItem
*
homeItem
=
NULL
;
// Setup home position at index 0
homeItem
=
qobject_cast
<
SimpleMissionItem
*>
(
_visualItems
->
get
(
0
));
if
(
!
homeItem
)
{
qWarning
()
<<
"homeItem not SimpleMissionItem"
;
return
;
}
homeItem
=
qobject_cast
<
MissionItem
*>
(
_missionItems
->
get
(
0
));
homeItem
->
setHomePositionSpecialCase
(
true
);
homeItem
->
setHomePositionSpecialCase
(
true
);
homeItem
->
setShowHomePosition
(
_editMode
);
homeItem
->
setShowHomePosition
(
_editMode
);
homeItem
->
setCommand
(
MAV_CMD_NAV_WAYPOINT
);
homeItem
->
missionItem
().
setCommand
(
MAV_CMD_NAV_WAYPOINT
);
homeItem
->
setFrame
(
MAV_FRAME_GLOBAL
);
homeItem
->
missionItem
().
setFrame
(
MAV_FRAME_GLOBAL
);
homeItem
->
setIsCurrentItem
(
true
);
homeItem
->
setIsCurrentItem
(
true
);
if
(
!
_editMode
&&
_activeVehicle
&&
_activeVehicle
->
homePositionAvailable
())
{
if
(
!
_editMode
&&
_activeVehicle
&&
_activeVehicle
->
homePositionAvailable
())
{
...
@@ -632,52 +712,70 @@ void MissionController::_initAllMissionItems(void)
...
@@ -632,52 +712,70 @@ void MissionController::_initAllMissionItems(void)
qDebug
()
<<
"home item"
<<
homeItem
->
coordinate
();
qDebug
()
<<
"home item"
<<
homeItem
->
coordinate
();
QmlObjectListModel
*
newComplexItems
=
new
QmlObjectListModel
(
this
);
QmlObjectListModel
*
newComplexItems
=
new
QmlObjectListModel
(
this
);
for
(
int
i
=
0
;
i
<
_visualItems
->
count
();
i
++
)
{
VisualMissionItem
*
item
=
qobject_cast
<
VisualMissionItem
*>
(
_visualItems
->
get
(
i
));
_initVisualItem
(
item
);
for
(
int
i
=
0
;
i
<
_missionItems
->
count
();
i
++
)
{
// Set up complex item list
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
_missionItems
->
get
(
i
));
if
(
!
item
->
isSimpleItem
())
{
ComplexMissionItem
*
complexItem
=
qobject_cast
<
ComplexMissionItem
*>
(
item
);
if
(
!
item
->
simpleItem
())
{
if
(
complexItem
)
{
newComplexItems
->
append
(
item
);
newComplexItems
->
append
(
item
);
}
else
{
qWarning
()
<<
"isSimpleItem == false, but not ComplexMissionItem"
;
}
}
}
_initMissionItem
(
item
);
}
}
delete
_complexMissionItems
;
if
(
_complexItems
)
{
_complexMissionItems
=
newComplexItems
;
_complexItems
->
deleteLater
();
}
_complexItems
=
newComplexItems
;
_recalcAll
();
_recalcAll
();
emit
mission
ItemsChanged
();
emit
visual
ItemsChanged
();
emit
complex
Mission
ItemsChanged
();
emit
complex
Visual
ItemsChanged
();
_
mission
Items
->
setDirty
(
false
);
_
visual
Items
->
setDirty
(
false
);
connect
(
_
mission
Items
,
&
QmlObjectListModel
::
dirtyChanged
,
this
,
&
MissionController
::
_dirtyChanged
);
connect
(
_
visual
Items
,
&
QmlObjectListModel
::
dirtyChanged
,
this
,
&
MissionController
::
_dirtyChanged
);
}
}
void
MissionController
::
_deinitAll
Mission
Items
(
void
)
void
MissionController
::
_deinitAll
Visual
Items
(
void
)
{
{
for
(
int
i
=
0
;
i
<
_
mission
Items
->
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_
visual
Items
->
count
();
i
++
)
{
_deinit
MissionItem
(
qobject_cast
<
MissionItem
*>
(
_mission
Items
->
get
(
i
)));
_deinit
VisualItem
(
qobject_cast
<
VisualMissionItem
*>
(
_visual
Items
->
get
(
i
)));
}
}
connect
(
_
mission
Items
,
&
QmlObjectListModel
::
dirtyChanged
,
this
,
&
MissionController
::
_dirtyChanged
);
connect
(
_
visual
Items
,
&
QmlObjectListModel
::
dirtyChanged
,
this
,
&
MissionController
::
_dirtyChanged
);
}
}
void
MissionController
::
_init
MissionItem
(
MissionItem
*
i
tem
)
void
MissionController
::
_init
VisualItem
(
VisualMissionItem
*
visualI
tem
)
{
{
_missionItems
->
setDirty
(
false
);
_visualItems
->
setDirty
(
false
);
connect
(
visualItem
,
&
VisualMissionItem
::
coordinateChanged
,
this
,
&
MissionController
::
_itemCoordinateChanged
);
connect
(
visualItem
,
&
VisualMissionItem
::
coordinateHasRelativeAltitudeChanged
,
this
,
&
MissionController
::
_recalcWaypointLines
);
connect
(
visualItem
,
&
VisualMissionItem
::
exitCoordinateHasRelativeAltitudeChanged
,
this
,
&
MissionController
::
_recalcWaypointLines
);
connect
(
item
,
&
MissionItem
::
commandChanged
,
this
,
&
MissionController
::
_itemCommandChanged
);
if
(
visualItem
->
isSimpleItem
())
{
connect
(
item
,
&
MissionItem
::
coordinateChanged
,
this
,
&
MissionController
::
_itemCoordinateChanged
);
// We need to track commandChanged on simple item since recalc has special handling for takeoff command
connect
(
item
,
&
MissionItem
::
frameChanged
,
this
,
&
MissionController
::
_itemFrameChanged
);
SimpleMissionItem
*
simpleItem
=
qobject_cast
<
SimpleMissionItem
*>
(
visualItem
);
if
(
simpleItem
)
{
connect
(
&
simpleItem
->
missionItem
().
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
MissionController
::
_itemCommandChanged
);
}
else
{
qWarning
()
<<
"isSimpleItem == true, yet not SimpleMissionItem"
;
}
}
}
}
void
MissionController
::
_deinit
MissionItem
(
MissionItem
*
i
tem
)
void
MissionController
::
_deinit
VisualItem
(
VisualMissionItem
*
visualI
tem
)
{
{
disconnect
(
item
,
&
MissionItem
::
commandChanged
,
this
,
&
MissionController
::
_itemCommandChanged
);
// Disconnect all signals
disconnect
(
item
,
&
MissionItem
::
coordinateChanged
,
this
,
&
MissionController
::
_itemCoordinateChanged
);
disconnect
(
visualItem
,
0
,
0
,
0
);
disconnect
(
item
,
&
MissionItem
::
frameChanged
,
this
,
&
MissionController
::
_itemFrameChanged
);
}
}
void
MissionController
::
_itemCoordinateChanged
(
const
QGeoCoordinate
&
coordinate
)
void
MissionController
::
_itemCoordinateChanged
(
const
QGeoCoordinate
&
coordinate
)
...
@@ -686,15 +784,8 @@ void MissionController::_itemCoordinateChanged(const QGeoCoordinate& coordinate)
...
@@ -686,15 +784,8 @@ void MissionController::_itemCoordinateChanged(const QGeoCoordinate& coordinate)
_recalcWaypointLines
();
_recalcWaypointLines
();
}
}
void
MissionController
::
_item
FrameChanged
(
int
frame
)
void
MissionController
::
_item
CommandChanged
(
void
)
{
{
Q_UNUSED
(
frame
);
_recalcWaypointLines
();
}
void
MissionController
::
_itemCommandChanged
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
)
{
Q_UNUSED
(
command
);;
_recalcChildItems
();
_recalcChildItems
();
_recalcWaypointLines
();
_recalcWaypointLines
();
}
}
...
@@ -732,20 +823,29 @@ void MissionController::_activeVehicleChanged(Vehicle* activeVehicle)
...
@@ -732,20 +823,29 @@ void MissionController::_activeVehicleChanged(Vehicle* activeVehicle)
_activeVehicleHomePositionChanged
(
_activeVehicle
->
homePosition
());
_activeVehicleHomePositionChanged
(
_activeVehicle
->
homePosition
());
_activeVehicleHomePositionAvailableChanged
(
_activeVehicle
->
homePositionAvailable
());
_activeVehicleHomePositionAvailableChanged
(
_activeVehicle
->
homePositionAvailable
());
}
}
// Whenever vehicle changes we need to update syncInProgress
emit
syncInProgressChanged
(
syncInProgress
());
}
}
void
MissionController
::
_activeVehicleHomePositionAvailableChanged
(
bool
homePositionAvailable
)
void
MissionController
::
_activeVehicleHomePositionAvailableChanged
(
bool
homePositionAvailable
)
{
{
if
(
!
_editMode
&&
_missionItems
)
{
if
(
!
_editMode
&&
_visualItems
)
{
qobject_cast
<
MissionItem
*>
(
_missionItems
->
get
(
0
))
->
setShowHomePosition
(
homePositionAvailable
);
SimpleMissionItem
*
homeItem
=
qobject_cast
<
SimpleMissionItem
*>
(
_visualItems
->
get
(
0
));
_recalcWaypointLines
();
if
(
homeItem
)
{
homeItem
->
setShowHomePosition
(
homePositionAvailable
);
_recalcWaypointLines
();
}
else
{
qWarning
()
<<
"Unabled to cast home item to SimpleMissionItem"
;
}
}
}
}
}
void
MissionController
::
_activeVehicleHomePositionChanged
(
const
QGeoCoordinate
&
homePosition
)
void
MissionController
::
_activeVehicleHomePositionChanged
(
const
QGeoCoordinate
&
homePosition
)
{
{
if
(
!
_editMode
&&
_
mission
Items
)
{
if
(
!
_editMode
&&
_
visual
Items
)
{
qobject_cast
<
MissionItem
*>
(
_mission
Items
->
get
(
0
))
->
setCoordinate
(
homePosition
);
qobject_cast
<
VisualMissionItem
*>
(
_visual
Items
->
get
(
0
))
->
setCoordinate
(
homePosition
);
_recalcWaypointLines
();
_recalcWaypointLines
();
}
}
}
}
...
@@ -784,9 +884,9 @@ void MissionController::_autoSyncSend(void)
...
@@ -784,9 +884,9 @@ void MissionController::_autoSyncSend(void)
{
{
qDebug
()
<<
"Auto-syncing with vehicle"
;
qDebug
()
<<
"Auto-syncing with vehicle"
;
_queuedSend
=
false
;
_queuedSend
=
false
;
if
(
_
mission
Items
)
{
if
(
_
visual
Items
)
{
sendMissionItems
();
sendMissionItems
();
_
mission
Items
->
setDirty
(
false
);
_
visual
Items
->
setDirty
(
false
);
}
}
}
}
...
@@ -798,27 +898,17 @@ void MissionController::_inProgressChanged(bool inProgress)
...
@@ -798,27 +898,17 @@ void MissionController::_inProgressChanged(bool inProgress)
}
}
}
}
QmlObjectListModel
*
MissionController
::
missionItems
(
void
)
{
return
_missionItems
;
}
QmlObjectListModel
*
MissionController
::
complexMissionItems
(
void
)
{
return
_complexMissionItems
;
}
bool
MissionController
::
_findLastAltitude
(
double
*
lastAltitude
)
bool
MissionController
::
_findLastAltitude
(
double
*
lastAltitude
)
{
{
bool
found
=
false
;
bool
found
=
false
;
double
foundAltitude
;
double
foundAltitude
;
// Don't use home position
// Don't use home position
for
(
int
i
=
1
;
i
<
_
mission
Items
->
count
();
i
++
)
{
for
(
int
i
=
1
;
i
<
_
visual
Items
->
count
();
i
++
)
{
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
_mission
Items
->
get
(
i
));
VisualMissionItem
*
item
=
qobject_cast
<
VisualMissionItem
*>
(
_visual
Items
->
get
(
i
));
if
(
item
->
specifiesCoordinate
()
&&
!
item
->
s
tandaloneCoordinate
())
{
if
(
item
->
specifiesCoordinate
()
&&
!
item
->
isS
tandaloneCoordinate
())
{
foundAltitude
=
item
->
param7
();
foundAltitude
=
item
->
exitCoordinate
().
altitude
();
found
=
true
;
found
=
true
;
}
}
}
}
...
@@ -835,12 +925,20 @@ bool MissionController::_findLastAcceptanceRadius(double* lastAcceptanceRadius)
...
@@ -835,12 +925,20 @@ bool MissionController::_findLastAcceptanceRadius(double* lastAcceptanceRadius)
bool
found
=
false
;
bool
found
=
false
;
double
foundAcceptanceRadius
;
double
foundAcceptanceRadius
;
for
(
int
i
=
0
;
i
<
_
mission
Items
->
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_
visual
Items
->
count
();
i
++
)
{
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
_mission
Items
->
get
(
i
));
VisualMissionItem
*
visualItem
=
qobject_cast
<
VisualMissionItem
*>
(
_visual
Items
->
get
(
i
));
if
((
MAV_CMD
)
item
->
command
()
==
MAV_CMD_NAV_WAYPOINT
)
{
if
(
visualItem
->
isSimpleItem
())
{
foundAcceptanceRadius
=
item
->
param2
();
SimpleMissionItem
*
simpleItem
=
qobject_cast
<
SimpleMissionItem
*>
(
visualItem
);
found
=
true
;
if
(
simpleItem
)
{
if
((
MAV_CMD
)
simpleItem
->
command
()
==
MAV_CMD_NAV_WAYPOINT
)
{
foundAcceptanceRadius
=
simpleItem
->
missionItem
().
param2
();
found
=
true
;
}
}
else
{
qWarning
()
<<
"isSimpleItem == true, yet not SimpleMissionItem"
;
}
}
}
}
}
...
@@ -864,21 +962,21 @@ double MissionController::_normalizeLon(double lon)
...
@@ -864,21 +962,21 @@ double MissionController::_normalizeLon(double lon)
}
}
/// Add the home position item to the front of the list
/// Add the home position item to the front of the list
void
MissionController
::
_addPlannedHomePosition
(
QmlObjectListModel
*
mission
Items
,
bool
addToCenter
)
void
MissionController
::
_addPlannedHomePosition
(
QmlObjectListModel
*
visual
Items
,
bool
addToCenter
)
{
{
MissionItem
*
homeItem
=
new
SimpleMissionItem
(
_activeVehicle
,
this
);
Simple
MissionItem
*
homeItem
=
new
SimpleMissionItem
(
_activeVehicle
,
this
);
mission
Items
->
insert
(
0
,
homeItem
);
visual
Items
->
insert
(
0
,
homeItem
);
if
(
mission
Items
->
count
()
>
1
&&
addToCenter
)
{
if
(
visual
Items
->
count
()
>
1
&&
addToCenter
)
{
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
mission
Items
->
get
(
1
));
VisualMissionItem
*
item
=
qobject_cast
<
VisualMissionItem
*>
(
visual
Items
->
get
(
1
));
double
north
=
_normalizeLat
(
item
->
coordinate
().
latitude
());
double
north
=
_normalizeLat
(
item
->
coordinate
().
latitude
());
double
south
=
north
;
double
south
=
north
;
double
east
=
_normalizeLon
(
item
->
coordinate
().
longitude
());
double
east
=
_normalizeLon
(
item
->
coordinate
().
longitude
());
double
west
=
east
;
double
west
=
east
;
for
(
int
i
=
2
;
i
<
mission
Items
->
count
();
i
++
)
{
for
(
int
i
=
2
;
i
<
visual
Items
->
count
();
i
++
)
{
item
=
qobject_cast
<
MissionItem
*>
(
mission
Items
->
get
(
i
));
item
=
qobject_cast
<
VisualMissionItem
*>
(
visual
Items
->
get
(
i
));
double
lat
=
_normalizeLat
(
item
->
coordinate
().
latitude
());
double
lat
=
_normalizeLat
(
item
->
coordinate
().
latitude
());
double
lon
=
_normalizeLon
(
item
->
coordinate
().
longitude
());
double
lon
=
_normalizeLon
(
item
->
coordinate
().
longitude
());
...
@@ -902,8 +1000,8 @@ void MissionController::_currentMissionItemChanged(int sequenceNumber)
...
@@ -902,8 +1000,8 @@ void MissionController::_currentMissionItemChanged(int sequenceNumber)
sequenceNumber
++
;
sequenceNumber
++
;
}
}
for
(
int
i
=
0
;
i
<
_
mission
Items
->
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_
visual
Items
->
count
();
i
++
)
{
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
_mission
Items
->
get
(
i
));
VisualMissionItem
*
item
=
qobject_cast
<
VisualMissionItem
*>
(
_visual
Items
->
get
(
i
));
item
->
setIsCurrentItem
(
item
->
sequenceNumber
()
==
sequenceNumber
);
item
->
setIsCurrentItem
(
item
->
sequenceNumber
()
==
sequenceNumber
);
}
}
}
}
...
@@ -931,6 +1029,9 @@ QStringList MissionController::getMobileMissionFiles(void)
...
@@ -931,6 +1029,9 @@ QStringList MissionController::getMobileMissionFiles(void)
bool
MissionController
::
syncInProgress
(
void
)
bool
MissionController
::
syncInProgress
(
void
)
{
{
qDebug
()
<<
_activeVehicle
->
missionManager
()
->
inProgress
();
if
(
_activeVehicle
)
{
return
_activeVehicle
->
missionManager
()
->
inProgress
();
return
_activeVehicle
->
missionManager
()
->
inProgress
();
}
else
{
return
false
;
}
}
}
src/MissionManager/MissionController.h
View file @
36d57743
...
@@ -30,7 +30,7 @@ This file is part of the QGROUNDCONTROL project
...
@@ -30,7 +30,7 @@ This file is part of the QGROUNDCONTROL project
#include "Vehicle.h"
#include "Vehicle.h"
#include "QGCLoggingCategory.h"
#include "QGCLoggingCategory.h"
#include "MavlinkQmlSingleton.h"
#include "MavlinkQmlSingleton.h"
#include "MissionItem.h"
#include "
Visual
MissionItem.h"
Q_DECLARE_LOGGING_CATEGORY
(
MissionControllerLog
)
Q_DECLARE_LOGGING_CATEGORY
(
MissionControllerLog
)
...
@@ -42,11 +42,11 @@ public:
...
@@ -42,11 +42,11 @@ public:
MissionController
(
QObject
*
parent
=
NULL
);
MissionController
(
QObject
*
parent
=
NULL
);
~
MissionController
();
~
MissionController
();
Q_PROPERTY
(
QmlObjectListModel
*
missionItems
READ
missionItems
NOTIFY
mission
ItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
visualItems
READ
visualItems
NOTIFY
visual
ItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
complex
MissionItems
READ
complexMissionItems
NOTIFY
complexMission
ItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
complex
VisualItems
READ
complexVisualItems
NOTIFY
complexVisual
ItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
waypointLines
READ
waypointLines
NOTIFY
waypointLinesChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
waypointLines
READ
waypointLines
NOTIFY
waypointLinesChanged
)
Q_PROPERTY
(
bool
autoSync
READ
autoSync
WRITE
setAutoSync
NOTIFY
autoSyncChanged
)
Q_PROPERTY
(
bool
autoSync
READ
autoSync
WRITE
setAutoSync
NOTIFY
autoSyncChanged
)
Q_PROPERTY
(
bool
syncInProgress
READ
syncInProgress
NOTIFY
syncInProgressChanged
)
Q_PROPERTY
(
bool
syncInProgress
READ
syncInProgress
NOTIFY
syncInProgressChanged
)
Q_INVOKABLE
void
start
(
bool
editMode
);
Q_INVOKABLE
void
start
(
bool
editMode
);
Q_INVOKABLE
void
getMissionItems
(
void
);
Q_INVOKABLE
void
getMissionItems
(
void
);
...
@@ -67,16 +67,17 @@ public:
...
@@ -67,16 +67,17 @@ public:
// Property accessors
// Property accessors
QmlObjectListModel
*
missionItems
(
void
);
QmlObjectListModel
*
visualItems
(
void
)
{
return
_visualItems
;
}
QmlObjectListModel
*
complexMissionItems
(
void
);
QmlObjectListModel
*
complexVisualItems
(
void
)
{
return
_complexItems
;
}
QmlObjectListModel
*
waypointLines
(
void
)
{
return
&
_waypointLines
;
}
QmlObjectListModel
*
waypointLines
(
void
)
{
return
&
_waypointLines
;
}
bool
autoSync
(
void
)
{
return
_autoSync
;
}
bool
autoSync
(
void
)
{
return
_autoSync
;
}
void
setAutoSync
(
bool
autoSync
);
void
setAutoSync
(
bool
autoSync
);
bool
syncInProgress
(
void
);
bool
syncInProgress
(
void
);
signals:
signals:
void
mission
ItemsChanged
(
void
);
void
visual
ItemsChanged
(
void
);
void
complex
Mission
ItemsChanged
(
void
);
void
complex
Visual
ItemsChanged
(
void
);
void
waypointLinesChanged
(
void
);
void
waypointLinesChanged
(
void
);
void
autoSyncChanged
(
bool
autoSync
);
void
autoSyncChanged
(
bool
autoSync
);
void
newItemsFromVehicle
(
void
);
void
newItemsFromVehicle
(
void
);
...
@@ -85,41 +86,40 @@ signals:
...
@@ -85,41 +86,40 @@ signals:
private
slots
:
private
slots
:
void
_newMissionItemsAvailableFromVehicle
();
void
_newMissionItemsAvailableFromVehicle
();
void
_itemCoordinateChanged
(
const
QGeoCoordinate
&
coordinate
);
void
_itemCoordinateChanged
(
const
QGeoCoordinate
&
coordinate
);
void
_itemFrameChanged
(
int
frame
);
void
_itemCommandChanged
(
void
);
void
_itemCommandChanged
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
);
void
_activeVehicleChanged
(
Vehicle
*
activeVehicle
);
void
_activeVehicleChanged
(
Vehicle
*
activeVehicle
);
void
_activeVehicleHomePositionAvailableChanged
(
bool
homePositionAvailable
);
void
_activeVehicleHomePositionAvailableChanged
(
bool
homePositionAvailable
);
void
_activeVehicleHomePositionChanged
(
const
QGeoCoordinate
&
homePosition
);
void
_activeVehicleHomePositionChanged
(
const
QGeoCoordinate
&
homePosition
);
void
_dirtyChanged
(
bool
dirty
);
void
_dirtyChanged
(
bool
dirty
);
void
_inProgressChanged
(
bool
inProgress
);
void
_inProgressChanged
(
bool
inProgress
);
void
_currentMissionItemChanged
(
int
sequenceNumber
);
void
_currentMissionItemChanged
(
int
sequenceNumber
);
void
_recalcWaypointLines
(
void
);
private:
private:
void
_recalcSequence
(
void
);
void
_recalcSequence
(
void
);
void
_recalcWaypointLines
(
void
);
void
_recalcChildItems
(
void
);
void
_recalcChildItems
(
void
);
void
_recalcAll
(
void
);
void
_recalcAll
(
void
);
void
_initAll
Mission
Items
(
void
);
void
_initAll
Visual
Items
(
void
);
void
_deinitAll
Mission
Items
(
void
);
void
_deinitAll
Visual
Items
(
void
);
void
_init
MissionItem
(
MissionItem
*
item
);
void
_init
VisualItem
(
Visual
MissionItem
*
item
);
void
_deinit
MissionItem
(
MissionItem
*
item
);
void
_deinit
VisualItem
(
Visual
MissionItem
*
item
);
void
_autoSyncSend
(
void
);
void
_autoSyncSend
(
void
);
void
_setupActiveVehicle
(
Vehicle
*
activeVehicle
,
bool
forceLoadFromVehicle
);
void
_setupActiveVehicle
(
Vehicle
*
activeVehicle
,
bool
forceLoadFromVehicle
);
void
_calcPrevWaypointValues
(
double
homeAlt
,
MissionItem
*
currentItem
,
MissionItem
*
prevItem
,
double
*
azimuth
,
double
*
distance
,
double
*
altDifference
);
void
_calcPrevWaypointValues
(
double
homeAlt
,
VisualMissionItem
*
currentItem
,
Visual
MissionItem
*
prevItem
,
double
*
azimuth
,
double
*
distance
,
double
*
altDifference
);
bool
_findLastAltitude
(
double
*
lastAltitude
);
bool
_findLastAltitude
(
double
*
lastAltitude
);
bool
_findLastAcceptanceRadius
(
double
*
lastAcceptanceRadius
);
bool
_findLastAcceptanceRadius
(
double
*
lastAcceptanceRadius
);
void
_addPlannedHomePosition
(
QmlObjectListModel
*
mission
Items
,
bool
addToCenter
);
void
_addPlannedHomePosition
(
QmlObjectListModel
*
visual
Items
,
bool
addToCenter
);
double
_normalizeLat
(
double
lat
);
double
_normalizeLat
(
double
lat
);
double
_normalizeLon
(
double
lon
);
double
_normalizeLon
(
double
lon
);
bool
_loadJsonMissionFile
(
const
QByteArray
&
bytes
,
QmlObjectListModel
*
mission
Items
,
QString
&
errorString
);
bool
_loadJsonMissionFile
(
const
QByteArray
&
bytes
,
QmlObjectListModel
*
visual
Items
,
QString
&
errorString
);
bool
_loadTextMissionFile
(
QTextStream
&
stream
,
QmlObjectListModel
*
mission
Items
,
QString
&
errorString
);
bool
_loadTextMissionFile
(
QTextStream
&
stream
,
QmlObjectListModel
*
visual
Items
,
QString
&
errorString
);
void
_loadMissionFromFile
(
const
QString
&
file
);
void
_loadMissionFromFile
(
const
QString
&
file
);
void
_saveMissionToFile
(
const
QString
&
file
);
void
_saveMissionToFile
(
const
QString
&
file
);
private:
private:
bool
_editMode
;
bool
_editMode
;
QmlObjectListModel
*
_
mission
Items
;
QmlObjectListModel
*
_
visual
Items
;
QmlObjectListModel
*
_complex
Mission
Items
;
QmlObjectListModel
*
_complexItems
;
QmlObjectListModel
_waypointLines
;
QmlObjectListModel
_waypointLines
;
Vehicle
*
_activeVehicle
;
Vehicle
*
_activeVehicle
;
bool
_autoSync
;
bool
_autoSync
;
...
...
src/MissionManager/MissionControllerManagerTest.cc
View file @
36d57743
...
@@ -63,7 +63,7 @@ void MissionControllerManagerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareTy
...
@@ -63,7 +63,7 @@ void MissionControllerManagerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareTy
}
}
QVERIFY
(
!
_missionManager
->
inProgress
());
QVERIFY
(
!
_missionManager
->
inProgress
());
QCOMPARE
(
_missionManager
->
missionItems
()
->
count
(),
0
);
QCOMPARE
(
_missionManager
->
missionItems
()
.
count
(),
0
);
_multiSpyMissionManager
->
clearAllSignals
();
_multiSpyMissionManager
->
clearAllSignals
();
}
}
...
...
src/MissionManager/MissionControllerTest.cc
View file @
36d57743
...
@@ -24,6 +24,7 @@
...
@@ -24,6 +24,7 @@
#include "MissionControllerTest.h"
#include "MissionControllerTest.h"
#include "LinkManager.h"
#include "LinkManager.h"
#include "MultiVehicleManager.h"
#include "MultiVehicleManager.h"
#include "SimpleMissionItem.h"
MissionControllerTest
::
MissionControllerTest
(
void
)
MissionControllerTest
::
MissionControllerTest
(
void
)
:
_multiSpyMissionController
(
NULL
)
:
_multiSpyMissionController
(
NULL
)
...
@@ -62,7 +63,7 @@ void MissionControllerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareType)
...
@@ -62,7 +63,7 @@ void MissionControllerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareType)
_rgMissionItemSignals
[
coordinateChangedSignalIndex
]
=
SIGNAL
(
coordinateChanged
(
const
QGeoCoordinate
&
));
_rgMissionItemSignals
[
coordinateChangedSignalIndex
]
=
SIGNAL
(
coordinateChanged
(
const
QGeoCoordinate
&
));
// MissionController signals
// MissionController signals
_rgMissionControllerSignals
[
missionItemsChangedSignalIndex
]
=
SIGNAL
(
mission
ItemsChanged
());
_rgMissionControllerSignals
[
visualItemsChangedSignalIndex
]
=
SIGNAL
(
visual
ItemsChanged
());
_rgMissionControllerSignals
[
waypointLinesChangedSignalIndex
]
=
SIGNAL
(
waypointLinesChanged
());
_rgMissionControllerSignals
[
waypointLinesChangedSignalIndex
]
=
SIGNAL
(
waypointLinesChanged
());
if
(
!
_missionController
)
{
if
(
!
_missionController
)
{
...
@@ -80,17 +81,17 @@ void MissionControllerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareType)
...
@@ -80,17 +81,17 @@ void MissionControllerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareType)
}
}
// All signals should some through on start
// All signals should some through on start
QCOMPARE
(
_multiSpyMissionController
->
checkOnlySignalsByMask
(
mission
ItemsChangedSignalMask
|
waypointLinesChangedSignalMask
),
true
);
QCOMPARE
(
_multiSpyMissionController
->
checkOnlySignalsByMask
(
visual
ItemsChangedSignalMask
|
waypointLinesChangedSignalMask
),
true
);
_multiSpyMissionController
->
clearAllSignals
();
_multiSpyMissionController
->
clearAllSignals
();
QmlObjectListModel
*
missionItems
=
_missionController
->
mission
Items
();
QmlObjectListModel
*
visualItems
=
_missionController
->
visual
Items
();
QVERIFY
(
mission
Items
);
QVERIFY
(
visual
Items
);
// Empty vehicle only has home position
// Empty vehicle only has home position
QCOMPARE
(
mission
Items
->
count
(),
1
);
QCOMPARE
(
visual
Items
->
count
(),
1
);
// Home position should be in first slot, but not yet valid
// Home position should be in first slot, but not yet valid
MissionItem
*
homeItem
=
qobject_cast
<
MissionItem
*>
(
mission
Items
->
get
(
0
));
SimpleMissionItem
*
homeItem
=
qobject_cast
<
SimpleMissionItem
*>
(
visual
Items
->
get
(
0
));
QVERIFY
(
homeItem
);
QVERIFY
(
homeItem
);
QCOMPARE
(
homeItem
->
homePosition
(),
true
);
QCOMPARE
(
homeItem
->
homePosition
(),
true
);
...
@@ -113,9 +114,9 @@ void MissionControllerTest::_testEmptyVehicleWorker(MAV_AUTOPILOT firmwareType)
...
@@ -113,9 +114,9 @@ void MissionControllerTest::_testEmptyVehicleWorker(MAV_AUTOPILOT firmwareType)
// FYI: A significant amount of empty vehicle testing is in _initForFirmwareType since that
// FYI: A significant amount of empty vehicle testing is in _initForFirmwareType since that
// sets up an empty vehicle
// sets up an empty vehicle
QmlObjectListModel
*
missionItems
=
_missionController
->
mission
Items
();
QmlObjectListModel
*
visualItems
=
_missionController
->
visual
Items
();
QVERIFY
(
mission
Items
);
QVERIFY
(
visual
Items
);
MissionItem
*
homeItem
=
qobject_cast
<
MissionItem
*>
(
mission
Items
->
get
(
0
));
SimpleMissionItem
*
homeItem
=
qobject_cast
<
SimpleMissionItem
*>
(
visual
Items
->
get
(
0
));
QVERIFY
(
homeItem
);
QVERIFY
(
homeItem
);
_setupMissionItemSignals
(
homeItem
);
_setupMissionItemSignals
(
homeItem
);
...
@@ -137,17 +138,17 @@ void MissionControllerTest::_testAddWaypointWorker(MAV_AUTOPILOT firmwareType)
...
@@ -137,17 +138,17 @@ void MissionControllerTest::_testAddWaypointWorker(MAV_AUTOPILOT firmwareType)
QGeoCoordinate
coordinate
(
37.803784
,
-
122.462276
);
QGeoCoordinate
coordinate
(
37.803784
,
-
122.462276
);
_missionController
->
insertSimpleMissionItem
(
coordinate
,
_missionController
->
mission
Items
()
->
count
());
_missionController
->
insertSimpleMissionItem
(
coordinate
,
_missionController
->
visual
Items
()
->
count
());
QCOMPARE
(
_multiSpyMissionController
->
checkOnlySignalsByMask
(
waypointLinesChangedSignalMask
),
true
);
QCOMPARE
(
_multiSpyMissionController
->
checkOnlySignalsByMask
(
waypointLinesChangedSignalMask
),
true
);
QmlObjectListModel
*
missionItems
=
_missionController
->
mission
Items
();
QmlObjectListModel
*
visualItems
=
_missionController
->
visual
Items
();
QVERIFY
(
mission
Items
);
QVERIFY
(
visual
Items
);
QCOMPARE
(
mission
Items
->
count
(),
2
);
QCOMPARE
(
visual
Items
->
count
(),
2
);
MissionItem
*
homeItem
=
qobject_cast
<
MissionItem
*>
(
mission
Items
->
get
(
0
));
SimpleMissionItem
*
homeItem
=
qobject_cast
<
SimpleMissionItem
*>
(
visual
Items
->
get
(
0
));
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
mission
Items
->
get
(
1
));
SimpleMissionItem
*
item
=
qobject_cast
<
SimpleMissionItem
*>
(
visual
Items
->
get
(
1
));
QVERIFY
(
homeItem
);
QVERIFY
(
homeItem
);
QVERIFY
(
item
);
QVERIFY
(
item
);
...
@@ -182,13 +183,13 @@ void MissionControllerTest::_testOfflineToOnlineWorker(MAV_AUTOPILOT firmwareTyp
...
@@ -182,13 +183,13 @@ void MissionControllerTest::_testOfflineToOnlineWorker(MAV_AUTOPILOT firmwareTyp
_missionController
=
new
MissionController
();
_missionController
=
new
MissionController
();
Q_CHECK_PTR
(
_missionController
);
Q_CHECK_PTR
(
_missionController
);
_missionController
->
start
(
true
/* editMode */
);
_missionController
->
start
(
true
/* editMode */
);
_missionController
->
insertSimpleMissionItem
(
QGeoCoordinate
(
37.803784
,
-
122.462276
),
_missionController
->
mission
Items
()
->
count
());
_missionController
->
insertSimpleMissionItem
(
QGeoCoordinate
(
37.803784
,
-
122.462276
),
_missionController
->
visual
Items
()
->
count
());
// Go online to empty vehicle
// Go online to empty vehicle
MissionControllerManagerTest
::
_initForFirmwareType
(
firmwareType
);
MissionControllerManagerTest
::
_initForFirmwareType
(
firmwareType
);
// Make sure our offline mission items are still there
// Make sure our offline mission items are still there
QCOMPARE
(
_missionController
->
mission
Items
()
->
count
(),
2
);
QCOMPARE
(
_missionController
->
visual
Items
()
->
count
(),
2
);
}
}
void
MissionControllerTest
::
_testOfflineToOnlineAPM
(
void
)
void
MissionControllerTest
::
_testOfflineToOnlineAPM
(
void
)
...
@@ -201,7 +202,7 @@ void MissionControllerTest::_testOfflineToOnlinePX4(void)
...
@@ -201,7 +202,7 @@ void MissionControllerTest::_testOfflineToOnlinePX4(void)
_testOfflineToOnlineWorker
(
MAV_AUTOPILOT_PX4
);
_testOfflineToOnlineWorker
(
MAV_AUTOPILOT_PX4
);
}
}
void
MissionControllerTest
::
_setupMissionItemSignals
(
MissionItem
*
item
)
void
MissionControllerTest
::
_setupMissionItemSignals
(
Simple
MissionItem
*
item
)
{
{
delete
_multiSpyMissionItem
;
delete
_multiSpyMissionItem
;
...
...
src/MissionManager/MissionControllerTest.h
View file @
36d57743
...
@@ -30,6 +30,7 @@
...
@@ -30,6 +30,7 @@
#include "MultiSignalSpy.h"
#include "MultiSignalSpy.h"
#include "MissionControllerManagerTest.h"
#include "MissionControllerManagerTest.h"
#include "MissionController.h"
#include "MissionController.h"
#include "SimpleMissionItem.h"
#include <QGeoCoordinate>
#include <QGeoCoordinate>
...
@@ -55,7 +56,7 @@ private:
...
@@ -55,7 +56,7 @@ private:
void
_testEmptyVehicleWorker
(
MAV_AUTOPILOT
firmwareType
);
void
_testEmptyVehicleWorker
(
MAV_AUTOPILOT
firmwareType
);
void
_testAddWaypointWorker
(
MAV_AUTOPILOT
firmwareType
);
void
_testAddWaypointWorker
(
MAV_AUTOPILOT
firmwareType
);
void
_testOfflineToOnlineWorker
(
MAV_AUTOPILOT
firmwareType
);
void
_testOfflineToOnlineWorker
(
MAV_AUTOPILOT
firmwareType
);
void
_setupMissionItemSignals
(
MissionItem
*
item
);
void
_setupMissionItemSignals
(
Simple
MissionItem
*
item
);
// MissiomItems signals
// MissiomItems signals
...
@@ -72,13 +73,13 @@ private:
...
@@ -72,13 +73,13 @@ private:
// MissionController signals
// MissionController signals
enum
{
enum
{
mission
ItemsChangedSignalIndex
=
0
,
visual
ItemsChangedSignalIndex
=
0
,
waypointLinesChangedSignalIndex
,
waypointLinesChangedSignalIndex
,
missionControllerMaxSignalIndex
missionControllerMaxSignalIndex
};
};
enum
{
enum
{
missionItemsChangedSignalMask
=
1
<<
mission
ItemsChangedSignalIndex
,
visualItemsChangedSignalMask
=
1
<<
visual
ItemsChangedSignalIndex
,
waypointLinesChangedSignalMask
=
1
<<
waypointLinesChangedSignalIndex
,
waypointLinesChangedSignalMask
=
1
<<
waypointLinesChangedSignalIndex
,
};
};
...
...
src/MissionManager/MissionItem.cc
View file @
36d57743
...
@@ -28,15 +28,6 @@ This file is part of the QGROUNDCONTROL project
...
@@ -28,15 +28,6 @@ This file is part of the QGROUNDCONTROL project
#include "QGCApplication.h"
#include "QGCApplication.h"
#include "JsonHelper.h"
#include "JsonHelper.h"
const
double
MissionItem
::
defaultAltitude
=
25.0
;
FactMetaData
*
MissionItem
::
_altitudeMetaData
=
NULL
;
FactMetaData
*
MissionItem
::
_commandMetaData
=
NULL
;
FactMetaData
*
MissionItem
::
_defaultParamMetaData
=
NULL
;
FactMetaData
*
MissionItem
::
_frameMetaData
=
NULL
;
FactMetaData
*
MissionItem
::
_latitudeMetaData
=
NULL
;
FactMetaData
*
MissionItem
::
_longitudeMetaData
=
NULL
;
const
char
*
MissionItem
::
_itemType
=
"missionItem"
;
const
char
*
MissionItem
::
_itemType
=
"missionItem"
;
const
char
*
MissionItem
::
_jsonTypeKey
=
"type"
;
const
char
*
MissionItem
::
_jsonTypeKey
=
"type"
;
const
char
*
MissionItem
::
_jsonIdKey
=
"id"
;
const
char
*
MissionItem
::
_jsonIdKey
=
"id"
;
...
@@ -49,40 +40,10 @@ const char* MissionItem::_jsonParam4Key = "param4";
...
@@ -49,40 +40,10 @@ const char* MissionItem::_jsonParam4Key = "param4";
const
char
*
MissionItem
::
_jsonAutoContinueKey
=
"autoContinue"
;
const
char
*
MissionItem
::
_jsonAutoContinueKey
=
"autoContinue"
;
const
char
*
MissionItem
::
_jsonCoordinateKey
=
"coordinate"
;
const
char
*
MissionItem
::
_jsonCoordinateKey
=
"coordinate"
;
struct
EnumInfo_s
{
MissionItem
::
MissionItem
(
QObject
*
parent
)
const
char
*
label
;
MAV_FRAME
frame
;
};
static
const
struct
EnumInfo_s
_rgMavFrameInfo
[]
=
{
{
"MAV_FRAME_GLOBAL"
,
MAV_FRAME_GLOBAL
},
{
"MAV_FRAME_LOCAL_NED"
,
MAV_FRAME_LOCAL_NED
},
{
"MAV_FRAME_MISSION"
,
MAV_FRAME_MISSION
},
{
"MAV_FRAME_GLOBAL_RELATIVE_ALT"
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
"MAV_FRAME_LOCAL_ENU"
,
MAV_FRAME_LOCAL_ENU
},
{
"MAV_FRAME_GLOBAL_INT"
,
MAV_FRAME_GLOBAL_INT
},
{
"MAV_FRAME_GLOBAL_RELATIVE_ALT_INT"
,
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
},
{
"MAV_FRAME_LOCAL_OFFSET_NED"
,
MAV_FRAME_LOCAL_OFFSET_NED
},
{
"MAV_FRAME_BODY_NED"
,
MAV_FRAME_BODY_NED
},
{
"MAV_FRAME_BODY_OFFSET_NED"
,
MAV_FRAME_BODY_OFFSET_NED
},
{
"MAV_FRAME_GLOBAL_TERRAIN_ALT"
,
MAV_FRAME_GLOBAL_TERRAIN_ALT
},
{
"MAV_FRAME_GLOBAL_TERRAIN_ALT_INT"
,
MAV_FRAME_GLOBAL_TERRAIN_ALT_INT
},
};
MissionItem
::
MissionItem
(
Vehicle
*
vehicle
,
QObject
*
parent
)
:
QObject
(
parent
)
:
QObject
(
parent
)
,
_vehicle
(
vehicle
)
,
_rawEdit
(
false
)
,
_dirty
(
false
)
,
_sequenceNumber
(
0
)
,
_sequenceNumber
(
0
)
,
_isCurrentItem
(
false
)
,
_isCurrentItem
(
false
)
,
_altDifference
(
0.0
)
,
_altPercent
(
0.0
)
,
_azimuth
(
0.0
)
,
_distance
(
0.0
)
,
_homePositionSpecialCase
(
false
)
,
_showHomePosition
(
false
)
,
_altitudeRelativeToHomeFact
(
0
,
"Altitude is relative to home"
,
FactMetaData
::
valueTypeUint32
)
,
_autoContinueFact
(
0
,
"AutoContinue"
,
FactMetaData
::
valueTypeUint32
)
,
_autoContinueFact
(
0
,
"AutoContinue"
,
FactMetaData
::
valueTypeUint32
)
,
_commandFact
(
0
,
""
,
FactMetaData
::
valueTypeUint32
)
,
_commandFact
(
0
,
""
,
FactMetaData
::
valueTypeUint32
)
,
_frameFact
(
0
,
""
,
FactMetaData
::
valueTypeUint32
)
,
_frameFact
(
0
,
""
,
FactMetaData
::
valueTypeUint32
)
...
@@ -93,32 +54,15 @@ MissionItem::MissionItem(Vehicle* vehicle, QObject* parent)
...
@@ -93,32 +54,15 @@ MissionItem::MissionItem(Vehicle* vehicle, QObject* parent)
,
_param5Fact
(
0
,
"Latitude:"
,
FactMetaData
::
valueTypeDouble
)
,
_param5Fact
(
0
,
"Latitude:"
,
FactMetaData
::
valueTypeDouble
)
,
_param6Fact
(
0
,
"Longitude:"
,
FactMetaData
::
valueTypeDouble
)
,
_param6Fact
(
0
,
"Longitude:"
,
FactMetaData
::
valueTypeDouble
)
,
_param7Fact
(
0
,
"Altitude:"
,
FactMetaData
::
valueTypeDouble
)
,
_param7Fact
(
0
,
"Altitude:"
,
FactMetaData
::
valueTypeDouble
)
,
_supportedCommandFact
(
0
,
"Command:"
,
FactMetaData
::
valueTypeUint32
)
,
_param1MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param2MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param3MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param4MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param5MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param6MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param7MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_syncingAltitudeRelativeToHomeAndFrame
(
false
)
,
_syncingHeadingDegreesAndParam4
(
false
)
,
_missionCommands
(
qgcApp
()
->
toolbox
()
->
missionCommands
())
{
{
// Need a good command and frame before we start passing signals around
// Need a good command and frame before we start passing signals around
_commandFact
.
setRawValue
(
MAV_CMD_NAV_WAYPOINT
);
_commandFact
.
setRawValue
(
MAV_CMD_NAV_WAYPOINT
);
_frameFact
.
setRawValue
(
MAV_FRAME_GLOBAL_RELATIVE_ALT
);
_frameFact
.
setRawValue
(
MAV_FRAME_GLOBAL_RELATIVE_ALT
);
_altitudeRelativeToHomeFact
.
setRawValue
(
true
);
_setupMetaData
();
_connectSignals
();
setAutoContinue
(
true
);
setAutoContinue
(
true
);
setDefaultsForCommand
();
}
}
MissionItem
::
MissionItem
(
Vehicle
*
vehicle
,
MissionItem
::
MissionItem
(
int
sequenceNumber
,
int
sequenceNumber
,
MAV_CMD
command
,
MAV_CMD
command
,
MAV_FRAME
frame
,
MAV_FRAME
frame
,
double
param1
,
double
param1
,
...
@@ -132,18 +76,8 @@ MissionItem::MissionItem(Vehicle* vehicle,
...
@@ -132,18 +76,8 @@ MissionItem::MissionItem(Vehicle* vehicle,
bool
isCurrentItem
,
bool
isCurrentItem
,
QObject
*
parent
)
QObject
*
parent
)
:
QObject
(
parent
)
:
QObject
(
parent
)
,
_vehicle
(
vehicle
)
,
_rawEdit
(
false
)
,
_dirty
(
false
)
,
_sequenceNumber
(
sequenceNumber
)
,
_sequenceNumber
(
sequenceNumber
)
,
_isCurrentItem
(
isCurrentItem
)
,
_isCurrentItem
(
isCurrentItem
)
,
_altDifference
(
0.0
)
,
_altPercent
(
0.0
)
,
_azimuth
(
0.0
)
,
_distance
(
0.0
)
,
_homePositionSpecialCase
(
false
)
,
_showHomePosition
(
false
)
,
_altitudeRelativeToHomeFact
(
0
,
"Altitude is relative to home"
,
FactMetaData
::
valueTypeUint32
)
,
_commandFact
(
0
,
""
,
FactMetaData
::
valueTypeUint32
)
,
_commandFact
(
0
,
""
,
FactMetaData
::
valueTypeUint32
)
,
_frameFact
(
0
,
""
,
FactMetaData
::
valueTypeUint32
)
,
_frameFact
(
0
,
""
,
FactMetaData
::
valueTypeUint32
)
,
_param1Fact
(
0
,
"Param1:"
,
FactMetaData
::
valueTypeDouble
)
,
_param1Fact
(
0
,
"Param1:"
,
FactMetaData
::
valueTypeDouble
)
...
@@ -153,32 +87,15 @@ MissionItem::MissionItem(Vehicle* vehicle,
...
@@ -153,32 +87,15 @@ MissionItem::MissionItem(Vehicle* vehicle,
,
_param5Fact
(
0
,
"Lat/X:"
,
FactMetaData
::
valueTypeDouble
)
,
_param5Fact
(
0
,
"Lat/X:"
,
FactMetaData
::
valueTypeDouble
)
,
_param6Fact
(
0
,
"Lon/Y:"
,
FactMetaData
::
valueTypeDouble
)
,
_param6Fact
(
0
,
"Lon/Y:"
,
FactMetaData
::
valueTypeDouble
)
,
_param7Fact
(
0
,
"Alt/Z:"
,
FactMetaData
::
valueTypeDouble
)
,
_param7Fact
(
0
,
"Alt/Z:"
,
FactMetaData
::
valueTypeDouble
)
,
_supportedCommandFact
(
0
,
"Command:"
,
FactMetaData
::
valueTypeUint32
)
,
_param1MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param2MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param3MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param4MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param5MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param6MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param7MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_syncingAltitudeRelativeToHomeAndFrame
(
false
)
,
_syncingHeadingDegreesAndParam4
(
false
)
,
_missionCommands
(
qgcApp
()
->
toolbox
()
->
missionCommands
())
{
{
// Need a good command and frame before we start passing signals around
// Need a good command and frame before we start passing signals around
_commandFact
.
setRawValue
(
MAV_CMD_NAV_WAYPOINT
);
_commandFact
.
setRawValue
(
MAV_CMD_NAV_WAYPOINT
);
_frameFact
.
setRawValue
(
MAV_FRAME_GLOBAL_RELATIVE_ALT
);
_frameFact
.
setRawValue
(
MAV_FRAME_GLOBAL_RELATIVE_ALT
);
_altitudeRelativeToHomeFact
.
setRawValue
(
true
);
_setupMetaData
();
_connectSignals
();
setCommand
(
command
);
setCommand
(
command
);
setFrame
(
frame
);
setFrame
(
frame
);
setAutoContinue
(
autoContinue
);
setAutoContinue
(
autoContinue
);
_syncFrameToAltitudeRelativeToHome
();
_param1Fact
.
setRawValue
(
param1
);
_param1Fact
.
setRawValue
(
param1
);
_param2Fact
.
setRawValue
(
param2
);
_param2Fact
.
setRawValue
(
param2
);
_param3Fact
.
setRawValue
(
param3
);
_param3Fact
.
setRawValue
(
param3
);
...
@@ -190,18 +107,8 @@ MissionItem::MissionItem(Vehicle* vehicle,
...
@@ -190,18 +107,8 @@ MissionItem::MissionItem(Vehicle* vehicle,
MissionItem
::
MissionItem
(
const
MissionItem
&
other
,
QObject
*
parent
)
MissionItem
::
MissionItem
(
const
MissionItem
&
other
,
QObject
*
parent
)
:
QObject
(
parent
)
:
QObject
(
parent
)
,
_vehicle
(
NULL
)
,
_rawEdit
(
false
)
,
_dirty
(
false
)
,
_sequenceNumber
(
0
)
,
_sequenceNumber
(
0
)
,
_isCurrentItem
(
false
)
,
_isCurrentItem
(
false
)
,
_altDifference
(
0.0
)
,
_altPercent
(
0.0
)
,
_azimuth
(
0.0
)
,
_distance
(
0.0
)
,
_homePositionSpecialCase
(
false
)
,
_showHomePosition
(
false
)
,
_altitudeRelativeToHomeFact
(
0
,
"Altitude is relative to home"
,
FactMetaData
::
valueTypeUint32
)
,
_commandFact
(
0
,
""
,
FactMetaData
::
valueTypeUint32
)
,
_commandFact
(
0
,
""
,
FactMetaData
::
valueTypeUint32
)
,
_frameFact
(
0
,
""
,
FactMetaData
::
valueTypeUint32
)
,
_frameFact
(
0
,
""
,
FactMetaData
::
valueTypeUint32
)
,
_param1Fact
(
0
,
"Param1:"
,
FactMetaData
::
valueTypeDouble
)
,
_param1Fact
(
0
,
"Param1:"
,
FactMetaData
::
valueTypeDouble
)
...
@@ -211,45 +118,21 @@ MissionItem::MissionItem(const MissionItem& other, QObject* parent)
...
@@ -211,45 +118,21 @@ MissionItem::MissionItem(const MissionItem& other, QObject* parent)
,
_param5Fact
(
0
,
"Lat/X:"
,
FactMetaData
::
valueTypeDouble
)
,
_param5Fact
(
0
,
"Lat/X:"
,
FactMetaData
::
valueTypeDouble
)
,
_param6Fact
(
0
,
"Lon/Y:"
,
FactMetaData
::
valueTypeDouble
)
,
_param6Fact
(
0
,
"Lon/Y:"
,
FactMetaData
::
valueTypeDouble
)
,
_param7Fact
(
0
,
"Alt/Z:"
,
FactMetaData
::
valueTypeDouble
)
,
_param7Fact
(
0
,
"Alt/Z:"
,
FactMetaData
::
valueTypeDouble
)
,
_supportedCommandFact
(
0
,
"Command:"
,
FactMetaData
::
valueTypeUint32
)
,
_param1MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param2MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param3MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param4MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_syncingAltitudeRelativeToHomeAndFrame
(
false
)
,
_syncingHeadingDegreesAndParam4
(
false
)
,
_missionCommands
(
qgcApp
()
->
toolbox
()
->
missionCommands
())
{
{
// Need a good command and frame before we start passing signals around
// Need a good command and frame before we start passing signals around
_commandFact
.
setRawValue
(
MAV_CMD_NAV_WAYPOINT
);
_commandFact
.
setRawValue
(
MAV_CMD_NAV_WAYPOINT
);
_frameFact
.
setRawValue
(
MAV_FRAME_GLOBAL_RELATIVE_ALT
);
_frameFact
.
setRawValue
(
MAV_FRAME_GLOBAL_RELATIVE_ALT
);
_altitudeRelativeToHomeFact
.
setRawValue
(
true
);
_setupMetaData
();
_connectSignals
();
*
this
=
other
;
*
this
=
other
;
}
}
const
MissionItem
&
MissionItem
::
operator
=
(
const
MissionItem
&
other
)
const
MissionItem
&
MissionItem
::
operator
=
(
const
MissionItem
&
other
)
{
{
_vehicle
=
other
.
_vehicle
;
setCommand
(
other
.
command
());
setCommand
(
other
.
command
());
setFrame
(
other
.
frame
());
setFrame
(
other
.
frame
());
setRawEdit
(
other
.
_rawEdit
);
setDirty
(
other
.
_dirty
);
setSequenceNumber
(
other
.
_sequenceNumber
);
setSequenceNumber
(
other
.
_sequenceNumber
);
setAutoContinue
(
other
.
autoContinue
());
setAutoContinue
(
other
.
autoContinue
());
setIsCurrentItem
(
other
.
_isCurrentItem
);
setIsCurrentItem
(
other
.
_isCurrentItem
);
setAltDifference
(
other
.
_altDifference
);
setAltPercent
(
other
.
_altPercent
);
setAzimuth
(
other
.
_azimuth
);
setDistance
(
other
.
_distance
);
setHomePositionSpecialCase
(
other
.
_homePositionSpecialCase
);
setShowHomePosition
(
other
.
_showHomePosition
);
_syncFrameToAltitudeRelativeToHome
();
_param1Fact
.
setRawValue
(
other
.
_param1Fact
.
rawValue
());
_param1Fact
.
setRawValue
(
other
.
_param1Fact
.
rawValue
());
_param2Fact
.
setRawValue
(
other
.
_param2Fact
.
rawValue
());
_param2Fact
.
setRawValue
(
other
.
_param2Fact
.
rawValue
());
...
@@ -261,96 +144,6 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
...
@@ -261,96 +144,6 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
return
*
this
;
return
*
this
;
}
}
void
MissionItem
::
_connectSignals
(
void
)
{
// Connect to change signals to track dirty state
connect
(
&
_param1Fact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_setDirtyFromSignal
);
connect
(
&
_param2Fact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_setDirtyFromSignal
);
connect
(
&
_param3Fact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_setDirtyFromSignal
);
connect
(
&
_param4Fact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_setDirtyFromSignal
);
connect
(
&
_param5Fact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_setDirtyFromSignal
);
connect
(
&
_param6Fact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_setDirtyFromSignal
);
connect
(
&
_param7Fact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_setDirtyFromSignal
);
connect
(
&
_frameFact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_setDirtyFromSignal
);
connect
(
&
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_setDirtyFromSignal
);
connect
(
this
,
&
MissionItem
::
sequenceNumberChanged
,
this
,
&
MissionItem
::
_setDirtyFromSignal
);
// Values from these facts must propogate back and forth between the real object storage
connect
(
&
_altitudeRelativeToHomeFact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_syncAltitudeRelativeToHomeToFrame
);
connect
(
this
,
&
MissionItem
::
frameChanged
,
this
,
&
MissionItem
::
_syncFrameToAltitudeRelativeToHome
);
// These are parameter coordinates, they must emit coordinateChanged signal
connect
(
&
_param5Fact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_sendCoordinateChanged
);
connect
(
&
_param6Fact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_sendCoordinateChanged
);
connect
(
&
_param7Fact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_sendCoordinateChanged
);
// The following changes may also change friendlyEditAllowed
connect
(
&
_autoContinueFact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_sendFriendlyEditAllowedChanged
);
connect
(
&
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_sendFriendlyEditAllowedChanged
);
connect
(
&
_frameFact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_sendFriendlyEditAllowedChanged
);
// When the command changes we need to set defaults. This must go out before the signals below so it must be registered first.
connect
(
&
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
setDefaultsForCommand
);
// Whenever these properties change the ui model changes as well
connect
(
this
,
&
MissionItem
::
commandChanged
,
this
,
&
MissionItem
::
_sendUiModelChanged
);
connect
(
this
,
&
MissionItem
::
rawEditChanged
,
this
,
&
MissionItem
::
_sendUiModelChanged
);
// These fact signals must alway signal out through MissionItem signals
connect
(
&
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_sendCommandChanged
);
connect
(
&
_frameFact
,
&
Fact
::
valueChanged
,
this
,
&
MissionItem
::
_sendFrameChanged
);
}
void
MissionItem
::
_setupMetaData
(
void
)
{
QStringList
enumStrings
;
QVariantList
enumValues
;
if
(
!
_altitudeMetaData
)
{
_altitudeMetaData
=
new
FactMetaData
(
FactMetaData
::
valueTypeDouble
);
_altitudeMetaData
->
setRawUnits
(
"meters"
);
_altitudeMetaData
->
setDecimalPlaces
(
2
);
enumStrings
.
clear
();
enumValues
.
clear
();
foreach
(
const
MAV_CMD
command
,
_missionCommands
->
commandsIds
())
{
const
MavCmdInfo
*
mavCmdInfo
=
_missionCommands
->
getMavCmdInfo
(
command
,
_vehicle
);
enumStrings
.
append
(
mavCmdInfo
->
rawName
());
enumValues
.
append
(
QVariant
(
mavCmdInfo
->
command
()));
}
_commandMetaData
=
new
FactMetaData
(
FactMetaData
::
valueTypeUint32
);
_commandMetaData
->
setEnumInfo
(
enumStrings
,
enumValues
);
_defaultParamMetaData
=
new
FactMetaData
(
FactMetaData
::
valueTypeDouble
);
_defaultParamMetaData
->
setDecimalPlaces
(
7
);
enumStrings
.
clear
();
enumValues
.
clear
();
for
(
size_t
i
=
0
;
i
<
sizeof
(
_rgMavFrameInfo
)
/
sizeof
(
_rgMavFrameInfo
[
0
]);
i
++
)
{
const
struct
EnumInfo_s
*
mavFrameInfo
=
&
_rgMavFrameInfo
[
i
];
enumStrings
.
append
(
mavFrameInfo
->
label
);
enumValues
.
append
(
QVariant
(
mavFrameInfo
->
frame
));
}
_frameMetaData
=
new
FactMetaData
(
FactMetaData
::
valueTypeUint32
);
_frameMetaData
->
setEnumInfo
(
enumStrings
,
enumValues
);
_latitudeMetaData
=
new
FactMetaData
(
FactMetaData
::
valueTypeDouble
);
_latitudeMetaData
->
setRawUnits
(
"deg"
);
_latitudeMetaData
->
setDecimalPlaces
(
7
);
_longitudeMetaData
=
new
FactMetaData
(
FactMetaData
::
valueTypeDouble
);
_longitudeMetaData
->
setRawUnits
(
"deg"
);
_longitudeMetaData
->
setDecimalPlaces
(
7
);
}
_commandFact
.
setMetaData
(
_commandMetaData
);
_frameFact
.
setMetaData
(
_frameMetaData
);
}
MissionItem
::~
MissionItem
()
MissionItem
::~
MissionItem
()
{
{
}
}
...
@@ -414,7 +207,9 @@ bool MissionItem::load(const QJsonObject& json, QString& errorString)
...
@@ -414,7 +207,9 @@ bool MissionItem::load(const QJsonObject& json, QString& errorString)
if
(
!
JsonHelper
::
toQGeoCoordinate
(
json
[
_jsonCoordinateKey
],
coordinate
,
true
/* altitudeRequired */
,
errorString
))
{
if
(
!
JsonHelper
::
toQGeoCoordinate
(
json
[
_jsonCoordinateKey
],
coordinate
,
true
/* altitudeRequired */
,
errorString
))
{
return
false
;
return
false
;
}
}
setCoordinate
(
coordinate
);
setParam5
(
coordinate
.
latitude
());
setParam6
(
coordinate
.
longitude
());
setParam7
(
coordinate
.
altitude
());
setIsCurrentItem
(
false
);
setIsCurrentItem
(
false
);
setSequenceNumber
(
json
[
_jsonIdKey
].
toInt
());
setSequenceNumber
(
json
[
_jsonIdKey
].
toInt
());
...
@@ -432,30 +227,23 @@ bool MissionItem::load(const QJsonObject& json, QString& errorString)
...
@@ -432,30 +227,23 @@ bool MissionItem::load(const QJsonObject& json, QString& errorString)
void
MissionItem
::
setSequenceNumber
(
int
sequenceNumber
)
void
MissionItem
::
setSequenceNumber
(
int
sequenceNumber
)
{
{
_sequenceNumber
=
sequenceNumber
;
if
(
_sequenceNumber
!=
sequenceNumber
)
{
emit
sequenceNumberChanged
(
_sequenceNumber
);
_sequenceNumber
=
sequenceNumber
;
emit
sequenceNumberChanged
(
_sequenceNumber
);
}
}
}
void
MissionItem
::
setCommand
(
MAV_CMD
command
)
void
MissionItem
::
setCommand
(
MAV_CMD
command
)
{
{
if
((
MAV_CMD
)
this
->
command
()
!=
command
)
{
if
((
MAV_CMD
)
this
->
command
()
!=
command
)
{
_commandFact
.
setRawValue
(
command
);
_commandFact
.
setRawValue
(
command
);
setDefaultsForCommand
();
emit
commandChanged
(
this
->
command
());
}
}
}
}
void
MissionItem
::
setCommand
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
)
{
setCommand
((
MAV_CMD
)
command
);
}
void
MissionItem
::
setFrame
(
MAV_FRAME
frame
)
void
MissionItem
::
setFrame
(
MAV_FRAME
frame
)
{
{
if
(
this
->
frame
()
!=
frame
)
{
if
(
this
->
frame
()
!=
frame
)
{
_frameFact
.
setRawValue
(
frame
);
_frameFact
.
setRawValue
(
frame
);
frameChanged
(
frame
);
}
}
}
}
...
@@ -523,175 +311,6 @@ void MissionItem::setParam7(double param)
...
@@ -523,175 +311,6 @@ void MissionItem::setParam7(double param)
}
}
}
}
bool
MissionItem
::
standaloneCoordinate
(
void
)
const
{
if
(
_missionCommands
->
contains
((
MAV_CMD
)
command
()))
{
return
_missionCommands
->
getMavCmdInfo
((
MAV_CMD
)
command
(),
_vehicle
)
->
standaloneCoordinate
();
}
else
{
return
false
;
}
}
bool
MissionItem
::
specifiesCoordinate
(
void
)
const
{
if
(
_missionCommands
->
contains
((
MAV_CMD
)
command
()))
{
return
_missionCommands
->
getMavCmdInfo
((
MAV_CMD
)
command
(),
_vehicle
)
->
specifiesCoordinate
();
}
else
{
return
false
;
}
}
QString
MissionItem
::
commandDescription
(
void
)
const
{
if
(
_missionCommands
->
contains
((
MAV_CMD
)
command
()))
{
return
_missionCommands
->
getMavCmdInfo
((
MAV_CMD
)
command
(),
_vehicle
)
->
description
();
}
else
{
qWarning
()
<<
"Should not ask for command description on unknown command"
;
return
QString
();
}
}
void
MissionItem
::
_clearParamMetaData
(
void
)
{
_param1MetaData
.
setRawUnits
(
""
);
_param1MetaData
.
setDecimalPlaces
(
FactMetaData
::
defaultDecimalPlaces
);
_param2MetaData
.
setRawUnits
(
""
);
_param2MetaData
.
setDecimalPlaces
(
FactMetaData
::
defaultDecimalPlaces
);
_param3MetaData
.
setRawUnits
(
""
);
_param3MetaData
.
setDecimalPlaces
(
FactMetaData
::
defaultDecimalPlaces
);
_param4MetaData
.
setRawUnits
(
""
);
_param4MetaData
.
setDecimalPlaces
(
FactMetaData
::
defaultDecimalPlaces
);
}
QmlObjectListModel
*
MissionItem
::
textFieldFacts
(
void
)
{
QmlObjectListModel
*
model
=
new
QmlObjectListModel
(
this
);
if
(
rawEdit
())
{
_param1Fact
.
_setName
(
"Param1:"
);
_param1Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_param1Fact
);
_param2Fact
.
_setName
(
"Param2:"
);
_param2Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_param2Fact
);
_param3Fact
.
_setName
(
"Param3:"
);
_param3Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_param3Fact
);
_param4Fact
.
_setName
(
"Param4:"
);
_param4Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_param4Fact
);
_param5Fact
.
_setName
(
"Lat/X:"
);
_param5Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_param5Fact
);
_param6Fact
.
_setName
(
"Lon/Y:"
);
_param6Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_param6Fact
);
_param7Fact
.
_setName
(
"Alt/Z:"
);
_param7Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_param7Fact
);
}
else
{
_clearParamMetaData
();
MAV_CMD
command
;
if
(
_homePositionSpecialCase
)
{
command
=
MAV_CMD_NAV_LAST
;
}
else
{
command
=
(
MAV_CMD
)
this
->
command
();
}
Fact
*
rgParamFacts
[
7
]
=
{
&
_param1Fact
,
&
_param2Fact
,
&
_param3Fact
,
&
_param4Fact
,
&
_param5Fact
,
&
_param6Fact
,
&
_param7Fact
};
FactMetaData
*
rgParamMetaData
[
7
]
=
{
&
_param1MetaData
,
&
_param2MetaData
,
&
_param3MetaData
,
&
_param4MetaData
,
&
_param5MetaData
,
&
_param6MetaData
,
&
_param7MetaData
};
bool
altitudeAdded
=
false
;
for
(
int
i
=
1
;
i
<=
7
;
i
++
)
{
const
QMap
<
int
,
MavCmdParamInfo
*>&
paramInfoMap
=
_missionCommands
->
getMavCmdInfo
(
command
,
_vehicle
)
->
paramInfoMap
();
if
(
paramInfoMap
.
contains
(
i
)
&&
paramInfoMap
[
i
]
->
enumStrings
().
count
()
==
0
)
{
Fact
*
paramFact
=
rgParamFacts
[
i
-
1
];
FactMetaData
*
paramMetaData
=
rgParamMetaData
[
i
-
1
];
MavCmdParamInfo
*
paramInfo
=
paramInfoMap
[
i
];
paramFact
->
_setName
(
paramInfo
->
label
());
paramMetaData
->
setDecimalPlaces
(
paramInfo
->
decimalPlaces
());
paramMetaData
->
setEnumInfo
(
paramInfo
->
enumStrings
(),
paramInfo
->
enumValues
());
paramMetaData
->
setRawUnits
(
paramInfo
->
units
());
paramFact
->
setMetaData
(
paramMetaData
);
model
->
append
(
paramFact
);
if
(
i
==
7
)
{
altitudeAdded
=
true
;
}
}
}
if
(
specifiesCoordinate
()
&&
!
altitudeAdded
)
{
_param7Fact
.
_setName
(
"Altitude:"
);
_param7Fact
.
setMetaData
(
_altitudeMetaData
);
model
->
append
(
&
_param7Fact
);
}
}
return
model
;
}
QmlObjectListModel
*
MissionItem
::
checkboxFacts
(
void
)
{
QmlObjectListModel
*
model
=
new
QmlObjectListModel
(
this
);
if
(
rawEdit
())
{
model
->
append
(
&
_autoContinueFact
);
}
else
if
(
specifiesCoordinate
()
&&
!
_homePositionSpecialCase
)
{
model
->
append
(
&
_altitudeRelativeToHomeFact
);
}
return
model
;
}
QmlObjectListModel
*
MissionItem
::
comboboxFacts
(
void
)
{
QmlObjectListModel
*
model
=
new
QmlObjectListModel
(
this
);
if
(
rawEdit
())
{
model
->
append
(
&
_commandFact
);
model
->
append
(
&
_frameFact
);
}
else
{
Fact
*
rgParamFacts
[
7
]
=
{
&
_param1Fact
,
&
_param2Fact
,
&
_param3Fact
,
&
_param4Fact
,
&
_param5Fact
,
&
_param6Fact
,
&
_param7Fact
};
FactMetaData
*
rgParamMetaData
[
7
]
=
{
&
_param1MetaData
,
&
_param2MetaData
,
&
_param3MetaData
,
&
_param4MetaData
,
&
_param5MetaData
,
&
_param6MetaData
,
&
_param7MetaData
};
MAV_CMD
command
;
if
(
_homePositionSpecialCase
)
{
command
=
MAV_CMD_NAV_LAST
;
}
else
{
command
=
(
MAV_CMD
)
this
->
command
();
}
for
(
int
i
=
1
;
i
<=
7
;
i
++
)
{
const
QMap
<
int
,
MavCmdParamInfo
*>&
paramInfoMap
=
_missionCommands
->
getMavCmdInfo
(
command
,
_vehicle
)
->
paramInfoMap
();
if
(
paramInfoMap
.
contains
(
i
)
&&
paramInfoMap
[
i
]
->
enumStrings
().
count
()
!=
0
)
{
Fact
*
paramFact
=
rgParamFacts
[
i
-
1
];
FactMetaData
*
paramMetaData
=
rgParamMetaData
[
i
-
1
];
MavCmdParamInfo
*
paramInfo
=
paramInfoMap
[
i
];
paramFact
->
_setName
(
paramInfo
->
label
());
paramMetaData
->
setDecimalPlaces
(
paramInfo
->
decimalPlaces
());
paramMetaData
->
setEnumInfo
(
paramInfo
->
enumStrings
(),
paramInfo
->
enumValues
());
paramMetaData
->
setRawUnits
(
paramInfo
->
units
());
paramFact
->
setMetaData
(
paramMetaData
);
model
->
append
(
paramFact
);
}
}
}
return
model
;
}
QGeoCoordinate
MissionItem
::
coordinate
(
void
)
const
{
return
QGeoCoordinate
(
_param5Fact
.
rawValue
().
toDouble
(),
_param6Fact
.
rawValue
().
toDouble
(),
_param7Fact
.
rawValue
().
toDouble
());
}
void
MissionItem
::
setCoordinate
(
const
QGeoCoordinate
&
coordinate
)
void
MissionItem
::
setCoordinate
(
const
QGeoCoordinate
&
coordinate
)
{
{
setParam5
(
coordinate
.
latitude
());
setParam5
(
coordinate
.
latitude
());
...
@@ -699,166 +318,7 @@ void MissionItem::setCoordinate(const QGeoCoordinate& coordinate)
...
@@ -699,166 +318,7 @@ void MissionItem::setCoordinate(const QGeoCoordinate& coordinate)
setParam7
(
coordinate
.
altitude
());
setParam7
(
coordinate
.
altitude
());
}
}
bool
MissionItem
::
friendlyEditAllowed
(
void
)
const
QGeoCoordinate
MissionItem
::
coordinate
(
void
)
const
{
if
(
_missionCommands
->
contains
((
MAV_CMD
)
command
())
&&
_missionCommands
->
getMavCmdInfo
((
MAV_CMD
)
command
(),
_vehicle
)
->
friendlyEdit
())
{
if
(
!
autoContinue
())
{
return
false
;
}
if
(
specifiesCoordinate
())
{
return
frame
()
==
MAV_FRAME_GLOBAL
||
frame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
;
}
return
true
;
}
return
false
;
}
bool
MissionItem
::
rawEdit
(
void
)
const
{
return
_rawEdit
||
!
friendlyEditAllowed
();
}
void
MissionItem
::
setRawEdit
(
bool
rawEdit
)
{
if
(
this
->
rawEdit
()
!=
rawEdit
)
{
_rawEdit
=
rawEdit
;
emit
rawEditChanged
(
this
->
rawEdit
());
}
}
void
MissionItem
::
setDirty
(
bool
dirty
)
{
if
(
!
_homePositionSpecialCase
||
!
dirty
)
{
// Home position never affects dirty bit
_dirty
=
dirty
;
// We want to emit dirtyChanged even if _dirty didn't change. This can be handy signal for
// any value within the item changing.
emit
dirtyChanged
(
_dirty
);
}
}
void
MissionItem
::
_setDirtyFromSignal
(
void
)
{
setDirty
(
true
);
}
void
MissionItem
::
setDistance
(
double
distance
)
{
_distance
=
distance
;
emit
distanceChanged
(
_distance
);
}
void
MissionItem
::
setAltDifference
(
double
altDifference
)
{
_altDifference
=
altDifference
;
emit
altDifferenceChanged
(
_altDifference
);
}
void
MissionItem
::
setAltPercent
(
double
altPercent
)
{
_altPercent
=
altPercent
;
emit
altPercentChanged
(
_altPercent
);
}
void
MissionItem
::
setAzimuth
(
double
azimuth
)
{
_azimuth
=
azimuth
;
emit
azimuthChanged
(
_azimuth
);
}
void
MissionItem
::
_sendCoordinateChanged
(
void
)
{
emit
coordinateChanged
(
coordinate
());
}
void
MissionItem
::
_syncAltitudeRelativeToHomeToFrame
(
const
QVariant
&
value
)
{
if
(
!
_syncingAltitudeRelativeToHomeAndFrame
)
{
_syncingAltitudeRelativeToHomeAndFrame
=
true
;
setFrame
(
value
.
toBool
()
?
MAV_FRAME_GLOBAL_RELATIVE_ALT
:
MAV_FRAME_GLOBAL
);
_syncingAltitudeRelativeToHomeAndFrame
=
false
;
}
}
void
MissionItem
::
_syncFrameToAltitudeRelativeToHome
(
void
)
{
if
(
!
_syncingAltitudeRelativeToHomeAndFrame
)
{
_syncingAltitudeRelativeToHomeAndFrame
=
true
;
_altitudeRelativeToHomeFact
.
setRawValue
(
relativeAltitude
());
_syncingAltitudeRelativeToHomeAndFrame
=
false
;
}
}
void
MissionItem
::
setDefaultsForCommand
(
void
)
{
// We set these global defaults first, then if there are param defaults they will get reset
setParam7
(
defaultAltitude
);
MAV_CMD
command
=
(
MAV_CMD
)
this
->
command
();
if
(
_missionCommands
->
contains
(
command
))
{
MavCmdInfo
*
mavCmdInfo
=
_missionCommands
->
getMavCmdInfo
(
command
,
_vehicle
);
foreach
(
const
MavCmdParamInfo
*
paramInfo
,
mavCmdInfo
->
paramInfoMap
())
{
Fact
*
rgParamFacts
[
7
]
=
{
&
_param1Fact
,
&
_param2Fact
,
&
_param3Fact
,
&
_param4Fact
,
&
_param5Fact
,
&
_param6Fact
,
&
_param7Fact
};
rgParamFacts
[
paramInfo
->
param
()
-
1
]
->
setRawValue
(
paramInfo
->
defaultValue
());
}
}
if
(
command
==
MAV_CMD_NAV_WAYPOINT
)
{
// We default all acceptance radius to 0. This allows flight controller to be in control of
// accept radius.
setParam2
(
0
);
}
setAutoContinue
(
true
);
setFrame
(
specifiesCoordinate
()
?
MAV_FRAME_GLOBAL_RELATIVE_ALT
:
MAV_FRAME_MISSION
);
setRawEdit
(
false
);
}
void
MissionItem
::
_sendUiModelChanged
(
void
)
{
emit
uiModelChanged
();
}
void
MissionItem
::
_sendFrameChanged
(
void
)
{
emit
frameChanged
(
frame
());
}
void
MissionItem
::
_sendCommandChanged
(
void
)
{
emit
commandChanged
(
command
());
}
QString
MissionItem
::
commandName
(
void
)
const
{
MAV_CMD
command
=
(
MAV_CMD
)
this
->
command
();
if
(
_missionCommands
->
contains
(
command
))
{
const
MavCmdInfo
*
mavCmdInfo
=
_missionCommands
->
getMavCmdInfo
(
command
,
_vehicle
);
return
mavCmdInfo
->
friendlyName
().
isEmpty
()
?
mavCmdInfo
->
rawName
()
:
mavCmdInfo
->
friendlyName
();
}
else
{
return
QString
(
"Unknown: %1"
).
arg
(
command
);
}
}
void
MissionItem
::
_sendFriendlyEditAllowedChanged
(
void
)
{
emit
friendlyEditAllowedChanged
(
friendlyEditAllowed
());
}
QString
MissionItem
::
category
(
void
)
const
{
return
qgcApp
()
->
toolbox
()
->
missionCommands
()
->
categoryFromCommand
(
command
());
}
void
MissionItem
::
setShowHomePosition
(
bool
showHomePosition
)
{
{
if
(
showHomePosition
!=
_showHomePosition
)
{
return
QGeoCoordinate
(
param5
(),
param6
(),
param7
());
_showHomePosition
=
showHomePosition
;
emit
showHomePositionChanged
(
_showHomePosition
);
}
}
}
src/MissionManager/MissionItem.h
View file @
36d57743
...
@@ -40,16 +40,22 @@
...
@@ -40,16 +40,22 @@
#include "QmlObjectListModel.h"
#include "QmlObjectListModel.h"
#include "MissionCommands.h"
#include "MissionCommands.h"
// Abstract base class for Simple and Complex MissionItem obejcts.
class
ComplexMissionItem
;
class
SimpleMissionItem
;
class
MissionController
;
#ifdef UNITTEST_BUILD
class
MissionItemTest
;
#endif
// Represents a Mavlink mission command.
class
MissionItem
:
public
QObject
class
MissionItem
:
public
QObject
{
{
Q_OBJECT
Q_OBJECT
public:
public:
MissionItem
(
Vehicle
*
vehicle
,
QObject
*
parent
=
NULL
);
MissionItem
(
QObject
*
parent
=
NULL
);
MissionItem
(
Vehicle
*
vehicle
,
MissionItem
(
int
sequenceNumber
,
int
sequenceNumber
,
MAV_CMD
command
,
MAV_CMD
command
,
MAV_FRAME
frame
,
MAV_FRAME
frame
,
double
param1
,
double
param1
,
...
@@ -69,179 +75,48 @@ public:
...
@@ -69,179 +75,48 @@ public:
const
MissionItem
&
operator
=
(
const
MissionItem
&
other
);
const
MissionItem
&
operator
=
(
const
MissionItem
&
other
);
Q_PROPERTY
(
double
altDifference
READ
altDifference
WRITE
setAltDifference
NOTIFY
altDifferenceChanged
)
///< Change in altitude from previous waypoint
MAV_CMD
command
(
void
)
const
{
return
(
MAV_CMD
)
_commandFact
.
rawValue
().
toInt
();
}
Q_PROPERTY
(
double
altPercent
READ
altPercent
WRITE
setAltPercent
NOTIFY
altPercentChanged
)
///< Percent of total altitude change in mission altitude
bool
isCurrentItem
(
void
)
const
{
return
_isCurrentItem
;
}
Q_PROPERTY
(
double
azimuth
READ
azimuth
WRITE
setAzimuth
NOTIFY
azimuthChanged
)
///< Azimuth to previous waypoint
int
sequenceNumber
(
void
)
const
{
return
_sequenceNumber
;
}
Q_PROPERTY
(
QString
category
READ
category
NOTIFY
commandChanged
)
MAV_FRAME
frame
(
void
)
const
{
return
(
MAV_FRAME
)
_frameFact
.
rawValue
().
toInt
();
}
Q_PROPERTY
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
READ
command
WRITE
setCommand
NOTIFY
commandChanged
)
bool
autoContinue
(
void
)
const
{
return
_autoContinueFact
.
rawValue
().
toBool
();
}
Q_PROPERTY
(
QString
commandDescription
READ
commandDescription
NOTIFY
commandChanged
)
double
param1
(
void
)
const
{
return
_param1Fact
.
rawValue
().
toDouble
();
}
Q_PROPERTY
(
QString
commandName
READ
commandName
NOTIFY
commandChanged
)
double
param2
(
void
)
const
{
return
_param2Fact
.
rawValue
().
toDouble
();
}
Q_PROPERTY
(
bool
dirty
READ
dirty
WRITE
setDirty
NOTIFY
dirtyChanged
)
double
param3
(
void
)
const
{
return
_param3Fact
.
rawValue
().
toDouble
();
}
Q_PROPERTY
(
double
distance
READ
distance
WRITE
setDistance
NOTIFY
distanceChanged
)
///< Distance to previous waypoint
double
param4
(
void
)
const
{
return
_param4Fact
.
rawValue
().
toDouble
();
}
Q_PROPERTY
(
bool
friendlyEditAllowed
READ
friendlyEditAllowed
NOTIFY
friendlyEditAllowedChanged
)
double
param5
(
void
)
const
{
return
_param5Fact
.
rawValue
().
toDouble
();
}
Q_PROPERTY
(
bool
homePosition
READ
homePosition
CONSTANT
)
///< true: This item is being used as a home position indicator
double
param6
(
void
)
const
{
return
_param6Fact
.
rawValue
().
toDouble
();
}
Q_PROPERTY
(
bool
isCurrentItem
READ
isCurrentItem
WRITE
setIsCurrentItem
NOTIFY
isCurrentItemChanged
)
double
param7
(
void
)
const
{
return
_param7Fact
.
rawValue
().
toDouble
();
}
Q_PROPERTY
(
bool
rawEdit
READ
rawEdit
WRITE
setRawEdit
NOTIFY
rawEditChanged
)
///< true: raw item editing with all params
QGeoCoordinate
coordinate
(
void
)
const
;
Q_PROPERTY
(
bool
relativeAltitude
READ
relativeAltitude
NOTIFY
frameChanged
)
Q_PROPERTY
(
int
sequenceNumber
READ
sequenceNumber
WRITE
setSequenceNumber
NOTIFY
sequenceNumberChanged
)
void
setCommand
(
MAV_CMD
command
);
Q_PROPERTY
(
bool
standaloneCoordinate
READ
standaloneCoordinate
NOTIFY
commandChanged
)
void
setSequenceNumber
(
int
sequenceNumber
);
Q_PROPERTY
(
bool
specifiesCoordinate
READ
specifiesCoordinate
NOTIFY
commandChanged
)
void
setIsCurrentItem
(
bool
isCurrentItem
);
Q_PROPERTY
(
bool
showHomePosition
READ
showHomePosition
WRITE
setShowHomePosition
NOTIFY
showHomePositionChanged
)
void
setFrame
(
MAV_FRAME
frame
);
void
setAutoContinue
(
bool
autoContinue
);
// Mission item has two coordinates associated with them:
void
setParam1
(
double
param1
);
// coordinate: This is the entry point for a waypoint line into the item. For a simple item it is also the location of the item
void
setParam2
(
double
param2
);
// exitCoordinate This is the exit point for a waypoint line coming out of the item. For a SimpleMissionItem this will be the same as
void
setParam3
(
double
param3
);
// coordinate. For a ComplexMissionItem it may be different than the entry coordinate.
void
setParam4
(
double
param4
);
Q_PROPERTY
(
QGeoCoordinate
coordinate
READ
coordinate
WRITE
setCoordinate
NOTIFY
coordinateChanged
)
void
setParam5
(
double
param5
);
Q_PROPERTY
(
QGeoCoordinate
exitCoordinate
READ
exitCoordinate
NOTIFY
exitCoordinateChanged
)
void
setParam6
(
double
param6
);
void
setParam7
(
double
param7
);
/// @return true: SimpleMissionItem, false: ComplexMissionItem
void
setCoordinate
(
const
QGeoCoordinate
&
coordinate
);
Q_PROPERTY
(
bool
simpleItem
READ
simpleItem
NOTIFY
simpleItemChanged
)
// These properties are used to display the editing ui
Q_PROPERTY
(
QmlObjectListModel
*
checkboxFacts
READ
checkboxFacts
NOTIFY
uiModelChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
comboboxFacts
READ
comboboxFacts
NOTIFY
uiModelChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
textFieldFacts
READ
textFieldFacts
NOTIFY
uiModelChanged
)
/// List of child mission items. Child mission item are subsequent mision items which do not specify a coordinate. They
/// are shown next to the part item in the ui.
Q_PROPERTY
(
QmlObjectListModel
*
childItems
READ
childItems
CONSTANT
)
// Property accesors
double
altDifference
(
void
)
const
{
return
_altDifference
;
}
double
altPercent
(
void
)
const
{
return
_altPercent
;
}
double
azimuth
(
void
)
const
{
return
_azimuth
;
}
QString
category
(
void
)
const
;
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
(
void
)
const
{
return
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
)
_commandFact
.
cookedValue
().
toInt
();
};
QString
commandDescription
(
void
)
const
;
QString
commandName
(
void
)
const
;
QGeoCoordinate
coordinate
(
void
)
const
;
bool
dirty
(
void
)
const
{
return
_dirty
;
}
double
distance
(
void
)
const
{
return
_distance
;
}
bool
friendlyEditAllowed
(
void
)
const
;
bool
homePosition
(
void
)
const
{
return
_homePositionSpecialCase
;
}
bool
isCurrentItem
(
void
)
const
{
return
_isCurrentItem
;
}
bool
rawEdit
(
void
)
const
;
int
sequenceNumber
(
void
)
const
{
return
_sequenceNumber
;
}
bool
standaloneCoordinate
(
void
)
const
;
bool
specifiesCoordinate
(
void
)
const
;
bool
showHomePosition
(
void
)
const
{
return
_showHomePosition
;
}
QmlObjectListModel
*
textFieldFacts
(
void
);
QmlObjectListModel
*
checkboxFacts
(
void
);
QmlObjectListModel
*
comboboxFacts
(
void
);
QmlObjectListModel
*
childItems
(
void
)
{
return
&
_childItems
;
}
void
setRawEdit
(
bool
rawEdit
);
void
setDirty
(
bool
dirty
);
void
setSequenceNumber
(
int
sequenceNumber
);
void
setIsCurrentItem
(
bool
isCurrentItem
);
void
setCoordinate
(
const
QGeoCoordinate
&
coordinate
);
void
setCommandByIndex
(
int
index
);
void
setCommand
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
);
void
setHomePositionSpecialCase
(
bool
homePositionSpecialCase
)
{
_homePositionSpecialCase
=
homePositionSpecialCase
;
}
void
setShowHomePosition
(
bool
showHomePosition
);
void
setAltDifference
(
double
altDifference
);
void
setAltPercent
(
double
altPercent
);
void
setAzimuth
(
double
azimuth
);
void
setDistance
(
double
distance
);
// C++ only methods
MAV_FRAME
frame
(
void
)
const
{
return
(
MAV_FRAME
)
_frameFact
.
rawValue
().
toInt
();
}
bool
autoContinue
(
void
)
const
{
return
_autoContinueFact
.
rawValue
().
toBool
();
}
double
param1
(
void
)
const
{
return
_param1Fact
.
rawValue
().
toDouble
();
}
double
param2
(
void
)
const
{
return
_param2Fact
.
rawValue
().
toDouble
();
}
double
param3
(
void
)
const
{
return
_param3Fact
.
rawValue
().
toDouble
();
}
double
param4
(
void
)
const
{
return
_param4Fact
.
rawValue
().
toDouble
();
}
double
param5
(
void
)
const
{
return
_param5Fact
.
rawValue
().
toDouble
();
}
double
param6
(
void
)
const
{
return
_param6Fact
.
rawValue
().
toDouble
();
}
double
param7
(
void
)
const
{
return
_param7Fact
.
rawValue
().
toDouble
();
}
void
setCommand
(
MAV_CMD
command
);
void
setFrame
(
MAV_FRAME
frame
);
void
setAutoContinue
(
bool
autoContinue
);
void
setParam1
(
double
param1
);
void
setParam2
(
double
param2
);
void
setParam3
(
double
param3
);
void
setParam4
(
double
param4
);
void
setParam5
(
double
param5
);
void
setParam6
(
double
param6
);
void
setParam7
(
double
param7
);
// C++ only methods
void
save
(
QJsonObject
&
json
);
void
save
(
QJsonObject
&
json
);
bool
load
(
QTextStream
&
loadStream
);
bool
load
(
QTextStream
&
loadStream
);
bool
load
(
const
QJsonObject
&
json
,
QString
&
errorString
);
bool
load
(
const
QJsonObject
&
json
,
QString
&
errorString
);
bool
relativeAltitude
(
void
)
{
return
frame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
;
}
bool
relativeAltitude
(
void
)
const
{
return
frame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
;
}
static
const
double
defaultAltitude
;
// Pure virtuals which must be provides by derived classes
virtual
bool
simpleItem
(
void
)
const
=
0
;
virtual
QGeoCoordinate
exitCoordinate
(
void
)
const
=
0
;
public
slots
:
void
setDefaultsForCommand
(
void
);
signals:
signals:
void
altDifferenceChanged
(
double
altDifference
);
void
altPercentChanged
(
double
altPercent
);
void
azimuthChanged
(
double
azimuth
);
void
commandChanged
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
);
void
coordinateChanged
(
const
QGeoCoordinate
&
coordinate
);
void
exitCoordinateChanged
(
const
QGeoCoordinate
&
exitCoordinate
);
void
dirtyChanged
(
bool
dirty
);
void
distanceChanged
(
double
distance
);
void
frameChanged
(
int
frame
);
void
friendlyEditAllowedChanged
(
bool
friendlyEditAllowed
);
void
headingDegreesChanged
(
double
heading
);
void
isCurrentItemChanged
(
bool
isCurrentItem
);
void
isCurrentItemChanged
(
bool
isCurrentItem
);
void
rawEditChanged
(
bool
rawEdit
);
void
sequenceNumberChanged
(
int
sequenceNumber
);
void
sequenceNumberChanged
(
int
sequenceNumber
);
void
uiModelChanged
(
void
);
void
showHomePositionChanged
(
bool
showHomePosition
);
void
simpleItemChanged
(
bool
simpleItem
);
private
slots
:
void
_setDirtyFromSignal
(
void
);
void
_sendCommandChanged
(
void
);
void
_sendCoordinateChanged
(
void
);
void
_sendFrameChanged
(
void
);
void
_sendFriendlyEditAllowedChanged
(
void
);
void
_sendUiModelChanged
(
void
);
void
_syncAltitudeRelativeToHomeToFrame
(
const
QVariant
&
value
);
void
_syncFrameToAltitudeRelativeToHome
(
void
);
private:
private:
void
_clearParamMetaData
(
void
);
void
_connectSignals
(
void
);
void
_setupMetaData
(
void
);
private:
Vehicle
*
_vehicle
;
///< Vehicle associated with this item, NULL for offline mode
bool
_rawEdit
;
bool
_dirty
;
int
_sequenceNumber
;
int
_sequenceNumber
;
bool
_isCurrentItem
;
bool
_isCurrentItem
;
double
_altDifference
;
///< Difference in altitude from previous waypoint
double
_altPercent
;
///< Percent of total altitude change in mission
double
_azimuth
;
///< Azimuth to previous waypoint
double
_distance
;
///< Distance to previous waypoint
bool
_homePositionSpecialCase
;
///< true: This item is being used as a ui home position indicator
bool
_showHomePosition
;
Fact
_altitudeRelativeToHomeFact
;
Fact
_autoContinueFact
;
Fact
_autoContinueFact
;
Fact
_commandFact
;
Fact
_commandFact
;
Fact
_frameFact
;
Fact
_frameFact
;
...
@@ -252,31 +127,8 @@ private:
...
@@ -252,31 +127,8 @@ private:
Fact
_param5Fact
;
Fact
_param5Fact
;
Fact
_param6Fact
;
Fact
_param6Fact
;
Fact
_param7Fact
;
Fact
_param7Fact
;
Fact
_supportedCommandFact
;
static
FactMetaData
*
_altitudeMetaData
;
// Keys for Json save
static
FactMetaData
*
_commandMetaData
;
static
FactMetaData
*
_defaultParamMetaData
;
static
FactMetaData
*
_frameMetaData
;
static
FactMetaData
*
_latitudeMetaData
;
static
FactMetaData
*
_longitudeMetaData
;
FactMetaData
_param1MetaData
;
FactMetaData
_param2MetaData
;
FactMetaData
_param3MetaData
;
FactMetaData
_param4MetaData
;
FactMetaData
_param5MetaData
;
FactMetaData
_param6MetaData
;
FactMetaData
_param7MetaData
;
/// This is used to reference any subsequent mission items which do not specify a coordinate.
QmlObjectListModel
_childItems
;
bool
_syncingAltitudeRelativeToHomeAndFrame
;
///< true: already in a sync signal, prevents signal loop
bool
_syncingHeadingDegreesAndParam4
;
///< true: already in a sync signal, prevents signal loop
const
MissionCommands
*
_missionCommands
;
static
const
char
*
_itemType
;
static
const
char
*
_itemType
;
static
const
char
*
_jsonTypeKey
;
static
const
char
*
_jsonTypeKey
;
static
const
char
*
_jsonIdKey
;
static
const
char
*
_jsonIdKey
;
...
@@ -288,6 +140,13 @@ private:
...
@@ -288,6 +140,13 @@ private:
static
const
char
*
_jsonParam4Key
;
static
const
char
*
_jsonParam4Key
;
static
const
char
*
_jsonAutoContinueKey
;
static
const
char
*
_jsonAutoContinueKey
;
static
const
char
*
_jsonCoordinateKey
;
static
const
char
*
_jsonCoordinateKey
;
friend
class
ComplexMissionItem
;
friend
class
SimpleMissionItem
;
friend
class
MissionController
;
#ifdef UNITTEST_BUILD
friend
class
MissionItemTest
;
#endif
};
};
#endif
#endif
src/MissionManager/MissionItemTest.cc
View file @
36d57743
...
@@ -22,171 +22,658 @@
...
@@ -22,171 +22,658 @@
======================================================================*/
======================================================================*/
#include "MissionItemTest.h"
#include "MissionItemTest.h"
#include "SimpleMissionItem.h"
#include "LinkManager.h"
#include "MultiVehicleManager.h"
const
MissionItemTest
::
ItemInfo_t
MissionItemTest
::
_rgItemInfo
[]
=
{
#include "MissionItem.h"
{
MAV_CMD_NAV_WAYPOINT
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
MAV_CMD_NAV_LOITER_UNLIM
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
MAV_CMD_NAV_LOITER_TURNS
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
MAV_CMD_NAV_LOITER_TIME
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
MAV_CMD_NAV_LAND
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
MAV_CMD_NAV_TAKEOFF
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
MAV_CMD_CONDITION_DELAY
,
MAV_FRAME_MISSION
},
{
MAV_CMD_DO_JUMP
,
MAV_FRAME_MISSION
},
};
const
MissionItemTest
::
FactValue_t
MissionItemTest
::
_rgFactValuesWaypoint
[]
=
{
#if 0
{
"Altitude:"
,
70.1234567
},
const MissionItemTest::TestCase_t MissionItemTest::_rgTestCases[] = {
{
"Hold:"
,
10.1234567
},
{ "0\t0\t3\t16\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 0, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_WAYPOINT, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t3\t17\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_UNLIM, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "2\t0\t3\t18\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 2, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TURNS, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "3\t0\t3\t19\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 3, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TIME, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "4\t0\t3\t21\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 4, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LAND, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "6\t0\t2\t112\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 5, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_CONDITION_DELAY, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } },
};
};
const size_t MissionItemTest::_cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]);
#endif
const
MissionItemTest
::
FactValue_t
MissionItemTest
::
_rgFactValuesLoiterUnlim
[]
=
{
MissionItemTest
::
MissionItemTest
(
void
)
{
"Altitude:"
,
70.1234567
},
{
{
"Radius:"
,
30.1234567
},
}
;
}
const
MissionItemTest
::
FactValue_t
MissionItemTest
::
_rgFactValuesLoiterTurns
[]
=
{
// Test property get/set
{
"Altitude:"
,
70.1234567
},
void
MissionItemTest
::
_testSetGet
(
void
)
{
"Radius:"
,
30.1234567
},
{
{
"Turns:"
,
10.1234567
},
MissionItem
missionItem
;
};
const
MissionItemTest
::
FactValue_t
MissionItemTest
::
_rgFactValuesLoiterTime
[]
=
{
missionItem
.
setSequenceNumber
(
1
);
{
"Altitude:"
,
70.1234567
},
QCOMPARE
(
missionItem
.
sequenceNumber
(),
1
);
{
"Radius:"
,
30.1234567
},
{
"Hold:"
,
10.1234567
},
};
const
MissionItemTest
::
FactValue_t
MissionItemTest
::
_rgFactValuesLand
[]
=
{
missionItem
.
setCommand
(
MAV_CMD_NAV_WAYPOINT
);
{
"Altitude:"
,
70.1234567
},
QCOMPARE
(
missionItem
.
command
(),
MAV_CMD_NAV_WAYPOINT
);
{
"Abort Alt:"
,
10.1234567
},
{
"Heading:"
,
40.1234567
},
};
const
MissionItemTest
::
FactValue_t
MissionItemTest
::
_rgFactValuesTakeoff
[]
=
{
missionItem
.
setFrame
(
MAV_FRAME_LOCAL_NED
);
{
"Altitude:"
,
70.1234567
},
QCOMPARE
(
missionItem
.
frame
(),
MAV_FRAME_LOCAL_NED
);
{
"Heading:"
,
40.1234567
},
QCOMPARE
(
missionItem
.
relativeAltitude
(),
false
);
{
"Pitch:"
,
10.1234567
},
missionItem
.
setFrame
(
MAV_FRAME_GLOBAL_RELATIVE_ALT
);
}
;
QCOMPARE
(
missionItem
.
relativeAltitude
(),
true
)
;
const
MissionItemTest
::
FactValue_t
MissionItemTest
::
_rgFactValuesConditionDelay
[]
=
{
missionItem
.
setParam1
(
1.0
);
{
"Hold:"
,
10.1234567
},
QCOMPARE
(
missionItem
.
param1
(),
1.0
);
};
const
MissionItemTest
::
FactValue_t
MissionItemTest
::
_rgFactValuesDoJump
[]
=
{
missionItem
.
setParam2
(
2.0
);
{
"Item #:"
,
10.1234567
},
QCOMPARE
(
missionItem
.
param2
(),
2.0
);
{
"Repeat:"
,
20.1234567
},
};
const
MissionItemTest
::
ItemExpected_t
MissionItemTest
::
_rgItemExpected
[]
=
{
missionItem
.
setParam3
(
3.0
);
{
sizeof
(
MissionItemTest
::
_rgFactValuesWaypoint
)
/
sizeof
(
MissionItemTest
::
_rgFactValuesWaypoint
[
0
]),
MissionItemTest
::
_rgFactValuesWaypoint
},
QCOMPARE
(
missionItem
.
param3
(),
3.0
);
{
sizeof
(
MissionItemTest
::
_rgFactValuesLoiterUnlim
)
/
sizeof
(
MissionItemTest
::
_rgFactValuesLoiterUnlim
[
0
]),
MissionItemTest
::
_rgFactValuesLoiterUnlim
},
{
sizeof
(
MissionItemTest
::
_rgFactValuesLoiterTurns
)
/
sizeof
(
MissionItemTest
::
_rgFactValuesLoiterTurns
[
0
]),
MissionItemTest
::
_rgFactValuesLoiterTurns
},
{
sizeof
(
MissionItemTest
::
_rgFactValuesLoiterTime
)
/
sizeof
(
MissionItemTest
::
_rgFactValuesLoiterTime
[
0
]),
MissionItemTest
::
_rgFactValuesLoiterTime
},
{
sizeof
(
MissionItemTest
::
_rgFactValuesLand
)
/
sizeof
(
MissionItemTest
::
_rgFactValuesLand
[
0
]),
MissionItemTest
::
_rgFactValuesLand
},
{
sizeof
(
MissionItemTest
::
_rgFactValuesTakeoff
)
/
sizeof
(
MissionItemTest
::
_rgFactValuesTakeoff
[
0
]),
MissionItemTest
::
_rgFactValuesTakeoff
},
{
sizeof
(
MissionItemTest
::
_rgFactValuesConditionDelay
)
/
sizeof
(
MissionItemTest
::
_rgFactValuesConditionDelay
[
0
]),
MissionItemTest
::
_rgFactValuesConditionDelay
},
{
sizeof
(
MissionItemTest
::
_rgFactValuesDoJump
)
/
sizeof
(
MissionItemTest
::
_rgFactValuesDoJump
[
0
]),
MissionItemTest
::
_rgFactValuesDoJump
},
};
MissionItemTest
::
MissionItemTest
(
void
)
missionItem
.
setParam4
(
4.0
);
QCOMPARE
(
missionItem
.
param4
(),
4.0
);
missionItem
.
setParam5
(
5.0
);
QCOMPARE
(
missionItem
.
param5
(),
5.0
);
missionItem
.
setParam6
(
6.0
);
QCOMPARE
(
missionItem
.
param6
(),
6.0
);
missionItem
.
setParam7
(
7.0
);
QCOMPARE
(
missionItem
.
param7
(),
7.0
);
QCOMPARE
(
missionItem
.
coordinate
(),
QGeoCoordinate
(
5.0
,
6.0
,
7.0
));
missionItem
.
setAutoContinue
(
false
);
QCOMPARE
(
missionItem
.
autoContinue
(),
false
);
missionItem
.
setIsCurrentItem
(
true
);
QCOMPARE
(
missionItem
.
isCurrentItem
(),
true
);
}
// Test basic signalling
void
MissionItemTest
::
_testSignals
(
void
)
{
{
MissionItem
missionItem
(
1
,
// sequenceNumber
MAV_CMD_NAV_WAYPOINT
,
// command
MAV_FRAME_GLOBAL_RELATIVE_ALT
,
// MAV_FRAME
1.0
,
1.0
,
1.0
,
1.0
,
1.0
,
1.0
,
1.0
,
// params
true
,
// autoContinue
true
);
// isCurrentItem
enum
{
isCurrentItemChangedIndex
=
0
,
sequenceNumberChangedIndex
,
maxSignalIndex
};
enum
{
isCurrentItemChangedMask
=
1
<<
isCurrentItemChangedIndex
,
sequenceNumberChangedIndexMask
=
1
<<
sequenceNumberChangedIndex
};
static
const
size_t
cMissionItemSignals
=
maxSignalIndex
;
const
char
*
rgMissionItemSignals
[
cMissionItemSignals
];
rgMissionItemSignals
[
isCurrentItemChangedIndex
]
=
SIGNAL
(
isCurrentItemChanged
(
bool
));
rgMissionItemSignals
[
sequenceNumberChangedIndex
]
=
SIGNAL
(
sequenceNumberChanged
(
int
));
MultiSignalSpy
*
multiSpyMissionItem
=
new
MultiSignalSpy
();
Q_CHECK_PTR
(
multiSpyMissionItem
);
QCOMPARE
(
multiSpyMissionItem
->
init
(
&
missionItem
,
rgMissionItemSignals
,
cMissionItemSignals
),
true
);
// Validate isCurrentItemChanged signalling
missionItem
.
setIsCurrentItem
(
true
);
QVERIFY
(
multiSpyMissionItem
->
checkNoSignals
());
missionItem
.
setIsCurrentItem
(
false
);
QVERIFY
(
multiSpyMissionItem
->
checkOnlySignalByMask
(
isCurrentItemChangedMask
));
QSignalSpy
*
spy
=
multiSpyMissionItem
->
getSpyByIndex
(
isCurrentItemChangedIndex
);
QList
<
QVariant
>
signalArgs
=
spy
->
takeFirst
();
QCOMPARE
(
signalArgs
.
count
(),
1
);
QCOMPARE
(
signalArgs
[
0
].
toBool
(),
false
);
multiSpyMissionItem
->
clearAllSignals
();
// Validate sequenceNumberChanged signalling
missionItem
.
setSequenceNumber
(
1
);
QVERIFY
(
multiSpyMissionItem
->
checkNoSignals
());
missionItem
.
setSequenceNumber
(
2
);
QVERIFY
(
multiSpyMissionItem
->
checkOnlySignalByMask
(
sequenceNumberChangedIndexMask
));
spy
=
multiSpyMissionItem
->
getSpyByIndex
(
sequenceNumberChangedIndex
);
signalArgs
=
spy
->
takeFirst
();
QCOMPARE
(
signalArgs
.
count
(),
1
);
QCOMPARE
(
signalArgs
[
0
].
toInt
(),
2
);
}
}
void
MissionItemTest
::
_test
(
void
)
// Test signalling associated with contained facts
void
MissionItemTest
::
_testFactSignals
(
void
)
{
{
MissionItem
missionItem
(
1
,
// sequenceNumber
MAV_CMD_NAV_WAYPOINT
,
// command
MAV_FRAME_GLOBAL_RELATIVE_ALT
,
// MAV_FRAME
1.0
,
2.0
,
3.0
,
4.0
,
5.0
,
6.0
,
7.0
,
// params
true
,
// autoContinue
true
);
// isCurrentItem
// command
QSignalSpy
commandSpy
(
&
missionItem
.
_commandFact
,
SIGNAL
(
valueChanged
(
QVariant
)));
missionItem
.
setCommand
(
MAV_CMD_NAV_WAYPOINT
);
QCOMPARE
(
commandSpy
.
count
(),
0
);
missionItem
.
setCommand
(
MAV_CMD_NAV_ALTITUDE_WAIT
);
QCOMPARE
(
commandSpy
.
count
(),
1
);
QList
<
QVariant
>
arguments
=
commandSpy
.
takeFirst
();
QCOMPARE
(
arguments
.
count
(),
1
);
QCOMPARE
((
MAV_CMD
)
arguments
.
at
(
0
).
toInt
(),
MAV_CMD_NAV_ALTITUDE_WAIT
);
// frame
QSignalSpy
frameSpy
(
&
missionItem
.
_frameFact
,
SIGNAL
(
valueChanged
(
QVariant
)));
missionItem
.
setFrame
(
MAV_FRAME_GLOBAL_RELATIVE_ALT
);
QCOMPARE
(
frameSpy
.
count
(),
0
);
missionItem
.
setFrame
(
MAV_FRAME_BODY_NED
);
QCOMPARE
(
frameSpy
.
count
(),
1
);
arguments
=
frameSpy
.
takeFirst
();
QCOMPARE
(
arguments
.
count
(),
1
);
QCOMPARE
((
MAV_FRAME
)
arguments
.
at
(
0
).
toInt
(),
MAV_FRAME_BODY_NED
);
// param1
QSignalSpy
param1Spy
(
&
missionItem
.
_param1Fact
,
SIGNAL
(
valueChanged
(
QVariant
)));
missionItem
.
setParam1
(
1.0
);
QCOMPARE
(
param1Spy
.
count
(),
0
);
missionItem
.
setParam1
(
2.0
);
QCOMPARE
(
param1Spy
.
count
(),
1
);
arguments
=
param1Spy
.
takeFirst
();
QCOMPARE
(
arguments
.
count
(),
1
);
QCOMPARE
(
arguments
.
at
(
0
).
toDouble
(),
2.0
);
// param2
QSignalSpy
param2Spy
(
&
missionItem
.
_param2Fact
,
SIGNAL
(
valueChanged
(
QVariant
)));
missionItem
.
setParam2
(
2.0
);
QCOMPARE
(
param2Spy
.
count
(),
0
);
missionItem
.
setParam2
(
3.0
);
QCOMPARE
(
param2Spy
.
count
(),
1
);
arguments
=
param2Spy
.
takeFirst
();
QCOMPARE
(
arguments
.
count
(),
1
);
QCOMPARE
(
arguments
.
at
(
0
).
toDouble
(),
3.0
);
// param3
QSignalSpy
param3Spy
(
&
missionItem
.
_param3Fact
,
SIGNAL
(
valueChanged
(
QVariant
)));
missionItem
.
setParam3
(
3.0
);
QCOMPARE
(
param3Spy
.
count
(),
0
);
missionItem
.
setParam3
(
4.0
);
QCOMPARE
(
param3Spy
.
count
(),
1
);
arguments
=
param3Spy
.
takeFirst
();
QCOMPARE
(
arguments
.
count
(),
1
);
QCOMPARE
(
arguments
.
at
(
0
).
toDouble
(),
4.0
);
// param4
QSignalSpy
param4Spy
(
&
missionItem
.
_param4Fact
,
SIGNAL
(
valueChanged
(
QVariant
)));
missionItem
.
setParam4
(
4.0
);
QCOMPARE
(
param4Spy
.
count
(),
0
);
missionItem
.
setParam4
(
5.0
);
QCOMPARE
(
param4Spy
.
count
(),
1
);
arguments
=
param4Spy
.
takeFirst
();
QCOMPARE
(
arguments
.
count
(),
1
);
QCOMPARE
(
arguments
.
at
(
0
).
toDouble
(),
5.0
);
// param6
QSignalSpy
param6Spy
(
&
missionItem
.
_param6Fact
,
SIGNAL
(
valueChanged
(
QVariant
)));
missionItem
.
setParam6
(
6.0
);
QCOMPARE
(
param6Spy
.
count
(),
0
);
missionItem
.
setParam6
(
7.0
);
QCOMPARE
(
param6Spy
.
count
(),
1
);
arguments
=
param6Spy
.
takeFirst
();
QCOMPARE
(
arguments
.
count
(),
1
);
QCOMPARE
(
arguments
.
at
(
0
).
toDouble
(),
7.0
);
// param7
QSignalSpy
param7Spy
(
&
missionItem
.
_param7Fact
,
SIGNAL
(
valueChanged
(
QVariant
)));
missionItem
.
setParam7
(
7.0
);
QCOMPARE
(
param7Spy
.
count
(),
0
);
missionItem
.
setParam7
(
8.0
);
QCOMPARE
(
param7Spy
.
count
(),
1
);
arguments
=
param7Spy
.
takeFirst
();
QCOMPARE
(
arguments
.
count
(),
1
);
QCOMPARE
(
arguments
.
at
(
0
).
toDouble
(),
8.0
);
}
void
MissionItemTest
::
_testLoadFromStream
(
void
)
{
MissionItem
missionItem
;
QString
testString
(
"10
\t
1
\t
3
\t
80
\t
10
\t
20
\t
30
\t
40
\t
-10
\t
-20
\t
-30
\t
1
\r\n
"
);
QTextStream
testStream
(
&
testString
,
QIODevice
::
ReadOnly
);
QVERIFY
(
missionItem
.
load
(
testStream
));
QCOMPARE
(
missionItem
.
sequenceNumber
(),
10
);
QCOMPARE
(
missionItem
.
isCurrentItem
(),
true
);
QCOMPARE
(
missionItem
.
frame
(),
(
MAV_FRAME
)
3
);
QCOMPARE
(
missionItem
.
command
(),
(
MAV_CMD
)
80
);
QCOMPARE
(
missionItem
.
param1
(),
10.0
);
QCOMPARE
(
missionItem
.
param2
(),
20.0
);
QCOMPARE
(
missionItem
.
param3
(),
30.0
);
QCOMPARE
(
missionItem
.
param4
(),
40.0
);
QCOMPARE
(
missionItem
.
param5
(),
-
10.0
);
QCOMPARE
(
missionItem
.
param6
(),
-
20.0
);
QCOMPARE
(
missionItem
.
param7
(),
-
30.0
);
QCOMPARE
(
missionItem
.
autoContinue
(),
true
);
}
void
MissionItemTest
::
_testLoadFromJson
(
void
)
{
MissionItem
missionItem
;
QString
errorString
;
QJsonArray
coordinateArray
=
{
-
10.0
,
-
20.0
,
-
30.0
};
QJsonObject
jsonObject
{
{
"autoContinue"
,
true
},
{
"command"
,
80
},
{
"frame"
,
3
},
{
"id"
,
10
},
{
"param1"
,
10
},
{
"param2"
,
20
},
{
"param3"
,
30
},
{
"param4"
,
40
},
{
"type"
,
"missionItem"
},
{
"coordinate"
,
coordinateArray
},
};
// Test missing key detection
QStringList
removeKeys
;
removeKeys
<<
"autoContinue"
<<
"command"
<<
"frame"
<<
"id"
<<
"param1"
<<
"param2"
<<
"param3"
<<
"param4"
<<
"type"
<<
"coordinate"
;
foreach
(
const
QString
&
removeKey
,
removeKeys
)
{
QJsonObject
badObject
=
jsonObject
;
badObject
.
remove
(
removeKey
);
QCOMPARE
(
missionItem
.
load
(
badObject
,
errorString
),
false
);
QVERIFY
(
!
errorString
.
isEmpty
());
qDebug
()
<<
errorString
;
}
// Test bad coordinate variations
QJsonObject
badObject
=
jsonObject
;
badObject
.
remove
(
"coordinate"
);
badObject
[
"coordinate"
]
=
10
;
QCOMPARE
(
missionItem
.
load
(
badObject
,
errorString
),
false
);
QVERIFY
(
!
errorString
.
isEmpty
());
qDebug
()
<<
errorString
;
QJsonArray
badCoordinateArray
=
{
-
10.0
,
-
20.0
};
badObject
=
jsonObject
;
badObject
.
remove
(
"coordinate"
);
badObject
[
"coordinate"
]
=
badCoordinateArray
;
QCOMPARE
(
missionItem
.
load
(
badObject
,
errorString
),
false
);
QVERIFY
(
!
errorString
.
isEmpty
());
qDebug
()
<<
errorString
;
badCoordinateArray
=
{
-
10.0
,
-
20.0
,
true
};
badObject
=
jsonObject
;
badObject
.
remove
(
"coordinate"
);
badObject
[
"coordinate"
]
=
badCoordinateArray
;
QCOMPARE
(
missionItem
.
load
(
badObject
,
errorString
),
false
);
QVERIFY
(
!
errorString
.
isEmpty
());
qDebug
()
<<
errorString
;
QJsonArray
badCoordinateArray2
=
{
1
,
2
};
badCoordinateArray
=
{
-
10.0
,
-
20.0
,
badCoordinateArray2
};
badObject
=
jsonObject
;
badObject
.
remove
(
"coordinate"
);
badObject
[
"coordinate"
]
=
badCoordinateArray
;
QCOMPARE
(
missionItem
.
load
(
badObject
,
errorString
),
false
);
QVERIFY
(
!
errorString
.
isEmpty
());
qDebug
()
<<
errorString
;
// Test bad type
badObject
=
jsonObject
;
badObject
.
remove
(
"type"
);
badObject
[
"type"
]
=
"foo"
;
QCOMPARE
(
missionItem
.
load
(
badObject
,
errorString
),
false
);
QVERIFY
(
!
errorString
.
isEmpty
());
qDebug
()
<<
errorString
;
// Test good load
QVERIFY
(
missionItem
.
load
(
jsonObject
,
errorString
));
QCOMPARE
(
missionItem
.
sequenceNumber
(),
10
);
QCOMPARE
(
missionItem
.
isCurrentItem
(),
false
);
QCOMPARE
(
missionItem
.
frame
(),
(
MAV_FRAME
)
3
);
QCOMPARE
(
missionItem
.
command
(),
(
MAV_CMD
)
80
);
QCOMPARE
(
missionItem
.
param1
(),
10.0
);
QCOMPARE
(
missionItem
.
param2
(),
20.0
);
QCOMPARE
(
missionItem
.
param3
(),
30.0
);
QCOMPARE
(
missionItem
.
param4
(),
40.0
);
QCOMPARE
(
missionItem
.
param5
(),
-
10.0
);
QCOMPARE
(
missionItem
.
param6
(),
-
20.0
);
QCOMPARE
(
missionItem
.
param7
(),
-
30.0
);
QCOMPARE
(
missionItem
.
autoContinue
(),
true
);
}
void
MissionItemTest
::
_testSaveToJson
(
void
)
{
MissionItem
missionItem
;
missionItem
.
setSequenceNumber
(
10
);
missionItem
.
setIsCurrentItem
(
true
);
missionItem
.
setFrame
((
MAV_FRAME
)
3
);
missionItem
.
setCommand
((
MAV_CMD
)
80
);
missionItem
.
setParam1
(
10.1234567
);
missionItem
.
setParam2
(
20.1234567
);
missionItem
.
setParam3
(
30.1234567
);
missionItem
.
setParam4
(
40.1234567
);
missionItem
.
setParam5
(
-
10.1234567
);
missionItem
.
setParam6
(
-
20.1234567
);
missionItem
.
setParam7
(
-
30.1234567
);
missionItem
.
setAutoContinue
(
true
);
// Round trip item
QJsonObject
jsonObject
;
QString
errorString
;
missionItem
.
save
(
jsonObject
);
QVERIFY
(
missionItem
.
load
(
jsonObject
,
errorString
));
QCOMPARE
(
missionItem
.
sequenceNumber
(),
10
);
QCOMPARE
(
missionItem
.
isCurrentItem
(),
false
);
QCOMPARE
(
missionItem
.
frame
(),
(
MAV_FRAME
)
3
);
QCOMPARE
(
missionItem
.
command
(),
(
MAV_CMD
)
80
);
QCOMPARE
(
missionItem
.
param1
(),
10.1234567
);
QCOMPARE
(
missionItem
.
param2
(),
20.1234567
);
QCOMPARE
(
missionItem
.
param3
(),
30.1234567
);
QCOMPARE
(
missionItem
.
param4
(),
40.1234567
);
QCOMPARE
(
missionItem
.
param5
(),
-
10.1234567
);
QCOMPARE
(
missionItem
.
param6
(),
-
20.1234567
);
QCOMPARE
(
missionItem
.
param7
(),
-
30.1234567
);
QCOMPARE
(
missionItem
.
autoContinue
(),
true
);
}
#if 0
#if 0
// FIXME: Update to json
void MissionItemTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode)
{
_mockLink->setMissionItemFailureMode(failureMode);
// Setup our test case data
QList<MissionItem*> missionItems;
// Editor has a home position item on the front, so we do the same
MissionItem* homeItem = new MissionItem(NULL /* Vehicle */, this);
homeItem->setCommand(MAV_CMD_NAV_WAYPOINT);
homeItem->setCoordinate(QGeoCoordinate(47.3769, 8.549444, 0));
homeItem->setSequenceNumber(0);
missionItems.append(homeItem);
for (size_t i=0; i<_cTestCases; i++) {
const TestCase_t* testCase = &_rgTestCases[i];
MissionItem* missionItem = new MissionItem(this);
QTextStream loadStream(testCase->itemStream, QIODevice::ReadOnly);
QVERIFY(missionItem->load(loadStream));
for (size_t i=0; i<sizeof(_rgItemInfo)/sizeof(_rgItemInfo[0]); i++) {
// Mission Manager expects to get 1-base sequence numbers for write
const ItemInfo_t* info = &_rgItemInfo[i];
missionItem->setSequenceNumber(missionItem->sequenceNumber() + 1);
const ItemExpected_t* expected = &_rgItemExpected[i];
qDebug() << "Command:" << info->command;
missionItems.append(missionItem);
}
// Send the items to the vehicle
_missionManager->writeMissionItems(missionItems);
// writeMissionItems should emit these signals before returning:
// inProgressChanged
// newMissionItemsAvailable
QVERIFY(_missionManager->inProgress());
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | newMissionItemsAvailableSignalMask), true);
_checkInProgressValues(true);
_multiSpyMissionManager->clearAllSignals();
if (failureMode == MockLinkMissionItemHandler::FailNone) {
// This should be clean run
MissionItem* item = new MissionItem(NULL, // Vehicle
// Wait for write sequence to complete. We should get:
1,
// inProgressChanged(false) signal
info->command,
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
info->frame,
QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true);
10.1234567,
20.1234567,
30.1234567,
40.1234567,
50.1234567,
60.1234567,
70.1234567,
true,
false);
// Validate the saving is working correctly
QString savedItemString;
QTextStream saveStream(&savedItemString, QIODevice::WriteOnly);
item->save(saveStream);
// Param floats to string with 18 digits or precision
QString paramStrings = "10.1234567000000002\t"
"20.1234566999999984\t"
"30.1234566999999984\t"
"40.1234566999999984\t"
"50.1234566999999984\t"
"60.1234566999999984\t"
"70.1234567000000055";
QString expectedItemString = QString("1\t0\t%1\t%2\t%3\t1\r\n").arg(info->frame).arg(info->command).arg(paramStrings);
QCOMPARE(savedItemString, expectedItemString);
// Validate that the fact values are correctly returned
// Validate inProgressChanged signal value
size_t factCount = 0;
_checkInProgressValues(false);
for (int i=0; i<item->textFieldFacts()->count(); i++) {
Fact* fact = qobject_cast<Fact*>(item->textFieldFacts()->get(i));
// Validate item count in mission manager
bool found = false;
int expectedCount = (int)_cTestCases;
for (size_t j=0; j<expected->cFactValues; j++) {
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
const FactValue_t* factValue = &expected->rgFactValues[j];
// Home position at position 0 comes from vehicle
expectedCount++;
if (factValue->name == fact->name()) {
QCOMPARE(fact->rawValue().toDouble(), factValue->value);
factCount ++;
found = true;
break;
}
}
if (!found) {
qDebug() << fact->name();
}
QVERIFY(found);
}
}
QCOMPARE(factCount, expected->cFactValues);
QCOMPARE(_missionManager->missionItems().count(), expectedCount);
} else {
// This should be a failed run
// Validate that loading is working correctly
setExpectedMessageBox(QMessageBox::Ok);
MissionItem* loadedItem = new MissionItem(NULL /* Vehicle */);
QTextStream loadStream(&savedItemString, QIODevice::ReadOnly);
// Wait for write sequence to complete. We should get:
QVERIFY(loadedItem->load(loadStream));
// inProgressChanged(false) signal
QCOMPARE(loadedItem->coordinate().latitude(), item->coordinate().latitude());
// error(errorCode, QString) signal
QCOMPARE(loadedItem->coordinate().longitude(), item->coordinate().longitude()
);
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime
);
QCOMPARE(
loadedItem->coordinate().altitude(), item->coordinate().altitude()
);
QCOMPARE(
_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | errorSignalMask), true
);
QCOMPARE(loadedItem->command(), item->command());
QCOMPARE(loadedItem->param1(), item->param1());
// Validate inProgressChanged signal value
QCOMPARE(loadedItem->param2(), item->param2()
);
_checkInProgressValues(false
);
QCOMPARE(loadedItem->param3(), item->param3());
QCOMPARE(loadedItem->param4(), item->param4());
// Validate error signal values
Q
COMPARE(loadedItem->autoContinue(), item->autoContinue()
);
Q
SignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex
);
Q
COMPARE(loadedItem->isCurrentItem(), item->isCurrentItem()
);
Q
List<QVariant> signalArgs = spy->takeFirst(
);
QCOMPARE(
loadedItem->frame(), item->frame()
);
QCOMPARE(
signalArgs.count(), 2
);
qDebug() << signalArgs[1].toString();
delete item;
delete loadedItem
;
checkExpectedMessageBox()
;
}
}
#endif
_multiSpyMissionManager->clearAllSignals();
}
}
void
MissionItemTest
::
_
testDefaultValues
(
void
)
void MissionItemTest::_
roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode
)
{
{
SimpleMissionItem
item
(
NULL
/* Vehicle */
);
_writeItems(MockLinkMissionItemHandler::FailNone);
_mockLink->setMissionItemFailureMode(failureMode);
// Read the items back from the vehicle
_missionManager->requestMissionItems();
// requestMissionItems should emit inProgressChanged signal before returning so no need to wait for it
QVERIFY(_missionManager->inProgress());
QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true);
_checkInProgressValues(true);
_multiSpyMissionManager->clearAllSignals();
if (failureMode == MockLinkMissionItemHandler::FailNone) {
// This should be clean run
// Now wait for read sequence to complete. We should get:
// inProgressChanged(false) signal to signal completion
// newMissionItemsAvailable signal
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
_checkInProgressValues(false);
} else {
// This should be a failed run
setExpectedMessageBox(QMessageBox::Ok);
// Wait for read sequence to complete. We should get:
// inProgressChanged(false) signal to signal completion
// error(errorCode, QString) signal
// newMissionItemsAvailable signal
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask | errorSignalMask), true);
// Validate inProgressChanged signal value
_checkInProgressValues(false);
// Validate error signal values
QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex);
QList<QVariant> signalArgs = spy->takeFirst();
QCOMPARE(signalArgs.count(), 2);
qDebug() << signalArgs[1].toString();
checkExpectedMessageBox();
}
_multiSpyMissionManager->clearAllSignals();
// Validate returned items
size_t cMissionItemsExpected;
if (failureMode == MockLinkMissionItemHandler::FailNone) {
cMissionItemsExpected = (int)_cTestCases;
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
// Home position at position 0 comes from vehicle
cMissionItemsExpected++;
}
} else {
cMissionItemsExpected = 0;
}
QCOMPARE(_missionManager->missionItems().count(), (int)cMissionItemsExpected);
size_t firstActualItem = 0;
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
// First item is home position, don't validate it
firstActualItem++;
}
item
.
setCommand
(
MAV_CMD_NAV_WAYPOINT
);
int testCaseIndex = 0;
item
.
setFrame
(
MAV_FRAME_GLOBAL_RELATIVE_ALT
);
for (size_t actualItemIndex=firstActualItem; actualItemIndex<cMissionItemsExpected; actualItemIndex++) {
QCOMPARE
(
item
.
param7
(),
MissionItem
::
defaultAltitude
);
const TestCase_t* testCase = &_rgTestCases[testCaseIndex];
int expectedSequenceNumber = testCase->expectedItem.sequenceNumber;
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
// Account for home position in first item
expectedSequenceNumber++;
}
MissionItem* actual = _missionManager->missionItems()[actualItemIndex];
qDebug() << "Test case" << testCaseIndex;
QCOMPARE(actual->sequenceNumber(), expectedSequenceNumber);
QCOMPARE(actual->coordinate().latitude(), testCase->expectedItem.coordinate.latitude());
QCOMPARE(actual->coordinate().longitude(), testCase->expectedItem.coordinate.longitude());
QCOMPARE(actual->coordinate().altitude(), testCase->expectedItem.coordinate.altitude());
QCOMPARE((int)actual->command(), (int)testCase->expectedItem.command);
QCOMPARE(actual->param1(), testCase->expectedItem.param1);
QCOMPARE(actual->param2(), testCase->expectedItem.param2);
QCOMPARE(actual->param3(), testCase->expectedItem.param3);
QCOMPARE(actual->param4(), testCase->expectedItem.param4);
QCOMPARE(actual->autoContinue(), testCase->expectedItem.autocontinue);
QCOMPARE(actual->frame(), testCase->expectedItem.frame);
testCaseIndex++;
}
}
}
void MissionItemTest::_testWriteFailureHandlingWorker(void)
{
/*
/// Called to send a MISSION_ACK message while the MissionManager is in idle state
void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); }
/// Called to send a MISSION_ITEM message while the MissionManager is in idle state
void sendUnexpectedMissionItem(void) { _missionItemHandler.sendUnexpectedMissionItem(); }
/// Called to send a MISSION_REQUEST message while the MissionManager is in idle state
void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); }
*/
typedef struct {
const char* failureText;
MockLinkMissionItemHandler::FailureMode_t failureMode;
} TestCase_t;
static const TestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone },
{ "FailWriteRequest0NoResponse", MockLinkMissionItemHandler::FailWriteRequest0NoResponse },
{ "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse },
{ "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence },
{ "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence },
{ "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck },
{ "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck },
{ "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse },
{ "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck },
{ "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests },
};
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
qDebug() << "TEST CASE " << rgTestCases[i].failureText;
_writeItems(rgTestCases[i].failureMode);
_mockLink->resetMissionItemHandler();
}
}
void MissionItemTest::_testReadFailureHandlingWorker(void)
{
/*
/// Called to send a MISSION_ACK message while the MissionManager is in idle state
void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); }
/// Called to send a MISSION_ITEM message while the MissionManager is in idle state
void sendUnexpectedMissionItem(void) { _missionItemHandler.sendUnexpectedMissionItem(); }
/// Called to send a MISSION_REQUEST message while the MissionManager is in idle state
void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); }
*/
typedef struct {
const char* failureText;
MockLinkMissionItemHandler::FailureMode_t failureMode;
} TestCase_t;
static const TestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone },
{ "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse },
{ "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse },
{ "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse },
{ "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence },
{ "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence },
{ "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck },
{ "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck },
};
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
qDebug() << "TEST CASE " << rgTestCases[i].failureText;
_roundTripItems(rgTestCases[i].failureMode);
_mockLink->resetMissionItemHandler();
_multiSpyMissionManager->clearAllSignals();
}
}
void MissionItemTest::_testWriteFailureHandlingAPM(void)
{
_initForFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA);
_testWriteFailureHandlingWorker();
}
void MissionItemTest::_testReadFailureHandlingAPM(void)
{
_initForFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA);
_testReadFailureHandlingWorker();
}
void MissionItemTest::_testWriteFailureHandlingPX4(void)
{
_initForFirmwareType(MAV_AUTOPILOT_PX4);
_testWriteFailureHandlingWorker();
}
void MissionItemTest::_testReadFailureHandlingPX4(void)
{
_initForFirmwareType(MAV_AUTOPILOT_PX4);
_testReadFailureHandlingWorker();
}
#endif
src/MissionManager/MissionItemTest.h
View file @
36d57743
...
@@ -25,16 +25,9 @@
...
@@ -25,16 +25,9 @@
#define MissionItemTest_H
#define MissionItemTest_H
#include "UnitTest.h"
#include "UnitTest.h"
#include "TCPLink.h"
#include "MultiSignalSpy.h"
#include "MultiSignalSpy.h"
#include <QGeoCoordinate>
/// Unit test for the MissionItem Object
/// @file
/// @brief FlightGear HIL Simulation unit tests
///
/// @author Don Gagne <don@thegagnes.com>
class
MissionItemTest
:
public
UnitTest
class
MissionItemTest
:
public
UnitTest
{
{
Q_OBJECT
Q_OBJECT
...
@@ -43,36 +36,12 @@ public:
...
@@ -43,36 +36,12 @@ public:
MissionItemTest
(
void
);
MissionItemTest
(
void
);
private
slots
:
private
slots
:
void
_test
(
void
);
void
_testSetGet
(
void
);
void
_testDefaultValues
(
void
);
void
_testSignals
(
void
);
void
_testFactSignals
(
void
);
private:
void
_testLoadFromStream
(
void
);
void
_testLoadFromJson
(
void
);
typedef
struct
{
void
_testSaveToJson
(
void
);
MAV_CMD
command
;
MAV_FRAME
frame
;
}
ItemInfo_t
;
typedef
struct
{
const
char
*
name
;
double
value
;
}
FactValue_t
;
typedef
struct
{
size_t
cFactValues
;
const
FactValue_t
*
rgFactValues
;
}
ItemExpected_t
;
static
const
ItemInfo_t
_rgItemInfo
[];
static
const
ItemExpected_t
_rgItemExpected
[];
static
const
FactValue_t
_rgFactValuesWaypoint
[];
static
const
FactValue_t
_rgFactValuesLoiterUnlim
[];
static
const
FactValue_t
_rgFactValuesLoiterTurns
[];
static
const
FactValue_t
_rgFactValuesLoiterTime
[];
static
const
FactValue_t
_rgFactValuesLand
[];
static
const
FactValue_t
_rgFactValuesTakeoff
[];
static
const
FactValue_t
_rgFactValuesConditionDelay
[];
static
const
FactValue_t
_rgFactValuesDoJump
[];
};
};
#endif
#endif
src/MissionManager/MissionManager.cc
View file @
36d57743
...
@@ -29,7 +29,6 @@
...
@@ -29,7 +29,6 @@
#include "FirmwarePlugin.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
#include "QGCApplication.h"
#include "SimpleMissionItem.h"
QGC_LOGGING_CATEGORY
(
MissionManagerLog
,
"MissionManagerLog"
)
QGC_LOGGING_CATEGORY
(
MissionManagerLog
,
"MissionManagerLog"
)
...
@@ -56,7 +55,7 @@ MissionManager::~MissionManager()
...
@@ -56,7 +55,7 @@ MissionManager::~MissionManager()
}
}
void
MissionManager
::
writeMissionItems
(
const
Q
mlObjectListModel
&
missionItems
)
void
MissionManager
::
writeMissionItems
(
const
Q
List
<
MissionItem
*>
&
missionItems
)
{
{
bool
skipFirstItem
=
!
_vehicle
->
firmwarePlugin
()
->
sendHomePositionToVehicle
();
bool
skipFirstItem
=
!
_vehicle
->
firmwarePlugin
()
->
sendHomePositionToVehicle
();
...
@@ -65,14 +64,15 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
...
@@ -65,14 +64,15 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
int
firstIndex
=
skipFirstItem
?
1
:
0
;
int
firstIndex
=
skipFirstItem
?
1
:
0
;
for
(
int
i
=
firstIndex
;
i
<
missionItems
.
count
();
i
++
)
{
for
(
int
i
=
firstIndex
;
i
<
missionItems
.
count
();
i
++
)
{
_missionItems
.
append
(
new
SimpleMissionItem
(
*
qobject_cast
<
const
SimpleMissionItem
*>
(
missionItems
[
i
])));
MissionItem
*
item
=
new
MissionItem
(
*
missionItems
[
i
]);
_missionItems
.
append
(
item
);
MissionItem
*
item
=
qobject_cast
<
MissionItem
*>
(
_missionItems
.
get
(
_missionItems
.
count
()
-
1
)
);
item
->
setIsCurrentItem
(
i
==
firstIndex
);
if
(
skipFirstItem
)
{
if
(
skipFirstItem
)
{
// Home is in sequence
1
, remainder of items start at sequence 1
// Home is in sequence
0
, remainder of items start at sequence 1
item
->
setSequenceNumber
(
item
->
sequenceNumber
()
-
1
);
item
->
setSequenceNumber
(
item
->
sequenceNumber
()
-
1
);
if
(
item
->
command
()
==
M
avlinkQmlSingleton
::
M
AV_CMD_DO_JUMP
)
{
if
(
item
->
command
()
==
MAV_CMD_DO_JUMP
)
{
item
->
setParam1
((
int
)
item
->
param1
()
-
1
);
item
->
setParam1
((
int
)
item
->
param1
()
-
1
);
}
}
}
}
...
@@ -253,8 +253,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
...
@@ -253,8 +253,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
_requestItemRetryCount
=
0
;
_requestItemRetryCount
=
0
;
_itemIndicesToRead
.
removeOne
(
missionItem
.
seq
);
_itemIndicesToRead
.
removeOne
(
missionItem
.
seq
);
MissionItem
*
item
=
new
SimpleMissionItem
(
_vehicle
,
MissionItem
*
item
=
new
MissionItem
(
missionItem
.
seq
,
missionItem
.
seq
,
(
MAV_CMD
)
missionItem
.
command
,
(
MAV_CMD
)
missionItem
.
command
,
(
MAV_FRAME
)
missionItem
.
frame
,
(
MAV_FRAME
)
missionItem
.
frame
,
missionItem
.
param1
,
missionItem
.
param1
,
...
@@ -268,7 +267,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
...
@@ -268,7 +267,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
missionItem
.
current
,
missionItem
.
current
,
this
);
this
);
if
(
item
->
command
()
==
M
avlinkQmlSingleton
::
M
AV_CMD_DO_JUMP
)
{
if
(
item
->
command
()
==
MAV_CMD_DO_JUMP
)
{
// Home is in position 0
// Home is in position 0
item
->
setParam1
((
int
)
item
->
param1
()
+
1
);
item
->
setParam1
((
int
)
item
->
param1
()
+
1
);
}
}
...
@@ -323,19 +322,19 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
...
@@ -323,19 +322,19 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
mavlink_message_t
messageOut
;
mavlink_message_t
messageOut
;
mavlink_mission_item_t
missionItem
;
mavlink_mission_item_t
missionItem
;
MissionItem
*
item
=
(
MissionItem
*
)
_missionItems
[
missionRequest
.
seq
];
MissionItem
*
item
=
_missionItems
[
missionRequest
.
seq
];
missionItem
.
target_system
=
_vehicle
->
id
();
missionItem
.
target_system
=
_vehicle
->
id
();
missionItem
.
target_component
=
MAV_COMP_ID_MISSIONPLANNER
;
missionItem
.
target_component
=
MAV_COMP_ID_MISSIONPLANNER
;
missionItem
.
seq
=
missionRequest
.
seq
;
missionItem
.
seq
=
missionRequest
.
seq
;
missionItem
.
command
=
item
->
command
();
missionItem
.
command
=
item
->
command
();
missionItem
.
x
=
item
->
coordinate
().
latitude
();
missionItem
.
y
=
item
->
coordinate
().
longitude
();
missionItem
.
z
=
item
->
coordinate
().
altitude
();
missionItem
.
param1
=
item
->
param1
();
missionItem
.
param1
=
item
->
param1
();
missionItem
.
param2
=
item
->
param2
();
missionItem
.
param2
=
item
->
param2
();
missionItem
.
param3
=
item
->
param3
();
missionItem
.
param3
=
item
->
param3
();
missionItem
.
param4
=
item
->
param4
();
missionItem
.
param4
=
item
->
param4
();
missionItem
.
x
=
item
->
param5
();
missionItem
.
y
=
item
->
param6
();
missionItem
.
z
=
item
->
param7
();
missionItem
.
frame
=
item
->
frame
();
missionItem
.
frame
=
item
->
frame
();
missionItem
.
current
=
missionRequest
.
seq
==
0
;
missionItem
.
current
=
missionRequest
.
seq
==
0
;
missionItem
.
autocontinue
=
item
->
autoContinue
();
missionItem
.
autocontinue
=
item
->
autoContinue
();
...
@@ -428,17 +427,6 @@ void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message)
...
@@ -428,17 +427,6 @@ void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message)
}
}
}
}
QmlObjectListModel
*
MissionManager
::
copyMissionItems
(
void
)
{
QmlObjectListModel
*
list
=
new
QmlObjectListModel
();
for
(
int
i
=
0
;
i
<
_missionItems
.
count
();
i
++
)
{
list
->
append
(
new
SimpleMissionItem
(
*
qobject_cast
<
const
SimpleMissionItem
*>
(
_missionItems
[
i
])));
}
return
list
;
}
void
MissionManager
::
_sendError
(
ErrorCode_t
errorCode
,
const
QString
&
errorMsg
)
void
MissionManager
::
_sendError
(
ErrorCode_t
errorCode
,
const
QString
&
errorMsg
)
{
{
qCDebug
(
MissionManagerLog
)
<<
"Sending error"
<<
errorCode
<<
errorMsg
;
qCDebug
(
MissionManagerLog
)
<<
"Sending error"
<<
errorCode
<<
errorMsg
;
...
...
src/MissionManager/MissionManager.h
View file @
36d57743
...
@@ -30,7 +30,7 @@
...
@@ -30,7 +30,7 @@
#include <QMutex>
#include <QMutex>
#include <QTimer>
#include <QTimer>
#include "
QmlObjectListModel
.h"
#include "
MissionItem
.h"
#include "QGCMAVLink.h"
#include "QGCMAVLink.h"
#include "QGCLoggingCategory.h"
#include "QGCLoggingCategory.h"
#include "LinkInterface.h"
#include "LinkInterface.h"
...
@@ -48,27 +48,15 @@ public:
...
@@ -48,27 +48,15 @@ public:
MissionManager
(
Vehicle
*
vehicle
);
MissionManager
(
Vehicle
*
vehicle
);
~
MissionManager
();
~
MissionManager
();
Q_PROPERTY
(
bool
inProgress
READ
inProgress
NOTIFY
inProgressChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
missionItems
READ
missionItems
CONSTANT
)
Q_PROPERTY
(
int
currentItem
READ
currentItem
NOTIFY
currentItemChanged
)
// Property accessors
bool
inProgress
(
void
);
bool
inProgress
(
void
);
QmlObjectListModel
*
missionItems
(
void
)
{
return
&
_missionItems
;
}
const
QList
<
MissionItem
*>&
missionItems
(
void
)
{
return
_missionItems
;
}
int
currentItem
(
void
)
{
return
_currentMissionItem
;
}
int
currentItem
(
void
)
{
return
_currentMissionItem
;
}
// C++ methods
void
requestMissionItems
(
void
);
void
requestMissionItems
(
void
);
/// Writes the specified set of mission items to the vehicle
/// Writes the specified set of mission items to the vehicle
/// @oaram missionItems Items to send to vehicle
/// @param missionItems Items to send to vehicle
void
writeMissionItems
(
const
QmlObjectListModel
&
missionItems
);
void
writeMissionItems
(
const
QList
<
MissionItem
*>&
missionItems
);
/// Returns a copy of the current set of mission items. Caller is responsible for
/// freeing returned object.
QmlObjectListModel
*
copyMissionItems
(
void
);
/// Error codes returned in error signal
/// Error codes returned in error signal
typedef
enum
{
typedef
enum
{
...
@@ -134,7 +122,7 @@ private:
...
@@ -134,7 +122,7 @@ private:
QMutex
_dataMutex
;
QMutex
_dataMutex
;
Q
mlObjectListModel
_missionItems
;
Q
List
<
MissionItem
*>
_missionItems
;
int
_currentMissionItem
;
int
_currentMissionItem
;
};
};
...
...
src/MissionManager/MissionManagerTest.cc
View file @
36d57743
...
@@ -24,7 +24,6 @@
...
@@ -24,7 +24,6 @@
#include "MissionManagerTest.h"
#include "MissionManagerTest.h"
#include "LinkManager.h"
#include "LinkManager.h"
#include "MultiVehicleManager.h"
#include "MultiVehicleManager.h"
#include "SimpleMissionItem.h"
const
MissionManagerTest
::
TestCase_t
MissionManagerTest
::
_rgTestCases
[]
=
{
const
MissionManagerTest
::
TestCase_t
MissionManagerTest
::
_rgTestCases
[]
=
{
{
"0
\t
0
\t
3
\t
16
\t
10
\t
20
\t
30
\t
40
\t
-10
\t
-20
\t
-30
\t
1
\r\n
"
,
{
0
,
QGeoCoordinate
(
-
10.0
,
-
20.0
,
-
30.0
),
MAV_CMD_NAV_WAYPOINT
,
10.0
,
20.0
,
30.0
,
40.0
,
true
,
false
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
}
},
{
"0
\t
0
\t
3
\t
16
\t
10
\t
20
\t
30
\t
40
\t
-10
\t
-20
\t
-30
\t
1
\r\n
"
,
{
0
,
QGeoCoordinate
(
-
10.0
,
-
20.0
,
-
30.0
),
MAV_CMD_NAV_WAYPOINT
,
10.0
,
20.0
,
30.0
,
40.0
,
true
,
false
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
}
},
...
@@ -46,32 +45,31 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
...
@@ -46,32 +45,31 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
_mockLink
->
setMissionItemFailureMode
(
failureMode
);
_mockLink
->
setMissionItemFailureMode
(
failureMode
);
// Setup our test case data
// Setup our test case data
Q
mlObjectListModel
*
list
=
new
QmlObjectListModel
()
;
Q
List
<
MissionItem
*>
missionItems
;
// Editor has a home position item on the front, so we do the same
// Editor has a home position item on the front, so we do the same
SimpleMissionItem
*
homeItem
=
new
SimpleMissionItem
(
NULL
/* Vehicle */
,
this
);
MissionItem
*
homeItem
=
new
MissionItem
(
NULL
/* Vehicle */
,
this
);
homeItem
->
setHomePositionSpecialCase
(
true
);
homeItem
->
setCommand
(
MAV_CMD_NAV_WAYPOINT
);
homeItem
->
setCommand
(
MavlinkQmlSingleton
::
MAV_CMD_NAV_WAYPOINT
);
homeItem
->
setCoordinate
(
QGeoCoordinate
(
47.3769
,
8.549444
,
0
));
homeItem
->
setCoordinate
(
QGeoCoordinate
(
47.3769
,
8.549444
,
0
));
homeItem
->
setSequenceNumber
(
0
);
homeItem
->
setSequenceNumber
(
0
);
list
->
insert
(
0
,
homeItem
);
missionItems
.
append
(
homeItem
);
for
(
size_t
i
=
0
;
i
<
_cTestCases
;
i
++
)
{
for
(
size_t
i
=
0
;
i
<
_cTestCases
;
i
++
)
{
const
TestCase_t
*
testCase
=
&
_rgTestCases
[
i
];
const
TestCase_t
*
testCase
=
&
_rgTestCases
[
i
];
SimpleMissionItem
*
item
=
new
SimpleMissionItem
(
NULL
/* Vehicle */
,
list
);
MissionItem
*
missionItem
=
new
MissionItem
(
this
);
QTextStream
loadStream
(
testCase
->
itemStream
,
QIODevice
::
ReadOnly
);
QTextStream
loadStream
(
testCase
->
itemStream
,
QIODevice
::
ReadOnly
);
QVERIFY
(
i
tem
->
load
(
loadStream
));
QVERIFY
(
missionI
tem
->
load
(
loadStream
));
// Mission Manager expects to get 1-base sequence numbers for write
// Mission Manager expects to get 1-base sequence numbers for write
item
->
setSequenceNumber
(
i
tem
->
sequenceNumber
()
+
1
);
missionItem
->
setSequenceNumber
(
missionI
tem
->
sequenceNumber
()
+
1
);
list
->
append
(
i
tem
);
missionItems
.
append
(
missionI
tem
);
}
}
// Send the items to the vehicle
// Send the items to the vehicle
_missionManager
->
writeMissionItems
(
*
list
);
_missionManager
->
writeMissionItems
(
missionItems
);
// writeMissionItems should emit these signals before returning:
// writeMissionItems should emit these signals before returning:
// inProgressChanged
// inProgressChanged
...
@@ -101,7 +99,7 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
...
@@ -101,7 +99,7 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
expectedCount
++
;
expectedCount
++
;
}
}
QCOMPARE
(
_missionManager
->
missionItems
()
->
count
(),
expectedCount
);
QCOMPARE
(
_missionManager
->
missionItems
()
.
count
(),
expectedCount
);
}
else
{
}
else
{
// This should be a failed run
// This should be a failed run
...
@@ -125,8 +123,6 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
...
@@ -125,8 +123,6 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
checkExpectedMessageBox
();
checkExpectedMessageBox
();
}
}
delete
list
;
list
=
NULL
;
_multiSpyMissionManager
->
clearAllSignals
();
_multiSpyMissionManager
->
clearAllSignals
();
}
}
...
@@ -196,7 +192,7 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
...
@@ -196,7 +192,7 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
cMissionItemsExpected
=
0
;
cMissionItemsExpected
=
0
;
}
}
QCOMPARE
(
_missionManager
->
missionItems
()
->
count
(),
(
int
)
cMissionItemsExpected
);
QCOMPARE
(
_missionManager
->
missionItems
()
.
count
(),
(
int
)
cMissionItemsExpected
);
size_t
firstActualItem
=
0
;
size_t
firstActualItem
=
0
;
if
(
_mockLink
->
getFirmwareType
()
==
MAV_AUTOPILOT_ARDUPILOTMEGA
)
{
if
(
_mockLink
->
getFirmwareType
()
==
MAV_AUTOPILOT_ARDUPILOTMEGA
)
{
...
@@ -214,7 +210,7 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
...
@@ -214,7 +210,7 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
expectedSequenceNumber
++
;
expectedSequenceNumber
++
;
}
}
MissionItem
*
actual
=
qobject_cast
<
MissionItem
*>
(
_missionManager
->
missionItems
()
->
get
(
actualItemIndex
))
;
MissionItem
*
actual
=
_missionManager
->
missionItems
()[
actualItemIndex
]
;
qDebug
()
<<
"Test case"
<<
testCaseIndex
;
qDebug
()
<<
"Test case"
<<
testCaseIndex
;
QCOMPARE
(
actual
->
sequenceNumber
(),
expectedSequenceNumber
);
QCOMPARE
(
actual
->
sequenceNumber
(),
expectedSequenceNumber
);
...
...
src/MissionManager/SimpleMissionItem.cc
View file @
36d57743
...
@@ -20,42 +20,568 @@ This file is part of the QGROUNDCONTROL project
...
@@ -20,42 +20,568 @@ This file is part of the QGROUNDCONTROL project
======================================================================*/
======================================================================*/
#include <QStringList>
#include <QDebug>
#include "SimpleMissionItem.h"
#include "SimpleMissionItem.h"
#include "FirmwarePluginManager.h"
#include "QGCApplication.h"
#include "JsonHelper.h"
const
double
SimpleMissionItem
::
defaultAltitude
=
25.0
;
FactMetaData
*
SimpleMissionItem
::
_altitudeMetaData
=
NULL
;
FactMetaData
*
SimpleMissionItem
::
_commandMetaData
=
NULL
;
FactMetaData
*
SimpleMissionItem
::
_defaultParamMetaData
=
NULL
;
FactMetaData
*
SimpleMissionItem
::
_frameMetaData
=
NULL
;
FactMetaData
*
SimpleMissionItem
::
_latitudeMetaData
=
NULL
;
FactMetaData
*
SimpleMissionItem
::
_longitudeMetaData
=
NULL
;
struct
EnumInfo_s
{
const
char
*
label
;
MAV_FRAME
frame
;
};
static
const
struct
EnumInfo_s
_rgMavFrameInfo
[]
=
{
{
"MAV_FRAME_GLOBAL"
,
MAV_FRAME_GLOBAL
},
{
"MAV_FRAME_LOCAL_NED"
,
MAV_FRAME_LOCAL_NED
},
{
"MAV_FRAME_MISSION"
,
MAV_FRAME_MISSION
},
{
"MAV_FRAME_GLOBAL_RELATIVE_ALT"
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
"MAV_FRAME_LOCAL_ENU"
,
MAV_FRAME_LOCAL_ENU
},
{
"MAV_FRAME_GLOBAL_INT"
,
MAV_FRAME_GLOBAL_INT
},
{
"MAV_FRAME_GLOBAL_RELATIVE_ALT_INT"
,
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
},
{
"MAV_FRAME_LOCAL_OFFSET_NED"
,
MAV_FRAME_LOCAL_OFFSET_NED
},
{
"MAV_FRAME_BODY_NED"
,
MAV_FRAME_BODY_NED
},
{
"MAV_FRAME_BODY_OFFSET_NED"
,
MAV_FRAME_BODY_OFFSET_NED
},
{
"MAV_FRAME_GLOBAL_TERRAIN_ALT"
,
MAV_FRAME_GLOBAL_TERRAIN_ALT
},
{
"MAV_FRAME_GLOBAL_TERRAIN_ALT_INT"
,
MAV_FRAME_GLOBAL_TERRAIN_ALT_INT
},
};
SimpleMissionItem
::
SimpleMissionItem
(
Vehicle
*
vehicle
,
QObject
*
parent
)
SimpleMissionItem
::
SimpleMissionItem
(
Vehicle
*
vehicle
,
QObject
*
parent
)
:
MissionItem
(
vehicle
,
parent
)
:
VisualMissionItem
(
vehicle
,
parent
)
,
_rawEdit
(
false
)
,
_homePositionSpecialCase
(
false
)
,
_showHomePosition
(
false
)
,
_altitudeRelativeToHomeFact
(
0
,
"Altitude is relative to home"
,
FactMetaData
::
valueTypeUint32
)
,
_supportedCommandFact
(
0
,
"Command:"
,
FactMetaData
::
valueTypeUint32
)
,
_param1MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param2MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param3MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param4MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param5MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param6MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param7MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_syncingAltitudeRelativeToHomeAndFrame
(
false
)
,
_syncingHeadingDegreesAndParam4
(
false
)
,
_missionCommands
(
qgcApp
()
->
toolbox
()
->
missionCommands
())
{
{
_altitudeRelativeToHomeFact
.
setRawValue
(
true
);
_setupMetaData
();
_connectSignals
();
setDefaultsForCommand
();
}
}
SimpleMissionItem
::
SimpleMissionItem
(
Vehicle
*
vehicle
,
SimpleMissionItem
::
SimpleMissionItem
(
Vehicle
*
vehicle
,
const
MissionItem
&
missionItem
,
QObject
*
parent
)
int
sequenceNumber
,
:
VisualMissionItem
(
vehicle
,
parent
)
MAV_CMD
command
,
,
_missionItem
(
missionItem
)
MAV_FRAME
frame
,
,
_rawEdit
(
false
)
double
param1
,
,
_dirty
(
false
)
double
param2
,
,
_homePositionSpecialCase
(
false
)
double
param3
,
,
_showHomePosition
(
false
)
double
param4
,
,
_altitudeRelativeToHomeFact
(
0
,
"Altitude is relative to home"
,
FactMetaData
::
valueTypeUint32
)
double
param5
,
,
_supportedCommandFact
(
0
,
"Command:"
,
FactMetaData
::
valueTypeUint32
)
double
param6
,
,
_param1MetaData
(
FactMetaData
::
valueTypeDouble
)
double
param7
,
,
_param2MetaData
(
FactMetaData
::
valueTypeDouble
)
bool
autoContinue
,
,
_param3MetaData
(
FactMetaData
::
valueTypeDouble
)
bool
isCurrentItem
,
,
_param4MetaData
(
FactMetaData
::
valueTypeDouble
)
QObject
*
parent
)
,
_param5MetaData
(
FactMetaData
::
valueTypeDouble
)
:
MissionItem
(
vehicle
,
sequenceNumber
,
command
,
frame
,
param1
,
param2
,
param3
,
param4
,
param5
,
param6
,
param7
,
autoContinue
,
isCurrentItem
,
parent
)
,
_param6MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param7MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_syncingAltitudeRelativeToHomeAndFrame
(
false
)
,
_syncingHeadingDegreesAndParam4
(
false
)
,
_missionCommands
(
qgcApp
()
->
toolbox
()
->
missionCommands
())
{
{
_altitudeRelativeToHomeFact
.
setRawValue
(
true
);
_setupMetaData
();
_connectSignals
();
_syncFrameToAltitudeRelativeToHome
();
}
}
SimpleMissionItem
::
SimpleMissionItem
(
const
SimpleMissionItem
&
other
,
QObject
*
parent
)
SimpleMissionItem
::
SimpleMissionItem
(
const
SimpleMissionItem
&
other
,
QObject
*
parent
)
:
MissionItem
(
other
,
parent
)
:
VisualMissionItem
(
other
,
parent
)
,
_missionItem
(
other
.
_vehicle
)
,
_rawEdit
(
false
)
,
_dirty
(
false
)
,
_homePositionSpecialCase
(
false
)
,
_showHomePosition
(
false
)
,
_altitudeRelativeToHomeFact
(
0
,
"Altitude is relative to home"
,
FactMetaData
::
valueTypeUint32
)
,
_supportedCommandFact
(
0
,
"Command:"
,
FactMetaData
::
valueTypeUint32
)
,
_param1MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param2MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param3MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_param4MetaData
(
FactMetaData
::
valueTypeDouble
)
,
_syncingAltitudeRelativeToHomeAndFrame
(
false
)
,
_syncingHeadingDegreesAndParam4
(
false
)
,
_missionCommands
(
qgcApp
()
->
toolbox
()
->
missionCommands
())
{
{
_setupMetaData
();
_connectSignals
();
*
this
=
other
;
}
}
const
SimpleMissionItem
&
SimpleMissionItem
::
operator
=
(
const
SimpleMissionItem
&
other
)
const
SimpleMissionItem
&
SimpleMissionItem
::
operator
=
(
const
SimpleMissionItem
&
other
)
{
{
static_cast
<
MissionItem
&>
(
*
this
)
=
other
;
static_cast
<
VisualMissionItem
&>
(
*
this
)
=
other
;
setRawEdit
(
other
.
_rawEdit
);
setDirty
(
other
.
_dirty
);
setHomePositionSpecialCase
(
other
.
_homePositionSpecialCase
);
setShowHomePosition
(
other
.
_showHomePosition
);
_syncFrameToAltitudeRelativeToHome
();
return
*
this
;
return
*
this
;
}
}
void
SimpleMissionItem
::
_connectSignals
(
void
)
{
// Connect to change signals to track dirty state
connect
(
&
_missionItem
.
_param1Fact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_setDirtyFromSignal
);
connect
(
&
_missionItem
.
_param2Fact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_setDirtyFromSignal
);
connect
(
&
_missionItem
.
_param3Fact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_setDirtyFromSignal
);
connect
(
&
_missionItem
.
_param4Fact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_setDirtyFromSignal
);
connect
(
&
_missionItem
.
_param5Fact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_setDirtyFromSignal
);
connect
(
&
_missionItem
.
_param6Fact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_setDirtyFromSignal
);
connect
(
&
_missionItem
.
_param7Fact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_setDirtyFromSignal
);
connect
(
&
_missionItem
.
_frameFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_setDirtyFromSignal
);
connect
(
&
_missionItem
.
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_setDirtyFromSignal
);
connect
(
&
_missionItem
,
&
MissionItem
::
sequenceNumberChanged
,
this
,
&
SimpleMissionItem
::
_setDirtyFromSignal
);
// Values from these facts must propogate back and forth between the real object storage
connect
(
&
_altitudeRelativeToHomeFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_syncAltitudeRelativeToHomeToFrame
);
connect
(
&
_missionItem
.
_frameFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_syncFrameToAltitudeRelativeToHome
);
// These are coordinate parameters, they must emit coordinateChanged signal
connect
(
&
_missionItem
.
_param5Fact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_sendCoordinateChanged
);
connect
(
&
_missionItem
.
_param6Fact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_sendCoordinateChanged
);
connect
(
&
_missionItem
.
_param7Fact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_sendCoordinateChanged
);
// The following changes may also change friendlyEditAllowed
connect
(
&
_missionItem
.
_autoContinueFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_sendFriendlyEditAllowedChanged
);
connect
(
&
_missionItem
.
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_sendFriendlyEditAllowedChanged
);
connect
(
&
_missionItem
.
_frameFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_sendFriendlyEditAllowedChanged
);
// A command change triggers a number of other changes as well.
connect
(
&
_missionItem
.
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
setDefaultsForCommand
);
connect
(
&
_missionItem
.
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
commandNameChanged
);
connect
(
&
_missionItem
.
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
commandDescriptionChanged
);
connect
(
&
_missionItem
.
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
specifiesCoordinateChanged
);
connect
(
&
_missionItem
.
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
isStandaloneCoordinateChanged
);
// Whenever these properties change the ui model changes as well
connect
(
this
,
&
SimpleMissionItem
::
commandChanged
,
this
,
&
SimpleMissionItem
::
_sendUiModelChanged
);
connect
(
this
,
&
SimpleMissionItem
::
rawEditChanged
,
this
,
&
SimpleMissionItem
::
_sendUiModelChanged
);
// These fact signals must alway signal out through SimpleMissionItem signals
connect
(
&
_missionItem
.
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_sendCommandChanged
);
connect
(
&
_missionItem
.
_frameFact
,
&
Fact
::
valueChanged
,
this
,
&
SimpleMissionItem
::
_sendFrameChanged
);
}
void
SimpleMissionItem
::
_setupMetaData
(
void
)
{
QStringList
enumStrings
;
QVariantList
enumValues
;
if
(
!
_altitudeMetaData
)
{
_altitudeMetaData
=
new
FactMetaData
(
FactMetaData
::
valueTypeDouble
);
_altitudeMetaData
->
setRawUnits
(
"meters"
);
_altitudeMetaData
->
setDecimalPlaces
(
2
);
enumStrings
.
clear
();
enumValues
.
clear
();
foreach
(
const
MAV_CMD
command
,
_missionCommands
->
commandsIds
())
{
const
MavCmdInfo
*
mavCmdInfo
=
_missionCommands
->
getMavCmdInfo
(
command
,
_vehicle
);
enumStrings
.
append
(
mavCmdInfo
->
rawName
());
enumValues
.
append
(
QVariant
(
mavCmdInfo
->
command
()));
}
_commandMetaData
=
new
FactMetaData
(
FactMetaData
::
valueTypeUint32
);
_commandMetaData
->
setEnumInfo
(
enumStrings
,
enumValues
);
_defaultParamMetaData
=
new
FactMetaData
(
FactMetaData
::
valueTypeDouble
);
_defaultParamMetaData
->
setDecimalPlaces
(
7
);
enumStrings
.
clear
();
enumValues
.
clear
();
for
(
size_t
i
=
0
;
i
<
sizeof
(
_rgMavFrameInfo
)
/
sizeof
(
_rgMavFrameInfo
[
0
]);
i
++
)
{
const
struct
EnumInfo_s
*
mavFrameInfo
=
&
_rgMavFrameInfo
[
i
];
enumStrings
.
append
(
mavFrameInfo
->
label
);
enumValues
.
append
(
QVariant
(
mavFrameInfo
->
frame
));
}
_frameMetaData
=
new
FactMetaData
(
FactMetaData
::
valueTypeUint32
);
_frameMetaData
->
setEnumInfo
(
enumStrings
,
enumValues
);
_latitudeMetaData
=
new
FactMetaData
(
FactMetaData
::
valueTypeDouble
);
_latitudeMetaData
->
setRawUnits
(
"deg"
);
_latitudeMetaData
->
setDecimalPlaces
(
7
);
_longitudeMetaData
=
new
FactMetaData
(
FactMetaData
::
valueTypeDouble
);
_longitudeMetaData
->
setRawUnits
(
"deg"
);
_longitudeMetaData
->
setDecimalPlaces
(
7
);
}
_missionItem
.
_commandFact
.
setMetaData
(
_commandMetaData
);
_missionItem
.
_frameFact
.
setMetaData
(
_frameMetaData
);
}
SimpleMissionItem
::~
SimpleMissionItem
()
{
}
bool
SimpleMissionItem
::
save
(
QJsonObject
&
missionObject
,
QJsonArray
&
missionItems
,
QString
&
errorString
)
{
Q_UNUSED
(
missionObject
);
Q_UNUSED
(
errorString
);
QJsonObject
itemObject
;
_missionItem
.
save
(
itemObject
);
missionItems
.
append
(
itemObject
);
return
true
;
}
bool
SimpleMissionItem
::
load
(
QTextStream
&
loadStream
)
{
return
_missionItem
.
load
(
loadStream
);
}
bool
SimpleMissionItem
::
load
(
const
QJsonObject
&
json
,
QString
&
errorString
)
{
return
_missionItem
.
load
(
json
,
errorString
);
}
bool
SimpleMissionItem
::
isStandaloneCoordinate
(
void
)
const
{
if
(
_missionCommands
->
contains
((
MAV_CMD
)
command
()))
{
return
_missionCommands
->
getMavCmdInfo
((
MAV_CMD
)
command
(),
_vehicle
)
->
isStandaloneCoordinate
();
}
else
{
return
false
;
}
}
bool
SimpleMissionItem
::
specifiesCoordinate
(
void
)
const
{
if
(
_missionCommands
->
contains
((
MAV_CMD
)
command
()))
{
return
_missionCommands
->
getMavCmdInfo
((
MAV_CMD
)
command
(),
_vehicle
)
->
specifiesCoordinate
();
}
else
{
return
false
;
}
}
QString
SimpleMissionItem
::
commandDescription
(
void
)
const
{
if
(
_missionCommands
->
contains
((
MAV_CMD
)
command
()))
{
return
_missionCommands
->
getMavCmdInfo
((
MAV_CMD
)
command
(),
_vehicle
)
->
description
();
}
else
{
qWarning
()
<<
"Should not ask for command description on unknown command"
;
return
commandName
();
}
}
QString
SimpleMissionItem
::
commandName
(
void
)
const
{
MAV_CMD
command
=
(
MAV_CMD
)
this
->
command
();
if
(
_missionCommands
->
contains
(
command
))
{
const
MavCmdInfo
*
mavCmdInfo
=
_missionCommands
->
getMavCmdInfo
(
command
,
_vehicle
);
return
mavCmdInfo
->
friendlyName
().
isEmpty
()
?
mavCmdInfo
->
rawName
()
:
mavCmdInfo
->
friendlyName
();
}
else
{
qWarning
()
<<
"Request for command name on unknown command"
;
return
QString
(
"Unknown: %1"
).
arg
(
command
);
}
}
void
SimpleMissionItem
::
_clearParamMetaData
(
void
)
{
_param1MetaData
.
setRawUnits
(
""
);
_param1MetaData
.
setDecimalPlaces
(
FactMetaData
::
defaultDecimalPlaces
);
_param2MetaData
.
setRawUnits
(
""
);
_param2MetaData
.
setDecimalPlaces
(
FactMetaData
::
defaultDecimalPlaces
);
_param3MetaData
.
setRawUnits
(
""
);
_param3MetaData
.
setDecimalPlaces
(
FactMetaData
::
defaultDecimalPlaces
);
_param4MetaData
.
setRawUnits
(
""
);
_param4MetaData
.
setDecimalPlaces
(
FactMetaData
::
defaultDecimalPlaces
);
}
QmlObjectListModel
*
SimpleMissionItem
::
textFieldFacts
(
void
)
{
QmlObjectListModel
*
model
=
new
QmlObjectListModel
(
this
);
if
(
rawEdit
())
{
_missionItem
.
_param1Fact
.
_setName
(
"Param1:"
);
_missionItem
.
_param1Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_missionItem
.
_param1Fact
);
_missionItem
.
_param2Fact
.
_setName
(
"Param2:"
);
_missionItem
.
_param2Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_missionItem
.
_param2Fact
);
_missionItem
.
_param3Fact
.
_setName
(
"Param3:"
);
_missionItem
.
_param3Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_missionItem
.
_param3Fact
);
_missionItem
.
_param4Fact
.
_setName
(
"Param4:"
);
_missionItem
.
_param4Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_missionItem
.
_param4Fact
);
_missionItem
.
_param5Fact
.
_setName
(
"Lat/X:"
);
_missionItem
.
_param5Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_missionItem
.
_param5Fact
);
_missionItem
.
_param6Fact
.
_setName
(
"Lon/Y:"
);
_missionItem
.
_param6Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_missionItem
.
_param6Fact
);
_missionItem
.
_param7Fact
.
_setName
(
"Alt/Z:"
);
_missionItem
.
_param7Fact
.
setMetaData
(
_defaultParamMetaData
);
model
->
append
(
&
_missionItem
.
_param7Fact
);
}
else
{
_clearParamMetaData
();
MAV_CMD
command
;
if
(
_homePositionSpecialCase
)
{
command
=
MAV_CMD_NAV_LAST
;
}
else
{
command
=
_missionItem
.
command
();
}
Fact
*
rgParamFacts
[
7
]
=
{
&
_missionItem
.
_param1Fact
,
&
_missionItem
.
_param2Fact
,
&
_missionItem
.
_param3Fact
,
&
_missionItem
.
_param4Fact
,
&
_missionItem
.
_param5Fact
,
&
_missionItem
.
_param6Fact
,
&
_missionItem
.
_param7Fact
};
FactMetaData
*
rgParamMetaData
[
7
]
=
{
&
_param1MetaData
,
&
_param2MetaData
,
&
_param3MetaData
,
&
_param4MetaData
,
&
_param5MetaData
,
&
_param6MetaData
,
&
_param7MetaData
};
bool
altitudeAdded
=
false
;
for
(
int
i
=
1
;
i
<=
7
;
i
++
)
{
const
QMap
<
int
,
MavCmdParamInfo
*>&
paramInfoMap
=
_missionCommands
->
getMavCmdInfo
(
command
,
_vehicle
)
->
paramInfoMap
();
if
(
paramInfoMap
.
contains
(
i
)
&&
paramInfoMap
[
i
]
->
enumStrings
().
count
()
==
0
)
{
Fact
*
paramFact
=
rgParamFacts
[
i
-
1
];
FactMetaData
*
paramMetaData
=
rgParamMetaData
[
i
-
1
];
MavCmdParamInfo
*
paramInfo
=
paramInfoMap
[
i
];
paramFact
->
_setName
(
paramInfo
->
label
());
paramMetaData
->
setDecimalPlaces
(
paramInfo
->
decimalPlaces
());
paramMetaData
->
setEnumInfo
(
paramInfo
->
enumStrings
(),
paramInfo
->
enumValues
());
paramMetaData
->
setRawUnits
(
paramInfo
->
units
());
paramFact
->
setMetaData
(
paramMetaData
);
model
->
append
(
paramFact
);
if
(
i
==
7
)
{
altitudeAdded
=
true
;
}
}
}
if
(
specifiesCoordinate
()
&&
!
altitudeAdded
)
{
_missionItem
.
_param7Fact
.
_setName
(
"Altitude:"
);
_missionItem
.
_param7Fact
.
setMetaData
(
_altitudeMetaData
);
model
->
append
(
&
_missionItem
.
_param7Fact
);
}
}
return
model
;
}
QmlObjectListModel
*
SimpleMissionItem
::
checkboxFacts
(
void
)
{
QmlObjectListModel
*
model
=
new
QmlObjectListModel
(
this
);
if
(
rawEdit
())
{
model
->
append
(
&
_missionItem
.
_autoContinueFact
);
}
else
if
(
specifiesCoordinate
()
&&
!
_homePositionSpecialCase
)
{
model
->
append
(
&
_altitudeRelativeToHomeFact
);
}
return
model
;
}
QmlObjectListModel
*
SimpleMissionItem
::
comboboxFacts
(
void
)
{
QmlObjectListModel
*
model
=
new
QmlObjectListModel
(
this
);
if
(
rawEdit
())
{
model
->
append
(
&
_missionItem
.
_commandFact
);
model
->
append
(
&
_missionItem
.
_frameFact
);
}
else
{
Fact
*
rgParamFacts
[
7
]
=
{
&
_missionItem
.
_param1Fact
,
&
_missionItem
.
_param2Fact
,
&
_missionItem
.
_param3Fact
,
&
_missionItem
.
_param4Fact
,
&
_missionItem
.
_param5Fact
,
&
_missionItem
.
_param6Fact
,
&
_missionItem
.
_param7Fact
};
FactMetaData
*
rgParamMetaData
[
7
]
=
{
&
_param1MetaData
,
&
_param2MetaData
,
&
_param3MetaData
,
&
_param4MetaData
,
&
_param5MetaData
,
&
_param6MetaData
,
&
_param7MetaData
};
MAV_CMD
command
;
if
(
_homePositionSpecialCase
)
{
command
=
MAV_CMD_NAV_LAST
;
}
else
{
command
=
(
MAV_CMD
)
this
->
command
();
}
for
(
int
i
=
1
;
i
<=
7
;
i
++
)
{
const
QMap
<
int
,
MavCmdParamInfo
*>&
paramInfoMap
=
_missionCommands
->
getMavCmdInfo
(
command
,
_vehicle
)
->
paramInfoMap
();
if
(
paramInfoMap
.
contains
(
i
)
&&
paramInfoMap
[
i
]
->
enumStrings
().
count
()
!=
0
)
{
Fact
*
paramFact
=
rgParamFacts
[
i
-
1
];
FactMetaData
*
paramMetaData
=
rgParamMetaData
[
i
-
1
];
MavCmdParamInfo
*
paramInfo
=
paramInfoMap
[
i
];
paramFact
->
_setName
(
paramInfo
->
label
());
paramMetaData
->
setDecimalPlaces
(
paramInfo
->
decimalPlaces
());
paramMetaData
->
setEnumInfo
(
paramInfo
->
enumStrings
(),
paramInfo
->
enumValues
());
paramMetaData
->
setRawUnits
(
paramInfo
->
units
());
paramFact
->
setMetaData
(
paramMetaData
);
model
->
append
(
paramFact
);
}
}
}
return
model
;
}
bool
SimpleMissionItem
::
friendlyEditAllowed
(
void
)
const
{
if
(
_missionCommands
->
contains
((
MAV_CMD
)
command
())
&&
_missionCommands
->
getMavCmdInfo
((
MAV_CMD
)
command
(),
_vehicle
)
->
friendlyEdit
())
{
if
(
!
_missionItem
.
autoContinue
())
{
return
false
;
}
if
(
specifiesCoordinate
())
{
return
_missionItem
.
frame
()
==
MAV_FRAME_GLOBAL
||
_missionItem
.
frame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
;
}
return
true
;
}
return
false
;
}
bool
SimpleMissionItem
::
rawEdit
(
void
)
const
{
return
_rawEdit
||
!
friendlyEditAllowed
();
}
void
SimpleMissionItem
::
setRawEdit
(
bool
rawEdit
)
{
if
(
this
->
rawEdit
()
!=
rawEdit
)
{
_rawEdit
=
rawEdit
;
emit
rawEditChanged
(
this
->
rawEdit
());
}
}
void
SimpleMissionItem
::
setDirty
(
bool
dirty
)
{
if
(
!
_homePositionSpecialCase
||
!
dirty
)
{
// Home position never affects dirty bit
_dirty
=
dirty
;
// We want to emit dirtyChanged even if _dirty didn't change. This can be handy signal for
// any value within the item changing.
emit
dirtyChanged
(
_dirty
);
}
}
void
SimpleMissionItem
::
_setDirtyFromSignal
(
void
)
{
setDirty
(
true
);
}
void
SimpleMissionItem
::
_sendCoordinateChanged
(
void
)
{
emit
coordinateChanged
(
coordinate
());
}
void
SimpleMissionItem
::
_syncAltitudeRelativeToHomeToFrame
(
const
QVariant
&
value
)
{
if
(
!
_syncingAltitudeRelativeToHomeAndFrame
)
{
_syncingAltitudeRelativeToHomeAndFrame
=
true
;
_missionItem
.
setFrame
(
value
.
toBool
()
?
MAV_FRAME_GLOBAL_RELATIVE_ALT
:
MAV_FRAME_GLOBAL
);
_syncingAltitudeRelativeToHomeAndFrame
=
false
;
}
}
void
SimpleMissionItem
::
_syncFrameToAltitudeRelativeToHome
(
void
)
{
if
(
!
_syncingAltitudeRelativeToHomeAndFrame
)
{
_syncingAltitudeRelativeToHomeAndFrame
=
true
;
_altitudeRelativeToHomeFact
.
setRawValue
(
relativeAltitude
());
_syncingAltitudeRelativeToHomeAndFrame
=
false
;
}
}
void
SimpleMissionItem
::
setDefaultsForCommand
(
void
)
{
// We set these global defaults first, then if there are param defaults they will get reset
_missionItem
.
setParam7
(
defaultAltitude
);
MAV_CMD
command
=
(
MAV_CMD
)
this
->
command
();
if
(
_missionCommands
->
contains
(
command
))
{
MavCmdInfo
*
mavCmdInfo
=
_missionCommands
->
getMavCmdInfo
(
command
,
_vehicle
);
foreach
(
const
MavCmdParamInfo
*
paramInfo
,
mavCmdInfo
->
paramInfoMap
())
{
Fact
*
rgParamFacts
[
7
]
=
{
&
_missionItem
.
_param1Fact
,
&
_missionItem
.
_param2Fact
,
&
_missionItem
.
_param3Fact
,
&
_missionItem
.
_param4Fact
,
&
_missionItem
.
_param5Fact
,
&
_missionItem
.
_param6Fact
,
&
_missionItem
.
_param7Fact
};
rgParamFacts
[
paramInfo
->
param
()
-
1
]
->
setRawValue
(
paramInfo
->
defaultValue
());
}
}
if
(
command
==
MAV_CMD_NAV_WAYPOINT
)
{
// We default all acceptance radius to 0. This allows flight controller to be in control of
// accept radius.
_missionItem
.
setParam2
(
0
);
}
_missionItem
.
setAutoContinue
(
true
);
_missionItem
.
setFrame
(
specifiesCoordinate
()
?
MAV_FRAME_GLOBAL_RELATIVE_ALT
:
MAV_FRAME_MISSION
);
setRawEdit
(
false
);
}
void
SimpleMissionItem
::
_sendUiModelChanged
(
void
)
{
emit
uiModelChanged
();
}
void
SimpleMissionItem
::
_sendFrameChanged
(
void
)
{
emit
frameChanged
(
_missionItem
.
frame
());
}
void
SimpleMissionItem
::
_sendCommandChanged
(
void
)
{
emit
commandChanged
(
command
());
}
void
SimpleMissionItem
::
_sendFriendlyEditAllowedChanged
(
void
)
{
emit
friendlyEditAllowedChanged
(
friendlyEditAllowed
());
}
QString
SimpleMissionItem
::
category
(
void
)
const
{
return
qgcApp
()
->
toolbox
()
->
missionCommands
()
->
categoryFromCommand
(
command
());
}
void
SimpleMissionItem
::
setShowHomePosition
(
bool
showHomePosition
)
{
if
(
showHomePosition
!=
_showHomePosition
)
{
_showHomePosition
=
showHomePosition
;
emit
showHomePositionChanged
(
_showHomePosition
);
}
}
void
SimpleMissionItem
::
setCommand
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
)
{
if
((
MAV_CMD
)
command
!=
_missionItem
.
command
())
{
_missionItem
.
setCommand
((
MAV_CMD
)
command
);
}
}
void
SimpleMissionItem
::
setCoordinate
(
const
QGeoCoordinate
&
coordinate
)
{
if
(
_missionItem
.
coordinate
()
!=
coordinate
)
{
qDebug
()
<<
_missionItem
.
coordinate
()
<<
coordinate
;
_missionItem
.
setCoordinate
(
coordinate
);
}
}
src/MissionManager/SimpleMissionItem.h
View file @
36d57743
...
@@ -24,39 +24,151 @@
...
@@ -24,39 +24,151 @@
#ifndef SimpleMissionItem_H
#ifndef SimpleMissionItem_H
#define SimpleMissionItem_H
#define SimpleMissionItem_H
#include "VisualMissionItem.h"
#include "MissionItem.h"
#include "MissionItem.h"
class
SimpleMissionItem
:
public
MissionItem
/// A SimpleMissionItem is used to represent a single MissionItem to the ui.
class
SimpleMissionItem
:
public
VisualMissionItem
{
{
Q_OBJECT
Q_OBJECT
public:
public:
SimpleMissionItem
(
Vehicle
*
vehicle
,
QObject
*
parent
=
NULL
);
SimpleMissionItem
(
Vehicle
*
vehicle
,
QObject
*
parent
=
NULL
);
SimpleMissionItem
(
Vehicle
*
vehicle
,
const
MissionItem
&
missionItem
,
QObject
*
parent
=
NULL
);
SimpleMissionItem
(
Vehicle
*
vehicle
,
int
sequenceNumber
,
MAV_CMD
command
,
MAV_FRAME
frame
,
double
param1
,
double
param2
,
double
param3
,
double
param4
,
double
param5
,
double
param6
,
double
param7
,
bool
autoContinue
,
bool
isCurrentItem
,
QObject
*
parent
=
NULL
);
SimpleMissionItem
(
const
SimpleMissionItem
&
other
,
QObject
*
parent
=
NULL
);
SimpleMissionItem
(
const
SimpleMissionItem
&
other
,
QObject
*
parent
=
NULL
);
~
SimpleMissionItem
();
const
SimpleMissionItem
&
operator
=
(
const
SimpleMissionItem
&
other
);
const
SimpleMissionItem
&
operator
=
(
const
SimpleMissionItem
&
other
);
Q_PROPERTY
(
QString
category
READ
category
NOTIFY
commandChanged
)
Q_PROPERTY
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
READ
command
WRITE
setCommand
NOTIFY
commandChanged
)
Q_PROPERTY
(
bool
friendlyEditAllowed
READ
friendlyEditAllowed
NOTIFY
friendlyEditAllowedChanged
)
Q_PROPERTY
(
bool
homePosition
READ
homePosition
CONSTANT
)
///< true: This item is being used as a home position indicator
Q_PROPERTY
(
bool
rawEdit
READ
rawEdit
WRITE
setRawEdit
NOTIFY
rawEditChanged
)
///< true: raw item editing with all params
Q_PROPERTY
(
bool
relativeAltitude
READ
relativeAltitude
NOTIFY
frameChanged
)
Q_PROPERTY
(
bool
showHomePosition
READ
showHomePosition
WRITE
setShowHomePosition
NOTIFY
showHomePositionChanged
)
// These properties are used to display the editing ui
Q_PROPERTY
(
QmlObjectListModel
*
checkboxFacts
READ
checkboxFacts
NOTIFY
uiModelChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
comboboxFacts
READ
comboboxFacts
NOTIFY
uiModelChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
textFieldFacts
READ
textFieldFacts
NOTIFY
uiModelChanged
)
// Property accesors
QString
category
(
void
)
const
;
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
(
void
)
const
{
return
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
)
_missionItem
.
_commandFact
.
cookedValue
().
toInt
();
};
bool
friendlyEditAllowed
(
void
)
const
;
bool
homePosition
(
void
)
const
{
return
_homePositionSpecialCase
;
}
bool
rawEdit
(
void
)
const
;
bool
showHomePosition
(
void
)
const
{
return
_showHomePosition
;
}
QmlObjectListModel
*
textFieldFacts
(
void
);
QmlObjectListModel
*
checkboxFacts
(
void
);
QmlObjectListModel
*
comboboxFacts
(
void
);
void
setRawEdit
(
bool
rawEdit
);
void
setCommandByIndex
(
int
index
);
void
setCommand
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
);
void
setHomePositionSpecialCase
(
bool
homePositionSpecialCase
)
{
_homePositionSpecialCase
=
homePositionSpecialCase
;
}
void
setShowHomePosition
(
bool
showHomePosition
);
void
setAltDifference
(
double
altDifference
);
void
setAltPercent
(
double
altPercent
);
void
setAzimuth
(
double
azimuth
);
void
setDistance
(
double
distance
);
bool
load
(
QTextStream
&
loadStream
);
bool
load
(
const
QJsonObject
&
json
,
QString
&
errorString
);
bool
relativeAltitude
(
void
)
{
return
_missionItem
.
frame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
;
}
// Overrides from MissionItem base class
static
const
double
defaultAltitude
;
bool
simpleItem
(
void
)
const
final
{
return
true
;
}
QGeoCoordinate
exitCoordinate
(
void
)
const
final
{
return
coordinate
();
}
MissionItem
&
missionItem
(
void
)
{
return
_missionItem
;
}
// Overrides from VisualMissionItem
bool
dirty
(
void
)
const
final
{
return
_dirty
;
}
bool
isSimpleItem
(
void
)
const
final
{
return
true
;
}
bool
isStandaloneCoordinate
(
void
)
const
final
;
bool
specifiesCoordinate
(
void
)
const
final
;
QString
commandDescription
(
void
)
const
final
;
QString
commandName
(
void
)
const
final
;
QGeoCoordinate
coordinate
(
void
)
const
final
{
return
_missionItem
.
coordinate
();
}
QGeoCoordinate
exitCoordinate
(
void
)
const
final
{
return
coordinate
();
}
bool
coordinateHasRelativeAltitude
(
void
)
const
final
{
return
_missionItem
.
relativeAltitude
();
}
bool
exitCoordinateHasRelativeAltitude
(
void
)
const
final
{
return
coordinateHasRelativeAltitude
();
}
bool
exitCoordinateSameAsEntry
(
void
)
const
final
{
return
true
;
}
void
setDirty
(
bool
dirty
)
final
;
void
setCoordinate
(
const
QGeoCoordinate
&
coordinate
);
bool
save
(
QJsonObject
&
missionObject
,
QJsonArray
&
missionItems
,
QString
&
errorString
)
final
;
public
slots
:
void
setDefaultsForCommand
(
void
);
signals:
void
commandChanged
(
int
command
);
void
coordinateChanged
(
const
QGeoCoordinate
&
coordinate
);
void
exitCoordinateChanged
(
const
QGeoCoordinate
&
exitCoordinate
);
//void dirtyChanged (bool dirty);
void
frameChanged
(
int
frame
);
void
friendlyEditAllowedChanged
(
bool
friendlyEditAllowed
);
void
headingDegreesChanged
(
double
heading
);
void
rawEditChanged
(
bool
rawEdit
);
void
uiModelChanged
(
void
);
void
showHomePositionChanged
(
bool
showHomePosition
);
private
slots
:
void
_setDirtyFromSignal
(
void
);
void
_sendCommandChanged
(
void
);
void
_sendCoordinateChanged
(
void
);
void
_sendFrameChanged
(
void
);
void
_sendFriendlyEditAllowedChanged
(
void
);
void
_sendUiModelChanged
(
void
);
void
_syncAltitudeRelativeToHomeToFrame
(
const
QVariant
&
value
);
void
_syncFrameToAltitudeRelativeToHome
(
void
);
private:
private:
void
_clearParamMetaData
(
void
);
void
_connectSignals
(
void
);
void
_setupMetaData
(
void
);
private:
MissionItem
_missionItem
;
bool
_rawEdit
;
bool
_dirty
;
bool
_homePositionSpecialCase
;
///< true: This item is being used as a ui home position indicator
bool
_showHomePosition
;
Fact
_altitudeRelativeToHomeFact
;
Fact
_supportedCommandFact
;
static
FactMetaData
*
_altitudeMetaData
;
static
FactMetaData
*
_commandMetaData
;
static
FactMetaData
*
_defaultParamMetaData
;
static
FactMetaData
*
_frameMetaData
;
static
FactMetaData
*
_latitudeMetaData
;
static
FactMetaData
*
_longitudeMetaData
;
FactMetaData
_param1MetaData
;
FactMetaData
_param2MetaData
;
FactMetaData
_param3MetaData
;
FactMetaData
_param4MetaData
;
FactMetaData
_param5MetaData
;
FactMetaData
_param6MetaData
;
FactMetaData
_param7MetaData
;
bool
_syncingAltitudeRelativeToHomeAndFrame
;
///< true: already in a sync signal, prevents signal loop
bool
_syncingHeadingDegreesAndParam4
;
///< true: already in a sync signal, prevents signal loop
const
MissionCommands
*
_missionCommands
;
};
};
#endif
#endif
src/MissionManager/SimpleMissionItemTest.cc
0 → 100644
View file @
36d57743
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
#include "SimpleMissionItemTest.h"
#include "SimpleMissionItem.h"
const
SimpleMissionItemTest
::
ItemInfo_t
SimpleMissionItemTest
::
_rgItemInfo
[]
=
{
{
MAV_CMD_NAV_WAYPOINT
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
MAV_CMD_NAV_LOITER_UNLIM
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
MAV_CMD_NAV_LOITER_TURNS
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
MAV_CMD_NAV_LOITER_TIME
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
MAV_CMD_NAV_LAND
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
MAV_CMD_NAV_TAKEOFF
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
},
{
MAV_CMD_CONDITION_DELAY
,
MAV_FRAME_MISSION
},
{
MAV_CMD_DO_JUMP
,
MAV_FRAME_MISSION
},
};
const
SimpleMissionItemTest
::
FactValue_t
SimpleMissionItemTest
::
_rgFactValuesWaypoint
[]
=
{
{
"Altitude:"
,
70.1234567
},
{
"Hold:"
,
10.1234567
},
};
const
SimpleMissionItemTest
::
FactValue_t
SimpleMissionItemTest
::
_rgFactValuesLoiterUnlim
[]
=
{
{
"Altitude:"
,
70.1234567
},
{
"Radius:"
,
30.1234567
},
};
const
SimpleMissionItemTest
::
FactValue_t
SimpleMissionItemTest
::
_rgFactValuesLoiterTurns
[]
=
{
{
"Altitude:"
,
70.1234567
},
{
"Radius:"
,
30.1234567
},
{
"Turns:"
,
10.1234567
},
};
const
SimpleMissionItemTest
::
FactValue_t
SimpleMissionItemTest
::
_rgFactValuesLoiterTime
[]
=
{
{
"Altitude:"
,
70.1234567
},
{
"Radius:"
,
30.1234567
},
{
"Hold:"
,
10.1234567
},
};
const
SimpleMissionItemTest
::
FactValue_t
SimpleMissionItemTest
::
_rgFactValuesLand
[]
=
{
{
"Altitude:"
,
70.1234567
},
{
"Abort Alt:"
,
10.1234567
},
{
"Heading:"
,
40.1234567
},
};
const
SimpleMissionItemTest
::
FactValue_t
SimpleMissionItemTest
::
_rgFactValuesTakeoff
[]
=
{
{
"Pitch:"
,
10.1234567
},
{
"Altitude:"
,
70.1234567
},
};
const
SimpleMissionItemTest
::
FactValue_t
SimpleMissionItemTest
::
_rgFactValuesConditionDelay
[]
=
{
{
"Hold:"
,
10.1234567
},
};
const
SimpleMissionItemTest
::
FactValue_t
SimpleMissionItemTest
::
_rgFactValuesDoJump
[]
=
{
{
"Item #:"
,
10.1234567
},
{
"Repeat:"
,
20.1234567
},
};
const
SimpleMissionItemTest
::
ItemExpected_t
SimpleMissionItemTest
::
_rgItemExpected
[]
=
{
{
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesWaypoint
)
/
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesWaypoint
[
0
]),
SimpleMissionItemTest
::
_rgFactValuesWaypoint
,
true
},
{
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesLoiterUnlim
)
/
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesLoiterUnlim
[
0
]),
SimpleMissionItemTest
::
_rgFactValuesLoiterUnlim
,
true
},
{
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesLoiterTurns
)
/
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesLoiterTurns
[
0
]),
SimpleMissionItemTest
::
_rgFactValuesLoiterTurns
,
true
},
{
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesLoiterTime
)
/
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesLoiterTime
[
0
]),
SimpleMissionItemTest
::
_rgFactValuesLoiterTime
,
true
},
{
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesLand
)
/
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesLand
[
0
]),
SimpleMissionItemTest
::
_rgFactValuesLand
,
true
},
{
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesTakeoff
)
/
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesTakeoff
[
0
]),
SimpleMissionItemTest
::
_rgFactValuesTakeoff
,
false
},
{
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesConditionDelay
)
/
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesConditionDelay
[
0
]),
SimpleMissionItemTest
::
_rgFactValuesConditionDelay
,
false
},
{
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesDoJump
)
/
sizeof
(
SimpleMissionItemTest
::
_rgFactValuesDoJump
[
0
]),
SimpleMissionItemTest
::
_rgFactValuesDoJump
,
false
},
};
SimpleMissionItemTest
::
SimpleMissionItemTest
(
void
)
{
}
void
SimpleMissionItemTest
::
_testEditorFacts
(
void
)
{
for
(
size_t
i
=
0
;
i
<
sizeof
(
_rgItemInfo
)
/
sizeof
(
_rgItemInfo
[
0
]);
i
++
)
{
const
ItemInfo_t
*
info
=
&
_rgItemInfo
[
i
];
const
ItemExpected_t
*
expected
=
&
_rgItemExpected
[
i
];
qDebug
()
<<
"Command:"
<<
info
->
command
;
MissionItem
missionItem
(
1
,
// sequence number
info
->
command
,
info
->
frame
,
10.1234567
,
// param 1-7
20.1234567
,
30.1234567
,
40.1234567
,
50.1234567
,
60.1234567
,
70.1234567
,
true
,
// autoContinue
false
);
// isCurrentItem
SimpleMissionItem
simpleMissionItem
(
NULL
/* Vehicle */
,
missionItem
);
// Validate that the fact values are correctly returned
size_t
factCount
=
0
;
for
(
int
i
=
0
;
i
<
simpleMissionItem
.
textFieldFacts
()
->
count
();
i
++
)
{
Fact
*
fact
=
qobject_cast
<
Fact
*>
(
simpleMissionItem
.
textFieldFacts
()
->
get
(
i
));
bool
found
=
false
;
for
(
size_t
j
=
0
;
j
<
expected
->
cFactValues
;
j
++
)
{
const
FactValue_t
*
factValue
=
&
expected
->
rgFactValues
[
j
];
if
(
factValue
->
name
==
fact
->
name
())
{
QCOMPARE
(
fact
->
rawValue
().
toDouble
(),
factValue
->
value
);
factCount
++
;
found
=
true
;
break
;
}
}
if
(
!
found
)
{
qDebug
()
<<
fact
->
name
();
}
QVERIFY
(
found
);
}
QCOMPARE
(
factCount
,
expected
->
cFactValues
);
int
expectedCount
=
expected
->
relativeAltCheckbox
?
1
:
0
;
QCOMPARE
(
simpleMissionItem
.
checkboxFacts
()
->
count
(),
expectedCount
);
}
}
void
SimpleMissionItemTest
::
_testDefaultValues
(
void
)
{
SimpleMissionItem
item
(
NULL
/* Vehicle */
);
item
.
missionItem
().
setCommand
(
MAV_CMD_NAV_WAYPOINT
);
item
.
missionItem
().
setFrame
(
MAV_FRAME_GLOBAL_RELATIVE_ALT
);
QCOMPARE
(
item
.
missionItem
().
param7
(),
SimpleMissionItem
::
defaultAltitude
);
}
void
SimpleMissionItemTest
::
_testSignals
(
void
)
{
enum
{
commandChangedIndex
=
0
,
coordinateChangedIndex
,
exitCoordinateChangedIndex
,
dirtyChangedIndex
,
frameChangedIndex
,
friendlyEditAllowedChangedIndex
,
headingDegreesChangedIndex
,
rawEditChangedIndex
,
uiModelChangedIndex
,
showHomePositionChangedIndex
,
maxSignalIndex
};
enum
{
commandChangedMask
=
1
<<
commandChangedIndex
,
coordinateChangedMask
=
1
<<
coordinateChangedIndex
,
exitCoordinateChangedMask
=
1
<<
exitCoordinateChangedIndex
,
dirtyChangedMask
=
1
<<
dirtyChangedIndex
,
frameChangedMask
=
1
<<
frameChangedIndex
,
friendlyEditAllowedChangedMask
=
1
<<
friendlyEditAllowedChangedIndex
,
headingDegreesChangedMask
=
1
<<
headingDegreesChangedIndex
,
rawEditChangedMask
=
1
<<
rawEditChangedIndex
,
uiModelChangedMask
=
1
<<
uiModelChangedIndex
,
showHomePositionChangedMask
=
1
<<
showHomePositionChangedIndex
,
};
static
const
size_t
cSimpleMissionItemSignals
=
maxSignalIndex
;
const
char
*
rgSimpleMissionItemSignals
[
cSimpleMissionItemSignals
];
rgSimpleMissionItemSignals
[
commandChangedIndex
]
=
SIGNAL
(
commandChanged
(
int
));
rgSimpleMissionItemSignals
[
coordinateChangedIndex
]
=
SIGNAL
(
coordinateChanged
(
const
QGeoCoordinate
&
));
rgSimpleMissionItemSignals
[
exitCoordinateChangedIndex
]
=
SIGNAL
(
exitCoordinateChanged
(
const
QGeoCoordinate
&
));
rgSimpleMissionItemSignals
[
dirtyChangedIndex
]
=
SIGNAL
(
dirtyChanged
(
bool
));
rgSimpleMissionItemSignals
[
frameChangedIndex
]
=
SIGNAL
(
frameChanged
(
int
));
rgSimpleMissionItemSignals
[
friendlyEditAllowedChangedIndex
]
=
SIGNAL
(
friendlyEditAllowedChanged
(
bool
));
rgSimpleMissionItemSignals
[
headingDegreesChangedIndex
]
=
SIGNAL
(
headingDegreesChanged
(
double
));
rgSimpleMissionItemSignals
[
rawEditChangedIndex
]
=
SIGNAL
(
rawEditChanged
(
bool
));
rgSimpleMissionItemSignals
[
uiModelChangedIndex
]
=
SIGNAL
(
uiModelChanged
());
rgSimpleMissionItemSignals
[
showHomePositionChangedIndex
]
=
SIGNAL
(
showHomePositionChanged
(
bool
));
MissionItem
missionItem
(
1
,
// sequence number
MAV_CMD_NAV_WAYPOINT
,
MAV_FRAME_GLOBAL_RELATIVE_ALT
,
10.1234567
,
// param 1-7
20.1234567
,
30.1234567
,
40.1234567
,
50.1234567
,
60.1234567
,
70.1234567
,
true
,
// autoContinue
false
);
// isCurrentItem
SimpleMissionItem
simpleMissionItem
(
NULL
/* Vehicle */
,
missionItem
);
// It's important top check that the right signals are emitted at the right time since that drives ui change.
// It's also important to check that things are not being over-signalled when they should not be, since that can lead
// to incorrect ui or perf impact of uneeded signals propogating ui change.
MultiSignalSpy
*
multiSpy
=
new
MultiSignalSpy
();
Q_CHECK_PTR
(
multiSpy
);
QCOMPARE
(
multiSpy
->
init
(
&
simpleMissionItem
,
rgSimpleMissionItemSignals
,
cSimpleMissionItemSignals
),
true
);
// Check commandChanged signalling. Call setCommand should trigger:
// commandChanged
// dirtyChanged
// uiModelChanged
// coordinateChanged - since altitude will be set back to default
simpleMissionItem
.
setCommand
(
MavlinkQmlSingleton
::
MAV_CMD_NAV_WAYPOINT
);
QVERIFY
(
multiSpy
->
checkNoSignals
());
simpleMissionItem
.
setCommand
(
MavlinkQmlSingleton
::
MAV_CMD_NAV_LOITER_TIME
);
QVERIFY
(
multiSpy
->
checkSignalsByMask
(
commandChangedMask
|
dirtyChangedMask
|
uiModelChangedMask
|
coordinateChangedMask
));
multiSpy
->
clearAllSignals
();
// Check coordinateChanged signalling. Calling setCoordinate should trigger:
// coordinateChanged
// dirtyChanged
// Check that changing to the same coordinate does not signal
simpleMissionItem
.
setCoordinate
(
QGeoCoordinate
(
50.1234567
,
60.1234567
,
SimpleMissionItem
::
defaultAltitude
));
QVERIFY
(
multiSpy
->
checkNoSignals
());
// Check that actually changing coordinate signals correctly
simpleMissionItem
.
setCoordinate
(
QGeoCoordinate
(
50.1234567
,
60.1234567
,
70.1234567
));
QVERIFY
(
multiSpy
->
checkOnlySignalByMask
(
coordinateChangedMask
|
dirtyChangedMask
));
multiSpy
->
clearAllSignals
();
// Check dirtyChanged signalling
// Reset param 1-5 for testing
simpleMissionItem
.
missionItem
().
setParam1
(
10.1234567
);
simpleMissionItem
.
missionItem
().
setParam2
(
20.1234567
);
simpleMissionItem
.
missionItem
().
setParam3
(
30.1234567
);
simpleMissionItem
.
missionItem
().
setParam4
(
40.1234567
);
multiSpy
->
clearAllSignals
();
simpleMissionItem
.
missionItem
().
setParam1
(
10.1234567
);
QVERIFY
(
multiSpy
->
checkNoSignals
());
simpleMissionItem
.
missionItem
().
setParam1
(
20.1234567
);
QVERIFY
(
multiSpy
->
checkOnlySignalByMask
(
dirtyChangedMask
));
multiSpy
->
clearAllSignals
();
simpleMissionItem
.
missionItem
().
setParam2
(
20.1234567
);
QVERIFY
(
multiSpy
->
checkNoSignals
());
simpleMissionItem
.
missionItem
().
setParam2
(
30.1234567
);
QVERIFY
(
multiSpy
->
checkOnlySignalByMask
(
dirtyChangedMask
));
multiSpy
->
clearAllSignals
();
simpleMissionItem
.
missionItem
().
setParam3
(
30.1234567
);
QVERIFY
(
multiSpy
->
checkNoSignals
());
simpleMissionItem
.
missionItem
().
setParam3
(
40.1234567
);
QVERIFY
(
multiSpy
->
checkOnlySignalByMask
(
dirtyChangedMask
));
multiSpy
->
clearAllSignals
();
simpleMissionItem
.
missionItem
().
setParam4
(
40.1234567
);
QVERIFY
(
multiSpy
->
checkNoSignals
());
simpleMissionItem
.
missionItem
().
setParam4
(
50.1234567
);
QVERIFY
(
multiSpy
->
checkOnlySignalByMask
(
dirtyChangedMask
));
multiSpy
->
clearAllSignals
();
// Check frameChanged signalling. Calling setFrame should signal:
// frameChanged
// dirtyChanged
// friendlyEditAllowedChanged - this signal is not very smart on when it gets sent
simpleMissionItem
.
setCommand
(
MavlinkQmlSingleton
::
MAV_CMD_NAV_WAYPOINT
);
simpleMissionItem
.
missionItem
().
setFrame
(
MAV_FRAME_GLOBAL_RELATIVE_ALT
);
multiSpy
->
clearAllSignals
();
simpleMissionItem
.
missionItem
().
setFrame
(
MAV_FRAME_GLOBAL_RELATIVE_ALT
);
QVERIFY
(
multiSpy
->
checkNoSignals
());
simpleMissionItem
.
missionItem
().
setFrame
(
MAV_FRAME_GLOBAL
);
QVERIFY
(
multiSpy
->
checkOnlySignalByMask
(
frameChangedMask
|
dirtyChangedMask
|
friendlyEditAllowedChangedMask
));
multiSpy
->
clearAllSignals
();
}
src/MissionManager/SimpleMissionItemTest.h
0 → 100644
View file @
36d57743
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
#ifndef SimpleMissionItemTest_H
#define SimpleMissionItemTest_H
#include "UnitTest.h"
#include "TCPLink.h"
#include "MultiSignalSpy.h"
#include <QGeoCoordinate>
/// Unit test for SimpleMissionItem
class
SimpleMissionItemTest
:
public
UnitTest
{
Q_OBJECT
public:
SimpleMissionItemTest
(
void
);
private
slots
:
void
_testSignals
(
void
);
void
_testEditorFacts
(
void
);
void
_testDefaultValues
(
void
);
private:
typedef
struct
{
MAV_CMD
command
;
MAV_FRAME
frame
;
}
ItemInfo_t
;
typedef
struct
{
const
char
*
name
;
double
value
;
}
FactValue_t
;
typedef
struct
{
size_t
cFactValues
;
const
FactValue_t
*
rgFactValues
;
bool
relativeAltCheckbox
;
}
ItemExpected_t
;
static
const
ItemInfo_t
_rgItemInfo
[];
static
const
ItemExpected_t
_rgItemExpected
[];
static
const
FactValue_t
_rgFactValuesWaypoint
[];
static
const
FactValue_t
_rgFactValuesLoiterUnlim
[];
static
const
FactValue_t
_rgFactValuesLoiterTurns
[];
static
const
FactValue_t
_rgFactValuesLoiterTime
[];
static
const
FactValue_t
_rgFactValuesLand
[];
static
const
FactValue_t
_rgFactValuesTakeoff
[];
static
const
FactValue_t
_rgFactValuesConditionDelay
[];
static
const
FactValue_t
_rgFactValuesDoJump
[];
};
#endif
src/MissionManager/VisualMissionItem.cc
0 → 100644
View file @
36d57743
/*===================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
#include <QStringList>
#include <QDebug>
#include "VisualMissionItem.h"
#include "FirmwarePluginManager.h"
#include "QGCApplication.h"
#include "JsonHelper.h"
VisualMissionItem
::
VisualMissionItem
(
Vehicle
*
vehicle
,
QObject
*
parent
)
:
QObject
(
parent
)
,
_vehicle
(
vehicle
)
,
_sequenceNumber
(
0
)
,
_isCurrentItem
(
false
)
,
_dirty
(
false
)
,
_altDifference
(
0.0
)
,
_altPercent
(
0.0
)
,
_azimuth
(
0.0
)
,
_distance
(
0.0
)
{
}
VisualMissionItem
::
VisualMissionItem
(
const
VisualMissionItem
&
other
,
QObject
*
parent
)
:
QObject
(
parent
)
,
_vehicle
(
NULL
)
,
_sequenceNumber
(
0
)
,
_isCurrentItem
(
false
)
,
_dirty
(
false
)
,
_altDifference
(
0.0
)
,
_altPercent
(
0.0
)
,
_azimuth
(
0.0
)
,
_distance
(
0.0
)
{
*
this
=
other
;
}
const
VisualMissionItem
&
VisualMissionItem
::
operator
=
(
const
VisualMissionItem
&
other
)
{
_vehicle
=
other
.
_vehicle
;
setSequenceNumber
(
other
.
_sequenceNumber
);
setIsCurrentItem
(
other
.
_isCurrentItem
);
setDirty
(
other
.
_dirty
);
setAltDifference
(
other
.
_altDifference
);
setAltPercent
(
other
.
_altPercent
);
setAzimuth
(
other
.
_azimuth
);
setDistance
(
other
.
_distance
);
return
*
this
;
}
VisualMissionItem
::~
VisualMissionItem
()
{
}
void
VisualMissionItem
::
setIsCurrentItem
(
bool
isCurrentItem
)
{
if
(
_isCurrentItem
!=
isCurrentItem
)
{
_isCurrentItem
=
isCurrentItem
;
emit
isCurrentItemChanged
(
isCurrentItem
);
}
}
void
VisualMissionItem
::
setDistance
(
double
distance
)
{
_distance
=
distance
;
emit
distanceChanged
(
_distance
);
}
void
VisualMissionItem
::
setAltDifference
(
double
altDifference
)
{
_altDifference
=
altDifference
;
emit
altDifferenceChanged
(
_altDifference
);
}
void
VisualMissionItem
::
setAltPercent
(
double
altPercent
)
{
_altPercent
=
altPercent
;
emit
altPercentChanged
(
_altPercent
);
}
void
VisualMissionItem
::
setAzimuth
(
double
azimuth
)
{
_azimuth
=
azimuth
;
emit
azimuthChanged
(
_azimuth
);
}
void
VisualMissionItem
::
setSequenceNumber
(
int
sequenceNumber
)
{
if
(
_sequenceNumber
!=
sequenceNumber
)
{
_sequenceNumber
=
sequenceNumber
;
emit
sequenceNumberChanged
(
_sequenceNumber
);
}
}
src/MissionManager/VisualMissionItem.h
0 → 100644
View file @
36d57743
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
#ifndef VisualMissionItem_H
#define VisualMissionItem_H
#include <QObject>
#include <QString>
#include <QtQml>
#include <QTextStream>
#include <QJsonObject>
#include <QGeoCoordinate>
#include "QGCMAVLink.h"
#include "QGC.h"
#include "MavlinkQmlSingleton.h"
#include "QmlObjectListModel.h"
#include "Fact.h"
#include "QGCLoggingCategory.h"
#include "QmlObjectListModel.h"
#include "MissionCommands.h"
// Abstract base class for all Simple and Complex visual mission objects.
class
VisualMissionItem
:
public
QObject
{
Q_OBJECT
public:
VisualMissionItem
(
Vehicle
*
vehicle
,
QObject
*
parent
=
NULL
);
VisualMissionItem
(
const
VisualMissionItem
&
other
,
QObject
*
parent
=
NULL
);
~
VisualMissionItem
();
const
VisualMissionItem
&
operator
=
(
const
VisualMissionItem
&
other
);
// The following properties are calulated/set by the MissionControll recalc methods
Q_PROPERTY
(
double
altDifference
READ
altDifference
WRITE
setAltDifference
NOTIFY
altDifferenceChanged
)
///< Change in altitude from previous waypoint
Q_PROPERTY
(
double
altPercent
READ
altPercent
WRITE
setAltPercent
NOTIFY
altPercentChanged
)
///< Percent of total altitude change in mission altitude
Q_PROPERTY
(
double
azimuth
READ
azimuth
WRITE
setAzimuth
NOTIFY
azimuthChanged
)
///< Azimuth to previous waypoint
Q_PROPERTY
(
double
distance
READ
distance
WRITE
setDistance
NOTIFY
distanceChanged
)
///< Distance to previous waypoint
// Visual mission items have two coordinates associated with them:
/// This is the entry point for a waypoint line into the item. For a simple item it is also the location of the item
Q_PROPERTY
(
QGeoCoordinate
coordinate
READ
coordinate
WRITE
setCoordinate
NOTIFY
coordinateChanged
)
/// @return true: coordinate.latitude is relative to home altitude
Q_PROPERTY
(
bool
coordinateHasRelativeAltitude
READ
coordinateHasRelativeAltitude
NOTIFY
coordinateHasRelativeAltitudeChanged
)
/// This is the exit point for a waypoint line coming out of the item.
Q_PROPERTY
(
QGeoCoordinate
exitCoordinate
READ
exitCoordinate
NOTIFY
exitCoordinateChanged
)
/// @return true: coordinate.latitude is relative to home altitude
Q_PROPERTY
(
bool
exitCoordinateHasRelativeAltitude
READ
exitCoordinateHasRelativeAltitude
NOTIFY
exitCoordinateHasRelativeAltitudeChanged
)
/// @return true: exitCoordinate and coordinate are the same value
Q_PROPERTY
(
bool
exitCoordinateSameAsEntry
READ
exitCoordinateSameAsEntry
NOTIFY
exitCoordinateSameAsEntry
)
// General properties associated with all types of visual mission items
Q_PROPERTY
(
QString
commandDescription
READ
commandDescription
NOTIFY
commandDescriptionChanged
)
Q_PROPERTY
(
QString
commandName
READ
commandName
NOTIFY
commandNameChanged
)
Q_PROPERTY
(
bool
dirty
READ
dirty
WRITE
setDirty
NOTIFY
dirtyChanged
)
///< Item is dirty and requires save/send
Q_PROPERTY
(
bool
isCurrentItem
READ
isCurrentItem
WRITE
setIsCurrentItem
NOTIFY
isCurrentItemChanged
)
Q_PROPERTY
(
int
sequenceNumber
READ
sequenceNumber
WRITE
setSequenceNumber
NOTIFY
sequenceNumberChanged
)
Q_PROPERTY
(
bool
specifiesCoordinate
READ
specifiesCoordinate
NOTIFY
specifiesCoordinateChanged
)
///< Item is associated with a coordinate position
Q_PROPERTY
(
bool
isStandaloneCoordinate
READ
isStandaloneCoordinate
NOTIFY
isStandaloneCoordinateChanged
)
///< Wayoint line does not go through item
Q_PROPERTY
(
bool
isSimpleItem
READ
isSimpleItem
NOTIFY
isSimpleItemChanged
)
///< Simple or Complex MissionItem
/// List of child mission items. Child mission item are subsequent mision items which do not specify a coordinate. They
/// are shown next to the exitCoordinate indidcator in the ui.
Q_PROPERTY
(
QmlObjectListModel
*
childItems
READ
childItems
CONSTANT
)
// Property accesors
double
altDifference
(
void
)
const
{
return
_altDifference
;
}
double
altPercent
(
void
)
const
{
return
_altPercent
;
}
double
azimuth
(
void
)
const
{
return
_azimuth
;
}
double
distance
(
void
)
const
{
return
_distance
;
}
bool
isCurrentItem
(
void
)
const
{
return
_isCurrentItem
;
}
int
sequenceNumber
(
void
)
const
{
return
_sequenceNumber
;
}
QmlObjectListModel
*
childItems
(
void
)
{
return
&
_childItems
;
}
void
setIsCurrentItem
(
bool
isCurrentItem
);
void
setAltDifference
(
double
altDifference
);
void
setAltPercent
(
double
altPercent
);
void
setAzimuth
(
double
azimuth
);
void
setDistance
(
double
distance
);
void
setSequenceNumber
(
int
sequenceNumber
);
Vehicle
*
vehicle
(
void
)
{
return
_vehicle
;
}
// Pure virtuals which must be provides by derived classes
virtual
bool
dirty
(
void
)
const
=
0
;
virtual
bool
isSimpleItem
(
void
)
const
=
0
;
virtual
bool
isStandaloneCoordinate
(
void
)
const
=
0
;
virtual
bool
specifiesCoordinate
(
void
)
const
=
0
;
virtual
QString
commandDescription
(
void
)
const
=
0
;
virtual
QString
commandName
(
void
)
const
=
0
;
virtual
QGeoCoordinate
coordinate
(
void
)
const
=
0
;
virtual
QGeoCoordinate
exitCoordinate
(
void
)
const
=
0
;
virtual
bool
coordinateHasRelativeAltitude
(
void
)
const
=
0
;
virtual
bool
exitCoordinateHasRelativeAltitude
(
void
)
const
=
0
;
virtual
bool
exitCoordinateSameAsEntry
(
void
)
const
=
0
;
virtual
void
setDirty
(
bool
dirty
)
=
0
;
virtual
void
setCoordinate
(
const
QGeoCoordinate
&
coordinate
)
=
0
;
/// Save the item in Json format
/// @param missionObject Top level object for entire mission file
/// @param missionItems Array of mission items
/// @return false: save failed, errorString set
virtual
bool
save
(
QJsonObject
&
missionObject
,
QJsonArray
&
missionItems
,
QString
&
errorString
)
=
0
;
signals:
void
altDifferenceChanged
(
double
altDifference
);
void
altPercentChanged
(
double
altPercent
);
void
azimuthChanged
(
double
azimuth
);
void
commandDescriptionChanged
(
void
);
void
commandNameChanged
(
void
);
void
coordinateChanged
(
const
QGeoCoordinate
&
coordinate
);
void
exitCoordinateChanged
(
const
QGeoCoordinate
&
exitCoordinate
);
void
dirtyChanged
(
bool
dirty
);
void
distanceChanged
(
double
distance
);
void
isCurrentItemChanged
(
bool
isCurrentItem
);
void
sequenceNumberChanged
(
int
sequenceNumber
);
void
isSimpleItemChanged
(
bool
isSimpleItem
);
void
specifiesCoordinateChanged
(
void
);
void
isStandaloneCoordinateChanged
(
void
);
void
coordinateHasRelativeAltitudeChanged
(
bool
coordinateHasRelativeAltitude
);
void
exitCoordinateHasRelativeAltitudeChanged
(
bool
exitCoordinateHasRelativeAltitude
);
void
exitCoordinateSameAsEntry
(
bool
exitCoordinateSameAsEntry
);
protected:
Vehicle
*
_vehicle
;
int
_sequenceNumber
;
bool
_isCurrentItem
;
bool
_dirty
;
double
_altDifference
;
///< Difference in altitude from previous waypoint
double
_altPercent
;
///< Percent of total altitude change in mission
double
_azimuth
;
///< Azimuth to previous waypoint
double
_distance
;
///< Distance to previous waypoint
/// This is used to reference any subsequent mission items which do not specify a coordinate.
QmlObjectListModel
_childItems
;
};
#endif
src/QmlControls/MissionItemEditor.qml
View file @
36d57743
...
@@ -82,14 +82,14 @@ Rectangle {
...
@@ -82,14 +82,14 @@ Rectangle {
}
}
MenuSeparator
{
MenuSeparator
{
visible
:
!
missionItem
.
s
impleItem
visible
:
missionItem
.
isS
impleItem
}
}
MenuItem
{
MenuItem
{
text
:
"
Show all values
"
text
:
"
Show all values
"
checkable
:
true
checkable
:
true
checked
:
missionItem
.
rawEdit
checked
:
missionItem
.
rawEdit
visible
:
!
missionItem
.
s
impleItem
visible
:
missionItem
.
isS
impleItem
onTriggered
:
{
onTriggered
:
{
if
(
missionItem
.
rawEdit
)
{
if
(
missionItem
.
rawEdit
)
{
...
@@ -114,7 +114,7 @@ Rectangle {
...
@@ -114,7 +114,7 @@ Rectangle {
anchors.rightMargin
:
ScreenTools
.
defaultFontPixelWidth
anchors.rightMargin
:
ScreenTools
.
defaultFontPixelWidth
anchors.left
:
label
.
right
anchors.left
:
label
.
right
anchors.right
:
hamburger
.
left
anchors.right
:
hamburger
.
left
visible
:
missionItem
.
sequenceNumber
!=
0
&&
missionItem
.
isCurrentItem
&&
!
missionItem
.
rawEdit
&&
missionItem
.
s
impleItem
visible
:
missionItem
.
sequenceNumber
!=
0
&&
missionItem
.
isCurrentItem
&&
!
missionItem
.
rawEdit
&&
missionItem
.
isS
impleItem
text
:
missionItem
.
commandName
text
:
missionItem
.
commandName
Component
{
Component
{
...
@@ -130,9 +130,9 @@ Rectangle {
...
@@ -130,9 +130,9 @@ Rectangle {
QGCLabel
{
QGCLabel
{
anchors.fill
:
commandPicker
anchors.fill
:
commandPicker
visible
:
missionItem
.
sequenceNumber
==
0
||
!
missionItem
.
isCurrentItem
||
!
missionItem
.
s
impleItem
visible
:
missionItem
.
sequenceNumber
==
0
||
!
missionItem
.
isCurrentItem
||
!
missionItem
.
isS
impleItem
verticalAlignment
:
Text
.
AlignVCenter
verticalAlignment
:
Text
.
AlignVCenter
text
:
missionItem
.
sequenceNumber
==
0
?
"
Home Position
"
:
(
missionItem
.
s
impleItem
?
missionItem
.
commandName
:
"
Survey
"
)
text
:
missionItem
.
sequenceNumber
==
0
?
"
Home Position
"
:
(
missionItem
.
isS
impleItem
?
missionItem
.
commandName
:
"
Survey
"
)
color
:
_outerTextColor
color
:
_outerTextColor
}
}
...
@@ -142,7 +142,7 @@ Rectangle {
...
@@ -142,7 +142,7 @@ Rectangle {
anchors.topMargin
:
_margin
anchors.topMargin
:
_margin
anchors.left
:
parent
.
left
anchors.left
:
parent
.
left
anchors.top
:
commandPicker
.
bottom
anchors.top
:
commandPicker
.
bottom
sourceComponent
:
missionItem
.
s
impleItem
?
simpleMissionItemEditor
:
complexMissionItemEditor
sourceComponent
:
missionItem
.
isS
impleItem
?
simpleMissionItemEditor
:
complexMissionItemEditor
/// How wide the loaded component should be
/// How wide the loaded component should be
property
real
availableWidth
:
_root
.
width
-
(
_margin
*
2
)
property
real
availableWidth
:
_root
.
width
-
(
_margin
*
2
)
...
...
src/QmlControls/QmlObjectListModel.cc
View file @
36d57743
...
@@ -220,3 +220,11 @@ void QmlObjectListModel::_childDirtyChanged(bool dirty)
...
@@ -220,3 +220,11 @@ void QmlObjectListModel::_childDirtyChanged(bool dirty)
// signal to know when a child has changed dirty state
// signal to know when a child has changed dirty state
emit
dirtyChanged
(
_dirty
);
emit
dirtyChanged
(
_dirty
);
}
}
void
QmlObjectListModel
::
deleteListAndContents
(
void
)
{
for
(
int
i
=
0
;
i
<
_objectList
.
count
();
i
++
)
{
_objectList
[
i
]
->
deleteLater
();
}
deleteLater
();
}
src/QmlControls/QmlObjectListModel.h
View file @
36d57743
...
@@ -60,6 +60,9 @@ public:
...
@@ -60,6 +60,9 @@ public:
int
indexOf
(
QObject
*
object
)
{
return
_objectList
.
indexOf
(
object
);
}
int
indexOf
(
QObject
*
object
)
{
return
_objectList
.
indexOf
(
object
);
}
template
<
class
T
>
T
value
(
int
index
)
{
return
qobject_cast
<
T
>
(
_objectList
[
index
]);
}
template
<
class
T
>
T
value
(
int
index
)
{
return
qobject_cast
<
T
>
(
_objectList
[
index
]);
}
/// Calls deleteLater on all items and this itself.
void
deleteListAndContents
(
void
);
signals:
signals:
void
countChanged
(
int
count
);
void
countChanged
(
int
count
);
void
dirtyChanged
(
bool
dirtyChanged
);
void
dirtyChanged
(
bool
dirtyChanged
);
...
...
src/Vehicle/Vehicle.cc
View file @
36d57743
...
@@ -923,11 +923,6 @@ void Vehicle::setActive(bool active)
...
@@ -923,11 +923,6 @@ void Vehicle::setActive(bool active)
_startJoystick
(
_active
);
_startJoystick
(
_active
);
}
}
QmlObjectListModel
*
Vehicle
::
missionItemsModel
(
void
)
{
return
missionManager
()
->
missionItems
();
}
bool
Vehicle
::
homePositionAvailable
(
void
)
bool
Vehicle
::
homePositionAvailable
(
void
)
{
{
return
_homePositionAvailable
;
return
_homePositionAvailable
;
...
...
src/Vehicle/Vehicle.h
View file @
36d57743
...
@@ -160,7 +160,6 @@ public:
...
@@ -160,7 +160,6 @@ public:
Q_PROPERTY
(
AutoPilotPlugin
*
autopilot
MEMBER
_autopilotPlugin
CONSTANT
)
Q_PROPERTY
(
AutoPilotPlugin
*
autopilot
MEMBER
_autopilotPlugin
CONSTANT
)
Q_PROPERTY
(
QGeoCoordinate
coordinate
READ
coordinate
NOTIFY
coordinateChanged
)
Q_PROPERTY
(
QGeoCoordinate
coordinate
READ
coordinate
NOTIFY
coordinateChanged
)
Q_PROPERTY
(
bool
coordinateValid
READ
coordinateValid
NOTIFY
coordinateValidChanged
)
Q_PROPERTY
(
bool
coordinateValid
READ
coordinateValid
NOTIFY
coordinateValidChanged
)
Q_PROPERTY
(
MissionManager
*
missionManager
MEMBER
_missionManager
CONSTANT
)
Q_PROPERTY
(
bool
homePositionAvailable
READ
homePositionAvailable
NOTIFY
homePositionAvailableChanged
)
Q_PROPERTY
(
bool
homePositionAvailable
READ
homePositionAvailable
NOTIFY
homePositionAvailableChanged
)
Q_PROPERTY
(
QGeoCoordinate
homePosition
READ
homePosition
NOTIFY
homePositionChanged
)
Q_PROPERTY
(
QGeoCoordinate
homePosition
READ
homePosition
NOTIFY
homePositionChanged
)
Q_PROPERTY
(
bool
armed
READ
armed
WRITE
setArmed
NOTIFY
armedChanged
)
Q_PROPERTY
(
bool
armed
READ
armed
WRITE
setArmed
NOTIFY
armedChanged
)
...
@@ -173,7 +172,6 @@ public:
...
@@ -173,7 +172,6 @@ public:
Q_PROPERTY
(
float
latitude
READ
latitude
NOTIFY
coordinateChanged
)
Q_PROPERTY
(
float
latitude
READ
latitude
NOTIFY
coordinateChanged
)
Q_PROPERTY
(
float
longitude
READ
longitude
NOTIFY
coordinateChanged
)
Q_PROPERTY
(
float
longitude
READ
longitude
NOTIFY
coordinateChanged
)
Q_PROPERTY
(
QString
currentState
READ
currentState
NOTIFY
currentStateChanged
)
Q_PROPERTY
(
QString
currentState
READ
currentState
NOTIFY
currentStateChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
missionItems
READ
missionItemsModel
CONSTANT
)
Q_PROPERTY
(
bool
messageTypeNone
READ
messageTypeNone
NOTIFY
messageTypeChanged
)
Q_PROPERTY
(
bool
messageTypeNone
READ
messageTypeNone
NOTIFY
messageTypeChanged
)
Q_PROPERTY
(
bool
messageTypeNormal
READ
messageTypeNormal
NOTIFY
messageTypeChanged
)
Q_PROPERTY
(
bool
messageTypeNormal
READ
messageTypeNormal
NOTIFY
messageTypeChanged
)
Q_PROPERTY
(
bool
messageTypeWarning
READ
messageTypeWarning
NOTIFY
messageTypeChanged
)
Q_PROPERTY
(
bool
messageTypeWarning
READ
messageTypeWarning
NOTIFY
messageTypeChanged
)
...
@@ -237,7 +235,6 @@ public:
...
@@ -237,7 +235,6 @@ public:
QGeoCoordinate
coordinate
(
void
)
{
return
_coordinate
;
}
QGeoCoordinate
coordinate
(
void
)
{
return
_coordinate
;
}
bool
coordinateValid
(
void
)
{
return
_coordinateValid
;
}
bool
coordinateValid
(
void
)
{
return
_coordinateValid
;
}
void
_setCoordinateValid
(
bool
coordinateValid
);
void
_setCoordinateValid
(
bool
coordinateValid
);
QmlObjectListModel
*
missionItemsModel
(
void
);
typedef
enum
{
typedef
enum
{
JoystickModeRC
,
///< Joystick emulates an RC Transmitter
JoystickModeRC
,
///< Joystick emulates an RC Transmitter
...
...
src/qgcunittest/MultiSignalSpy.cc
View file @
36d57743
...
@@ -92,7 +92,8 @@ bool MultiSignalSpy::_checkSignalByMaskWorker(quint16 mask, bool multipleSignals
...
@@ -92,7 +92,8 @@ bool MultiSignalSpy::_checkSignalByMaskWorker(quint16 mask, bool multipleSignals
QSignalSpy
*
spy
=
_rgSpys
[
i
];
QSignalSpy
*
spy
=
_rgSpys
[
i
];
Q_ASSERT
(
spy
!=
NULL
);
Q_ASSERT
(
spy
!=
NULL
);
if
((
multipleSignalsAllowed
&&
spy
->
count
()
==
0
)
||
spy
->
count
()
!=
1
)
{
if
((
multipleSignalsAllowed
&&
spy
->
count
()
==
0
)
||
(
!
multipleSignalsAllowed
&&
spy
->
count
()
!=
1
))
{
qDebug
()
<<
"Failed index:"
<<
i
;
_printSignalState
();
_printSignalState
();
return
false
;
return
false
;
}
}
...
...
src/qgcunittest/UnitTestList.cc
View file @
36d57743
...
@@ -32,6 +32,7 @@
...
@@ -32,6 +32,7 @@
#include "LinkManagerTest.h"
#include "LinkManagerTest.h"
#include "MessageBoxTest.h"
#include "MessageBoxTest.h"
#include "MissionItemTest.h"
#include "MissionItemTest.h"
#include "SimpleMissionItemTest.h"
#include "MissionControllerTest.h"
#include "MissionControllerTest.h"
#include "MissionManagerTest.h"
#include "MissionManagerTest.h"
#include "RadioConfigTest.h"
#include "RadioConfigTest.h"
...
@@ -47,6 +48,7 @@ UT_REGISTER_TEST(LinkManagerTest)
...
@@ -47,6 +48,7 @@ UT_REGISTER_TEST(LinkManagerTest)
UT_REGISTER_TEST
(
MavlinkLogTest
)
UT_REGISTER_TEST
(
MavlinkLogTest
)
UT_REGISTER_TEST
(
MessageBoxTest
)
UT_REGISTER_TEST
(
MessageBoxTest
)
UT_REGISTER_TEST
(
MissionItemTest
)
UT_REGISTER_TEST
(
MissionItemTest
)
UT_REGISTER_TEST
(
SimpleMissionItemTest
)
UT_REGISTER_TEST
(
MissionControllerTest
)
UT_REGISTER_TEST
(
MissionControllerTest
)
UT_REGISTER_TEST
(
MissionManagerTest
)
UT_REGISTER_TEST
(
MissionManagerTest
)
UT_REGISTER_TEST
(
RadioConfigTest
)
UT_REGISTER_TEST
(
RadioConfigTest
)
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment