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
3b151cb4
Commit
3b151cb4
authored
Dec 16, 2019
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
123
parent
8a973ac5
Changes
4
Show whitespace changes
Inline
Side-by-side
Showing
4 changed files
with
369 additions
and
283 deletions
+369
-283
FlightDisplayWimaMenu.qml
src/FlightDisplay/FlightDisplayWimaMenu.qml
+303
-251
WimaController.cc
src/Wima/WimaController.cc
+60
-30
WimaController.h
src/Wima/WimaController.h
+1
-1
WimaPlaner.cc
src/Wima/WimaPlaner.cc
+5
-1
No files found.
src/FlightDisplay/FlightDisplayWimaMenu.qml
View file @
3b151cb4
...
...
@@ -19,14 +19,16 @@ import QGroundControl.FactControls 1.0
Item
{
id
:
_root
height
:
600
width
:
300
height
:
mainFrame
.
height
width
:
mainFrame
.
width
property
var
maxHeight
:
500
property
var
maxWidth
:
300
property
var
wimaController
// must be provided by the user
property
var
planMasterController
// must be provided by the user
property
bool
_controllerValid
:
planMasterController
!==
undefined
property
real
_controllerProgressPct
:
_controllerValid
?
planMasterController
.
missionController
.
progressPct
:
0
property
double
_margins
:
ScreenTools
.
defaultPixelWidth
*
0.3
signal
initSmartRTL
();
...
...
@@ -58,22 +60,48 @@ Item {
id
:
mainFrame
anchors.left
:
parent
.
left
anchors.bottom
:
parent
.
bottom
height
:
enableWima
.
enableWimaBoolean
?
parent
.
height
:
enableWima
.
height
width
:
enableWima
.
enableWimaBoolean
?
parent
.
width
:
enableWima
.
width
height
:
enableWima
.
enableWimaBoolean
?
Math
.
min
(
parent
.
maxHeight
,
enableWima
.
height
+
flickable
.
contentHeight
+
2
*
_margins
)
:
enableWima
.
height
+
_margins
width
:
enableWima
.
enableWimaBoolean
?
Math
.
min
(
parent
.
maxWidth
,
flickable
.
contentWidth
+
2
*
_margins
)
:
enableWima
.
width
color
:
enableWima
.
enableWimaBoolean
?
qgcPal
.
window
:
"
transparent
"
radius
:
ScreenTools
.
defaultFontPixelHeight
/
4
clip
:
false
Component.onCompleted
:
{
console
.
log
(
'
onCompleted
'
)
console
.
log
(
'
height
'
)
console
.
log
(
height
)
console
.
log
(
'
width
'
)
console
.
log
(
width
)
}
onHeightChanged
:
{
console
.
log
(
'
height
'
)
console
.
log
(
height
)
_root
.
height
=
mainFrame
.
height
}
onWidthChanged
:
{
console
.
log
(
'
width
'
)
console
.
log
(
width
)
_root
.
width
=
mainFrame
.
width
}
// checkbox to enable/ disable wima
SliderSwitch
{
id
:
enableWima
anchors.horizontalCenter
:
parent
.
horizontalCenter
anchors.topMargin
:
ScreenTools
.
defaultFontPixelHeight
*
0.25
anchors.topMargin
:
_margins
anchors.top
:
parent
.
top
confirmText
:
enableWimaBoolean
?
qsTr
(
"
disable WiMA
"
)
:
qsTr
(
"
enable WiMA
"
)
property
var
enableWimaFact
:
wimaController
.
enableWimaController
property
bool
enableWimaBoolean
:
enableWimaFact
.
value
Component.onCompleted
:
{
enableWimaFact
.
value
=
false
}
onAccept
:
{
if
(
enableWimaBoolean
)
{
enableWimaFact
.
value
=
false
...
...
@@ -83,27 +111,53 @@ Item {
}
}
Item
{
id
:
flickableWrapper
anchors.top
:
enableWima
.
bottom
anchors.left
:
parent
.
left
anchors.right
:
parent
.
right
anchors.bottom
:
parent
.
bottom
anchors.margins
:
ScreenTools
.
defaultFontPixelHeight
*
0.5
/*MouseArea {
anchors.fill: enableWima
QGCFlickable
{
clip
:
true
anchors.fill
:
parent
contentHeight
:
wrapperItem
.
height
contentWidth
:
wrapperItem
.
width
hoverEnabled: true
Item
{
id
:
wrapperItem
width
:
Math
.
max
(
flickableWrapper
.
width
,
mainColumn
.
width
)
height
:
mainColumn
.
height
property var enableWimaFact: wimaController.enableWimaController
property bool enableWimaBoolean: enableWimaFact.value
onEntered: {
timer.stop()
enableWima.visible = true
}
onExited: {
if (enableWimaBoolean === false) {
timer.start()
}
}
}
Timer {
id: timer
interval: 1000
running: false
repeat: false
onTriggered: enableWima.visible = false
}*/
property
int
itemWidth
:
120
QGCFlickable
{
id
:
flickable
clip
:
false
anchors.top
:
enableWima
.
bottom
anchors.left
:
parent
.
left
contentHeight
:
mainColumn
.
height
contentWidth
:
mainColumn
.
width
onContentHeightChanged
:
{
console
.
log
(
'
contentHeight
'
)
console
.
log
(
contentHeight
)
mainFrame
.
height
=
enableWima
.
enableWimaBoolean
?
Math
.
min
(
parent
.
maxHeight
,
enableWima
.
height
+
flickable
.
contentHeight
+
2
*
_margins
)
:
enableWima
.
height
+
_margins
}
onContentWidthChanged
:
{
console
.
log
(
'
contentWidth
'
)
console
.
log
(
contentWidth
)
mainFrame
.
width
=
enableWima
.
enableWimaBoolean
?
Math
.
min
(
parent
.
maxWidth
,
flickable
.
contentWidth
+
2
*
_margins
)
:
enableWima
.
width
}
Column
{
id
:
mainColumn
anchors.horizontalCenter
:
parent
.
horizontalCenter
...
...
@@ -327,8 +381,6 @@ Item {
}
}
}
// settingsColumn
}
// wrapperItem
}
// QGCFlickable
}
// Item
}
}
src/Wima/WimaController.cc
View file @
3b151cb4
...
...
@@ -32,8 +32,8 @@ WimaController::WimaController(QObject *parent)
,
_flightSpeed
(
settingsGroup
,
_metaDataMap
[
flightSpeedName
])
,
_altitude
(
settingsGroup
,
_metaDataMap
[
altitudeName
])
,
_reverse
(
settingsGroup
,
_metaDataMap
[
reverseName
])
,
_endWaypointIndex
(
0
)
,
_startWaypointIndex
(
0
)
,
_lastMissionPhaseReached
(
false
)
,
_uploadOverrideRequired
(
false
)
,
_phaseDistance
(
-
1
)
,
_phaseDuration
(
-
1
)
...
...
@@ -42,7 +42,7 @@ WimaController::WimaController(QObject *parent)
,
_executingSmartRTL
(
false
)
{
_nextPhaseStartWaypointIndex
.
setRawValue
(
int
(
1
));
//
_nextPhaseStartWaypointIndex.setRawValue(int(1));
_showAllMissionItems
.
setRawValue
(
true
);
_showCurrentMissionItems
.
setRawValue
(
true
);
connect
(
&
_overlapWaypoints
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateNextWaypoint
);
...
...
@@ -50,6 +50,7 @@ WimaController::WimaController(QObject *parent)
connect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
connect
(
&
_flightSpeed
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateSpeed
);
connect
(
&
_altitude
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
updateAltitude
);
connect
(
&
_reverse
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
reverseChangedHandler
);
// setup low battery handling
connect
(
&
_checkBatteryTimer
,
&
QTimer
::
timeout
,
this
,
&
WimaController
::
checkBatteryLevel
);
...
...
@@ -219,18 +220,34 @@ void WimaController::nextPhase()
void
WimaController
::
previousPhase
()
{
if
(
_nextPhaseStartWaypointIndex
.
rawValue
().
toInt
()
>
0
)
{
_lastMissionPhaseReached
=
false
;
_nextPhaseStartWaypointIndex
.
setRawValue
(
std
::
max
(
_startWaypointIndex
bool
reverseBool
=
_reverse
.
rawValue
().
toBool
();
if
(
!
reverseBool
){
int
startIndex
=
_nextPhaseStartWaypointIndex
.
rawValue
().
toInt
();
if
(
startIndex
>
0
)
{
_nextPhaseStartWaypointIndex
.
setRawValue
(
1
+
std
::
max
(
_startWaypointIndex
-
_maxWaypointsPerPhase
.
rawValue
().
toInt
()
+
_overlapWaypoints
.
rawValue
().
toInt
(),
1
));
+
_overlapWaypoints
.
rawValue
().
toInt
(),
0
));
}
}
else
{
int
startIndex
=
_nextPhaseStartWaypointIndex
.
rawValue
().
toInt
();
if
(
startIndex
<=
_missionItems
.
count
())
{
_nextPhaseStartWaypointIndex
.
setRawValue
(
1
+
std
::
min
(
_startWaypointIndex
+
_maxWaypointsPerPhase
.
rawValue
().
toInt
()
-
_overlapWaypoints
.
rawValue
().
toInt
(),
_missionItems
.
count
()
-
1
));
}
}
}
void
WimaController
::
resetPhase
()
{
_lastMissionPhaseReached
=
false
;
bool
reverseBool
=
_reverse
.
rawValue
().
toBool
();
if
(
!
reverseBool
)
{
_nextPhaseStartWaypointIndex
.
setRawValue
(
int
(
1
));
}
else
{
_nextPhaseStartWaypointIndex
.
setRawValue
(
_missionItems
.
count
());
}
}
bool
WimaController
::
uploadToVehicle
()
...
...
@@ -450,7 +467,6 @@ bool WimaController::fetchContainerData()
emit
currentWaypointPathChanged
();
_localPlanDataValid
=
false
;
_lastMissionPhaseReached
=
false
;
if
(
_container
==
nullptr
)
{
qWarning
(
"WimaController::fetchContainerData(): No container assigned!"
);
...
...
@@ -536,7 +552,8 @@ bool WimaController::fetchContainerData()
// set _nextPhaseStartWaypointIndex to 1
disconnect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
_nextPhaseStartWaypointIndex
.
setRawValue
(
int
(
1
));
bool
reverse
=
_reverse
.
rawValue
().
toBool
();
_nextPhaseStartWaypointIndex
.
setRawValue
(
reverse
?
_missionItems
.
count
()
:
int
(
1
));
connect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
if
(
!
calcNextPhase
())
...
...
@@ -554,46 +571,51 @@ bool WimaController::fetchContainerData()
bool
WimaController
::
calcNextPhase
()
{
if
(
_missionItems
.
count
()
<
1
||
_lastMissionPhaseReached
)
if
(
_missionItems
.
count
()
<
1
)
{
_startWaypointIndex
=
0
;
_endWaypointIndex
=
0
;
return
false
;
}
_currentMissionItems
.
clearAndDeleteContents
();
_currentWaypointPath
.
clear
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
bool
reverse
Bool
=
_reverse
.
rawValue
().
toBool
();
// Reverses the phase direction. Phases go from high to low waypoint numbers, if true.
_startWaypoin
tIndex
=
_nextPhaseStartWaypointIndex
.
rawValue
().
toInt
()
-
1
;
if
(
!
reverse
Bool
)
{
if
(
_startWaypoin
tIndex
>
_missionItems
.
count
()
-
1
)
bool
reverse
=
_reverse
.
rawValue
().
toBool
();
// Reverses the phase direction. Phases go from high to low waypoint numbers, if true.
int
star
tIndex
=
_nextPhaseStartWaypointIndex
.
rawValue
().
toInt
()
-
1
;
if
(
!
reverse
)
{
if
(
star
tIndex
>
_missionItems
.
count
()
-
1
)
return
false
;
}
else
{
if
(
_startWaypoin
tIndex
<
0
)
if
(
star
tIndex
<
0
)
return
false
;
}
_startWaypointIndex
=
startIndex
;
int
maxWaypointsPerPhase
Int
=
_maxWaypointsPerPhase
.
rawValue
().
toInt
();
int
maxWaypointsPerPhase
=
_maxWaypointsPerPhase
.
rawValue
().
toInt
();
// determine end waypoint index
if
(
!
reverseBool
)
{
_endWaypointIndex
=
std
::
min
(
_startWaypointIndex
+
maxWaypointsPerPhaseInt
-
1
,
_missionItems
.
count
()
-
1
);
bool
lastMissionPhaseReached
=
false
;
if
(
!
reverse
)
{
_endWaypointIndex
=
std
::
min
(
_startWaypointIndex
+
maxWaypointsPerPhase
-
1
,
_missionItems
.
count
()
-
1
);
if
(
_endWaypointIndex
==
_missionItems
.
count
()
-
1
)
_
lastMissionPhaseReached
=
true
;
lastMissionPhaseReached
=
true
;
}
else
{
_endWaypointIndex
=
std
::
max
(
_startWaypointIndex
-
maxWaypointsPerPhase
Int
+
1
,
0
);
_endWaypointIndex
=
std
::
max
(
_startWaypointIndex
-
maxWaypointsPerPhase
+
1
,
0
);
if
(
_endWaypointIndex
==
0
)
_
lastMissionPhaseReached
=
true
;
lastMissionPhaseReached
=
true
;
}
// extract waypoints
QList
<
QGeoCoordinate
>
geoCoordinateList
;
// list with potential waypoints (from _missionItems), for _currentMissionItems
if
(
!
reverse
Bool
)
{
if
(
!
reverse
)
{
if
(
!
extractCoordinateList
(
_missionItems
,
geoCoordinateList
,
_startWaypointIndex
,
_endWaypointIndex
))
{
qWarning
(
"WimaController::calcNextPhase(): error on waypoint extraction."
);
return
false
;
...
...
@@ -614,9 +636,9 @@ bool WimaController::calcNextPhase()
// set start waypoint index for next phase
if
(
!
_
lastMissionPhaseReached
)
{
if
(
!
lastMissionPhaseReached
)
{
disconnect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
if
(
!
reverse
Bool
)
{
if
(
!
reverse
)
{
int
untruncated
=
std
::
max
(
_endWaypointIndex
+
1
-
_overlapWaypoints
.
rawValue
().
toInt
(),
0
);
int
truncated
=
std
::
min
(
untruncated
,
_missionItems
.
count
()
-
1
);
_nextPhaseStartWaypointIndex
.
setRawValue
(
truncated
+
1
);
...
...
@@ -747,7 +769,6 @@ void WimaController::updateNextWaypoint()
void
WimaController
::
recalcCurrentPhase
()
{
_lastMissionPhaseReached
=
false
;
disconnect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
_nextPhaseStartWaypointIndex
.
setRawValue
(
_startWaypointIndex
+
1
);
connect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
...
...
@@ -845,6 +866,15 @@ void WimaController::enableDisableLowBatteryHandling(QVariant enable)
}
}
void
WimaController
::
reverseChangedHandler
()
{
disconnect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
_nextPhaseStartWaypointIndex
.
setRawValue
(
_endWaypointIndex
+
1
);
connect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
calcNextPhase
);
calcNextPhase
();
}
void
WimaController
::
_setPhaseDistance
(
double
distance
)
{
if
(
!
qFuzzyCompare
(
distance
,
_phaseDistance
))
{
...
...
@@ -996,7 +1026,7 @@ void WimaController::_setVehicleHasLowBattery(bool batteryLow)
bool
WimaController
::
_executeSmartRTL
(
QString
&
errorSring
)
{
Q_UNUSED
(
errorSring
)
;
Q_UNUSED
(
errorSring
)
_executingSmartRTL
=
true
;
connect
(
masterController
()
->
managerVehicle
(),
&
Vehicle
::
flyingChanged
,
this
,
&
WimaController
::
smartRTLCleanUp
);
forceUploadToVehicle
();
...
...
src/Wima/WimaController.h
View file @
3b151cb4
...
...
@@ -177,6 +177,7 @@ private slots:
void
checkBatteryLevel
(
void
);
void
smartRTLCleanUp
(
bool
flying
);
// cleans up after successfull smart RTL
void
enableDisableLowBatteryHandling
(
QVariant
enable
);
void
reverseChangedHandler
();
private:
void
_setPhaseDistance
(
double
distance
);
...
...
@@ -224,7 +225,6 @@ private:
// (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
bool
_lastMissionPhaseReached
;
bool
_uploadOverrideRequired
;
// Is set to true if uploadToVehicle() did not suceed because the vehicle is not inside the service area.
// The user can override the upload lock with a slider, this will reset this variable to false.
double
_phaseDistance
;
// the lenth in meters of the current phase
...
...
src/Wima/WimaPlaner.cc
View file @
3b151cb4
...
...
@@ -708,7 +708,11 @@ WimaPlanData WimaPlaner::toPlanData()
// convert mission items to mavlink commands
QObject
deleteObject
;
// used to automatically delete content of rgMissionItems after this function call
QList
<
MissionItem
*>
rgMissionItems
;
MissionController
::
convertToMissionItems
(
_missionController
->
visualItems
(),
rgMissionItems
,
&
deleteObject
);
QmlObjectListModel
*
visualItems
=
_missionController
->
visualItems
();
QmlObjectListModel
visualItemsToCopy
;
for
(
int
i
=
_arrivalPathLength
+
1
;
i
<
visualItems
->
count
()
-
_returnPathLength
;
i
++
)
visualItemsToCopy
.
append
(
visualItems
->
get
(
i
));
MissionController
::
convertToMissionItems
(
&
visualItemsToCopy
,
rgMissionItems
,
&
deleteObject
);
// store mavlink commands
planData
.
append
(
rgMissionItems
);
...
...
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