Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
ca9e82a3
Commit
ca9e82a3
authored
Feb 04, 2011
by
pixhawk
Browse files
Fixed minor visual issues in multiple views
parent
c28456d4
Changes
7
Hide whitespace changes
Inline
Side-by-side
images/earth.html
View file @
ca9e82a3
...
...
@@ -253,9 +253,9 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
// FIXME Currently invalid conversion from right-handed z-down to z-up frame
planeOrient
.
setRoll
(((
roll
/
M_PI
))
*
180.0
+
180.0
);
planeOrient
.
setTilt
(((
pitch
/
M_PI
))
*
180.0
+
180.0
);
planeOrient
.
setHeading
(((
yaw
/
M_PI
))
*
180.0
-
90.0
+
180.0
);
planeOrient
.
setHeading
(((
yaw
/
M_PI
))
*
180.0
-
90.0
);
currFollowHeading
=
((
yaw
/
M_PI
))
*
180.0
+
180.0
;
currFollowHeading
=
((
yaw
/
M_PI
))
*
180.0
;
planeLoc
.
setLatitude
(
lastLat
);
planeLoc
.
setLongitude
(
lastLon
);
...
...
src/comm/MAVLinkSimulationMAV.cc
View file @
ca9e82a3
...
...
@@ -155,11 +155,32 @@ void MAVLinkSimulationMAV::mainloop()
// 25 Hz execution
if
(
timer25Hz
<=
0
)
{
// The message container to be used for sending
mavlink_message_t
ret
;
// Send which controllers are active
mavlink_control_status_t
control_status
;
control_status
.
control_att
=
1
;
control_status
.
control_pos_xy
=
1
;
control_status
.
control_pos_yaw
=
1
;
control_status
.
control_pos_z
=
1
;
mavlink_msg_control_status_encode
(
systemid
,
MAV_COMP_ID_IMU
,
&
ret
,
&
control_status
);
link
->
sendMAVLinkMessage
(
&
ret
);
// Send actual controller outputs
// This message just shows the direction
// and magnitude of the control output
mavlink_position_controller_output_t
pos
;
pos
.
x
=
sin
(
yaw
)
*
127.0
f
;
pos
.
y
=
cos
(
yaw
)
*
127.0
f
;
pos
.
z
=
0
;
mavlink_msg_position_controller_output_encode
(
systemid
,
MAV_COMP_ID_IMU
,
&
ret
,
&
pos
);
link
->
sendMAVLinkMessage
(
&
ret
);
// Send a named value with name "FLOAT" and 0.5f as value
// The message container to be used for sending
mavlink_message_t
ret
;
//
mavlink_message_t ret;
// The C-struct holding the data to be sent
mavlink_named_value_float_t
val
;
...
...
src/comm/MAVLinkSimulationWaypointPlanner.cc
View file @
ca9e82a3
...
...
@@ -535,7 +535,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{
mavlink_local_position_t
pos
;
mavlink_msg_local_position_decode
(
msg
,
&
pos
);
qDebug
()
<<
"Received new position: x:"
<<
pos
.
x
<<
"| y:"
<<
pos
.
y
<<
"| z:"
<<
pos
.
z
;
//
qDebug() << "Received new position: x:" << pos.x << "| y:" << pos.y << "| z:" << pos.z;
posReached
=
false
;
...
...
@@ -1004,7 +1004,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
if
(
cur_wp
->
autocontinue
)
{
cur_wp
->
current
=
0
;
if
(
current_active_wp_id
==
waypoints
->
size
()
-
1
&&
waypoints
->
size
()
>
1
)
if
(
current_active_wp_id
==
waypoints
->
size
()
-
1
&&
waypoints
->
size
()
>
0
)
{
//the last waypoint was reached, if auto continue is
//activated restart the waypoint list from the beginning
...
...
src/ui/HDDisplay.h
View file @
ca9e82a3
...
...
@@ -65,7 +65,7 @@ public:
public
slots
:
/** @brief Update a HDD value */
void
updateValue
(
const
int
uasId
,
const
QString
&
name
,
const
QString
&
unit
,
const
double
value
,
const
quint64
msec
);
void
setActiveUAS
(
UASInterface
*
uas
);
virtual
void
setActiveUAS
(
UASInterface
*
uas
);
/** @brief Removes a plot item by the action data */
void
removeItemByAction
();
...
...
src/ui/HSIDisplay.cc
View file @
ca9e82a3
...
...
@@ -37,7 +37,6 @@ This file is part of the QGROUNDCONTROL project
#include
<QDoubleSpinBox>
#include
"UASManager.h"
#include
"HSIDisplay.h"
#include
"MG.h"
#include
"QGC.h"
#include
"Waypoint.h"
#include
"UASWaypointManager.h"
...
...
@@ -74,9 +73,9 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
vz
(
0
),
speed
(
0
),
localAvailable
(
0
),
roll
(
0
),
pitch
(
0
),
yaw
(
1
.0
f
),
// FIXME Should be 0
roll
(
0
.0
f
),
pitch
(
0
.0
f
),
yaw
(
0
.0
f
),
bodyXSetCoordinate
(
0.0
f
),
bodyYSetCoordinate
(
0.0
f
),
bodyZSetCoordinate
(
0.0
f
),
...
...
@@ -96,23 +95,20 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
visionFix
(
0
),
laserFix
(
0
),
mavInitialized
(
false
),
bottomMargin
(
3
.0
f
),
topMargin
(
3
.0
f
)
bottomMargin
(
10
.0
f
),
topMargin
(
10
.0
f
)
{
connect
(
UASManager
::
instance
(),
SIGNAL
(
activeUASSet
(
UASInterface
*
)),
this
,
SLOT
(
setActiveUAS
(
UASInterface
*
)));
refreshTimer
->
setInterval
(
updateInterval
);
columns
=
1
;
// this->setScene(new QGraphicsScene(-metricWidth/2.0f, -metricWidth/2.0f, metricWidth, metricWidth, this));
this
->
setAutoFillBackground
(
true
);
vwidth
=
80
;
vheight
=
80
;
xCenterPos
=
vwidth
/
2.0
f
;
yCenterPos
=
vheight
/
2.0
f
+
topMargin
-
bottomMargin
;
qDebug
()
<<
"CENTER"
<<
xCenterPos
<<
yCenterPos
;
//
qDebug() << "CENTER" << xCenterPos << yCenterPos;
// Add interaction elements
QHBoxLayout
*
layout
=
new
QHBoxLayout
(
this
);
...
...
@@ -121,24 +117,57 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
QDoubleSpinBox
*
spinBox
=
new
QDoubleSpinBox
(
this
);
spinBox
->
setMinimum
(
0.1
);
spinBox
->
setMaximum
(
9999
);
spinBox
->
setValue
(
metricWidth
);
spinBox
->
setToolTip
(
tr
(
"Ground width in meters shown on instrument"
));
spinBox
->
setStatusTip
(
tr
(
"Ground width in meters shown on instrument"
));
connect
(
spinBox
,
SIGNAL
(
valueChanged
(
double
)),
this
,
SLOT
(
setMetricWidth
(
double
)));
connect
(
this
,
SIGNAL
(
metricWidthChanged
(
double
)),
spinBox
,
SLOT
(
setValue
(
double
)));
layout
->
addWidget
(
spinBox
);
layout
->
setAlignment
(
spinBox
,
Qt
::
AlignBottom
|
Qt
::
AlignLeft
);
this
->
setLayout
(
layout
);
this
->
setVisible
(
false
);
uas
=
NULL
;
resetMAVState
();
// Do first update
setMetricWidth
(
metricWidth
);
}
void
HSIDisplay
::
resetMAVState
()
{
mavInitialized
=
false
;
attControlKnown
=
false
;
attControlEnabled
=
false
;
xyControlKnown
=
false
;
xyControlEnabled
=
false
;
zControlKnown
=
false
;
zControlEnabled
=
false
;
yawControlKnown
=
false
;
yawControlEnabled
=
false
;
// Draw position lock indicators
positionFixKnown
=
false
;
positionFix
=
0
;
visionFixKnown
=
false
;
visionFix
=
0
;
gpsFixKnown
=
false
;
gpsFix
=
0
;
iruFixKnown
=
false
;
iruFix
=
0
;
// Data
setPointKnown
=
false
;
localAvailable
=
0
;
globalAvailable
=
0
;
}
void
HSIDisplay
::
paintEvent
(
QPaintEvent
*
event
)
{
Q_UNUSED
(
event
);
//paintGL();
static
quint64
interval
=
0
;
//qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
interval
=
MG
::
TIME
::
getGroundTimeNow
();
//
static quint64 interval = 0;
//
//qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
//
interval = MG::TIME::getGroundTimeNow();
renderOverlay
();
}
...
...
@@ -149,7 +178,7 @@ void HSIDisplay::renderOverlay()
#endif
// Center location of the HSI gauge items
float
bottomMargin
=
3.0
f
;
//
float bottomMargin = 3.0f;
// Size of the ring instrument
//const float margin = 0.1f; // 10% margin of total width on each side
...
...
@@ -167,10 +196,6 @@ void HSIDisplay::renderOverlay()
painter
.
setRenderHint
(
QPainter
::
Antialiasing
,
true
);
painter
.
setRenderHint
(
QPainter
::
HighQualityAntialiasing
,
true
);
// Draw background
//painter.fillRect(QRect(0, 0, width(), height()), backgroundColor);
// Draw base instrument
// ----------------------
painter
.
setBrush
(
Qt
::
NoBrush
);
...
...
@@ -182,7 +207,7 @@ void HSIDisplay::renderOverlay()
const
int
ringCount
=
2
;
for
(
int
i
=
0
;
i
<
ringCount
;
i
++
)
{
float
radius
=
(
vwidth
-
topMargin
-
bottomMargin
)
/
(
2.0
f
*
i
+
1
)
/
2.0
f
-
bottomMargin
/
2.0
f
;
float
radius
=
(
vwidth
-
(
topMargin
+
bottomMargin
)
*
0.3
f
)
/
(
1.3
f
*
i
+
1
)
/
2.0
f
-
bottomMargin
/
2.0
f
;
drawCircle
(
xCenterPos
,
yCenterPos
,
radius
,
0.1
f
,
ringColor
,
&
painter
);
}
...
...
@@ -192,7 +217,7 @@ void HSIDisplay::renderOverlay()
painter
.
rotate
((
yaw
/
(
M_PI
))
*
180.0
f
);
paintText
(
tr
(
"N"
),
ringColor
,
3.5
f
,
-
1.0
f
,
-
baseRadius
-
5.5
f
,
&
painter
);
paintText
(
tr
(
"S"
),
ringColor
,
3.5
f
,
-
1.0
f
,
+
baseRadius
+
1.5
f
,
&
painter
);
paintText
(
tr
(
"E"
),
ringColor
,
3.5
f
,
+
baseRadius
+
2.0
f
,
-
1.
7
5
f
,
&
painter
);
paintText
(
tr
(
"E"
),
ringColor
,
3.5
f
,
+
baseRadius
+
2.0
f
,
-
1.
2
5
f
,
&
painter
);
paintText
(
tr
(
"W"
),
ringColor
,
3.5
f
,
-
baseRadius
-
5.5
f
,
-
1.75
f
,
&
painter
);
painter
.
rotate
((
-
yaw
/
(
M_PI
))
*
180.0
f
);
painter
.
translate
(
-
(
xCenterPos
)
*
scalingFactor
,
-
(
yCenterPos
)
*
scalingFactor
);
...
...
@@ -255,23 +280,35 @@ void HSIDisplay::renderOverlay()
paintText
(
str
,
ringColor
,
3.0
f
,
10.0
f
,
vheight
-
5.0
f
,
&
painter
);
}
if
(
globalAvailable
>
0
)
{
// Position
QString
str
;
str
.
sprintf
(
"%05.2f lat %06.2f lon %06.2f alt"
,
lat
,
lon
,
alt
);
paintText
(
str
,
ringColor
,
2.2
f
,
xCenterPos
+
baseRadius
-
30.75
f
,
vheight
-
5.0
f
,
&
painter
);
// Speed
str
.
sprintf
(
"%05.2f m/s"
,
speed
);
paintText
(
str
,
ringColor
,
2.2
f
,
10.0
f
,
vheight
-
5.0
f
,
&
painter
);
}
// Draw waypoints
drawWaypoints
(
painter
);
// Draw status flags
drawStatusFlag
(
2
,
1
,
tr
(
"ATT"
),
attControlEnabled
,
painter
);
drawStatusFlag
(
22
,
1
,
tr
(
"PXY"
),
xyControlEnabled
,
painter
);
drawStatusFlag
(
44
,
1
,
tr
(
"PZ"
),
zControlEnabled
,
painter
);
drawStatusFlag
(
66
,
1
,
tr
(
"YAW"
),
yawControlEnabled
,
painter
);
drawStatusFlag
(
2
,
1
,
tr
(
"ATT"
),
attControlEnabled
,
attControlKnown
,
painter
);
drawStatusFlag
(
22
,
1
,
tr
(
"PXY"
),
xyControlEnabled
,
xyControlKnown
,
painter
);
drawStatusFlag
(
44
,
1
,
tr
(
"PZ"
),
zControlEnabled
,
zControlKnown
,
painter
);
drawStatusFlag
(
66
,
1
,
tr
(
"YAW"
),
yawControlEnabled
,
yawControlKnown
,
painter
);
// Draw position lock indicators
drawPositionLock
(
2
,
5
,
tr
(
"POS"
),
positionFix
,
painter
);
drawPositionLock
(
22
,
5
,
tr
(
"VIS"
),
visionFix
,
painter
);
drawPositionLock
(
44
,
5
,
tr
(
"GPS"
),
gpsFix
,
painter
);
drawPositionLock
(
66
,
5
,
tr
(
"IRU"
),
iruFix
,
painter
);
drawPositionLock
(
2
,
5
,
tr
(
"POS"
),
positionFix
,
positionFixKnown
,
painter
);
drawPositionLock
(
22
,
5
,
tr
(
"VIS"
),
visionFix
,
visionFixKnown
,
painter
);
drawPositionLock
(
44
,
5
,
tr
(
"GPS"
),
gpsFix
,
gpsFixKnown
,
painter
);
drawPositionLock
(
66
,
5
,
tr
(
"IRU"
),
iruFix
,
iruFixKnown
,
painter
);
}
void
HSIDisplay
::
drawStatusFlag
(
float
x
,
float
y
,
QString
label
,
bool
status
,
QPainter
&
painter
)
void
HSIDisplay
::
drawStatusFlag
(
float
x
,
float
y
,
QString
label
,
bool
status
,
bool
known
,
QPainter
&
painter
)
{
paintText
(
label
,
QGC
::
colorCyan
,
2.6
f
,
x
,
y
+
0.35
f
,
&
painter
);
QColor
statusColor
(
250
,
250
,
250
);
...
...
@@ -284,46 +321,93 @@ void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, QP
painter
.
setBrush
(
QGC
::
colorDarkYellow
);
}
painter
.
setPen
(
Qt
::
NoPen
);
painter
.
drawRect
(
QRect
(
refToScreenX
(
x
+
7.3
f
),
refToScreenY
(
y
+
0.05
),
refToScreenX
(
7.0
f
),
refToScreenY
(
4.0
f
)));
float
indicatorWidth
=
refToScreenX
(
7.0
f
);
float
indicatorHeight
=
refToScreenY
(
4.0
f
);
painter
.
drawRect
(
QRect
(
refToScreenX
(
x
+
7.3
f
),
refToScreenY
(
y
+
0.05
),
indicatorWidth
,
indicatorHeight
));
paintText
((
status
)
?
tr
(
"ON"
)
:
tr
(
"OFF"
),
statusColor
,
2.6
f
,
x
+
7.9
f
,
y
+
0.35
f
,
&
painter
);
// Cross out instrument if state unknown
if
(
!
known
)
{
QPen
pen
(
Qt
::
yellow
);
pen
.
setWidth
(
2
);
painter
.
setPen
(
pen
);
// Top left to bottom right
QPointF
p1
,
p2
,
p3
,
p4
;
p1
.
setX
(
refToScreenX
(
x
));
p1
.
setY
(
refToScreenX
(
y
));
p2
.
setX
(
p1
.
x
()
+
indicatorWidth
+
refToScreenX
(
7.3
f
));
p2
.
setY
(
p1
.
y
()
+
indicatorHeight
);
painter
.
drawLine
(
p1
,
p2
);
// Bottom left to top right
p3
.
setX
(
refToScreenX
(
x
));
p3
.
setY
(
refToScreenX
(
y
)
+
indicatorHeight
);
p4
.
setX
(
p1
.
x
()
+
indicatorWidth
+
refToScreenX
(
7.3
f
));
p4
.
setY
(
p1
.
y
());
painter
.
drawLine
(
p3
,
p4
);
}
}
void
HSIDisplay
::
drawPositionLock
(
float
x
,
float
y
,
QString
label
,
int
status
,
QPainter
&
painter
)
void
HSIDisplay
::
drawPositionLock
(
float
x
,
float
y
,
QString
label
,
int
status
,
bool
known
,
QPainter
&
painter
)
{
paintText
(
label
,
QGC
::
colorCyan
,
2.6
f
,
x
,
y
+
0.35
f
,
&
painter
);
QColor
negStatusColor
(
200
,
20
,
20
);
QColor
posStatusColor
(
20
,
200
,
20
);
QColor
statusColor
(
250
,
250
,
250
);
if
(
status
>
0
&&
status
<
4
)
{
painter
.
setBrush
(
posStatusColor
);
}
else
{
painter
.
setBrush
(
negStatusColor
);
}
paintText
(
label
,
QGC
::
colorCyan
,
2.6
f
,
x
,
y
+
0.35
f
,
&
painter
);
QColor
negStatusColor
(
200
,
20
,
20
);
QColor
posStatusColor
(
20
,
200
,
20
);
QColor
statusColor
(
250
,
250
,
250
);
if
(
status
>
0
&&
status
<
4
)
{
painter
.
setBrush
(
posStatusColor
);
}
else
{
painter
.
setBrush
(
negStatusColor
);
}
// Lock text
QString
lockText
;
switch
(
status
)
{
case
1
:
lockText
=
tr
(
"LOC"
);
break
;
case
2
:
lockText
=
tr
(
"2D"
);
break
;
case
3
:
lockText
=
tr
(
"3D"
);
break
;
default:
lockText
=
tr
(
"NO"
);
break
;
}
// Lock text
QString
lockText
;
switch
(
status
)
{
case
1
:
lockText
=
tr
(
"LOC"
);
break
;
case
2
:
lockText
=
tr
(
"2D"
);
break
;
case
3
:
lockText
=
tr
(
"3D"
);
break
;
default:
lockText
=
tr
(
"NO"
);
break
;
}
painter
.
setPen
(
Qt
::
NoPen
);
painter
.
drawRect
(
QRect
(
refToScreenX
(
x
+
7.3
f
),
refToScreenY
(
y
+
0.05
),
refToScreenX
(
7.0
f
),
refToScreenY
(
4.0
f
)));
paintText
(
lockText
,
statusColor
,
2.6
f
,
x
+
7.9
f
,
y
+
0.35
f
,
&
painter
);
float
indicatorWidth
=
refToScreenX
(
7.0
f
);
float
indicatorHeight
=
refToScreenY
(
4.0
f
);
painter
.
setPen
(
Qt
::
NoPen
);
painter
.
drawRect
(
QRect
(
refToScreenX
(
x
+
7.3
f
),
refToScreenY
(
y
+
0.05
),
refToScreenX
(
7.0
f
),
refToScreenY
(
4.0
f
)));
paintText
(
lockText
,
statusColor
,
2.6
f
,
x
+
7.9
f
,
y
+
0.35
f
,
&
painter
);
// Cross out instrument if state unknown
if
(
!
known
)
{
QPen
pen
(
Qt
::
yellow
);
pen
.
setWidth
(
2
);
painter
.
setPen
(
pen
);
// Top left to bottom right
QPointF
p1
,
p2
,
p3
,
p4
;
p1
.
setX
(
refToScreenX
(
x
));
p1
.
setY
(
refToScreenX
(
y
));
p2
.
setX
(
p1
.
x
()
+
indicatorWidth
+
refToScreenX
(
7.3
f
));
p2
.
setY
(
p1
.
y
()
+
indicatorHeight
);
painter
.
drawLine
(
p1
,
p2
);
// Bottom left to top right
p3
.
setX
(
refToScreenX
(
x
));
p3
.
setY
(
refToScreenX
(
y
)
+
indicatorHeight
);
p4
.
setX
(
p1
.
x
()
+
indicatorWidth
+
refToScreenX
(
7.3
f
));
p4
.
setY
(
p1
.
y
());
painter
.
drawLine
(
p3
,
p4
);
}
}
void
HSIDisplay
::
updatePositionLock
(
UASInterface
*
uas
,
bool
lock
)
...
...
@@ -335,16 +419,19 @@ void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock)
void
HSIDisplay
::
updateAttitudeControllerEnabled
(
bool
enabled
)
{
attControlEnabled
=
enabled
;
attControlKnown
=
true
;
}
void
HSIDisplay
::
updatePositionXYControllerEnabled
(
bool
enabled
)
{
xyControlEnabled
=
enabled
;
xyControlKnown
=
true
;
}
void
HSIDisplay
::
updatePositionZControllerEnabled
(
bool
enabled
)
{
zControlEnabled
=
enabled
;
zControlKnown
=
true
;
}
QPointF
HSIDisplay
::
metricWorldToBody
(
QPointF
world
)
...
...
@@ -481,6 +568,8 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
connect
(
uas
,
SIGNAL
(
irUltraSoundLocalizationChanged
(
UASInterface
*
,
int
)),
this
,
SLOT
(
updateInfraredUltrasoundLocalization
(
UASInterface
*
,
int
)));
this
->
uas
=
uas
;
resetMAVState
();
}
void
HSIDisplay
::
updateSpeed
(
UASInterface
*
uas
,
double
vx
,
double
vy
,
double
vz
,
quint64
time
)
...
...
@@ -550,6 +639,7 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir
bodyZSetCoordinate
=
zDesired
;
bodyYawSet
=
yawDesired
;
mavInitialized
=
true
;
setPointKnown
=
true
;
// qDebug() << "Received setpoint at x: " << x << "metric y:" << y;
// posXSet = xDesired;
...
...
@@ -592,6 +682,7 @@ void HSIDisplay::updateSatellite(int uasid, int satid, float elevation, float az
void
HSIDisplay
::
updatePositionYawControllerEnabled
(
bool
enabled
)
{
yawControlEnabled
=
enabled
;
yawControlKnown
=
true
;
}
/**
...
...
@@ -601,6 +692,7 @@ void HSIDisplay::updateLocalization(UASInterface* uas, int fix)
{
Q_UNUSED
(
uas
);
positionFix
=
fix
;
positionFixKnown
=
true
;
}
/**
* @param fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D localization, 3: 3D localization
...
...
@@ -609,6 +701,7 @@ void HSIDisplay::updateGpsLocalization(UASInterface* uas, int fix)
{
Q_UNUSED
(
uas
);
gpsFix
=
fix
;
gpsFixKnown
=
true
;
}
/**
* @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
...
...
@@ -617,6 +710,7 @@ void HSIDisplay::updateVisionLocalization(UASInterface* uas, int fix)
{
Q_UNUSED
(
uas
);
visionFix
=
fix
;
visionFixKnown
=
true
;
}
/**
...
...
@@ -626,6 +720,7 @@ void HSIDisplay::updateInfraredUltrasoundLocalization(UASInterface* uas, int fix
{
Q_UNUSED
(
uas
);
iruFix
=
fix
;
iruFixKnown
=
true
;
}
QColor
HSIDisplay
::
getColorForSNR
(
float
snr
)
...
...
@@ -656,22 +751,25 @@ QColor HSIDisplay::getColorForSNR(float snr)
void
HSIDisplay
::
drawSetpointXY
(
float
x
,
float
y
,
float
yaw
,
const
QColor
&
color
,
QPainter
&
painter
)
{
float
radius
=
vwidth
/
20.0
f
;
QPen
pen
(
color
);
pen
.
setWidthF
(
refLineWidthToPen
(
0.4
f
));
pen
.
setColor
(
color
);
painter
.
setPen
(
pen
);
painter
.
setBrush
(
Qt
::
NoBrush
);
QPointF
in
(
x
,
y
);
// Transform from body to world coordinates
in
=
metricWorldToBody
(
in
);
// Scale from metric to screen reference coordinates
QPointF
p
=
metricBodyToRef
(
in
);
drawCircle
(
p
.
x
(),
p
.
y
(),
radius
,
0.4
f
,
color
,
&
painter
);
radius
*=
0.8
;
drawLine
(
p
.
x
(),
p
.
y
(),
p
.
x
()
+
sin
(
yaw
)
*
radius
,
p
.
y
()
-
cos
(
yaw
)
*
radius
,
refLineWidthToPen
(
0.4
f
),
color
,
&
painter
);
painter
.
setBrush
(
color
);
drawCircle
(
p
.
x
(),
p
.
y
(),
radius
*
0.1
f
,
0.1
f
,
color
,
&
painter
);
if
(
setPointKnown
)
{
float
radius
=
vwidth
/
20.0
f
;
QPen
pen
(
color
);
pen
.
setWidthF
(
refLineWidthToPen
(
0.4
f
));
pen
.
setColor
(
color
);
painter
.
setPen
(
pen
);
painter
.
setBrush
(
Qt
::
NoBrush
);
QPointF
in
(
x
,
y
);
// Transform from body to world coordinates
in
=
metricWorldToBody
(
in
);
// Scale from metric to screen reference coordinates
QPointF
p
=
metricBodyToRef
(
in
);
drawCircle
(
p
.
x
(),
p
.
y
(),
radius
,
0.4
f
,
color
,
&
painter
);
radius
*=
0.8
;
drawLine
(
p
.
x
(),
p
.
y
(),
p
.
x
()
+
sin
(
yaw
)
*
radius
,
p
.
y
()
-
cos
(
yaw
)
*
radius
,
refLineWidthToPen
(
0.4
f
),
color
,
&
painter
);
painter
.
setBrush
(
color
);
drawCircle
(
p
.
x
(),
p
.
y
(),
radius
*
0.1
f
,
0.1
f
,
color
,
&
painter
);
}
}
void
HSIDisplay
::
drawWaypoints
(
QPainter
&
painter
)
...
...
@@ -699,7 +797,19 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
for
(
int
i
=
0
;
i
<
list
.
size
();
i
++
)
{
QPointF
in
(
list
.
at
(
i
)
->
getX
(),
list
.
at
(
i
)
->
getY
());
QPointF
in
;
if
(
list
.
at
(
i
)
->
getFrame
()
==
MAV_FRAME_LOCAL
)
{
// Do not transform
in
=
QPointF
(
list
.
at
(
i
)
->
getX
(),
list
.
at
(
i
)
->
getY
());
}
else
{
// Transform to local coordinates first
double
x
=
list
.
at
(
i
)
->
getX
();
double
y
=
list
.
at
(
i
)
->
getY
();
in
=
QPointF
(
x
,
y
);
}
// Transform from world to body coordinates
in
=
metricWorldToBody
(
in
);
// Scale from metric to screen reference coordinates
...
...
@@ -865,7 +975,7 @@ void HSIDisplay::drawPositionDirection(float xRef, float yRef, float radius, con
painter
->
setBrush
(
indexBrush
);
drawPolygon
(
p
,
painter
);
//
qDebug() << "DRAWING POS SETPOINT X:" << posXSet << "Y:" << posYSet << angle;
qDebug
()
<<
"DRAWING POS SETPOINT X:"
<<
posXSet
<<
"Y:"
<<
posYSet
<<
angle
;
}
void
HSIDisplay
::
drawAttitudeDirection
(
float
xRef
,
float
yRef
,
float
radius
,
const
QColor
&
color
,
QPainter
*
painter
)
...
...
src/ui/HSIDisplay.h
View file @
ca9e82a3
...
...
@@ -81,6 +81,8 @@ public slots:
/** @brief Update state from joystick */
void
updateJoystick
(
double
roll
,
double
pitch
,
double
yaw
,
double
thrust
,
int
xHat
,
int
yHat
);
void
pressKey
(
int
key
);
/** @brief Reset the state of the view */
void
resetMAVState
();
signals:
void
metricWidthChanged
(
double
width
);
...
...
@@ -92,8 +94,10 @@ protected slots:
void
drawPositionDirection
(
float
xRef
,
float
yRef
,
float
radius
,
const
QColor
&
color
,
QPainter
*
painter
);
void
drawAttitudeDirection
(
float
xRef
,
float
yRef
,
float
radius
,
const
QColor
&
color
,
QPainter
*
painter
);
void
drawAltitudeSetpoint
(
float
xRef
,
float
yRef
,
float
radius
,
const
QColor
&
color
,
QPainter
*
painter
);
void
drawStatusFlag
(
float
x
,
float
y
,
QString
label
,
bool
status
,
QPainter
&
painter
);
void
drawPositionLock
(
float
x
,
float
y
,
QString
label
,
int
status
,
QPainter
&
painter
);
/** @brief Draw a status flag indicator */
void
drawStatusFlag
(
float
x
,
float
y
,
QString
label
,
bool
status
,
bool
known
,
QPainter
&
painter
);
/** @brief Draw a position lock indicator */
void
drawPositionLock
(
float
x
,
float
y
,
QString
label
,
int
status
,
bool
known
,
QPainter
&
painter
);
void
setBodySetpointCoordinateXY
(
double
x
,
double
y
);
void
setBodySetpointCoordinateZ
(
double
z
);
/** @brief Send the current ui setpoint coordinates as new setpoint to the MAV */
...
...
@@ -231,6 +235,20 @@ protected:
float
bottomMargin
;
///< Margin on the bottom of the page, in virtual coordinates
float
topMargin
;
///< Margin on top of the page, in virtual coordinates
bool
attControlKnown
;
///< Attitude control status known flag
bool
xyControlKnown
;
///< XY control status known flag
bool
zControlKnown
;
///< Z control status known flag
bool
yawControlKnown
;
///< Yaw control status known flag
// Position lock indicators
bool
positionFixKnown
;
///< Position fix status known flag
bool
visionFixKnown
;
///< Vision fix status known flag
bool
gpsFixKnown
;
///< GPS fix status known flag
bool
iruFixKnown
;
///< Infrared/Ultrasound fix status known flag
// Data indicators
bool
setPointKnown
;
///< Controller setpoint known status flag
private:
};
...
...
src/ui/MainWindow.cc
View file @
ca9e82a3
...
...
@@ -160,6 +160,25 @@ MainWindow::MainWindow(QWidget *parent):
MainWindow
::~
MainWindow
()
{
delete
mavlink
;
delete
joystick
;
// Get and delete all dockwidgets and contained
// widgets
QObjectList
childList
(
this
->
children
()
);
QObjectList
::
iterator
i
;
QDockWidget
*
dockWidget
;
for
(
i
=
childList
.
begin
();
i
!=
childList
.
end
();
++
i
)
{
dockWidget
=
dynamic_cast
<
QDockWidget
*>
(
*
i
);
if
(
dockWidget
)
{
// Remove dock widget from main window
removeDockWidget
(
dockWidget
);
delete
dockWidget
->
widget
();
delete
dockWidget
;
}
}
}
/**
...
...
@@ -884,7 +903,6 @@ void MainWindow::closeEvent(QCloseEvent *event)
//settings.setValue("windowState", saveState());
aboutToCloseFlag
=
true
;
mavlink
->
storeSettings
();
joystick
->
deleteLater
();
// Save the last current view in any case
settings
.
setValue
(
"CURRENT_VIEW"
,
currentView
);
// Save the current window state, but only if a system is connected (else no real number of widgets would be present)
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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