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
af1056d5
Commit
af1056d5
authored
Aug 23, 2014
by
Thomas Gubler
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
handle wgs84 altitude correctly
also adds AMSL output in ft
parent
6836a23a
Changes
19
Show whitespace changes
Inline
Side-by-side
Showing
19 changed files
with
82 additions
and
41 deletions
+82
-41
UAS.cc
src/uas/UAS.cc
+9
-7
UAS.h
src/uas/UAS.h
+29
-2
UASInterface.h
src/uas/UASInterface.h
+2
-2
UASWaypointManager.cc
src/uas/UASWaypointManager.cc
+4
-3
UASWaypointManager.h
src/uas/UASWaypointManager.h
+1
-1
senseSoarMAV.cpp
src/uas/senseSoarMAV.cpp
+1
-1
HSIDisplay.cc
src/ui/HSIDisplay.cc
+6
-5
HSIDisplay.h
src/ui/HSIDisplay.h
+1
-1
HUD.cc
src/ui/HUD.cc
+5
-4
HUD.h
src/ui/HUD.h
+1
-1
PrimaryFlightDisplay.cc
src/ui/PrimaryFlightDisplay.cc
+10
-4
PrimaryFlightDisplay.h
src/ui/PrimaryFlightDisplay.h
+2
-1
QGCToolBar.cc
src/ui/QGCToolBar.cc
+4
-4
QGCToolBar.h
src/ui/QGCToolBar.h
+1
-1
QGCMapWidget.cc
src/ui/map/QGCMapWidget.cc
+1
-1
QGCGoogleEarthView.cc
src/ui/map3D/QGCGoogleEarthView.cc
+1
-1
UASControlParameters.cpp
src/ui/uas/UASControlParameters.cpp
+1
-1
UASQuickView.cc
src/ui/uas/UASQuickView.cc
+2
-0
UASView.cc
src/ui/uas/UASView.cc
+1
-1
No files found.
src/uas/UAS.cc
View file @
af1056d5
...
...
@@ -109,6 +109,8 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
latitude
(
0.0
),
longitude
(
0.0
),
altitudeAMSL
(
0.0
),
altitudeAMSLFT
(
0.0
),
altitudeWGS84
(
0.0
),
altitudeRelative
(
0.0
),
globalEstimatorActive
(
false
),
...
...
@@ -820,7 +822,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
if
(
!
isnan
(
hud
.
airspeed
))
setAirSpeed
(
hud
.
airspeed
);
speedZ
=
-
hud
.
climb
;
emit
altitudeChanged
(
this
,
altitudeAMSL
,
altitudeRelative
,
-
speedZ
,
time
);
emit
altitudeChanged
(
this
,
altitudeAMSL
,
altitude
WGS84
,
altitude
Relative
,
-
speedZ
,
time
);
emit
speedChanged
(
this
,
groundSpeed
,
airSpeed
,
time
);
}
break
;
...
...
@@ -879,7 +881,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
setLatitude
(
pos
.
lat
/
(
double
)
1E7
);
setLongitude
(
pos
.
lon
/
(
double
)
1E7
);
setAltitude
AMSL
(
pos
.
alt
/
1000.0
);
setAltitude
WGS84
(
pos
.
alt
/
1000.0
);
setAltitudeRelative
(
pos
.
relative_alt
/
1000.0
);
globalEstimatorActive
=
true
;
...
...
@@ -888,8 +890,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
speedY
=
pos
.
vy
/
100.0
;
speedZ
=
pos
.
vz
/
100.0
;
emit
globalPositionChanged
(
this
,
getLatitude
(),
getLongitude
(),
getAltitudeAMSL
(),
time
);
emit
altitudeChanged
(
this
,
altitudeAMSL
,
altitudeRelative
,
-
speedZ
,
time
);
emit
globalPositionChanged
(
this
,
getLatitude
(),
getLongitude
(),
getAltitudeAMSL
(),
getAltitudeWGS84
(),
time
);
emit
altitudeChanged
(
this
,
altitudeAMSL
,
altitude
WGS84
,
altitude
Relative
,
-
speedZ
,
time
);
// We had some frame mess here, global and local axes were mixed.
emit
velocityChanged_NED
(
this
,
speedX
,
speedY
,
speedZ
,
time
);
...
...
@@ -938,9 +940,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
if
(
!
globalEstimatorActive
)
{
setLatitude
(
latitude_gps
);
setLongitude
(
longitude_gps
);
setAltitude
AMSL
(
altitude_gps
);
emit
globalPositionChanged
(
this
,
getLatitude
(),
getLongitude
(),
getAltitudeAMSL
(),
time
);
emit
altitudeChanged
(
this
,
altitudeAMSL
,
altitudeRelative
,
-
speedZ
,
time
);
setAltitude
WGS84
(
altitude_gps
);
emit
globalPositionChanged
(
this
,
getLatitude
(),
getLongitude
(),
getAltitudeAMSL
(),
getAltitudeWGS84
(),
time
);
emit
altitudeChanged
(
this
,
altitudeAMSL
,
altitude
WGS84
,
altitude
Relative
,
-
speedZ
,
time
);
float
vel
=
pos
.
vel
/
100.0
f
;
// Smaller than threshold and not NaN
...
...
src/uas/UAS.h
View file @
af1056d5
...
...
@@ -110,6 +110,8 @@ public:
Q_PROPERTY
(
double
groundSpeed
READ
getGroundSpeed
WRITE
setGroundSpeed
NOTIFY
groundSpeedChanged
)
Q_PROPERTY
(
double
bearingToWaypoint
READ
getBearingToWaypoint
WRITE
setBearingToWaypoint
NOTIFY
bearingToWaypointChanged
)
Q_PROPERTY
(
double
altitudeAMSL
READ
getAltitudeAMSL
WRITE
setAltitudeAMSL
NOTIFY
altitudeAMSLChanged
)
Q_PROPERTY
(
double
altitudeAMSLFT
READ
getAltitudeAMSLFT
NOTIFY
altitudeAMSLFTChanged
)
Q_PROPERTY
(
double
altitudeWGS84
READ
getAltitudeWGS84
WRITE
setAltitudeWGS84
NOTIFY
altitudeWGS84Changed
)
Q_PROPERTY
(
double
altitudeRelative
READ
getAltitudeRelative
WRITE
setAltitudeRelative
NOTIFY
altitudeRelativeChanged
)
void
setGroundSpeed
(
double
val
)
...
...
@@ -197,7 +199,10 @@ public:
{
altitudeAMSL
=
val
;
emit
altitudeAMSLChanged
(
val
,
"altitudeAMSL"
);
emit
valueChanged
(
this
->
uasId
,
"altitudeAMSL"
,
"m"
,
QVariant
(
val
),
getUnixTime
());
emit
valueChanged
(
this
->
uasId
,
"altitudeAMSL"
,
"m"
,
QVariant
(
altitudeAMSL
),
getUnixTime
());
altitudeAMSLFT
=
3
.
28084
*
altitudeAMSL
;
emit
altitudeAMSLFTChanged
(
val
,
"altitudeAMSLFT"
);
emit
valueChanged
(
this
->
uasId
,
"altitudeAMSLFT"
,
"m"
,
QVariant
(
altitudeAMSLFT
),
getUnixTime
());
}
double
getAltitudeAMSL
()
const
...
...
@@ -205,6 +210,24 @@ public:
return
altitudeAMSL
;
}
double
getAltitudeAMSLFT
()
const
{
return
altitudeAMSLFT
;
}
void
setAltitudeWGS84
(
double
val
)
{
altitudeWGS84
=
val
;
emit
altitudeWGS84Changed
(
val
,
"altitudeWGS84"
);
emit
valueChanged
(
this
->
uasId
,
"altitudeWGS84"
,
"m"
,
QVariant
(
val
),
getUnixTime
());
}
double
getAltitudeWGS84
()
const
{
return
altitudeWGS84
;
}
void
setAltitudeRelative
(
double
val
)
{
altitudeRelative
=
val
;
...
...
@@ -453,7 +476,9 @@ protected: //COMMENTS FOR TEST UNIT
double
latitude
;
///< Global latitude as estimated by position estimator
double
longitude
;
///< Global longitude as estimated by position estimator
double
altitudeAMSL
;
///< Global altitude as estimated by position estimator
double
altitudeAMSL
;
///< Global altitude as estimated by position estimator, AMSL
double
altitudeAMSLFT
;
///< Global altitude as estimated by position estimator, AMSL
double
altitudeWGS84
;
///< Global altitude as estimated by position estimator, WGS84
double
altitudeRelative
;
///< Altitude above home as estimated by position estimator
double
satelliteCount
;
///< Number of satellites visible to raw GPS
...
...
@@ -963,6 +988,8 @@ signals:
void
longitudeChanged
(
double
val
,
QString
name
);
void
latitudeChanged
(
double
val
,
QString
name
);
void
altitudeAMSLChanged
(
double
val
,
QString
name
);
void
altitudeAMSLFTChanged
(
double
val
,
QString
name
);
void
altitudeWGS84Changed
(
double
val
,
QString
name
);
void
altitudeRelativeChanged
(
double
val
,
QString
name
);
void
rollChanged
(
double
val
,
QString
name
);
void
pitchChanged
(
double
val
,
QString
name
);
...
...
src/uas/UASInterface.h
View file @
af1056d5
...
...
@@ -525,8 +525,8 @@ signals:
void
userPositionSetPointsChanged
(
int
uasid
,
float
xDesired
,
float
yDesired
,
float
zDesired
,
float
yawDesired
);
void
localPositionChanged
(
UASInterface
*
,
double
x
,
double
y
,
double
z
,
quint64
usec
);
void
localPositionChanged
(
UASInterface
*
,
int
component
,
double
x
,
double
y
,
double
z
,
quint64
usec
);
void
globalPositionChanged
(
UASInterface
*
,
double
lat
,
double
lon
,
double
alt
,
quint64
usec
);
void
altitudeChanged
(
UASInterface
*
,
double
altitudeAMSL
,
double
altitudeRelative
,
double
climbRate
,
quint64
usec
);
void
globalPositionChanged
(
UASInterface
*
,
double
lat
,
double
lon
,
double
alt
AMSL
,
double
altWGS84
,
quint64
usec
);
void
altitudeChanged
(
UASInterface
*
,
double
altitudeAMSL
,
double
altitude
WGS84
,
double
altitude
Relative
,
double
climbRate
,
quint64
usec
);
/** @brief Update the status of one satellite used for localization */
void
gpsSatelliteStatusChanged
(
int
uasid
,
int
satid
,
float
azimuth
,
float
direction
,
float
snr
,
bool
used
);
...
...
src/uas/UASWaypointManager.cc
View file @
af1056d5
...
...
@@ -55,7 +55,7 @@ UASWaypointManager::UASWaypointManager(UAS* _uas)
uasid
=
uas
->
getUASID
();
connect
(
&
protocol_timer
,
SIGNAL
(
timeout
()),
this
,
SLOT
(
timeout
()));
connect
(
uas
,
SIGNAL
(
localPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
handleLocalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
handleGlobalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
handleGlobalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
double
,
quint64
)));
}
else
{
...
...
@@ -119,11 +119,12 @@ void UASWaypointManager::handleLocalPositionChanged(UASInterface* mav, double x,
}
}
void
UASWaypointManager
::
handleGlobalPositionChanged
(
UASInterface
*
mav
,
double
lat
,
double
lon
,
double
alt
,
quint64
time
)
void
UASWaypointManager
::
handleGlobalPositionChanged
(
UASInterface
*
mav
,
double
lat
,
double
lon
,
double
alt
AMSL
,
double
altWGS84
,
quint64
time
)
{
Q_UNUSED
(
mav
);
Q_UNUSED
(
time
);
Q_UNUSED
(
alt
);
Q_UNUSED
(
altAMSL
);
Q_UNUSED
(
altWGS84
);
Q_UNUSED
(
lon
);
Q_UNUSED
(
lat
);
if
(
waypointsEditable
.
count
()
>
0
&&
currentWaypointEditable
&&
(
currentWaypointEditable
->
getFrame
()
==
MAV_FRAME_GLOBAL
||
currentWaypointEditable
->
getFrame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
))
...
...
src/uas/UASWaypointManager.h
View file @
af1056d5
...
...
@@ -145,7 +145,7 @@ public slots:
void
notifyOfChangeViewOnly
(
Waypoint
*
wp
);
///< Notifies manager to changes to a viewonly waypoint, e.g. some widget wants to change "current"
/*@}*/
void
handleLocalPositionChanged
(
UASInterface
*
mav
,
double
x
,
double
y
,
double
z
,
quint64
time
);
void
handleGlobalPositionChanged
(
UASInterface
*
mav
,
double
lat
,
double
lon
,
double
alt
,
quint64
time
);
void
handleGlobalPositionChanged
(
UASInterface
*
mav
,
double
lat
,
double
lon
,
double
alt
AMSL
,
double
altWGS84
,
quint64
time
);
signals:
void
waypointEditableListChanged
(
void
);
///< emits signal that the list of editable waypoints has been changed
...
...
src/uas/senseSoarMAV.cpp
View file @
af1056d5
...
...
@@ -115,7 +115,7 @@ void senseSoarMAV::receiveMessage(LinkInterface *link, mavlink_message_t message
emit
valueChanged
(
uasId
,
"latitude"
,
"deg"
,
this
->
latitude
,
time
);
emit
valueChanged
(
uasId
,
"longitude"
,
"deg"
,
this
->
longitude
,
time
);
emit
valueChanged
(
uasId
,
"altitude"
,
"m"
,
this
->
altitude
,
time
);
emit
globalPositionChanged
(
this
,
this
->
latitude
,
this
->
long
itude
,
this
->
altitude
,
time
);
emit
globalPositionChanged
(
this
,
this
->
latitude
,
this
->
longitude
,
this
->
alt
itude
,
this
->
altitude
,
time
);
break
;
}
case
MAVLINK_MSG_ID_OBS_QFF
:
...
...
src/ui/HSIDisplay.cc
View file @
af1056d5
...
...
@@ -920,7 +920,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
if
(
this
->
uas
!=
NULL
)
{
disconnect
(
this
->
uas
,
SIGNAL
(
gpsSatelliteStatusChanged
(
int
,
int
,
float
,
float
,
float
,
bool
)),
this
,
SLOT
(
updateSatellite
(
int
,
int
,
float
,
float
,
float
,
bool
)));
disconnect
(
this
->
uas
,
SIGNAL
(
localPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateLocalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
attitudeThrustSetPointChanged
(
UASInterface
*
,
float
,
float
,
float
,
float
,
quint64
)),
this
,
SLOT
(
updateAttitudeSetpoints
(
UASInterface
*
,
float
,
float
,
float
,
float
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
positionSetPointsChanged
(
int
,
float
,
float
,
float
,
float
,
quint64
)),
this
,
SLOT
(
updatePositionSetpoints
(
int
,
float
,
float
,
float
,
float
,
quint64
)));
disconnect
(
uas
,
SIGNAL
(
userPositionSetPointsChanged
(
int
,
float
,
float
,
float
,
float
)),
this
,
SLOT
(
updateUserPositionSetpoints
(
int
,
float
,
float
,
float
,
float
)));
...
...
@@ -955,8 +955,8 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
this
,
SLOT
(
updateSatellite
(
int
,
int
,
float
,
float
,
float
,
bool
)));
connect
(
uas
,
SIGNAL
(
localPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateLocalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
attitudeThrustSetPointChanged
(
UASInterface
*
,
float
,
float
,
float
,
float
,
quint64
)),
this
,
SLOT
(
updateAttitudeSetpoints
(
UASInterface
*
,
float
,
float
,
float
,
float
,
quint64
)));
connect
(
uas
,
SIGNAL
(
positionSetPointsChanged
(
int
,
float
,
float
,
float
,
float
,
quint64
)),
...
...
@@ -1161,11 +1161,12 @@ void HSIDisplay::updateLocalPosition(UASInterface*, double x, double y, double z
localAvailable
=
usec
;
}
void
HSIDisplay
::
updateGlobalPosition
(
UASInterface
*
,
double
lat
,
double
lon
,
double
alt
,
quint64
usec
)
void
HSIDisplay
::
updateGlobalPosition
(
UASInterface
*
,
double
lat
,
double
lon
,
double
alt
AMSL
,
double
altWGS84
,
quint64
usec
)
{
Q_UNUSED
(
altAMSL
);
this
->
lat
=
lat
;
this
->
lon
=
lon
;
this
->
alt
=
alt
;
this
->
alt
=
alt
WGS84
;
globalAvailable
=
usec
;
}
...
...
src/ui/HSIDisplay.h
View file @
af1056d5
...
...
@@ -60,7 +60,7 @@ public slots:
void
updateUserPositionSetpoints
(
int
uasid
,
float
xDesired
,
float
yDesired
,
float
zDesired
,
float
yawDesired
);
void
updatePositionSetpoints
(
int
uasid
,
float
xDesired
,
float
yDesired
,
float
zDesired
,
float
yawDesired
,
quint64
usec
);
void
updateLocalPosition
(
UASInterface
*
,
double
x
,
double
y
,
double
z
,
quint64
usec
);
void
updateGlobalPosition
(
UASInterface
*
,
double
lat
,
double
lon
,
double
alt
,
quint64
usec
);
void
updateGlobalPosition
(
UASInterface
*
,
double
lat
,
double
lon
,
double
alt
AMSL
,
double
altWGS84
,
quint64
usec
);
void
updateSpeed
(
UASInterface
*
uas
,
double
vx
,
double
vy
,
double
vz
,
quint64
time
);
void
updatePositionLock
(
UASInterface
*
uas
,
bool
lock
);
void
updateAttitudeControllerEnabled
(
bool
enabled
);
...
...
src/ui/HUD.cc
View file @
af1056d5
...
...
@@ -285,7 +285,7 @@ void HUD::setActiveUAS(UASInterface* uas)
disconnect
(
this
->
uas
,
SIGNAL
(
heartbeat
(
UASInterface
*
)),
this
,
SLOT
(
receiveHeartbeat
(
UASInterface
*
)));
disconnect
(
this
->
uas
,
SIGNAL
(
localPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateLocalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
velocityChanged_NEDspeedChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateSpeed
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
waypointSelected
(
int
,
int
)),
this
,
SLOT
(
selectWaypoint
(
int
,
int
)));
...
...
@@ -307,7 +307,7 @@ void HUD::setActiveUAS(UASInterface* uas)
connect
(
uas
,
SIGNAL
(
heartbeat
(
UASInterface
*
)),
this
,
SLOT
(
receiveHeartbeat
(
UASInterface
*
)));
connect
(
uas
,
SIGNAL
(
localPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateLocalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
velocityChanged_NED
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateSpeed
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
waypointSelected
(
int
,
int
)),
this
,
SLOT
(
selectWaypoint
(
int
,
int
)));
...
...
@@ -387,13 +387,14 @@ void HUD::updateLocalPosition(UASInterface* uas,double x,double y,double z,quint
this
->
zPos
=
z
;
}
void
HUD
::
updateGlobalPosition
(
UASInterface
*
uas
,
double
lat
,
double
lon
,
double
altitude
,
quint64
timestamp
)
void
HUD
::
updateGlobalPosition
(
UASInterface
*
uas
,
double
lat
,
double
lon
,
double
altitude
AMSL
,
double
altitudeWGS84
,
quint64
timestamp
)
{
Q_UNUSED
(
uas
);
Q_UNUSED
(
altitudeAMSL
);
Q_UNUSED
(
timestamp
);
this
->
lat
=
lat
;
this
->
lon
=
lon
;
this
->
alt
=
altitude
;
this
->
alt
=
altitude
WGS84
;
}
void
HUD
::
updateSpeed
(
UASInterface
*
uas
,
double
x
,
double
y
,
double
z
,
quint64
timestamp
)
...
...
src/ui/HUD.h
View file @
af1056d5
...
...
@@ -74,7 +74,7 @@ public slots:
void
receiveHeartbeat
(
UASInterface
*
);
void
updateThrust
(
UASInterface
*
,
double
);
void
updateLocalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
);
void
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
);
void
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
);
void
updateSpeed
(
UASInterface
*
,
double
,
double
,
double
,
quint64
);
void
updateState
(
UASInterface
*
,
QString
);
void
updateMode
(
int
id
,
QString
mode
,
QString
description
);
...
...
src/ui/PrimaryFlightDisplay.cc
View file @
af1056d5
...
...
@@ -218,7 +218,7 @@ void PrimaryFlightDisplay::forgetUAS(UASInterface* uas)
disconnect
(
this
->
uas
,
SIGNAL
(
attitudeChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAttitude
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
attitudeChanged
(
UASInterface
*
,
int
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAttitude
(
UASInterface
*
,
int
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
speedChanged
(
UASInterface
*
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateSpeed
(
UASInterface
*
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
altitudeChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAltitude
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
altitudeChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAltitude
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
navigationControllerErrorsChanged
(
UASInterface
*
,
double
,
double
,
double
)),
this
,
SLOT
(
updateNavigationControllerErrors
(
UASInterface
*
,
double
,
double
,
double
)));
}
}
...
...
@@ -241,7 +241,7 @@ void PrimaryFlightDisplay::setActiveUAS(UASInterface* uas)
connect
(
uas
,
SIGNAL
(
attitudeChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAttitude
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
attitudeChanged
(
UASInterface
*
,
int
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAttitude
(
UASInterface
*
,
int
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
speedChanged
(
UASInterface
*
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateSpeed
(
UASInterface
*
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
altitudeChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAltitude
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
altitudeChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAltitude
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
navigationControllerErrorsChanged
(
UASInterface
*
,
double
,
double
,
double
)),
this
,
SLOT
(
updateNavigationControllerErrors
(
UASInterface
*
,
double
,
double
,
double
)));
// Set new UAS
...
...
@@ -319,14 +319,19 @@ void PrimaryFlightDisplay::updateSpeed(UASInterface* uas, double _groundSpeed, d
airSpeed
=
_airSpeed
;
}
void
PrimaryFlightDisplay
::
updateAltitude
(
UASInterface
*
uas
,
double
_altitudeAMSL
,
double
_altitudeRelative
,
double
_climbRate
,
quint64
timestamp
)
{
void
PrimaryFlightDisplay
::
updateAltitude
(
UASInterface
*
uas
,
double
_altitudeAMSL
,
double
_altitude
WGS84
,
double
_altitude
Relative
,
double
_climbRate
,
quint64
timestamp
)
{
Q_UNUSED
(
uas
);
Q_UNUSED
(
timestamp
);
Q_UNUSED
(
_altitudeWGS84
)
if
(
fabsf
(
altitudeAMSL
-
_altitudeAMSL
)
>
0.5
f
)
{
_valuesChanged
=
true
;
}
if
(
fabsf
(
altitudeWGS84
-
_altitudeWGS84
)
>
0.5
f
)
{
_valuesChanged
=
true
;
}
if
(
fabsf
(
altitudeRelative
-
_altitudeRelative
)
>
0.5
f
)
{
_valuesChanged
=
true
;
}
...
...
@@ -336,6 +341,7 @@ void PrimaryFlightDisplay::updateAltitude(UASInterface* uas, double _altitudeAMS
}
altitudeAMSL
=
_altitudeAMSL
;
altitudeWGS84
=
_altitudeWGS84
;
altitudeRelative
=
_altitudeRelative
;
climbRate
=
_climbRate
;
}
...
...
@@ -892,7 +898,7 @@ void PrimaryFlightDisplay::drawAltimeter(
QRectF
area
)
{
float
primaryAltitude
=
altitude
AMSL
;
float
primaryAltitude
=
altitude
WGS84
;
float
secondaryAltitude
=
0
;
painter
.
resetTransform
();
...
...
src/ui/PrimaryFlightDisplay.h
View file @
af1056d5
...
...
@@ -19,7 +19,7 @@ public slots:
void
updateAttitude
(
UASInterface
*
uas
,
int
component
,
double
roll
,
double
pitch
,
double
yaw
,
quint64
timestamp
);
void
updateSpeed
(
UASInterface
*
uas
,
double
_groundSpeed
,
double
_airSpeed
,
quint64
timestamp
);
void
updateAltitude
(
UASInterface
*
uas
,
double
_altitudeAMSL
,
double
_altitudeRelative
,
double
_climbRate
,
quint64
timestamp
);
void
updateAltitude
(
UASInterface
*
uas
,
double
_altitudeAMSL
,
double
_altitude
WGS84
,
double
_altitude
Relative
,
double
_climbRate
,
quint64
timestamp
);
void
updateNavigationControllerErrors
(
UASInterface
*
uas
,
double
altitudeError
,
double
speedError
,
double
xtrackError
);
/** @brief Set the currently monitored UAS */
...
...
@@ -104,6 +104,7 @@ private:
float
heading
;
float
altitudeAMSL
;
float
altitudeWGS84
;
float
altitudeRelative
;
// APM: GPS and baro mix above home (GPS) altitude. This value comes from the GLOBAL_POSITION_INT message.
...
...
src/ui/QGCToolBar.cc
View file @
af1056d5
...
...
@@ -51,13 +51,13 @@ QGCToolBar::QGCToolBar(QWidget *parent) :
// Do not load UI, wait for actions
}
void
QGCToolBar
::
globalPositionChanged
(
UASInterface
*
uas
,
double
lat
,
double
lon
,
double
alt
,
quint64
usec
)
void
QGCToolBar
::
globalPositionChanged
(
UASInterface
*
uas
,
double
lat
,
double
lon
,
double
alt
AMSL
,
double
altWGS84
,
quint64
usec
)
{
Q_UNUSED
(
uas
);
Q_UNUSED
(
lat
);
Q_UNUSED
(
lon
);
Q_UNUSED
(
usec
);
altitudeMSL
=
alt
;
altitudeMSL
=
alt
AMSL
;
changed
=
true
;
}
...
...
@@ -370,7 +370,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active)
disconnect
(
mav
,
SIGNAL
(
batteryChanged
(
UASInterface
*
,
double
,
double
,
double
,
int
)),
this
,
SLOT
(
updateBatteryRemaining
(
UASInterface
*
,
double
,
double
,
double
,
int
)));
disconnect
(
mav
,
SIGNAL
(
armingChanged
(
bool
)),
this
,
SLOT
(
updateArmingState
(
bool
)));
disconnect
(
mav
,
SIGNAL
(
heartbeatTimeout
(
bool
,
unsigned
int
)),
this
,
SLOT
(
heartbeatTimeout
(
bool
,
unsigned
int
)));
disconnect
(
active
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
disconnect
(
active
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)));
if
(
mav
->
getWaypointManager
())
{
disconnect
(
mav
->
getWaypointManager
(),
SIGNAL
(
currentWaypointChanged
(
quint16
)),
this
,
SLOT
(
updateCurrentWaypoint
(
quint16
)));
...
...
@@ -395,7 +395,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active)
connect
(
mav
,
SIGNAL
(
batteryChanged
(
UASInterface
*
,
double
,
double
,
double
,
int
)),
this
,
SLOT
(
updateBatteryRemaining
(
UASInterface
*
,
double
,
double
,
double
,
int
)));
connect
(
mav
,
SIGNAL
(
armingChanged
(
bool
)),
this
,
SLOT
(
updateArmingState
(
bool
)));
connect
(
mav
,
SIGNAL
(
heartbeatTimeout
(
bool
,
unsigned
int
)),
this
,
SLOT
(
heartbeatTimeout
(
bool
,
unsigned
int
)));
connect
(
mav
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
mav
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)));
if
(
mav
->
getWaypointManager
())
{
connect
(
mav
->
getWaypointManager
(),
SIGNAL
(
currentWaypointChanged
(
quint16
)),
this
,
SLOT
(
updateCurrentWaypoint
(
quint16
)));
...
...
src/ui/QGCToolBar.h
View file @
af1056d5
...
...
@@ -78,7 +78,7 @@ public slots:
/** @brief Update connection timeout time */
void
heartbeatTimeout
(
bool
timeout
,
unsigned
int
ms
);
/** @brief Update global position */
void
globalPositionChanged
(
UASInterface
*
uas
,
double
lat
,
double
lon
,
double
alt
,
quint64
usec
);
void
globalPositionChanged
(
UASInterface
*
uas
,
double
lat
,
double
lon
,
double
alt
AMSL
,
double
altWGS84
,
quint64
usec
);
/** @brief Create or connect link */
void
connectLink
(
bool
connect
);
/** @brief Clear status string */
...
...
src/ui/map/QGCMapWidget.cc
View file @
af1056d5
...
...
@@ -353,7 +353,7 @@ void QGCMapWidget::mouseDoubleClickEvent(QMouseEvent* event)
*/
void
QGCMapWidget
::
addUAS
(
UASInterface
*
uas
)
{
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
systemSpecsChanged
(
int
)),
this
,
SLOT
(
updateSystemSpecs
(
int
)));
if
(
!
waypointLines
.
value
(
uas
->
getUASID
(),
NULL
))
{
waypointLines
.
insert
(
uas
->
getUASID
(),
new
QGraphicsItemGroup
(
map
));
...
...
src/ui/map3D/QGCGoogleEarthView.cc
View file @
af1056d5
...
...
@@ -206,7 +206,7 @@ void QGCGoogleEarthView::addUAS(UASInterface* uas)
if
(
trailEnabled
)
javaScript
(
QString
(
"showTrail(%1);"
).
arg
(
uas
->
getUASID
()));
// Automatically receive further position updates
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)));
// Receive waypoint updates
// Connect the waypoint manager / data storage to the UI
connect
(
uas
->
getWaypointManager
(),
SIGNAL
(
waypointEditableListChanged
(
int
)),
this
,
SLOT
(
updateWaypointList
(
int
)));
...
...
src/ui/uas/UASControlParameters.cpp
View file @
af1056d5
...
...
@@ -65,7 +65,7 @@ void UASControlParameters::changedMode(int mode)
void
UASControlParameters
::
activeUasSet
(
UASInterface
*
uas
)
{
if
(
uas
)
{
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
velocityChanged_NED
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
speedChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
attitudeChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAttitude
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
modeChanged
(
int
,
QString
,
QString
)),
this
,
SLOT
(
updateMode
(
int
,
QString
,
QString
)));
...
...
src/ui/uas/UASQuickView.cc
View file @
af1056d5
...
...
@@ -31,6 +31,8 @@ UASQuickView::UASQuickView(QWidget *parent) : QWidget(parent),
if
(
uasPropertyValueMap
.
size
()
==
0
)
{
valueEnabled
(
"altitudeAMSL"
);
valueEnabled
(
"altitudeAMSLFT"
);
valueEnabled
(
"altitudeWGS84"
);
valueEnabled
(
"altitudeRelative"
);
valueEnabled
(
"groundSpeed"
);
valueEnabled
(
"distToWaypoint"
);
...
...
src/ui/uas/UASView.cc
View file @
af1056d5
...
...
@@ -92,7 +92,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
connect
(
uas
,
SIGNAL
(
heartbeat
(
UASInterface
*
)),
this
,
SLOT
(
receiveHeartbeat
(
UASInterface
*
)));
connect
(
uas
,
SIGNAL
(
thrustChanged
(
UASInterface
*
,
double
)),
this
,
SLOT
(
updateThrust
(
UASInterface
*
,
double
)));
connect
(
uas
,
SIGNAL
(
localPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateLocalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
velocityChanged_NED
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateSpeed
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
statusChanged
(
UASInterface
*
,
QString
,
QString
)),
this
,
SLOT
(
updateState
(
UASInterface
*
,
QString
,
QString
)));
connect
(
uas
,
SIGNAL
(
modeChanged
(
int
,
QString
,
QString
)),
this
,
SLOT
(
updateMode
(
int
,
QString
,
QString
)));
...
...
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