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
c03f446d
Commit
c03f446d
authored
Feb 06, 2011
by
pixhawk
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Fixed minor issues in HUD
parent
da2aaa92
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
39 additions
and
24 deletions
+39
-24
MAVLinkSimulationMAV.cc
src/comm/MAVLinkSimulationMAV.cc
+11
-11
HUD.cc
src/ui/HUD.cc
+24
-12
HUD.h
src/ui/HUD.h
+4
-1
No files found.
src/comm/MAVLinkSimulationMAV.cc
View file @
c03f446d
...
...
@@ -70,16 +70,16 @@ void MAVLinkSimulationMAV::mainloop()
// 10 Hz execution
if
(
timer10Hz
<=
0
)
{
if
(
!
firstWP
)
{
double
radPer100ms
=
0.00006
;
double
altPer100ms
=
1.0
;
double
xm
=
(
nextSPX
-
x
);
double
ym
=
(
nextSPY
-
y
);
double
zm
=
(
nextSPZ
-
z
);
double
radPer100ms
=
0.00006
;
double
altPer100ms
=
0.4
;
double
xm
=
(
nextSPX
-
x
);
double
ym
=
(
nextSPY
-
y
);
double
zm
=
(
nextSPZ
-
z
);
float
zsign
=
(
zm
<
0
)
?
-
1.0
f
:
1.0
f
;
float
zsign
=
(
zm
<
0
)
?
-
1.0
f
:
1.0
f
;
if
(
!
firstWP
)
{
//float trueyaw = atan2f(xm, ym);
float
newYaw
=
atan2f
(
xm
,
ym
);
...
...
@@ -121,9 +121,9 @@ void MAVLinkSimulationMAV::mainloop()
pos
.
alt
=
z
*
1000.0
;
pos
.
lat
=
y
*
1E7
;
pos
.
lon
=
x
*
1E7
;
pos
.
vx
=
sin
(
yaw
)
*
10.0
f
;
pos
.
vy
=
cos
(
yaw
)
*
10.0
f
;
pos
.
vz
=
0
;
pos
.
vx
=
sin
(
yaw
)
*
10.0
f
*
100.0
f
;
pos
.
vy
=
cos
(
yaw
)
*
10.0
f
*
100.0
f
;
pos
.
vz
=
altPer100ms
*
10.0
f
*
100.0
f
*
zsign
*-
1.0
f
;
mavlink_msg_global_position_int_encode
(
systemid
,
MAV_COMP_ID_IMU
,
&
msg
,
&
pos
);
link
->
sendMAVLinkMessage
(
&
msg
);
planner
.
handleMessage
(
msg
);
...
...
src/ui/HUD.cc
View file @
c03f446d
...
...
@@ -37,9 +37,7 @@ This file is part of the QGROUNDCONTROL project
#include <QDebug>
#include <cmath>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include <qmath.h>
#include <limits>
#include "UASManager.h"
...
...
@@ -126,6 +124,9 @@ HUD::HUD(int width, int height, QWidget* parent)
xSpeed
(
0.0
),
ySpeed
(
0.0
),
zSpeed
(
0.0
),
lastSpeedUpdate
(
0
),
totalSpeed
(
0.0
),
totalAcc
(
0.0
),
lat
(
0.0
),
lon
(
0.0
),
alt
(
0.0
),
...
...
@@ -277,6 +278,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
(
speedChanged
(
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
)));
...
...
@@ -299,6 +301,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
(
speedChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateSpeed
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
waypointSelected
(
int
,
int
)),
this
,
SLOT
(
selectWaypoint
(
int
,
int
)));
...
...
@@ -334,12 +337,7 @@ void HUD::updateAttitude(UASInterface* uas, double roll, double pitch, double ya
void
HUD
::
updateBattery
(
UASInterface
*
uas
,
double
voltage
,
double
percent
,
int
seconds
)
{
Q_UNUSED
(
uas
);
// this->voltage = voltage;
// this->timeRemaining = seconds;
// this->percentRemaining = percent;
fuelStatus
.
sprintf
(
"BAT [%02.0f % | %05.2fV] (%02dm:%02ds)"
,
percent
,
voltage
,
seconds
/
60
,
seconds
%
60
);
fuelStatus
=
tr
(
"BAT [%1% | %2V] (%3:%4)"
).
arg
(
percent
,
2
,
'f'
,
0
,
QChar
(
'0'
)).
arg
(
voltage
,
4
,
'f'
,
1
,
QChar
(
'0'
)).
arg
(
seconds
/
60
,
2
,
10
,
QChar
(
'0'
)).
arg
(
seconds
%
60
,
2
,
10
,
QChar
(
'0'
));
if
(
percent
<
20.0
f
)
{
fuelColor
=
warningColor
;
...
...
@@ -390,6 +388,9 @@ void HUD::updateSpeed(UASInterface* uas,double x,double y,double z,quint64 times
this
->
xSpeed
=
x
;
this
->
ySpeed
=
y
;
this
->
zSpeed
=
z
;
double
newTotalSpeed
=
sqrt
(
xSpeed
*
xSpeed
+
ySpeed
*
ySpeed
+
zSpeed
*
zSpeed
);
totalAcc
=
(
newTotalSpeed
-
totalSpeed
)
/
((
double
)(
lastSpeedUpdate
-
timestamp
)
/
1000.0
);
totalSpeed
=
newTotalSpeed
;
}
/**
...
...
@@ -814,15 +815,26 @@ void HUD::paintHUD()
drawChangeRateStrip
(
-
51.0
f
,
-
50.0
f
,
15.0
f
,
-
1.0
f
,
1.0
f
,
-
zSpeed
,
&
painter
);
// CHANGE RATE STRIPS
drawChangeRateStrip
(
49.0
f
,
-
50.0
f
,
15.0
f
,
-
1.0
f
,
1.0
f
,
xSpeed
,
&
painter
);
drawChangeRateStrip
(
49.0
f
,
-
50.0
f
,
15.0
f
,
-
1.0
f
,
1.0
f
,
totalAcc
,
&
painter
);
// GAUGES
// Left altitude gauge
drawChangeIndicatorGauge
(
-
vGaugeSpacing
,
-
15.0
f
,
10.0
f
,
2.0
f
,
-
zPos
,
defaultColor
,
&
painter
,
false
);
float
gaugeAltitude
;
if
(
this
->
alt
!=
0
)
{
gaugeAltitude
=
alt
;
}
else
{
gaugeAltitude
=
-
zPos
;
}
drawChangeIndicatorGauge
(
-
vGaugeSpacing
,
-
15.0
f
,
10.0
f
,
2.0
f
,
gaugeAltitude
,
defaultColor
,
&
painter
,
false
);
// Right speed gauge
drawChangeIndicatorGauge
(
vGaugeSpacing
,
-
15.0
f
,
10.0
f
,
5.0
f
,
x
Speed
,
defaultColor
,
&
painter
,
false
);
drawChangeIndicatorGauge
(
vGaugeSpacing
,
-
15.0
f
,
10.0
f
,
5.0
f
,
total
Speed
,
defaultColor
,
&
painter
,
false
);
// Waypoint name
...
...
src/ui/HUD.h
View file @
c03f446d
...
...
@@ -130,7 +130,7 @@ protected:
void
contextMenuEvent
(
QContextMenuEvent
*
event
);
void
createActions
();
static
const
int
updateInterval
=
5
0
;
static
const
int
updateInterval
=
4
0
;
QImage
*
image
;
///< Double buffer image
QImage
glImage
;
///< The background / camera image
...
...
@@ -198,6 +198,9 @@ protected:
double
xSpeed
;
double
ySpeed
;
double
zSpeed
;
quint64
lastSpeedUpdate
;
double
totalSpeed
;
double
totalAcc
;
double
lat
;
double
lon
;
double
alt
;
...
...
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