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
609e71ac
Commit
609e71ac
authored
Oct 18, 2019
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Wima Menu in flight view improved
parent
ced7b198
Changes
13
Hide whitespace changes
Inline
Side-by-side
Showing
13 changed files
with
81 additions
and
57 deletions
+81
-57
FlightDisplayViewMap.qml
src/FlightDisplay/FlightDisplayViewMap.qml
+3
-1
FlightDisplayWimaMenu.qml
src/FlightDisplay/FlightDisplayWimaMenu.qml
+6
-6
WimaPlanMapItems.qml
src/FlightMap/MapItems/WimaPlanMapItems.qml
+5
-4
SimpleMissionItem.cc
src/MissionManager/SimpleMissionItem.cc
+2
-2
SimpleMissionItem.h
src/MissionManager/SimpleMissionItem.h
+1
-1
StructureScanComplexItem.cc
src/MissionManager/StructureScanComplexItem.cc
+1
-1
StructureScanComplexItem.h
src/MissionManager/StructureScanComplexItem.h
+1
-1
TransectStyleComplexItem.cc
src/MissionManager/TransectStyleComplexItem.cc
+1
-1
TransectStyleComplexItem.h
src/MissionManager/TransectStyleComplexItem.h
+1
-1
VisualMissionItem.cc
src/MissionManager/VisualMissionItem.cc
+7
-1
VisualMissionItem.h
src/MissionManager/VisualMissionItem.h
+1
-1
WimaController.cc
src/Wima/WimaController.cc
+45
-33
WimaController.h
src/Wima/WimaController.h
+7
-4
No files found.
src/FlightDisplay/FlightDisplayViewMap.qml
View file @
609e71ac
...
@@ -202,7 +202,9 @@ FlightMap {
...
@@ -202,7 +202,9 @@ FlightMap {
// Add wima Areas to the Map
// Add wima Areas to the Map
MapItemView
{
MapItemView
{
model
:
wimaController
.
visualItems
property
bool
_enableWima
:
wimaController
.
enableWimaController
.
value
model
:
_enableWima
?
wimaController
.
visualItems
:
0
delegate
:
MapPolygon
{
delegate
:
MapPolygon
{
path
:
object
.
path
;
path
:
object
.
path
;
...
...
src/FlightDisplay/FlightDisplayWimaMenu.qml
View file @
609e71ac
...
@@ -120,16 +120,16 @@ Item {
...
@@ -120,16 +120,16 @@ Item {
// Buttons
// Buttons
QGCButton
{
QGCButton
{
id
:
button
Next
MissionPhase
id
:
button
Previous
MissionPhase
text
:
qsTr
(
"
Next Pha
se
"
)
text
:
qsTr
(
"
Rever
se
"
)
onClicked
:
wimaController
.
next
Phase
();
onClicked
:
wimaController
.
previous
Phase
();
Layout.fillWidth
:
true
Layout.fillWidth
:
true
}
}
QGCButton
{
QGCButton
{
id
:
button
Previous
MissionPhase
id
:
button
Next
MissionPhase
text
:
qsTr
(
"
Previous Phase
"
)
text
:
qsTr
(
"
Forward
"
)
onClicked
:
wimaController
.
previous
Phase
();
onClicked
:
wimaController
.
next
Phase
();
Layout.fillWidth
:
true
Layout.fillWidth
:
true
}
}
...
...
src/FlightMap/MapItems/WimaPlanMapItems.qml
View file @
609e71ac
...
@@ -33,10 +33,11 @@ Item {
...
@@ -33,10 +33,11 @@ Item {
property
bool
_showAllItems
:
wimaController
.
showAllMissionItems
.
value
property
bool
_showAllItems
:
wimaController
.
showAllMissionItems
.
value
property
bool
_showCurrentItems
:
wimaController
.
showCurrentMissionItems
.
value
property
bool
_showCurrentItems
:
wimaController
.
showCurrentMissionItems
.
value
property
bool
_wimaEnabled
:
wimaController
.
enableWimaController
.
value
// Add the mission item visuals to the map
// Add the mission item visuals to the map
Repeater
{
Repeater
{
model
:
largeMapView
?
(
_showAllItems
?
wimaController
.
missionItems
:
0
)
:
0
model
:
largeMapView
?
(
_showAllItems
?
(
_wimaEnabled
?
wimaController
.
missionItems
:
0
)
:
0
)
:
0
z
:
QGroundControl
.
zOrderWaypointIndicators
-
2
z
:
QGroundControl
.
zOrderWaypointIndicators
-
2
delegate
:
WimaMissionItemMapVisual
{
delegate
:
WimaMissionItemMapVisual
{
...
@@ -53,7 +54,7 @@ Item {
...
@@ -53,7 +54,7 @@ Item {
// Add the current mission item visuals to the map
// Add the current mission item visuals to the map
Repeater
{
Repeater
{
model
:
largeMapView
?
(
_showCurrentItems
?
wimaController
.
currentMissionItems
:
0
)
:
0
model
:
largeMapView
?
(
_showCurrentItems
?
(
_wimaEnabled
?
wimaController
.
currentMissionItems
:
0
)
:
0
)
:
0
z
:
QGroundControl
.
zOrderWaypointIndicators
-
1
z
:
QGroundControl
.
zOrderWaypointIndicators
-
1
delegate
:
WimaMissionItemMapVisual
{
delegate
:
WimaMissionItemMapVisual
{
...
@@ -85,7 +86,7 @@ Item {
...
@@ -85,7 +86,7 @@ Item {
MapPolyline
{
MapPolyline
{
line.width
:
3
line.width
:
3
line.color
:
_showAllItems
?
mIlineColor
:
"
transparent
"
line.color
:
_showAllItems
?
(
_wimaEnabled
?
mIlineColor
:
"
transparent
"
)
:
"
transparent
"
z
:
QGroundControl
.
zOrderWaypointLines
-
2
z
:
QGroundControl
.
zOrderWaypointLines
-
2
path
:
wimaController
.
waypointPath
path
:
wimaController
.
waypointPath
}
}
...
@@ -96,7 +97,7 @@ Item {
...
@@ -96,7 +97,7 @@ Item {
MapPolyline
{
MapPolyline
{
line.width
:
3
line.width
:
3
line.color
:
_showCurrentItems
?
cMIlineColor
:
"
transparent
"
line.color
:
_showCurrentItems
?
(
_wimaEnabled
?
cMIlineColor
:
"
transparent
"
)
:
"
transparent
"
z
:
QGroundControl
.
zOrderWaypointLines
-
1
z
:
QGroundControl
.
zOrderWaypointLines
-
1
path
:
wimaController
.
currentWaypointPath
path
:
wimaController
.
currentWaypointPath
}
}
...
...
src/MissionManager/SimpleMissionItem.cc
View file @
609e71ac
...
@@ -168,7 +168,7 @@ SimpleMissionItem::SimpleMissionItem(const SimpleMissionItem& other, bool flyVie
...
@@ -168,7 +168,7 @@ SimpleMissionItem::SimpleMissionItem(const SimpleMissionItem& other, bool flyVie
_editorQml
=
QStringLiteral
(
"qrc:/qml/SimpleItemEditor.qml"
);
_editorQml
=
QStringLiteral
(
"qrc:/qml/SimpleItemEditor.qml"
);
_altitudeFact
.
setRawValue
(
other
.
_altitudeFact
.
rawValue
());
_altitudeFact
.
setRawValue
(
other
.
_altitudeFact
.
rawValue
());
_amslAltAboveTerrainFact
.
setRawValue
(
other
.
_amslAltAboveTerrainFact
.
rawValue
());
_amslAltAboveTerrainFact
.
setRawValue
(
other
.
_amslAltAboveTerrainFact
.
rawValue
());
_setupMetaData
();
_setupMetaData
();
_connectSignals
();
_connectSignals
();
...
@@ -954,7 +954,7 @@ void SimpleMissionItem::applyNewAltitude(double newAltitude)
...
@@ -954,7 +954,7 @@ void SimpleMissionItem::applyNewAltitude(double newAltitude)
}
}
}
}
void
SimpleMissionItem
::
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
void
SimpleMissionItem
::
setMissionFlightStatus
(
const
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
{
{
// If user has not already set speed/gimbal, set defaults from previous items.
// If user has not already set speed/gimbal, set defaults from previous items.
VisualMissionItem
::
setMissionFlightStatus
(
missionFlightStatus
);
VisualMissionItem
::
setMissionFlightStatus
(
missionFlightStatus
);
...
...
src/MissionManager/SimpleMissionItem.h
View file @
609e71ac
...
@@ -112,7 +112,7 @@ public:
...
@@ -112,7 +112,7 @@ public:
QString
mapVisualQML
(
void
)
const
final
{
return
QStringLiteral
(
"SimpleItemMapVisual.qml"
);
}
QString
mapVisualQML
(
void
)
const
final
{
return
QStringLiteral
(
"SimpleItemMapVisual.qml"
);
}
void
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
void
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
void
applyNewAltitude
(
double
newAltitude
)
final
;
void
applyNewAltitude
(
double
newAltitude
)
final
;
void
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
void
setMissionFlightStatus
(
const
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
bool
readyForSave
(
void
)
const
final
;
bool
readyForSave
(
void
)
const
final
;
double
additionalTimeDelay
(
void
)
const
final
;
double
additionalTimeDelay
(
void
)
const
final
;
...
...
src/MissionManager/StructureScanComplexItem.cc
View file @
609e71ac
...
@@ -441,7 +441,7 @@ int StructureScanComplexItem::cameraShots(void) const
...
@@ -441,7 +441,7 @@ int StructureScanComplexItem::cameraShots(void) const
return
_cameraShots
;
return
_cameraShots
;
}
}
void
StructureScanComplexItem
::
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
void
StructureScanComplexItem
::
setMissionFlightStatus
(
const
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
{
{
ComplexMissionItem
::
setMissionFlightStatus
(
missionFlightStatus
);
ComplexMissionItem
::
setMissionFlightStatus
(
missionFlightStatus
);
if
(
!
qFuzzyCompare
(
_cruiseSpeed
,
missionFlightStatus
.
vehicleSpeed
))
{
if
(
!
qFuzzyCompare
(
_cruiseSpeed
,
missionFlightStatus
.
vehicleSpeed
))
{
...
...
src/MissionManager/StructureScanComplexItem.h
View file @
609e71ac
...
@@ -86,7 +86,7 @@ public:
...
@@ -86,7 +86,7 @@ public:
double
specifiedGimbalYaw
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalYaw
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalPitch
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalPitch
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
void
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
void
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
void
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
void
setMissionFlightStatus
(
const
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
void
applyNewAltitude
(
double
newAltitude
)
final
;
void
applyNewAltitude
(
double
newAltitude
)
final
;
double
additionalTimeDelay
(
void
)
const
final
{
return
0
;
}
double
additionalTimeDelay
(
void
)
const
final
{
return
0
;
}
...
...
src/MissionManager/TransectStyleComplexItem.cc
View file @
609e71ac
...
@@ -291,7 +291,7 @@ double TransectStyleComplexItem::greatestDistanceTo(const QGeoCoordinate &other)
...
@@ -291,7 +291,7 @@ double TransectStyleComplexItem::greatestDistanceTo(const QGeoCoordinate &other)
return
greatestDistance
;
return
greatestDistance
;
}
}
void
TransectStyleComplexItem
::
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
void
TransectStyleComplexItem
::
setMissionFlightStatus
(
const
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
{
{
ComplexMissionItem
::
setMissionFlightStatus
(
missionFlightStatus
);
ComplexMissionItem
::
setMissionFlightStatus
(
missionFlightStatus
);
if
(
!
qFuzzyCompare
(
_cruiseSpeed
,
missionFlightStatus
.
vehicleSpeed
))
{
if
(
!
qFuzzyCompare
(
_cruiseSpeed
,
missionFlightStatus
.
vehicleSpeed
))
{
...
...
src/MissionManager/TransectStyleComplexItem.h
View file @
609e71ac
...
@@ -98,7 +98,7 @@ public:
...
@@ -98,7 +98,7 @@ public:
double
specifiedFlightSpeed
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedFlightSpeed
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalYaw
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalYaw
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalPitch
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalPitch
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
void
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
void
setMissionFlightStatus
(
const
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
bool
readyForSave
(
void
)
const
override
;
bool
readyForSave
(
void
)
const
override
;
QString
commandDescription
(
void
)
const
override
{
return
tr
(
"Transect"
);
}
QString
commandDescription
(
void
)
const
override
{
return
tr
(
"Transect"
);
}
QString
commandName
(
void
)
const
override
{
return
tr
(
"Transect"
);
}
QString
commandName
(
void
)
const
override
{
return
tr
(
"Transect"
);
}
...
...
src/MissionManager/VisualMissionItem.cc
View file @
609e71ac
...
@@ -85,8 +85,14 @@ const VisualMissionItem& VisualMissionItem::operator=(const VisualMissionItem& o
...
@@ -85,8 +85,14 @@ const VisualMissionItem& VisualMissionItem::operator=(const VisualMissionItem& o
setAltDifference
(
other
.
_altDifference
);
setAltDifference
(
other
.
_altDifference
);
setAltPercent
(
other
.
_altPercent
);
setAltPercent
(
other
.
_altPercent
);
setTerrainPercent
(
other
.
_terrainPercent
);
setTerrainPercent
(
other
.
_terrainPercent
);
setTerrainCollision
(
other
.
_terrainCollision
);
setAzimuth
(
other
.
_azimuth
);
setAzimuth
(
other
.
_azimuth
);
setDistance
(
other
.
_distance
);
setDistance
(
other
.
_distance
);
_missionGimbalYaw
=
other
.
_missionGimbalYaw
;
_missionVehicleYaw
=
other
.
_missionVehicleYaw
;
_setBoundingCube
(
other
.
_boundingCube
);
setMissionFlightStatus
(
other
.
_missionFlightStatus
);
// _childItems // necessary here?
return
*
this
;
return
*
this
;
}
}
...
@@ -151,7 +157,7 @@ void VisualMissionItem::setAzimuth(double azimuth)
...
@@ -151,7 +157,7 @@ void VisualMissionItem::setAzimuth(double azimuth)
}
}
}
}
void
VisualMissionItem
::
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
void
VisualMissionItem
::
setMissionFlightStatus
(
const
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
{
{
_missionFlightStatus
=
missionFlightStatus
;
_missionFlightStatus
=
missionFlightStatus
;
if
(
qIsNaN
(
_missionFlightStatus
.
gimbalYaw
)
&&
qIsNaN
(
_missionGimbalYaw
))
{
if
(
qIsNaN
(
_missionFlightStatus
.
gimbalYaw
)
&&
qIsNaN
(
_missionGimbalYaw
))
{
...
...
src/MissionManager/VisualMissionItem.h
View file @
609e71ac
...
@@ -128,7 +128,7 @@ public:
...
@@ -128,7 +128,7 @@ public:
/// Update item to mission flight status at point where this item appears in mission.
/// Update item to mission flight status at point where this item appears in mission.
/// IMPORTANT: Overrides must call base class implementation
/// IMPORTANT: Overrides must call base class implementation
virtual
void
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
);
virtual
void
setMissionFlightStatus
(
const
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
);
virtual
bool
coordinateHasRelativeAltitude
(
void
)
const
=
0
;
virtual
bool
coordinateHasRelativeAltitude
(
void
)
const
=
0
;
virtual
bool
exitCoordinateHasRelativeAltitude
(
void
)
const
=
0
;
virtual
bool
exitCoordinateHasRelativeAltitude
(
void
)
const
=
0
;
...
...
src/Wima/WimaController.cc
View file @
609e71ac
...
@@ -23,16 +23,16 @@ WimaController::WimaController(QObject *parent)
...
@@ -23,16 +23,16 @@ WimaController::WimaController(QObject *parent)
,
_enableWimaController
(
settingsGroup
,
_metaDataMap
[
enableWimaControllerName
])
,
_enableWimaController
(
settingsGroup
,
_metaDataMap
[
enableWimaControllerName
])
,
_overlapWaypoints
(
settingsGroup
,
_metaDataMap
[
overlapWaypointsName
])
,
_overlapWaypoints
(
settingsGroup
,
_metaDataMap
[
overlapWaypointsName
])
,
_maxWaypointsPerPhase
(
settingsGroup
,
_metaDataMap
[
maxWaypointsPerPhaseName
])
,
_maxWaypointsPerPhase
(
settingsGroup
,
_metaDataMap
[
maxWaypointsPerPhaseName
])
,
_
s
tartWaypointIndex
(
settingsGroup
,
_metaDataMap
[
startWaypointIndexName
])
,
_
nestPhaseS
tartWaypointIndex
(
settingsGroup
,
_metaDataMap
[
startWaypointIndexName
])
,
_showAllMissionItems
(
settingsGroup
,
_metaDataMap
[
showAllMissionItemsName
])
,
_showAllMissionItems
(
settingsGroup
,
_metaDataMap
[
showAllMissionItemsName
])
,
_showCurrentMissionItems
(
settingsGroup
,
_metaDataMap
[
showCurrentMissionItemsName
])
,
_showCurrentMissionItems
(
settingsGroup
,
_metaDataMap
[
showCurrentMissionItemsName
])
{
{
_
s
tartWaypointIndex
.
setRawValue
(
int
(
1
));
_
nestPhaseS
tartWaypointIndex
.
setRawValue
(
int
(
1
));
_showAllMissionItems
.
setRawValue
(
true
);
_showAllMissionItems
.
setRawValue
(
true
);
_showCurrentMissionItems
.
setRawValue
(
true
);
_showCurrentMissionItems
.
setRawValue
(
true
);
connect
(
&
_overlapWaypoints
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateNextWaypoint
);
connect
(
&
_overlapWaypoints
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateNextWaypoint
);
connect
(
&
_maxWaypointsPerPhase
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateCurrentMissionItems
);
connect
(
&
_maxWaypointsPerPhase
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
recalcCurrentPhase
);
connect
(
&
_
startWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateCurrentMissionItems
);
connect
(
&
_
nestPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
}
}
QmlObjectListModel
*
WimaController
::
visualItems
()
QmlObjectListModel
*
WimaController
::
visualItems
()
...
@@ -109,7 +109,7 @@ Fact *WimaController::showCurrentMissionItems()
...
@@ -109,7 +109,7 @@ Fact *WimaController::showCurrentMissionItems()
Fact
*
WimaController
::
startWaypointIndex
()
Fact
*
WimaController
::
startWaypointIndex
()
{
{
return
&
_
s
tartWaypointIndex
;
return
&
_
nestPhaseS
tartWaypointIndex
;
}
}
void
WimaController
::
setMasterController
(
PlanMasterController
*
masterC
)
void
WimaController
::
setMasterController
(
PlanMasterController
*
masterC
)
...
@@ -134,11 +134,11 @@ void WimaController::setDataContainer(WimaDataContainer *container)
...
@@ -134,11 +134,11 @@ void WimaController::setDataContainer(WimaDataContainer *container)
{
{
if
(
container
!=
nullptr
)
{
if
(
container
!=
nullptr
)
{
if
(
_container
!=
nullptr
)
{
if
(
_container
!=
nullptr
)
{
disconnect
(
_container
,
&
WimaDataContainer
::
dataValidChanged
,
this
,
&
WimaController
::
containerDataValidChanged
);
disconnect
(
_container
,
&
WimaDataContainer
::
dataValidChanged
,
this
,
&
WimaController
::
fetchContainerData
);
}
}
_container
=
container
;
_container
=
container
;
connect
(
_container
,
&
WimaDataContainer
::
dataValidChanged
,
this
,
&
WimaController
::
containerDataValidChanged
);
connect
(
_container
,
&
WimaDataContainer
::
dataValidChanged
,
this
,
&
WimaController
::
fetchContainerData
);
emit
dataContainerChanged
();
emit
dataContainerChanged
();
}
}
...
@@ -146,22 +146,21 @@ void WimaController::setDataContainer(WimaDataContainer *container)
...
@@ -146,22 +146,21 @@ void WimaController::setDataContainer(WimaDataContainer *container)
void
WimaController
::
nextPhase
()
void
WimaController
::
nextPhase
()
{
{
updateCurrentMissionItems
();
calcNextPhase
();
}
}
void
WimaController
::
previousPhase
()
void
WimaController
::
previousPhase
()
{
{
if
(
_startWaypointIndex
.
rawValue
().
toInt
()
>
0
)
{
if
(
_nestPhaseStartWaypointIndex
.
rawValue
().
toInt
()
>
0
)
{
_nestPhaseStartWaypointIndex
.
setRawValue
(
1
+
std
::
max
(
_startWaypointIndex
_startWaypointIndex
.
setRawValue
(
_startWaypointIndex
.
rawValue
().
toInt
()
-
_maxWaypointsPerPhase
.
rawValue
().
toInt
()
-
_maxWaypointsPerPhase
.
rawValue
().
toInt
()
+
_overlapWaypoints
.
rawValue
().
toInt
());
+
_overlapWaypoints
.
rawValue
().
toInt
(),
0
));
}
}
}
}
void
WimaController
::
resetPhase
()
void
WimaController
::
resetPhase
()
{
{
_
s
tartWaypointIndex
.
setRawValue
(
int
(
1
));
_
nestPhaseS
tartWaypointIndex
.
setRawValue
(
int
(
1
));
}
}
void
WimaController
::
uploadToVehicle
()
void
WimaController
::
uploadToVehicle
()
...
@@ -300,7 +299,7 @@ bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVa
...
@@ -300,7 +299,7 @@ bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVa
*
*
* \sa WimaDataContainer, WimaPlaner, WimaPlanData
* \sa WimaDataContainer, WimaPlaner, WimaPlanData
*/
*/
void
WimaController
::
containerDataValidChanged
(
bool
valid
)
void
WimaController
::
fetchContainerData
(
bool
valid
)
{
{
if
(
valid
)
{
if
(
valid
)
{
if
(
_container
==
nullptr
)
{
if
(
_container
==
nullptr
)
{
...
@@ -403,6 +402,10 @@ void WimaController::containerDataValidChanged(bool valid)
...
@@ -403,6 +402,10 @@ void WimaController::containerDataValidChanged(bool valid)
return
;
return
;
}
}
SimpleMissionItem
*
visualItemCopy
=
new
SimpleMissionItem
(
*
visualItem
,
true
,
this
);
SimpleMissionItem
*
visualItemCopy
=
new
SimpleMissionItem
(
*
visualItem
,
true
,
this
);
qWarning
()
<<
visualItem
->
missionGimbalYaw
();
qWarning
()
<<
visualItemCopy
->
missionGimbalYaw
();
qWarning
(
""
);
_missionItems
.
append
(
visualItemCopy
);
_missionItems
.
append
(
visualItemCopy
);
}
}
...
@@ -410,8 +413,8 @@ void WimaController::containerDataValidChanged(bool valid)
...
@@ -410,8 +413,8 @@ void WimaController::containerDataValidChanged(bool valid)
_localPlanDataValid
=
true
;
_localPlanDataValid
=
true
;
updateWaypointPath
();
updateWaypointPath
();
_
s
tartWaypointIndex
.
setRawValue
(
int
(
1
));
_
nestPhaseS
tartWaypointIndex
.
setRawValue
(
int
(
1
));
updateCurrentMissionItems
();
calcNextPhase
();
}
else
{
}
else
{
_localPlanDataValid
=
false
;
_localPlanDataValid
=
false
;
...
@@ -428,21 +431,21 @@ void WimaController::containerDataValidChanged(bool valid)
...
@@ -428,21 +431,21 @@ void WimaController::containerDataValidChanged(bool valid)
#endif
#endif
}
}
void
WimaController
::
updateCurrentMissionItems
()
void
WimaController
::
calcNextPhase
()
{
{
int
startWaypointIndexInt
=
_s
tartWaypointIndex
.
rawValue
().
toInt
()
-
1
;
_startWaypointIndex
=
_nestPhaseS
tartWaypointIndex
.
rawValue
().
toInt
()
-
1
;
// check if data was fetched and mission end is not reached yet
// check if data was fetched and mission end is not reached yet
if
(
_missionItems
.
count
()
<
1
||
!
_localPlanDataValid
||
startWaypointIndexInt
>=
_missionItems
.
count
()
-
2
)
if
(
_missionItems
.
count
()
<
1
||
!
_localPlanDataValid
||
_startWaypointIndex
>=
_missionItems
.
count
()
-
2
)
return
;
return
;
int
maxWaypointsPerPhaseInt
=
_maxWaypointsPerPhase
.
rawValue
().
toInt
();
int
maxWaypointsPerPhaseInt
=
_maxWaypointsPerPhase
.
rawValue
().
toInt
();
// determine end waypoint index
// determine end waypoint index
_endWaypointIndex
=
std
::
min
(
startWaypointIndexInt
+
maxWaypointsPerPhaseInt
-
1
,
_missionItems
.
count
()
-
2
);
// -2 -> last item is land item
_endWaypointIndex
=
std
::
min
(
_startWaypointIndex
+
maxWaypointsPerPhaseInt
-
1
,
_missionItems
.
count
()
-
2
);
// -2 -> last item is land item
// extract waypoints
// extract waypoints
QList
<
QGeoCoordinate
>
geoCoordinateList
;
// list with potential waypoints (from _missionItems), for _currentMissionItems
QList
<
QGeoCoordinate
>
geoCoordinateList
;
// list with potential waypoints (from _missionItems), for _currentMissionItems
if
(
!
extractCoordinateList
(
_missionItems
,
geoCoordinateList
,
startWaypointIndexInt
,
_endWaypointIndex
))
{
if
(
!
extractCoordinateList
(
_missionItems
,
geoCoordinateList
,
_startWaypointIndex
,
_endWaypointIndex
))
{
qWarning
(
"WimaController::updateCurrentMissionItems(): error on waypoint extraction."
);
qWarning
(
"WimaController::updateCurrentMissionItems(): error on waypoint extraction."
);
_currentMissionItems
.
clear
();
_currentMissionItems
.
clear
();
return
;
return
;
...
@@ -450,16 +453,17 @@ void WimaController::updateCurrentMissionItems()
...
@@ -450,16 +453,17 @@ void WimaController::updateCurrentMissionItems()
// set start waypoint index for next phase
// set start waypoint index for next phase
if
(
_endWaypointIndex
<
_missionItems
.
count
()
-
2
)
{
if
(
_endWaypointIndex
<
_missionItems
.
count
()
-
2
)
{
_startWaypointIndexList
.
append
(
_
s
tartWaypointIndex
.
rawValue
().
toInt
());
_startWaypointIndexList
.
append
(
_
nestPhaseS
tartWaypointIndex
.
rawValue
().
toInt
());
disconnect
(
&
_startWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateCurrentMissionItems
);
disconnect
(
&
_nestPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
_startWaypointIndex
.
setRawValue
(
std
::
max
(
_endWaypointIndex
+
2
-
_overlapWaypoints
.
rawValue
().
toInt
(),
1
));
int
temp
=
1
+
std
::
max
(
_endWaypointIndex
+
1
-
_overlapWaypoints
.
rawValue
().
toInt
(),
0
);
connect
(
&
_startWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateCurrentMissionItems
);
_nestPhaseStartWaypointIndex
.
setRawValue
(
std
::
min
(
temp
,
_missionItems
.
count
()
-
1
));
connect
(
&
_nestPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
}
}
else
{
else
{
disconnect
(
&
_
startWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateCurrentMissionItems
);
disconnect
(
&
_
nestPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
_
s
tartWaypointIndex
.
setRawValue
(
_missionItems
.
count
()
-
1
);
// marks that end of mission is reached
_
nestPhaseS
tartWaypointIndex
.
setRawValue
(
_missionItems
.
count
()
-
1
);
// marks that end of mission is reached
connect
(
&
_
startWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateCurrentMissionItems
);
connect
(
&
_
nestPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
}
}
// calculate path from home to first waypoint
// calculate path from home to first waypoint
...
@@ -538,7 +542,10 @@ void WimaController::updateCurrentMissionItems()
...
@@ -538,7 +542,10 @@ void WimaController::updateCurrentMissionItems()
return
;
return
;
}
}
SimpleMissionItem
*
visualItemCopy
=
new
SimpleMissionItem
(
*
visualItem
,
true
,
this
);
SimpleMissionItem
*
visualItemCopy
=
new
SimpleMissionItem
(
*
visualItem
,
true
,
this
);
qWarning
()
<<
visualItem
->
missionGimbalYaw
();
qWarning
()
<<
visualItemCopy
->
missionGimbalYaw
();
qWarning
(
""
);
_currentMissionItems
.
append
(
visualItemCopy
);
_currentMissionItems
.
append
(
visualItemCopy
);
}
}
...
@@ -564,11 +571,16 @@ void WimaController::updateCurrentPath()
...
@@ -564,11 +571,16 @@ void WimaController::updateCurrentPath()
void
WimaController
::
updateNextWaypoint
()
void
WimaController
::
updateNextWaypoint
()
{
{
if
(
_endWaypointIndex
<
_missionItems
.
count
()
-
2
)
{
if
(
_endWaypointIndex
<
_missionItems
.
count
()
-
2
)
{
disconnect
(
&
_
startWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateCurrentMissionItems
);
disconnect
(
&
_
nestPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
_
startWaypointIndex
.
setRawValue
(
_endWaypointIndex
+
2
-
_overlapWaypoints
.
rawValue
().
toInt
(
));
_
nestPhaseStartWaypointIndex
.
setRawValue
(
1
+
std
::
max
(
_endWaypointIndex
+
1
-
_overlapWaypoints
.
rawValue
().
toInt
(),
0
));
connect
(
&
_
startWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateCurrentMissionItems
);
connect
(
&
_
nestPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
}
}
}
}
void
WimaController
::
recalcCurrentPhase
()
{
_nestPhaseStartWaypointIndex
.
setRawValue
(
_startWaypointIndex
+
1
);
}
src/Wima/WimaController.h
View file @
609e71ac
...
@@ -137,11 +137,12 @@ signals:
...
@@ -137,11 +137,12 @@ signals:
void
currentWaypointPathChanged
(
void
);
void
currentWaypointPathChanged
(
void
);
private
slots
:
private
slots
:
void
containerDataValidChanged
(
bool
valid
);
void
fetchContainerData
(
bool
valid
);
void
updateCurrentMissionItems
(
void
);
void
calcNextPhase
(
void
);
void
updateWaypointPath
(
void
);
void
updateWaypointPath
(
void
);
void
updateCurrentPath
(
void
);
void
updateCurrentPath
(
void
);
void
updateNextWaypoint
(
void
);
void
updateNextWaypoint
(
void
);
void
recalcCurrentPhase
(
void
);
private:
private:
...
@@ -169,11 +170,13 @@ private:
...
@@ -169,11 +170,13 @@ private:
SettingsFact
_enableWimaController
;
// enables or disables the wimaControler
SettingsFact
_enableWimaController
;
// enables or disables the wimaControler
SettingsFact
_overlapWaypoints
;
// determines the number of overlapping waypoints between two consecutive mission phases
SettingsFact
_overlapWaypoints
;
// determines the number of overlapping waypoints between two consecutive mission phases
SettingsFact
_maxWaypointsPerPhase
;
// determines the maximum number waypoints per phase
SettingsFact
_maxWaypointsPerPhase
;
// determines the maximum number waypoints per phase
SettingsFact
_
s
tartWaypointIndex
;
// index (displayed on the map, -1 to get index of item in _missionItems) of the mission item
SettingsFact
_
nestPhaseS
tartWaypointIndex
;
// index (displayed on the map, -1 to get index of item in _missionItems) of the mission item
//
stored in _missionItems defining the first element of _currentMissionItems
//
defining the first element of the next phase
SettingsFact
_showAllMissionItems
;
// bool value, Determines whether the mission items of the overall mission are displayed or not.
SettingsFact
_showAllMissionItems
;
// bool value, Determines whether the mission items of the overall mission are displayed or not.
SettingsFact
_showCurrentMissionItems
;
// bool value, Determines whether the mission items of the current mission phase are displayed or not.
SettingsFact
_showCurrentMissionItems
;
// bool value, Determines whether the mission items of the current mission phase are displayed or not.
int
_endWaypointIndex
;
// indes of the mission item stored in _missionItems defining the last element
int
_endWaypointIndex
;
// indes of the mission item stored in _missionItems defining the last element
// (which is not part of the return path) of _currentMissionItem
// (which is not part of the return path) of _currentMissionItem
int
_startWaypointIndex
;
// indes of the mission item stored in _missionItems defining the first element
// (which is not part of the arrival path) of _currentMissionItem
};
};
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