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
fead2b5e
Commit
fead2b5e
authored
Dec 03, 2015
by
Don Gagne
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
commit
parent
44407a35
Changes
5
Hide whitespace changes
Inline
Side-by-side
Showing
5 changed files
with
61 additions
and
43 deletions
+61
-43
MissionEditor.qml
src/MissionEditor/MissionEditor.qml
+23
-21
MissionController.cc
src/MissionManager/MissionController.cc
+21
-19
MissionController.h
src/MissionManager/MissionController.h
+1
-1
MissionItem.cc
src/MissionManager/MissionItem.cc
+10
-1
MissionItem.h
src/MissionManager/MissionItem.h
+6
-1
No files found.
src/MissionEditor/MissionEditor.qml
View file @
fead2b5e
...
@@ -59,7 +59,8 @@ QGCView {
...
@@ -59,7 +59,8 @@ 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
.
missionItems
property
var
_missionItems
:
controller
.
missionItems
property
var
_currentMissionItem
property
bool
gpsLock
:
_activeVehicle
?
_activeVehicle
.
coordinateValid
:
false
property
bool
gpsLock
:
_activeVehicle
?
_activeVehicle
.
coordinateValid
:
false
property
bool
_firstGpsLock
:
true
property
bool
_firstGpsLock
:
true
...
@@ -92,16 +93,6 @@ QGCView {
...
@@ -92,16 +93,6 @@ QGCView {
}
}
}
}
function
showDistance
(
missionItem
)
{
if
(
missionItem
.
distance
<
0.0
)
{
waypointValuesDisplay
.
visible
=
false
}
else
{
waypointValuesDisplay
.
azimuth
=
missionItem
.
azimuth
waypointValuesDisplay
.
distance
=
missionItem
.
distance
waypointValuesDisplay
.
visible
=
true
}
}
MissionController
{
MissionController
{
id
:
controller
id
:
controller
...
@@ -131,8 +122,14 @@ QGCView {
...
@@ -131,8 +122,14 @@ QGCView {
}
}
function
setCurrentItem
(
index
)
{
function
setCurrentItem
(
index
)
{
_currentMissionItem
=
undefined
for
(
var
i
=
0
;
i
<
_missionItems
.
count
;
i
++
)
{
for
(
var
i
=
0
;
i
<
_missionItems
.
count
;
i
++
)
{
_missionItems
.
get
(
i
).
isCurrentItem
=
(
i
==
index
)
if
(
i
==
index
)
{
_currentMissionItem
=
_missionItems
.
get
(
i
)
_currentMissionItem
.
isCurrentItem
=
true
}
else
{
_missionItems
.
get
(
i
).
isCurrentItem
=
false
}
}
}
}
}
...
@@ -300,7 +297,6 @@ QGCView {
...
@@ -300,7 +297,6 @@ QGCView {
itemDragger
.
missionItem
.
coordinate
=
coordinate
itemDragger
.
missionItem
.
coordinate
=
coordinate
editorMap
.
latitude
=
itemDragger
.
missionItem
.
coordinate
.
latitude
editorMap
.
latitude
=
itemDragger
.
missionItem
.
coordinate
.
latitude
editorMap
.
longitude
=
itemDragger
.
missionItem
.
coordinate
.
longitude
editorMap
.
longitude
=
itemDragger
.
missionItem
.
coordinate
.
longitude
_root
.
showDistance
(
itemDragger
.
missionItem
)
}
}
}
}
}
}
...
@@ -328,7 +324,6 @@ QGCView {
...
@@ -328,7 +324,6 @@ QGCView {
function
updateItemIndicator
()
function
updateItemIndicator
()
{
{
if
(
object
.
isCurrentItem
)
{
if
(
object
.
isCurrentItem
)
{
_root
.
showDistance
(
object
)
if
(
object
.
specifiesCoordinate
)
{
if
(
object
.
specifiesCoordinate
)
{
// Setup our drag item
// Setup our drag item
if
(
object
.
sequenceNumber
!=
0
)
{
if
(
object
.
sequenceNumber
!=
0
)
{
...
@@ -908,13 +903,14 @@ QGCView {
...
@@ -908,13 +903,14 @@ QGCView {
radius
:
ScreenTools
.
defaultFontPixelWidth
radius
:
ScreenTools
.
defaultFontPixelWidth
color
:
qgcPal
.
window
color
:
qgcPal
.
window
opacity
:
0.80
opacity
:
0.80
visible
:
false
visible
:
_currentMissionItem
?
_currentMissionItem
.
distance
!=
-
1
:
false
property
real
azimuth
:
0
property
real
distance
:
0
readonly
property
real
margins
:
ScreenTools
.
defaultFontPixelWidth
readonly
property
real
margins
:
ScreenTools
.
defaultFontPixelWidth
property
real
_altDifference
:
_currentMissionItem
?
_currentMissionItem
.
altDifference
:
0
property
real
_azimuth
:
_currentMissionItem
?
_currentMissionItem
.
azimuth
:
0
property
real
_distance
:
_currentMissionItem
?
_currentMissionItem
.
distance
:
0
Column
{
Column
{
id
:
valuesColumn
id
:
valuesColumn
anchors.leftMargin
:
parent
.
margins
anchors.leftMargin
:
parent
.
margins
...
@@ -923,15 +919,21 @@ QGCView {
...
@@ -923,15 +919,21 @@ QGCView {
anchors.top
:
parent
.
top
anchors.top
:
parent
.
top
QGCLabel
{
QGCLabel
{
id
:
distanceLabel
color
:
qgcPal
.
text
color
:
qgcPal
.
text
text
:
"
Azimuth:
"
+
Math
.
round
(
waypointValuesDisplay
.
azimuth
)
text
:
"
Distance:
"
+
Math
.
round
(
waypointValuesDisplay
.
_distance
)
+
"
meters
"
}
QGCLabel
{
color
:
qgcPal
.
text
text
:
"
Alt diff:
"
+
Math
.
round
(
waypointValuesDisplay
.
_altDifference
)
+
"
meters
"
}
}
QGCLabel
{
QGCLabel
{
id
:
distanceLabel
color
:
qgcPal
.
text
color
:
qgcPal
.
text
text
:
"
Distance:
"
+
Math
.
round
(
waypointValuesDisplay
.
distance
)
+
"
meters
"
text
:
"
Azimuth:
"
+
Math
.
round
(
waypointValuesDisplay
.
_azimuth
)
}
}
}
}
}
}
}
// FlightMap
}
// FlightMap
...
...
src/MissionManager/MissionController.cc
View file @
fead2b5e
...
@@ -280,28 +280,28 @@ void MissionController::saveMissionToFile(void)
...
@@ -280,28 +280,28 @@ void MissionController::saveMissionToFile(void)
_missionItems
->
setDirty
(
false
);
_missionItems
->
setDirty
(
false
);
}
}
void
MissionController
::
_calcPrevWaypointValues
(
bool
homePositionValid
,
double
homeAlt
,
MissionItem
*
item1
,
MissionItem
*
item2
,
double
*
azimuth
,
double
*
dista
nce
)
void
MissionController
::
_calcPrevWaypointValues
(
bool
homePositionValid
,
double
homeAlt
,
MissionItem
*
currentItem
,
MissionItem
*
prevItem
,
double
*
azimuth
,
double
*
distance
,
double
*
altDiffere
nce
)
{
{
QGeoCoordinate
c
oord1
=
item1
->
coordinate
();
QGeoCoordinate
c
urrentCoord
=
currentItem
->
coordinate
();
QGeoCoordinate
coord2
=
item2
->
coordinate
();
QGeoCoordinate
prevCoord
=
prevItem
->
coordinate
();
bool
distanceOk
=
false
;
bool
distanceOk
=
false
;
// Convert to fixed altitudes
// Convert to fixed altitudes
qCDebug
(
MissionControllerLog
)
<<
homePositionValid
<<
homeAlt
qCDebug
(
MissionControllerLog
)
<<
homePositionValid
<<
homeAlt
<<
item1
->
relativeAltitude
()
<<
item1
->
coordinate
().
altitude
()
<<
currentItem
->
relativeAltitude
()
<<
currentItem
->
coordinate
().
altitude
()
<<
item2
->
relativeAltitude
()
<<
item2
->
coordinate
().
altitude
();
<<
prevItem
->
relativeAltitude
()
<<
prevItem
->
coordinate
().
altitude
();
if
(
homePositionValid
)
{
if
(
homePositionValid
)
{
distanceOk
=
true
;
distanceOk
=
true
;
if
(
item1
->
relativeAltitude
())
{
if
(
currentItem
->
relativeAltitude
())
{
c
oord1
.
setAltitude
(
homeAlt
+
coord1
.
altitude
());
c
urrentCoord
.
setAltitude
(
homeAlt
+
currentCoord
.
altitude
());
}
}
if
(
item2
->
relativeAltitude
())
{
if
(
prevItem
->
relativeAltitude
())
{
coord2
.
setAltitude
(
homeAlt
+
coord2
.
altitude
());
prevCoord
.
setAltitude
(
homeAlt
+
prevCoord
.
altitude
());
}
}
}
else
{
}
else
{
if
(
item1
->
relativeAltitude
()
&&
item2
->
relativeAltitude
())
{
if
(
prevItem
->
relativeAltitude
()
&&
currentItem
->
relativeAltitude
())
{
distanceOk
=
true
;
distanceOk
=
true
;
}
}
}
}
...
@@ -309,11 +309,13 @@ void MissionController::_calcPrevWaypointValues(bool homePositionValid, double h
...
@@ -309,11 +309,13 @@ void MissionController::_calcPrevWaypointValues(bool homePositionValid, double h
qCDebug
(
MissionControllerLog
)
<<
"distanceOk"
<<
distanceOk
;
qCDebug
(
MissionControllerLog
)
<<
"distanceOk"
<<
distanceOk
;
if
(
distanceOk
)
{
if
(
distanceOk
)
{
*
distance
=
item1
->
coordinate
().
distanceTo
(
item2
->
coordinate
());
*
altDifference
=
currentCoord
.
altitude
()
-
prevCoord
.
altitude
();
*
azimuth
=
item1
->
coordinate
().
azimuthTo
(
item2
->
coordinate
());
*
distance
=
prevCoord
.
distanceTo
(
currentCoord
);
*
azimuth
=
prevCoord
.
azimuthTo
(
currentCoord
);
}
else
{
}
else
{
*
altDifference
=
0.0
;
*
azimuth
=
0.0
;
*
azimuth
=
0.0
;
*
distance
=
-
1.0
;
*
distance
=
-
1.0
;
// Signals no values
}
}
}
}
...
@@ -349,11 +351,11 @@ void MissionController::_recalcWaypointLines(void)
...
@@ -349,11 +351,11 @@ void MissionController::_recalcWaypointLines(void)
if
(
item
->
command
()
==
MavlinkQmlSingleton
::
MAV_CMD_NAV_TAKEOFF
)
{
if
(
item
->
command
()
==
MavlinkQmlSingleton
::
MAV_CMD_NAV_TAKEOFF
)
{
// The first coordinate we hit is a takeoff command so link back to home position if valid
// The first coordinate we hit is a takeoff command so link back to home position if valid
if
(
homePositionValid
)
{
if
(
homePositionValid
)
{
double
azimuth
,
distance
;
double
azimuth
,
distance
,
altDifference
;
_waypointLines
.
append
(
new
CoordinateVector
(
homeItem
->
coordinate
(),
item
->
coordinate
()));
_waypointLines
.
append
(
new
CoordinateVector
(
homeItem
->
coordinate
(),
item
->
coordinate
()));
_calcPrevWaypointValues
(
homePositionValid
,
homeAlt
,
homeItem
,
item
,
&
azimuth
,
&
dista
nce
);
_calcPrevWaypointValues
(
homePositionValid
,
homeAlt
,
item
,
homeItem
,
&
azimuth
,
&
distance
,
&
altDiffere
nce
);
qDebug
()
<<
azimuth
<<
distance
;
item
->
setAltDifference
(
altDifference
)
;
item
->
setAzimuth
(
azimuth
);
item
->
setAzimuth
(
azimuth
);
item
->
setDistance
(
distance
);
item
->
setDistance
(
distance
);
}
}
...
@@ -362,12 +364,12 @@ void MissionController::_recalcWaypointLines(void)
...
@@ -362,12 +364,12 @@ void MissionController::_recalcWaypointLines(void)
}
}
firstCoordinateItem
=
false
;
firstCoordinateItem
=
false
;
}
else
if
(
!
lastCoordinateItem
->
homePosition
()
||
lastCoordinateItem
->
homePositionValid
())
{
}
else
if
(
!
lastCoordinateItem
->
homePosition
()
||
lastCoordinateItem
->
homePositionValid
())
{
double
azimuth
,
distance
;
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
// is an invalid home position we skip the line
// is an invalid home position we skip the line
_calcPrevWaypointValues
(
homePositionValid
,
homeAlt
,
lastCoordinateItem
,
item
,
&
azimuth
,
&
dista
nce
);
_calcPrevWaypointValues
(
homePositionValid
,
homeAlt
,
item
,
lastCoordinateItem
,
&
azimuth
,
&
distance
,
&
altDiffere
nce
);
qDebug
()
<<
azimuth
<<
distance
;
item
->
setAltDifference
(
altDifference
)
;
item
->
setAzimuth
(
azimuth
);
item
->
setAzimuth
(
azimuth
);
item
->
setDistance
(
distance
);
item
->
setDistance
(
distance
);
_waypointLines
.
append
(
new
CoordinateVector
(
lastCoordinateItem
->
coordinate
(),
item
->
coordinate
()));
_waypointLines
.
append
(
new
CoordinateVector
(
lastCoordinateItem
->
coordinate
(),
item
->
coordinate
()));
...
...
src/MissionManager/MissionController.h
View file @
fead2b5e
...
@@ -94,7 +94,7 @@ private:
...
@@ -94,7 +94,7 @@ private:
void
_autoSyncSend
(
void
);
void
_autoSyncSend
(
void
);
void
_setupMissionItems
(
bool
loadFromVehicle
,
bool
forceLoad
);
void
_setupMissionItems
(
bool
loadFromVehicle
,
bool
forceLoad
);
void
_setupActiveVehicle
(
Vehicle
*
activeVehicle
,
bool
forceLoadFromVehicle
);
void
_setupActiveVehicle
(
Vehicle
*
activeVehicle
,
bool
forceLoadFromVehicle
);
void
_calcPrevWaypointValues
(
bool
homePositionValid
,
double
homeAlt
,
MissionItem
*
item1
,
MissionItem
*
item2
,
double
*
azimuth
,
double
*
dista
nce
);
void
_calcPrevWaypointValues
(
bool
homePositionValid
,
double
homeAlt
,
MissionItem
*
currentItem
,
MissionItem
*
prevItem
,
double
*
azimuth
,
double
*
distance
,
double
*
altDiffere
nce
);
bool
_findLastAltitude
(
double
*
lastAltitude
);
bool
_findLastAltitude
(
double
*
lastAltitude
);
bool
_findLastAcceptanceRadius
(
double
*
lastAcceptanceRadius
);
bool
_findLastAcceptanceRadius
(
double
*
lastAcceptanceRadius
);
...
...
src/MissionManager/MissionItem.cc
View file @
fead2b5e
...
@@ -138,6 +138,7 @@ MissionItem::MissionItem(int sequenceNumber,
...
@@ -138,6 +138,7 @@ MissionItem::MissionItem(int sequenceNumber,
,
_dirty
(
false
)
,
_dirty
(
false
)
,
_sequenceNumber
(
sequenceNumber
)
,
_sequenceNumber
(
sequenceNumber
)
,
_isCurrentItem
(
isCurrentItem
)
,
_isCurrentItem
(
isCurrentItem
)
,
_altDifference
(
0.0
)
,
_azimuth
(
0.0
)
,
_azimuth
(
0.0
)
,
_distance
(
0.0
)
,
_distance
(
0.0
)
,
_homePositionSpecialCase
(
false
)
,
_homePositionSpecialCase
(
false
)
...
@@ -194,6 +195,7 @@ MissionItem::MissionItem(const MissionItem& other, QObject* parent)
...
@@ -194,6 +195,7 @@ MissionItem::MissionItem(const MissionItem& other, QObject* parent)
,
_dirty
(
false
)
,
_dirty
(
false
)
,
_sequenceNumber
(
0
)
,
_sequenceNumber
(
0
)
,
_isCurrentItem
(
false
)
,
_isCurrentItem
(
false
)
,
_altDifference
(
0.0
)
,
_azimuth
(
0.0
)
,
_azimuth
(
0.0
)
,
_distance
(
0.0
)
,
_distance
(
0.0
)
,
_homePositionSpecialCase
(
false
)
,
_homePositionSpecialCase
(
false
)
...
@@ -237,7 +239,8 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
...
@@ -237,7 +239,8 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
setSequenceNumber
(
other
.
_sequenceNumber
);
setSequenceNumber
(
other
.
_sequenceNumber
);
setAutoContinue
(
other
.
autoContinue
());
setAutoContinue
(
other
.
autoContinue
());
setIsCurrentItem
(
other
.
_isCurrentItem
);
setIsCurrentItem
(
other
.
_isCurrentItem
);
setAzimuth
(
other
.
_distance
);
setAltDifference
(
other
.
_altDifference
);
setAzimuth
(
other
.
_azimuth
);
setDistance
(
other
.
_distance
);
setDistance
(
other
.
_distance
);
setHomePositionSpecialCase
(
other
.
_homePositionSpecialCase
);
setHomePositionSpecialCase
(
other
.
_homePositionSpecialCase
);
setHomePositionValid
(
other
.
_homePositionValid
);
setHomePositionValid
(
other
.
_homePositionValid
);
...
@@ -717,6 +720,12 @@ void MissionItem::setDistance(double distance)
...
@@ -717,6 +720,12 @@ void MissionItem::setDistance(double distance)
emit
distanceChanged
(
_distance
);
emit
distanceChanged
(
_distance
);
}
}
void
MissionItem
::
setAltDifference
(
double
altDifference
)
{
_altDifference
=
altDifference
;
emit
altDifferenceChanged
(
_altDifference
);
}
void
MissionItem
::
setAzimuth
(
double
azimuth
)
void
MissionItem
::
setAzimuth
(
double
azimuth
)
{
{
_azimuth
=
azimuth
;
_azimuth
=
azimuth
;
...
...
src/MissionManager/MissionItem.h
View file @
fead2b5e
...
@@ -68,6 +68,7 @@ public:
...
@@ -68,6 +68,7 @@ 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
Q_PROPERTY
(
double
azimuth
READ
azimuth
WRITE
setAzimuth
NOTIFY
azimuthChanged
)
///< Azimuth to previous waypoint
Q_PROPERTY
(
double
azimuth
READ
azimuth
WRITE
setAzimuth
NOTIFY
azimuthChanged
)
///< Azimuth to previous waypoint
Q_PROPERTY
(
QString
category
READ
category
NOTIFY
commandChanged
)
Q_PROPERTY
(
QString
category
READ
category
NOTIFY
commandChanged
)
Q_PROPERTY
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
READ
command
WRITE
setCommand
NOTIFY
commandChanged
)
Q_PROPERTY
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
READ
command
WRITE
setCommand
NOTIFY
commandChanged
)
...
@@ -96,6 +97,7 @@ public:
...
@@ -96,6 +97,7 @@ public:
// Property accesors
// Property accesors
double
altDifference
(
void
)
const
{
return
_altDifference
;
}
double
azimuth
(
void
)
const
{
return
_azimuth
;
}
double
azimuth
(
void
)
const
{
return
_azimuth
;
}
QString
category
(
void
)
const
;
QString
category
(
void
)
const
;
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
(
void
)
const
{
return
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
)
_commandFact
.
cookedValue
().
toInt
();
};
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
(
void
)
const
{
return
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
)
_commandFact
.
cookedValue
().
toInt
();
};
...
@@ -134,8 +136,9 @@ public:
...
@@ -134,8 +136,9 @@ public:
void
setHomePositionValid
(
bool
homePositionValid
);
void
setHomePositionValid
(
bool
homePositionValid
);
void
setHomePositionSpecialCase
(
bool
homePositionSpecialCase
)
{
_homePositionSpecialCase
=
homePositionSpecialCase
;
}
void
setHomePositionSpecialCase
(
bool
homePositionSpecialCase
)
{
_homePositionSpecialCase
=
homePositionSpecialCase
;
}
void
set
Distance
(
double
dista
nce
);
void
set
AltDifference
(
double
altDiffere
nce
);
void
setAzimuth
(
double
azimuth
);
void
setAzimuth
(
double
azimuth
);
void
setDistance
(
double
distance
);
// C++ only methods
// C++ only methods
...
@@ -173,6 +176,7 @@ public slots:
...
@@ -173,6 +176,7 @@ public slots:
void
setDefaultsForCommand
(
void
);
void
setDefaultsForCommand
(
void
);
signals:
signals:
void
altDifferenceChanged
(
double
altDifference
);
void
azimuthChanged
(
double
azimuth
);
void
azimuthChanged
(
double
azimuth
);
void
commandChanged
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
);
void
commandChanged
(
MavlinkQmlSingleton
::
Qml_MAV_CMD
command
);
void
coordinateChanged
(
const
QGeoCoordinate
&
coordinate
);
void
coordinateChanged
(
const
QGeoCoordinate
&
coordinate
);
...
@@ -212,6 +216,7 @@ private:
...
@@ -212,6 +216,7 @@ private:
bool
_dirty
;
bool
_dirty
;
int
_sequenceNumber
;
int
_sequenceNumber
;
bool
_isCurrentItem
;
bool
_isCurrentItem
;
double
_altDifference
;
///< Difference in altitude from previous waypoint
double
_azimuth
;
///< Azimuth to previous waypoint
double
_azimuth
;
///< Azimuth to previous waypoint
double
_distance
;
///< Distance to previous waypoint
double
_distance
;
///< Distance to previous waypoint
bool
_homePositionSpecialCase
;
///< true: This item is being used as a ui home position indicator
bool
_homePositionSpecialCase
;
///< true: This item is being used as a ui home position indicator
...
...
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