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
c2a4f4d2
Commit
c2a4f4d2
authored
Feb 10, 2012
by
Bryant Mairs
Browse files
Options
Browse Files
Download
Plain Diff
Merge branch 'master' of
https://github.com/mavlink/qgroundcontrol
parents
216b0236
bfc8f211
Changes
27
Show whitespace changes
Inline
Side-by-side
Showing
27 changed files
with
1314 additions
and
335 deletions
+1314
-335
mavconn.qgw
files/pixhawk/hexarotor/widgets/mavconn.qgw
+27
-3
colorbars.png
images/status/colorbars.png
+0
-0
qgroundcontrol.pri
qgroundcontrol.pri
+26
-5
qgroundcontrol.pro
qgroundcontrol.pro
+4
-2
qgroundcontrol.qrc
qgroundcontrol.qrc
+1
-0
QGC.cc
src/QGC.cc
+9
-0
QGC.h
src/QGC.h
+2
-0
MAVLinkProtocol.cc
src/comm/MAVLinkProtocol.cc
+41
-1
QGCUASParamManager.h
src/uas/QGCUASParamManager.h
+14
-2
UAS.cc
src/uas/UAS.cc
+117
-14
UAS.h
src/uas/UAS.h
+49
-3
UASInterface.h
src/uas/UASInterface.h
+13
-3
UASWaypointManager.cc
src/uas/UASWaypointManager.cc
+5
-0
UASWaypointManager.h
src/uas/UASWaypointManager.h
+1
-0
HSIDisplay.cc
src/ui/HSIDisplay.cc
+12
-0
HSIDisplay.h
src/ui/HSIDisplay.h
+1
-0
HUD.h
src/ui/HUD.h
+1
-1
QGCRGBDView.cc
src/ui/QGCRGBDView.cc
+30
-6
QGCRGBDView.h
src/ui/QGCRGBDView.h
+3
-1
WaypointList.cc
src/ui/WaypointList.cc
+4
-1
ObstacleGroupNode.cc
src/ui/map3D/ObstacleGroupNode.cc
+8
-12
ObstacleGroupNode.h
src/ui/map3D/ObstacleGroupNode.h
+3
-1
Pixhawk3DWidget.cc
src/ui/map3D/Pixhawk3DWidget.cc
+529
-252
Pixhawk3DWidget.h
src/ui/map3D/Pixhawk3DWidget.h
+27
-26
Q3DWidget.cc
src/ui/map3D/Q3DWidget.cc
+2
-2
gpl.cc
src/ui/map3D/gpl.cc
+333
-0
gpl.h
src/ui/map3D/gpl.h
+52
-0
No files found.
files/pixhawk/hexrotor/widgets/mavconn.qgw
→
files/pixhawk/hex
a
rotor/widgets/mavconn.qgw
View file @
c2a4f4d2
[MAVCONN%20Control]
QGC_TOOL_WIDGET_ITEMS\1\TYPE=COMMANDBUTTON
QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_DESCRIPTION=
START Recording
QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_DESCRIPTION=
DO: Control Video
QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_BUTTONTEXT=CAPTURE
QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_COMMANDID=200
QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAMS_VISIBLE=false
...
...
@@ -12,7 +12,7 @@ QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM5=0
QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM6=0
QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM7=0
QGC_TOOL_WIDGET_ITEMS\2\TYPE=COMMANDBUTTON
QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_DESCRIPTION=
STOP Recording
QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_DESCRIPTION=
DO: Control Video
QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_BUTTONTEXT=STOP
QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_COMMANDID=200
QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAMS_VISIBLE=false
...
...
@@ -35,4 +35,28 @@ QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM4=0
QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM5=0
QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM6=0
QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM7=0
QGC_TOOL_WIDGET_ITEMS\size=3
QGC_TOOL_WIDGET_ITEMS\4\TYPE=SLIDER
QGC_TOOL_WIDGET_ITEMS\4\QGC_PARAM_SLIDER_DESCRIPTION=Setpoint ON<->OFF
QGC_TOOL_WIDGET_ITEMS\4\QGC_PARAM_SLIDER_PARAMID=
QGC_TOOL_WIDGET_ITEMS\4\QGC_PARAM_SLIDER_COMPONENTID=0
QGC_TOOL_WIDGET_ITEMS\4\QGC_PARAM_SLIDER_MIN=1
QGC_TOOL_WIDGET_ITEMS\4\QGC_PARAM_SLIDER_MAX=0
QGC_TOOL_WIDGET_ITEMS\5\TYPE=SLIDER
QGC_TOOL_WIDGET_ITEMS\5\QGC_PARAM_SLIDER_DESCRIPTION=Glob. Loc ON<->OFF
QGC_TOOL_WIDGET_ITEMS\5\QGC_PARAM_SLIDER_PARAMID=
QGC_TOOL_WIDGET_ITEMS\5\QGC_PARAM_SLIDER_COMPONENTID=0
QGC_TOOL_WIDGET_ITEMS\5\QGC_PARAM_SLIDER_MIN=1
QGC_TOOL_WIDGET_ITEMS\5\QGC_PARAM_SLIDER_MAX=0
QGC_TOOL_WIDGET_ITEMS\6\TYPE=SLIDER
QGC_TOOL_WIDGET_ITEMS\6\QGC_PARAM_SLIDER_DESCRIPTION=GPS ENU HL<->ASL
QGC_TOOL_WIDGET_ITEMS\6\QGC_PARAM_SLIDER_PARAMID=
QGC_TOOL_WIDGET_ITEMS\6\QGC_PARAM_SLIDER_COMPONENTID=0
QGC_TOOL_WIDGET_ITEMS\6\QGC_PARAM_SLIDER_MIN=0
QGC_TOOL_WIDGET_ITEMS\6\QGC_PARAM_SLIDER_MAX=1
QGC_TOOL_WIDGET_ITEMS\7\TYPE=SLIDER
QGC_TOOL_WIDGET_ITEMS\7\QGC_PARAM_SLIDER_DESCRIPTION=Yaw PX<->ASL
QGC_TOOL_WIDGET_ITEMS\7\QGC_PARAM_SLIDER_PARAMID=
QGC_TOOL_WIDGET_ITEMS\7\QGC_PARAM_SLIDER_COMPONENTID=0
QGC_TOOL_WIDGET_ITEMS\7\QGC_PARAM_SLIDER_MIN=0
QGC_TOOL_WIDGET_ITEMS\7\QGC_PARAM_SLIDER_MAX=1
QGC_TOOL_WIDGET_ITEMS\size=7
images/status/colorbars.png
0 → 100644
View file @
c2a4f4d2
6.42 KB
qgroundcontrol.pri
View file @
c2a4f4d2
...
...
@@ -333,6 +333,10 @@ linux-g++-64 {
-losgQt \
-lOpenThreads
exists(/usr/local/lib64) {
LIBS += -L/usr/local/lib64
}
DEFINES += QGC_OSG_ENABLED
DEFINES += QGC_OSG_QT_ENABLED
}
...
...
@@ -358,11 +362,28 @@ linux-g++-64 {
}
# Validated copy commands
QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR
QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$TARGETDIR
QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR
QMAKE_POST_LINK += && mkdir -p $$TARGETDIRimages
QMAKE_POST_LINK += && cp -rf $$BASEDIR/images/Vera.ttf $$TARGETDIR/images/Vera.ttf
debug {
!exists($$TARGETDIR/debug){
QMAKE_POST_LINK += && mkdir -p $$TARGETDIR/debug
}
DESTDIR = $$TARGETDIR/debug
QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/debug
QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$TARGETDIR/debug
QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR/debug
QMAKE_POST_LINK += && mkdir -p $$TARGETDIR/debug/images
QMAKE_POST_LINK += && cp -rf $$BASEDIR/images/Vera.ttf $$TARGETDIR/debug/images/Vera.ttf
}
release {
!exists($$TARGETDIR/release){
QMAKE_POST_LINK += && mkdir -p $$TARGETDIR/release
}
DESTDIR = $$TARGETDIR/release
QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/release
QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$TARGETDIR/release
QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR/release
QMAKE_POST_LINK += && mkdir -p $$TARGETDIR/release/images
QMAKE_POST_LINK += && cp -rf $$BASEDIR/images/Vera.ttf $$TARGETDIR/release/images/Vera.ttf
}
# osg/osgEarth dynamic casts might fail without this compiler option.
# see http://osgearth.org/wiki/FAQ for details.
...
...
qgroundcontrol.pro
View file @
c2a4f4d2
...
...
@@ -361,7 +361,8 @@ HEADERS += src/MG.h \
src
/
ui
/
mavlink
/
QGCMAVLinkMessageSender
.
h
\
src
/
ui
/
firmwareupdate
/
QGCFirmwareUpdateWidget
.
h
\
src
/
ui
/
QGCPluginHost
.
h
\
src
/
ui
/
firmwareupdate
/
QGCPX4FirmwareUpdate
.
h
src
/
ui
/
firmwareupdate
/
QGCPX4FirmwareUpdate
.
h
\
src
/
ui
/
map3D
/
gpl
.
h
#
Google
Earth
is
only
supported
on
Mac
OS
and
Windows
with
Visual
Studio
Compiler
macx
|
macx
-
g
++
|
macx
-
g
++
42
|
win32
-
msvc2008
|
win32
-
msvc2010
::
HEADERS
+=
src
/
ui
/
map3D
/
QGCGoogleEarthView
.
h
...
...
@@ -494,7 +495,8 @@ SOURCES += src/main.cc \
src
/
ui
/
mavlink
/
QGCMAVLinkMessageSender
.
cc
\
src
/
ui
/
firmwareupdate
/
QGCFirmwareUpdateWidget
.
cc
\
src
/
ui
/
QGCPluginHost
.
cc
\
src
/
ui
/
firmwareupdate
/
QGCPX4FirmwareUpdate
.
cc
src
/
ui
/
firmwareupdate
/
QGCPX4FirmwareUpdate
.
cc
\
src
/
ui
/
map3D
/
gpl
.
cc
#
Enable
Google
Earth
only
on
Mac
OS
and
Windows
with
Visual
Studio
compiler
macx
|
macx
-
g
++
|
macx
-
g
++
42
|
win32
-
msvc2008
|
win32
-
msvc2010
::
SOURCES
+=
src
/
ui
/
map3D
/
QGCGoogleEarthView
.
cc
...
...
qgroundcontrol.qrc
View file @
c2a4f4d2
...
...
@@ -75,6 +75,7 @@
<file>images/status/audio-volume-medium.svg</file>
<file>images/status/audio-volume-low.svg</file>
<file>images/status/audio-volume-high.svg</file>
<file>images/status/colorbars.png</file>
<file>images/style-mission.css</file>
<file>images/splash.png</file>
<file>audio/alert.wav</file>
...
...
src/QGC.cc
View file @
c2a4f4d2
...
...
@@ -47,6 +47,15 @@ quint64 groundTimeMilliseconds()
return
static_cast
<
quint64
>
(
seconds
+
(
time
.
time
().
msec
()));
}
qreal
groundTimeSeconds
()
{
QDateTime
time
=
QDateTime
::
currentDateTime
();
time
=
time
.
toUTC
();
/* Return time in seconds unit */
quint64
seconds
=
time
.
toTime_t
();
return
static_cast
<
qreal
>
(
seconds
+
(
time
.
time
().
msec
()
/
1000.0
));
}
float
limitAngleToPMPIf
(
float
angle
)
{
if
(
angle
>
-
20
*
M_PI
&&
angle
<
20
*
M_PI
)
...
...
src/QGC.h
View file @
c2a4f4d2
...
...
@@ -77,6 +77,8 @@ const QColor colorBlack(0, 0, 0);
quint64
groundTimeUsecs
();
/** @brief Get the current ground time in milliseconds */
quint64
groundTimeMilliseconds
();
/** @brief Get the current ground time in seconds */
qreal
groundTimeSeconds
();
/** @brief Returns the angle limited to -pi - pi */
float
limitAngleToPMPIf
(
float
angle
);
/** @brief Returns the angle limited to -pi - pi */
...
...
src/comm/MAVLinkProtocol.cc
View file @
c2a4f4d2
...
...
@@ -31,6 +31,10 @@
#include "QGCMAVLinkUASFactory.h"
#include "QGC.h"
#ifdef QGC_PROTOBUF_ENABLED
#include <google/protobuf/descriptor.h>
#endif
/**
* The default constructor will create a new MAVLink object sending heartbeats at
...
...
@@ -198,6 +202,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
// }
//#endif
#ifdef QGC_PROTOBUF_ENABLED
if
(
message
.
msgid
==
MAVLINK_MSG_ID_EXTENDED_MESSAGE
)
{
mavlink_extended_message_t
extended_message
;
...
...
@@ -218,10 +223,45 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
std
::
tr1
::
shared_ptr
<
google
::
protobuf
::
Message
>
protobuf_msg
;
if
(
protobufManager
.
getMessage
(
protobuf_msg
))
{
const
google
::
protobuf
::
Descriptor
*
descriptor
=
protobuf_msg
->
GetDescriptor
();
if
(
!
descriptor
)
{
continue
;
}
const
google
::
protobuf
::
FieldDescriptor
*
headerField
=
descriptor
->
FindFieldByName
(
"header"
);
if
(
!
headerField
)
{
continue
;
}
const
google
::
protobuf
::
Descriptor
*
headerDescriptor
=
headerField
->
message_type
();
if
(
!
headerDescriptor
)
{
continue
;
}
const
google
::
protobuf
::
FieldDescriptor
*
sourceSysIdField
=
headerDescriptor
->
FindFieldByName
(
"source_sysid"
);
if
(
!
sourceSysIdField
)
{
continue
;
}
const
google
::
protobuf
::
Reflection
*
reflection
=
protobuf_msg
->
GetReflection
();
const
google
::
protobuf
::
Message
&
headerMsg
=
reflection
->
GetMessage
(
*
protobuf_msg
,
headerField
);
const
google
::
protobuf
::
Reflection
*
headerReflection
=
headerMsg
.
GetReflection
();
int
source_sysid
=
headerReflection
->
GetInt32
(
headerMsg
,
sourceSysIdField
);
UASInterface
*
uas
=
UASManager
::
instance
()
->
getUASForId
(
source_sysid
);
if
(
uas
!=
NULL
)
{
emit
extendedMessageReceived
(
link
,
protobuf_msg
);
}
}
}
position
+=
extended_message
.
extended_payload_len
;
...
...
src/uas/QGCUASParamManager.h
View file @
c2a4f4d2
...
...
@@ -20,8 +20,20 @@ public:
QList
<
QVariant
>
getParameterValues
(
int
component
)
const
{
return
parameters
.
value
(
component
)
->
values
();
}
QVariant
getParameterValue
(
int
component
,
const
QString
&
parameter
)
const
{
return
parameters
.
value
(
component
)
->
value
(
parameter
);
bool
getParameterValue
(
int
component
,
const
QString
&
parameter
,
QVariant
&
value
)
const
{
if
(
!
parameters
.
contains
(
component
))
{
return
false
;
}
if
(
!
parameters
.
value
(
component
)
->
contains
(
parameter
))
{
return
false
;
}
value
=
parameters
.
value
(
component
)
->
value
(
parameter
);
return
true
;
}
virtual
bool
isParamMinKnown
(
const
QString
&
param
)
=
0
;
...
...
src/uas/UAS.cc
View file @
c2a4f4d2
...
...
@@ -27,6 +27,10 @@
#include "LinkManager.h"
#include "SerialLink.h"
#ifdef QGC_PROTOBUF_ENABLED
#include <google/protobuf/descriptor.h>
#endif
UAS
::
UAS
(
MAVLinkProtocol
*
protocol
,
int
id
)
:
UASInterface
(),
uasId
(
id
),
startTime
(
QGC
::
groundTimeMilliseconds
()),
...
...
@@ -71,6 +75,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
pitch
(
0.0
),
yaw
(
0.0
),
statusTimeout
(
new
QTimer
(
this
)),
#ifdef QGC_PROTOBUF_ENABLED
receivedPointCloudTimestamp
(
0.0
),
receivedRGBDImageTimestamp
(
0.0
),
receivedObstacleListTimestamp
(
0.0
),
receivedPathTimestamp
(
0.0
),
#endif
paramsOnceRequested
(
false
),
airframe
(
QGC_AIRFRAME_EASYSTAR
),
attitudeKnown
(
false
),
...
...
@@ -230,9 +240,21 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
bool
multiComponentSourceDetected
=
false
;
bool
wrongComponent
=
false
;
switch
(
message
.
compid
)
{
case
MAV_COMP_ID_IMU_2
:
// Prefer IMU 2 over IMU 1 (FIXME)
componentID
[
message
.
msgid
]
=
MAV_COMP_ID_IMU_2
;
break
;
default:
// Do nothing
break
;
}
// Store component ID
if
(
componentID
[
message
.
msgid
]
==
-
1
)
{
// Prefer the first component
componentID
[
message
.
msgid
]
=
message
.
compid
;
}
else
...
...
@@ -440,9 +462,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// compass = 0.0f;
// }
attitudeKnown
=
true
;
emit
attitudeChanged
(
this
,
roll
,
pitch
,
yaw
,
time
);
emit
attitudeChanged
(
this
,
message
.
compid
,
roll
,
pitch
,
yaw
,
time
);
emit
attitudeSpeedChanged
(
uasId
,
attitude
.
rollspeed
,
attitude
.
pitchspeed
,
attitude
.
yawspeed
,
time
);
}
break
;
...
...
@@ -478,9 +500,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
mavlink_local_position_ned_t
pos
;
mavlink_msg_local_position_ned_decode
(
&
message
,
&
pos
);
quint64
time
=
getUnixTime
(
pos
.
time_boot_ms
);
// Emit position always with component ID
emit
localPositionChanged
(
this
,
message
.
compid
,
pos
.
x
,
pos
.
y
,
pos
.
z
,
time
);
if
(
!
wrongComponent
)
{
localX
=
pos
.
x
;
localY
=
pos
.
y
;
localZ
=
pos
.
z
;
// Emit
emit
localPositionChanged
(
this
,
pos
.
x
,
pos
.
y
,
pos
.
z
,
time
);
emit
speedChanged
(
this
,
pos
.
vx
,
pos
.
vy
,
pos
.
vz
,
time
);
...
...
@@ -492,6 +524,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
positionLock
=
true
;
isLocalPositionKnown
=
true
;
}
}
break
;
case
MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE
:
{
mavlink_global_vision_position_estimate_t
pos
;
mavlink_msg_global_vision_position_estimate_decode
(
&
message
,
&
pos
);
quint64
time
=
getUnixTime
(
pos
.
usec
);
emit
localPositionChanged
(
this
,
message
.
compid
,
pos
.
x
,
pos
.
y
,
pos
.
z
,
time
);
emit
attitudeChanged
(
this
,
message
.
compid
,
pos
.
roll
,
pos
.
pitch
,
pos
.
yaw
,
time
);
}
break
;
case
MAVLINK_MSG_ID_GLOBAL_POSITION_INT
:
//std::cerr << std::endl;
...
...
@@ -786,6 +828,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit
positionSetPointsChanged
(
uasId
,
p
.
x
,
p
.
y
,
p
.
z
,
p
.
yaw
,
QGC
::
groundTimeUsecs
());
}
break
;
case
MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT
:
{
mavlink_set_local_position_setpoint_t
p
;
mavlink_msg_set_local_position_setpoint_decode
(
&
message
,
&
p
);
emit
userPositionSetPointsChanged
(
uasId
,
p
.
x
,
p
.
y
,
p
.
z
,
p
.
yaw
);
}
break
;
case
MAVLINK_MSG_ID_STATUSTEXT
:
{
QByteArray
b
;
...
...
@@ -955,7 +1004,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
#endif
// Messages to ignore
case
MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT
:
case
MAVLINK_MSG_ID_RAW_IMU
:
case
MAVLINK_MSG_ID_SCALED_IMU
:
case
MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT
:
...
...
@@ -988,27 +1036,82 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
#ifdef QGC_PROTOBUF_ENABLED
void
UAS
::
receiveExtendedMessage
(
LinkInterface
*
link
,
std
::
tr1
::
shared_ptr
<
google
::
protobuf
::
Message
>
message
)
{
if
(
!
link
)
return
;
if
(
!
link
)
{
return
;
}
if
(
!
links
->
contains
(
link
))
{
addLink
(
link
);
}
const
google
::
protobuf
::
Descriptor
*
descriptor
=
message
->
GetDescriptor
();
if
(
!
descriptor
)
{
return
;
}
const
google
::
protobuf
::
FieldDescriptor
*
headerField
=
descriptor
->
FindFieldByName
(
"header"
);
if
(
!
headerField
)
{
return
;
}
const
google
::
protobuf
::
Descriptor
*
headerDescriptor
=
headerField
->
message_type
();
if
(
!
headerDescriptor
)
{
return
;
}
const
google
::
protobuf
::
FieldDescriptor
*
sourceSysIdField
=
headerDescriptor
->
FindFieldByName
(
"source_sysid"
);
if
(
!
sourceSysIdField
)
{
return
;
}
const
google
::
protobuf
::
Reflection
*
reflection
=
message
->
GetReflection
();
const
google
::
protobuf
::
Message
&
headerMsg
=
reflection
->
GetMessage
(
*
message
,
headerField
);
const
google
::
protobuf
::
Reflection
*
headerReflection
=
headerMsg
.
GetReflection
();
int
source_sysid
=
headerReflection
->
GetInt32
(
headerMsg
,
sourceSysIdField
);
if
(
source_sysid
!=
uasId
)
{
return
;
}
if
(
message
->
GetTypeName
()
==
pointCloud
.
GetTypeName
())
{
receivedPointCloudTimestamp
=
QGC
::
groundTimeSeconds
();
pointCloudMutex
.
lock
();
pointCloud
.
CopyFrom
(
*
message
);
pointCloudMutex
.
unlock
();
emit
pointCloudChanged
(
this
);
}
else
if
(
message
->
GetTypeName
()
==
rgbdImage
.
GetTypeName
())
{
receivedRGBDImageTimestamp
=
QGC
::
groundTimeSeconds
();
rgbdImageMutex
.
lock
();
rgbdImage
.
CopyFrom
(
*
message
);
rgbdImageMutex
.
unlock
();
emit
rgbdImageChanged
(
this
);
}
else
if
(
message
->
GetTypeName
()
==
obstacleList
.
GetTypeName
())
{
receivedObstacleListTimestamp
=
QGC
::
groundTimeSeconds
();
obstacleListMutex
.
lock
();
obstacleList
.
CopyFrom
(
*
message
);
obstacleListMutex
.
unlock
();
emit
obstacleListChanged
(
this
);
}
else
if
(
message
->
GetTypeName
()
==
path
.
GetTypeName
())
{
receivedPathTimestamp
=
QGC
::
groundTimeSeconds
();
pathMutex
.
lock
();
path
.
CopyFrom
(
*
message
);
pathMutex
.
unlock
();
emit
pathChanged
(
this
);
}
}
#endif
...
...
src/uas/UAS.h
View file @
c2a4f4d2
...
...
@@ -135,17 +135,49 @@ public:
bool
getSelected
()
const
;
#ifdef QGC_PROTOBUF_ENABLED
px
::
PointCloudXYZRGB
getPointCloud
()
const
{
px
::
PointCloudXYZRGB
getPointCloud
()
{
QMutexLocker
locker
(
&
pointCloudMutex
);
return
pointCloud
;
}
px
::
RGBDImage
getRGBDImage
()
const
{
px
::
PointCloudXYZRGB
getPointCloud
(
qreal
&
receivedTimestamp
)
{
receivedTimestamp
=
receivedPointCloudTimestamp
;
QMutexLocker
locker
(
&
pointCloudMutex
);
return
pointCloud
;
}
px
::
RGBDImage
getRGBDImage
()
{
QMutexLocker
locker
(
&
rgbdImageMutex
);
return
rgbdImage
;
}
px
::
RGBDImage
getRGBDImage
(
qreal
&
receivedTimestamp
)
{
receivedTimestamp
=
receivedRGBDImageTimestamp
;
QMutexLocker
locker
(
&
rgbdImageMutex
);
return
rgbdImage
;
}
px
::
ObstacleList
getObstacleList
()
const
{
px
::
ObstacleList
getObstacleList
()
{
QMutexLocker
locker
(
&
obstacleListMutex
);
return
obstacleList
;
}
px
::
ObstacleList
getObstacleList
(
qreal
&
receivedTimestamp
)
{
receivedTimestamp
=
receivedObstacleListTimestamp
;
QMutexLocker
locker
(
&
obstacleListMutex
);
return
obstacleList
;
}
px
::
Path
getPath
()
{
QMutexLocker
locker
(
&
pathMutex
);
return
path
;
}
px
::
Path
getPath
(
qreal
&
receivedTimestamp
)
{
receivedTimestamp
=
receivedPathTimestamp
;
QMutexLocker
locker
(
&
pathMutex
);
return
path
;
}
#endif
friend
class
UASWaypointManager
;
...
...
@@ -233,8 +265,20 @@ protected: //COMMENTS FOR TEST UNIT
#ifdef QGC_PROTOBUF_ENABLED
px
::
PointCloudXYZRGB
pointCloud
;
QMutex
pointCloudMutex
;
qreal
receivedPointCloudTimestamp
;
px
::
RGBDImage
rgbdImage
;
QMutex
rgbdImageMutex
;
qreal
receivedRGBDImageTimestamp
;
px
::
ObstacleList
obstacleList
;
QMutex
obstacleListMutex
;
qreal
receivedObstacleListTimestamp
;
px
::
Path
path
;
QMutex
pathMutex
;
qreal
receivedPathTimestamp
;
#endif
QMap
<
int
,
QMap
<
QString
,
QVariant
>*
>
parameters
;
///< All parameters
...
...
@@ -573,6 +617,8 @@ signals:
void
rgbdImageChanged
(
UASInterface
*
uas
);
/** @brief Obstacle list data has been changed */
void
obstacleListChanged
(
UASInterface
*
uas
);
/** @brief Path data has been changed */
void
pathChanged
(
UASInterface
*
uas
);
#endif
/** @brief HIL controls have changed */
void
hilControlsChanged
(
uint64_t
time
,
float
rollAilerons
,
float
pitchElevator
,
float
yawRudder
,
float
throttle
,
uint8_t
systemMode
,
uint8_t
navMode
);
...
...
src/uas/UASInterface.h
View file @
c2a4f4d2
...
...
@@ -95,9 +95,14 @@ public:
virtual
bool
getSelected
()
const
=
0
;
#ifdef QGC_PROTOBUF_ENABLED
virtual
px
::
PointCloudXYZRGB
getPointCloud
()
const
=
0
;
virtual
px
::
RGBDImage
getRGBDImage
()
const
=
0
;
virtual
px
::
ObstacleList
getObstacleList
()
const
=
0
;
virtual
px
::
PointCloudXYZRGB
getPointCloud
()
=
0
;
virtual
px
::
PointCloudXYZRGB
getPointCloud
(
qreal
&
receivedTimestamp
)
=
0
;
virtual
px
::
RGBDImage
getRGBDImage
()
=
0
;
virtual
px
::
RGBDImage
getRGBDImage
(
qreal
&
receivedTimestamp
)
=
0
;
virtual
px
::
ObstacleList
getObstacleList
()
=
0
;
virtual
px
::
ObstacleList
getObstacleList
(
qreal
&
receivedTimestamp
)
=
0
;
virtual
px
::
Path
getPath
()
=
0
;
virtual
px
::
Path
getPath
(
qreal
&
receivedTimestamp
)
=
0
;
#endif
virtual
bool
isArmed
()
const
=
0
;
...
...
@@ -442,10 +447,15 @@ signals:
void
thrustChanged
(
UASInterface
*
,
double
thrust
);
void
heartbeat
(
UASInterface
*
uas
);
void
attitudeChanged
(
UASInterface
*
,
double
roll
,
double
pitch
,
double
yaw
,
quint64
usec
);
void
attitudeChanged
(
UASInterface
*
,
int
component
,
double
roll
,
double
pitch
,
double
yaw
,
quint64
usec
);
void
attitudeSpeedChanged
(
int
uas
,
double
rollspeed
,
double
pitchspeed
,
double
yawspeed
,
quint64
usec
);
void
attitudeThrustSetPointChanged
(
UASInterface
*
,
double
rollDesired
,
double
pitchDesired
,
double
yawDesired
,
double
thrustDesired
,
quint64
usec
);
/** @brief The MAV set a new setpoint in the local (not body) NED X, Y, Z frame */
void
positionSetPointsChanged
(
int
uasid
,
float
xDesired
,
float
yDesired
,
float
zDesired
,
float
yawDesired
,
quint64
usec
);
/** @brief A user (or an autonomous mission or obstacle avoidance planner) requested to set a new setpoint */
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
(
int
uasid
,
double
altitude
);
/** @brief Update the status of one satellite used for localization */
...
...
src/uas/UASWaypointManager.cc
View file @
c2a4f4d2
...
...
@@ -61,6 +61,11 @@ UASWaypointManager::UASWaypointManager(UAS* _uas)
}
}
UASWaypointManager
::~
UASWaypointManager
()
{
}
void
UASWaypointManager
::
timeout
()
{
if
(
current_retries
>
0
)
{
...
...
src/uas/UASWaypointManager.h
View file @
c2a4f4d2
...
...
@@ -65,6 +65,7 @@ private:
public:
UASWaypointManager
(
UAS
*
uas
=
NULL
);
///< Standard constructor
~
UASWaypointManager
();
/** @name Received message handlers */
/*@{*/
...
...
src/ui/HSIDisplay.cc
View file @
c2a4f4d2
...
...
@@ -668,6 +668,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
disconnect
(
this
->
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
attitudeThrustSetPointChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAttitudeSetpoints
(
UASInterface
*
,
double
,
double
,
double
,
double
,
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
)));
disconnect
(
this
->
uas
,
SIGNAL
(
speedChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateSpeed
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
attitudeChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAttitude
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
...
...
@@ -688,6 +689,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
attitudeThrustSetPointChanged
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAttitudeSetpoints
(
UASInterface
*
,
double
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
positionSetPointsChanged
(
int
,
float
,
float
,
float
,
float
,
quint64
)),
this
,
SLOT
(
updatePositionSetpoints
(
int
,
float
,
float
,
float
,
float
,
quint64
)));
connect
(
uas
,
SIGNAL
(
userPositionSetPointsChanged
(
int
,
float
,
float
,
float
,
float
)),
this
,
SLOT
(
updateUserPositionSetpoints
(
int
,
float
,
float
,
float
,
float
)));
connect
(
uas
,
SIGNAL
(
speedChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateSpeed
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
attitudeChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAttitude
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
...
...
@@ -811,6 +813,16 @@ void HSIDisplay::updateAttitude(UASInterface* uas, double roll, double pitch, do
this
->
yaw
=
yaw
;
}
void
HSIDisplay
::
updateUserPositionSetpoints
(
int
uasid
,
float
xDesired
,
float
yDesired
,
float
zDesired
,
float
yawDesired
)
{
uiXSetCoordinate
=
xDesired
;
uiYSetCoordinate
=
yDesired
;
uiZSetCoordinate
=
zDesired
;
uiYawSet
=
yawDesired
;
userXYSetPointSet
=
true
;
userSetPointSet
=
true
;
}
void
HSIDisplay
::
updatePositionSetpoints
(
int
uasid
,
float
xDesired
,
float
yDesired
,
float
zDesired
,
float
yawDesired
,
quint64
usec
)
{
Q_UNUSED
(
usec
);
...
...
src/ui/HSIDisplay.h
View file @
c2a4f4d2
...
...
@@ -57,6 +57,7 @@ public slots:
void
updateSatellite
(
int
uasid
,
int
satid
,
float
azimuth
,
float
direction
,
float
snr
,
bool
used
);
void
updateAttitudeSetpoints
(
UASInterface
*
,
double
rollDesired
,
double
pitchDesired
,
double
yawDesired
,
double
thrustDesired
,
quint64
usec
);
void
updateAttitude
(
UASInterface
*
uas
,
double
roll
,
double
pitch
,
double
yaw
,
quint64
time
);
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
);
...
...
src/ui/HUD.h
View file @
c2a4f4d2
...
...
@@ -61,7 +61,7 @@ public slots:
//void paintGL();
/** @brief Set the currently monitored UAS */
void
setActiveUAS
(
UASInterface
*
uas
);
v
irtual
v
oid
setActiveUAS
(
UASInterface
*
uas
);
void
updateAttitude
(
UASInterface
*
uas
,
double
roll
,
double
pitch
,
double
yaw
,
quint64
timestamp
);
// void updateAttitudeThrustSetPoint(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
...
...
src/ui/QGCRGBDView.cc
View file @
c2a4f4d2
...
...
@@ -21,16 +21,40 @@ QGCRGBDView::QGCRGBDView(int width, int height, QWidget *parent) :
enableDepthAction
->
setChecked
(
depthEnabled
);
connect
(
enableDepthAction
,
SIGNAL
(
triggered
(
bool
)),
this
,
SLOT
(
enableDepth
(
bool
)));
connect
(
UASManager
::
instance
(),
SIGNAL
(
UASCreated
(
UASInterface
*
)),
this
,
SLOT
(
addUAS
(
UASInterface
*
)));
connect
(
UASManager
::
instance
(),
SIGNAL
(
activeUASSet
(
UASInterface
*
)),
this
,
SLOT
(
setActiveUAS
(
UASInterface
*
)));
clearData
();
}
void
QGCRGBDView
::
addUAS
(
UASInterface
*
uas
)
void
QGCRGBDView
::
setActiveUAS
(
UASInterface
*
uas
)
{
// TODO Enable multi-uas support
if
(
this
->
uas
!=
NULL
)
{
// Disconnect any previously connected active MAV
disconnect
(
this
->
uas
,
SIGNAL
(
rgbdImageChanged
(
UASInterface
*
)),
this
,
SLOT
(
updateData
(
UASInterface
*
)));
clearData
();
}
if
(
uas
)
{
// Now connect the new UAS
// Setup communication
connect
(
uas
,
SIGNAL
(
rgbdImageChanged
(
UASInterface
*
)),
this
,
SLOT
(
updateData
(
UASInterface
*
)));
}
HUD
::
setActiveUAS
(
uas
);
}
void
QGCRGBDView
::
clearData
(
void
)
{
QImage
offlineImg
;
qDebug
()
<<
offlineImg
.
load
(
":/images/status/colorbars.png"
);
glImage
=
QGLWidget
::
convertToGLFormat
(
offlineImg
);
}
void
QGCRGBDView
::
contextMenuEvent
(
QContextMenuEvent
*
event
)
void
QGCRGBDView
::
contextMenuEvent
(
QContextMenuEvent
*
event
)
{
QMenu
menu
(
this
);
// Update actions
...
...
@@ -248,7 +272,7 @@ void QGCRGBDView::updateData(UASInterface *uas)
{
if
(
depth
[
c
]
!=
0
)
{
int
idx
=
fminf
(
depth
[
c
],
7.0
f
)
/
7
.0
f
*
127.0
f
;
int
idx
=
fminf
(
depth
[
c
],
10.0
f
)
/
10
.0
f
*
127.0
f
;
idx
=
127
-
idx
;
pixel
[
0
]
=
colormapJet
[
idx
][
2
]
*
255.0
f
;
...
...
src/ui/QGCRGBDView.h
View file @
c2a4f4d2
...
...
@@ -12,7 +12,9 @@ public:
signals:
public
slots
:
void
addUAS
(
UASInterface
*
uas
);
void
setActiveUAS
(
UASInterface
*
uas
);
void
clearData
(
void
);
void
enableRGB
(
bool
enabled
);
void
enableDepth
(
bool
enabled
);
void
updateData
(
UASInterface
*
uas
);
...
...
src/ui/WaypointList.cc
View file @
c2a4f4d2
...
...
@@ -155,7 +155,10 @@ void WaypointList::updateAttitude(UASInterface* uas, double roll, double pitch,
void
WaypointList
::
setUAS
(
UASInterface
*
uas
)
{
//if (this->uas == NULL && uas != NULL)
if
(
this
->
uas
==
NULL
&&
uas
!=
NULL
)
{
WPM
=
uas
->
getWaypointManager
();
}
if
(
this
->
uas
==
NULL
)
{
this
->
uas
=
uas
;
...
...
src/ui/map3D/ObstacleGroupNode.cc
View file @
c2a4f4d2
...
...
@@ -48,25 +48,21 @@ ObstacleGroupNode::init(void)
}
void
ObstacleGroupNode
::
update
(
MAV_FRAME
frame
,
UASInterface
*
uas
)
ObstacleGroupNode
::
clear
(
void
)
{
if
(
!
uas
||
frame
==
MAV_FRAME_GLOBAL
)
{
return
;
}
double
robotX
=
uas
->
getLocalX
();
double
robotY
=
uas
->
getLocalY
();
double
robotZ
=
uas
->
getLocalZ
();
if
(
getNumChildren
()
>
0
)
{
removeChild
(
0
,
getNumChildren
());
}
}
osg
::
ref_ptr
<
osg
::
Geode
>
geode
=
new
osg
::
Geode
;
void
ObstacleGroupNode
::
update
(
double
robotX
,
double
robotY
,
double
robotZ
,
const
px
::
ObstacleList
&
obstacleList
)
{
clear
();
px
::
ObstacleList
obstacleList
=
uas
->
getObstacleList
()
;
osg
::
ref_ptr
<
osg
::
Geode
>
geode
=
new
osg
::
Geode
;
for
(
int
i
=
0
;
i
<
obstacleList
.
obstacles_size
();
++
i
)
{
...
...
src/ui/map3D/ObstacleGroupNode.h
View file @
c2a4f4d2
...
...
@@ -42,7 +42,9 @@ public:
ObstacleGroupNode
();
void
init
(
void
);
void
update
(
MAV_FRAME
frame
,
UASInterface
*
uas
);
void
clear
(
void
);
void
update
(
double
robotX
,
double
robotY
,
double
robotZ
,
const
px
::
ObstacleList
&
obstacleList
);
};
#endif // OBSTACLEGROUPNODE_H
src/ui/map3D/Pixhawk3DWidget.cc
View file @
c2a4f4d2
...
...
@@ -43,6 +43,7 @@
#include "UASManager.h"
#include "QGC.h"
#include "gpl.h"
#ifdef QGC_PROTOBUF_ENABLED
#include <tr1/memory>
...
...
@@ -52,15 +53,18 @@
Pixhawk3DWidget
::
Pixhawk3DWidget
(
QWidget
*
parent
)
:
Q3DWidget
(
parent
)
,
uas
(
NULL
)
,
kMessageTimeout
(
4.0
)
,
mode
(
DEFAULT_MODE
)
,
selectedWpIndex
(
-
1
)
,
displayGrid
(
true
)
,
displayTrail
(
false
)
,
displayLocalGrid
(
false
)
,
displayWorldGrid
(
true
)
,
displayTrails
(
true
)
,
displayImagery
(
true
)
,
displayWaypoints
(
true
)
,
displayRGBD2D
(
false
)
,
displayRGBD3D
(
true
)
,
displayObstacleList
(
true
)
,
displayPath
(
true
)
,
enableRGBDColor
(
false
)
,
enableTarget
(
false
)
,
followCamera
(
true
)
...
...
@@ -76,14 +80,20 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
vehicleModel
=
PixhawkCheetahGeode
::
instance
();
egocentricMap
->
addChild
(
vehicleModel
);
// generate grid model
gridNode
=
createGrid
();
rollingMap
->
addChild
(
gridNode
);
// generate grid models
localGridNode
=
createLocalGrid
();
rollingMap
->
addChild
(
localGridNode
);
worldGridNode
=
createWorldGrid
();
allocentricMap
->
addChild
(
worldGridNode
);
// generate empty trail model
trailNode
=
createTrail
()
;
trailNode
=
new
osg
::
Geode
;
rollingMap
->
addChild
(
trailNode
);
orientationNode
=
new
osg
::
Group
;
rollingMap
->
addChild
(
orientationNode
);
// generate map model
mapNode
=
createMap
();
rollingMap
->
addChild
(
mapNode
);
...
...
@@ -105,6 +115,11 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
obstacleGroupNode
=
new
ObstacleGroupNode
;
obstacleGroupNode
->
init
();
rollingMap
->
addChild
(
obstacleGroupNode
);
// generate path model
pathNode
=
new
osg
::
Geode
;
pathNode
->
addDrawable
(
createTrail
(
osg
::
Vec4
(
1.0
f
,
0.8
f
,
0.0
f
,
1.0
f
)));
rollingMap
->
addChild
(
pathNode
);
#endif
setupHUD
();
...
...
@@ -132,15 +147,148 @@ Pixhawk3DWidget::~Pixhawk3DWidget()
void
Pixhawk3DWidget
::
setActiveUAS
(
UASInterface
*
uas
)
{
if
(
this
->
uas
!=
NULL
&&
this
->
uas
!=
uas
)
if
(
this
->
uas
==
uas
)
{
return
;
}
if
(
this
->
uas
!=
NULL
)
{
// Disconnect any previously connected active MAV
//disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
disconnect
(
this
->
uas
,
SIGNAL
(
localPositionChanged
(
UASInterface
*
,
int
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
addToTrails
(
UASInterface
*
,
int
,
double
,
double
,
double
,
quint64
)));
disconnect
(
this
->
uas
,
SIGNAL
(
attitudeChanged
(
UASInterface
*
,
int
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAttitude
(
UASInterface
*
,
int
,
double
,
double
,
double
,
quint64
)));
}
connect
(
uas
,
SIGNAL
(
localPositionChanged
(
UASInterface
*
,
int
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
addToTrails
(
UASInterface
*
,
int
,
double
,
double
,
double
,
quint64
)));
connect
(
uas
,
SIGNAL
(
attitudeChanged
(
UASInterface
*
,
int
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateAttitude
(
UASInterface
*
,
int
,
double
,
double
,
double
,
quint64
)));
trails
.
clear
();
trailNode
->
removeDrawables
(
0
,
trailNode
->
getNumDrawables
());
orientationNode
->
removeChildren
(
0
,
orientationNode
->
getNumChildren
());
this
->
uas
=
uas
;
}
void
Pixhawk3DWidget
::
addToTrails
(
UASInterface
*
uas
,
int
component
,
double
x
,
double
y
,
double
z
,
quint64
time
)
{
if
(
this
->
uas
->
getUASID
()
!=
uas
->
getUASID
())
{
return
;
}
if
(
!
trails
.
contains
(
component
))
{
trails
[
component
]
=
QVarLengthArray
<
osg
::
Vec3d
,
10000
>
();
trailDrawableIdxs
[
component
]
=
trails
.
size
()
-
1
;
osg
::
Vec4
color
((
float
)
qrand
()
/
RAND_MAX
,
(
float
)
qrand
()
/
RAND_MAX
,
(
float
)
qrand
()
/
RAND_MAX
,
0.5
);
trailNode
->
addDrawable
(
createTrail
(
color
));
double
radius
=
0.5
;
osg
::
ref_ptr
<
osg
::
Group
>
group
=
new
osg
::
Group
;
// cone indicates waypoint orientation
osg
::
ref_ptr
<
osg
::
ShapeDrawable
>
sd
=
new
osg
::
ShapeDrawable
;
double
coneRadius
=
radius
/
2.0
;
osg
::
ref_ptr
<
osg
::
Cone
>
cone
=
new
osg
::
Cone
(
osg
::
Vec3d
(
0.0
,
0.0
,
0.0
),
coneRadius
,
radius
*
2.0
);
sd
->
setShape
(
cone
);
sd
->
getOrCreateStateSet
()
->
setMode
(
GL_BLEND
,
osg
::
StateAttribute
::
ON
);
sd
->
setColor
(
color
);
osg
::
ref_ptr
<
osg
::
Geode
>
geode
=
new
osg
::
Geode
;
geode
->
addDrawable
(
sd
);
osg
::
ref_ptr
<
osg
::
PositionAttitudeTransform
>
pat
=
new
osg
::
PositionAttitudeTransform
;
pat
->
addChild
(
geode
);
pat
->
setAttitude
(
osg
::
Quat
(
-
M_PI_2
,
osg
::
Vec3d
(
1.0
f
,
0.0
f
,
0.0
f
),
M_PI_2
,
osg
::
Vec3d
(
0.0
f
,
1.0
f
,
0.0
f
),
0.0
,
osg
::
Vec3d
(
0.0
f
,
0.0
f
,
1.0
f
)));
group
->
addChild
(
pat
);
// cylinder indicates waypoint position
sd
=
new
osg
::
ShapeDrawable
;
osg
::
ref_ptr
<
osg
::
Cylinder
>
cylinder
=
new
osg
::
Cylinder
(
osg
::
Vec3d
(
0.0
,
0.0
,
0.0
),
radius
,
0
);
sd
->
setShape
(
cylinder
);
sd
->
getOrCreateStateSet
()
->
setMode
(
GL_BLEND
,
osg
::
StateAttribute
::
ON
);
sd
->
setColor
(
color
);
geode
=
new
osg
::
Geode
;
geode
->
addDrawable
(
sd
);
group
->
addChild
(
geode
);
pat
=
new
osg
::
PositionAttitudeTransform
;
orientationNode
->
addChild
(
pat
);
pat
->
addChild
(
group
);
}
QVarLengthArray
<
osg
::
Vec3d
,
10000
>&
trail
=
trails
[
component
];
bool
addToTrail
=
false
;
if
(
trail
.
size
()
>
0
)
{
if
(
fabs
(
x
-
trail
[
trail
.
size
()
-
1
].
x
())
>
0.01
f
||
fabs
(
y
-
trail
[
trail
.
size
()
-
1
].
y
())
>
0.01
f
||
fabs
(
z
-
trail
[
trail
.
size
()
-
1
].
z
())
>
0.01
f
)
{
addToTrail
=
true
;
}
}
else
{
addToTrail
=
true
;
}
if
(
addToTrail
)
{
osg
::
Vec3d
p
(
x
,
y
,
z
);
if
(
trail
.
size
()
==
trail
.
capacity
())
{
memcpy
(
trail
.
data
(),
trail
.
data
()
+
1
,
(
trail
.
size
()
-
1
)
*
sizeof
(
osg
::
Vec3d
));
trail
[
trail
.
size
()
-
1
]
=
p
;
}
else
{
trail
.
append
(
p
);
}
}
}
void
Pixhawk3DWidget
::
updateAttitude
(
UASInterface
*
uas
,
int
component
,
double
roll
,
double
pitch
,
double
yaw
,
quint64
time
)
{
if
(
this
->
uas
->
getUASID
()
!=
uas
->
getUASID
())
{
return
;
}
if
(
!
trails
.
contains
(
component
))
{
return
;
}
int
idx
=
trailDrawableIdxs
[
component
];
osg
::
PositionAttitudeTransform
*
pat
=
dynamic_cast
<
osg
::
PositionAttitudeTransform
*>
(
orientationNode
->
getChild
(
idx
));
pat
->
setAttitude
(
osg
::
Quat
(
-
yaw
,
osg
::
Vec3d
(
0.0
f
,
0.0
f
,
1.0
f
),
0.0
,
osg
::
Vec3d
(
1.0
f
,
0.0
f
,
0.0
f
),
0.0
,
osg
::
Vec3d
(
0.0
f
,
1.0
f
,
0.0
f
)));
}
void
Pixhawk3DWidget
::
selectFrame
(
QString
text
)
{
...
...
@@ -159,33 +307,46 @@ Pixhawk3DWidget::selectFrame(QString text)
}
void
Pixhawk3DWidget
::
showGrid
(
int32_t
state
)
Pixhawk3DWidget
::
show
Local
Grid
(
int32_t
state
)
{
if
(
state
==
Qt
::
Checked
)
{
displayGrid
=
true
;
display
Local
Grid
=
true
;
}
else
{
displayGrid
=
false
;
display
Local
Grid
=
false
;
}
}
void
Pixhawk3DWidget
::
show
Trail
(
int32_t
state
)
Pixhawk3DWidget
::
show
WorldGrid
(
int32_t
state
)
{
if
(
state
==
Qt
::
Checked
)
{
if
(
!
displayTrail
)
displayWorldGrid
=
true
;
}
else
{
trail
.
clear
()
;
displayWorldGrid
=
false
;
}
}
displayTrail
=
true
;
void
Pixhawk3DWidget
::
showTrails
(
int32_t
state
)
{
if
(
state
==
Qt
::
Checked
)
{
if
(
!
displayTrails
)
{
trails
.
clear
();
}
displayTrails
=
true
;
}
else
{
displayTrail
=
false
;
displayTrail
s
=
false
;
}
}
...
...
@@ -276,7 +437,7 @@ Pixhawk3DWidget::selectTargetHeading(void)
p
.
set
(
cursorWorldCoords
.
first
,
cursorWorldCoords
.
second
);
}
target
.
z
()
=
atan2
(
p
.
y
()
-
target
.
y
(),
p
.
x
()
-
target
.
x
(
));
target
.
setW
(
atan2
(
p
.
y
()
-
target
.
y
(),
p
.
x
()
-
target
.
x
()
));
}
void
...
...
@@ -286,6 +447,10 @@ Pixhawk3DWidget::selectTarget(void)
{
return
;
}
if
(
!
uas
->
getParamManager
())
{
return
;
}
if
(
frame
==
MAV_FRAME_GLOBAL
)
{
...
...
@@ -295,7 +460,14 @@ Pixhawk3DWidget::selectTarget(void)
getGlobalCursorPosition
(
cachedMousePos
.
x
(),
cachedMousePos
.
y
(),
altitude
);
target
.
set
(
cursorWorldCoords
.
first
,
cursorWorldCoords
.
second
,
0.0
);
QVariant
zTarget
;
if
(
!
uas
->
getParamManager
()
->
getParameterValue
(
MAV_COMP_ID_PATHPLANNER
,
"TARGET-ALT"
,
zTarget
))
{
zTarget
=
-
altitude
;
}
target
=
QVector4D
(
cursorWorldCoords
.
first
,
cursorWorldCoords
.
second
,
zTarget
.
toReal
(),
0.0
);
}
else
if
(
frame
==
MAV_FRAME_LOCAL_NED
)
{
...
...
@@ -304,7 +476,14 @@ Pixhawk3DWidget::selectTarget(void)
std
::
pair
<
double
,
double
>
cursorWorldCoords
=
getGlobalCursorPosition
(
cachedMousePos
.
x
(),
cachedMousePos
.
y
(),
-
z
);
target
.
set
(
cursorWorldCoords
.
first
,
cursorWorldCoords
.
second
,
0.0
);
QVariant
zTarget
;
if
(
!
uas
->
getParamManager
()
->
getParameterValue
(
MAV_COMP_ID_PATHPLANNER
,
"TARGET-ALT"
,
zTarget
))
{
zTarget
=
z
;
}
target
=
QVector4D
(
cursorWorldCoords
.
first
,
cursorWorldCoords
.
second
,
zTarget
.
toReal
(),
0.0
);
}
enableTarget
=
true
;
...
...
@@ -317,8 +496,8 @@ Pixhawk3DWidget::setTarget(void)
{
selectTargetHeading
();
uas
->
setTargetPosition
(
target
.
x
(),
target
.
y
(),
0.0
,
osg
::
RadiansToDegrees
(
target
.
z
()));
uas
->
setTargetPosition
(
target
.
x
(),
target
.
y
(),
target
.
z
()
,
osg
::
RadiansToDegrees
(
target
.
w
()));
}
void
...
...
@@ -587,13 +766,17 @@ Pixhawk3DWidget::buildLayout(void)
frameComboBox
->
addItem
(
"Global"
);
frameComboBox
->
setFixedWidth
(
70
);
QCheckBox
*
gridCheckBox
=
new
QCheckBox
(
this
);
gridCheckBox
->
setText
(
"Grid"
);
gridCheckBox
->
setChecked
(
displayGrid
);
QCheckBox
*
localGridCheckBox
=
new
QCheckBox
(
this
);
localGridCheckBox
->
setText
(
"Local Grid"
);
localGridCheckBox
->
setChecked
(
displayLocalGrid
);
QCheckBox
*
worldGridCheckBox
=
new
QCheckBox
(
this
);
worldGridCheckBox
->
setText
(
"World Grid"
);
worldGridCheckBox
->
setChecked
(
displayWorldGrid
);
QCheckBox
*
trailCheckBox
=
new
QCheckBox
(
this
);
trailCheckBox
->
setText
(
"Trail"
);
trailCheckBox
->
setChecked
(
displayTrail
);
trailCheckBox
->
setText
(
"Trail
s
"
);
trailCheckBox
->
setChecked
(
displayTrail
s
);
QCheckBox
*
waypointsCheckBox
=
new
QCheckBox
(
this
);
waypointsCheckBox
->
setText
(
"Waypoints"
);
...
...
@@ -623,17 +806,18 @@ Pixhawk3DWidget::buildLayout(void)
layout
->
setMargin
(
0
);
layout
->
setSpacing
(
2
);
layout
->
addWidget
(
frameComboBox
,
0
,
10
);
layout
->
addWidget
(
gridCheckBox
,
2
,
0
);
layout
->
addWidget
(
trailCheckBox
,
2
,
1
);
layout
->
addWidget
(
waypointsCheckBox
,
2
,
2
);
layout
->
addItem
(
new
QSpacerItem
(
10
,
0
,
QSizePolicy
::
Expanding
,
QSizePolicy
::
Expanding
),
2
,
3
);
layout
->
addWidget
(
mapLabel
,
2
,
4
);
layout
->
addWidget
(
mapComboBox
,
2
,
5
);
layout
->
addWidget
(
modelLabel
,
2
,
6
);
layout
->
addWidget
(
modelComboBox
,
2
,
7
);
layout
->
addItem
(
new
QSpacerItem
(
10
,
0
,
QSizePolicy
::
Expanding
,
QSizePolicy
::
Expanding
),
2
,
8
);
layout
->
addWidget
(
recenterButton
,
2
,
9
);
layout
->
addWidget
(
followCameraCheckBox
,
2
,
10
);
layout
->
addWidget
(
localGridCheckBox
,
2
,
0
);
layout
->
addWidget
(
worldGridCheckBox
,
2
,
1
);
layout
->
addWidget
(
trailCheckBox
,
2
,
2
);
layout
->
addWidget
(
waypointsCheckBox
,
2
,
3
);
layout
->
addItem
(
new
QSpacerItem
(
10
,
0
,
QSizePolicy
::
Expanding
,
QSizePolicy
::
Expanding
),
2
,
4
);
layout
->
addWidget
(
mapLabel
,
2
,
5
);
layout
->
addWidget
(
mapComboBox
,
2
,
6
);
layout
->
addWidget
(
modelLabel
,
2
,
7
);
layout
->
addWidget
(
modelComboBox
,
2
,
8
);
layout
->
addItem
(
new
QSpacerItem
(
10
,
0
,
QSizePolicy
::
Expanding
,
QSizePolicy
::
Expanding
),
2
,
9
);
layout
->
addWidget
(
recenterButton
,
2
,
10
);
layout
->
addWidget
(
followCameraCheckBox
,
2
,
11
);
layout
->
setRowStretch
(
0
,
1
);
layout
->
setRowStretch
(
1
,
100
);
layout
->
setRowStretch
(
2
,
1
);
...
...
@@ -641,10 +825,12 @@ Pixhawk3DWidget::buildLayout(void)
connect
(
frameComboBox
,
SIGNAL
(
currentIndexChanged
(
QString
)),
this
,
SLOT
(
selectFrame
(
QString
)));
connect
(
gridCheckBox
,
SIGNAL
(
stateChanged
(
int
)),
this
,
SLOT
(
showGrid
(
int
)));
connect
(
localGridCheckBox
,
SIGNAL
(
stateChanged
(
int
)),
this
,
SLOT
(
showLocalGrid
(
int
)));
connect
(
worldGridCheckBox
,
SIGNAL
(
stateChanged
(
int
)),
this
,
SLOT
(
showWorldGrid
(
int
)));
connect
(
trailCheckBox
,
SIGNAL
(
stateChanged
(
int
)),
this
,
SLOT
(
showTrail
(
int
)));
this
,
SLOT
(
showTrail
s
(
int
)));
connect
(
waypointsCheckBox
,
SIGNAL
(
stateChanged
(
int
)),
this
,
SLOT
(
showWaypoints
(
int
)));
connect
(
mapComboBox
,
SIGNAL
(
currentIndexChanged
(
int
)),
...
...
@@ -668,13 +854,16 @@ void
Pixhawk3DWidget
::
display
(
void
)
{
// set node visibility
rollingMap
->
setChildValue
(
gridNode
,
displayGrid
);
rollingMap
->
setChildValue
(
trailNode
,
displayTrail
);
allocentricMap
->
setChildValue
(
worldGridNode
,
displayWorldGrid
);
rollingMap
->
setChildValue
(
localGridNode
,
displayLocalGrid
);
rollingMap
->
setChildValue
(
trailNode
,
displayTrails
);
rollingMap
->
setChildValue
(
orientationNode
,
displayTrails
);
rollingMap
->
setChildValue
(
mapNode
,
displayImagery
);
rollingMap
->
setChildValue
(
waypointGroupNode
,
displayWaypoints
);
rollingMap
->
setChildValue
(
targetNode
,
enableTarget
);
#ifdef QGC_PROTOBUF_ENABLED
rollingMap
->
setChildValue
(
obstacleGroupNode
,
displayObstacleList
);
rollingMap
->
setChildValue
(
pathNode
,
displayPath
);
#endif
rollingMap
->
setChildValue
(
rgbd3DNode
,
displayRGBD3D
);
hudGroup
->
setChildValue
(
rgb2DGeode
,
displayRGBD2D
);
...
...
@@ -712,9 +901,9 @@ Pixhawk3DWidget::display(void)
robotPitch
,
osg
::
Vec3d
(
1.0
f
,
0.0
f
,
0.0
f
),
robotRoll
,
osg
::
Vec3d
(
0.0
f
,
1.0
f
,
0.0
f
)));
if
(
displayTrail
)
if
(
displayTrail
s
)
{
updateTrail
(
robotX
,
robotY
,
robotZ
);
updateTrail
s
(
robotX
,
robotY
,
robotZ
);
}
if
(
frame
==
MAV_FRAME_GLOBAL
&&
displayImagery
)
...
...
@@ -729,7 +918,7 @@ Pixhawk3DWidget::display(void)
if
(
enableTarget
)
{
updateTarget
(
robotX
,
robotY
);
updateTarget
(
robotX
,
robotY
,
robotZ
);
}
#ifdef QGC_PROTOBUF_ENABLED
...
...
@@ -740,7 +929,12 @@ Pixhawk3DWidget::display(void)
if
(
displayObstacleList
)
{
updateObstacles
();
updateObstacles
(
robotX
,
robotY
,
robotZ
);
}
if
(
displayPath
)
{
updatePath
(
robotX
,
robotY
,
robotZ
);
}
#endif
...
...
@@ -774,6 +968,10 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
case
'O'
:
displayObstacleList
=
!
displayObstacleList
;
break
;
case
'p'
:
case
'P'
:
displayPath
=
!
displayPath
;
break
;
}
}
...
...
@@ -816,6 +1014,20 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
Q3DWidget
::
mousePressEvent
(
event
);
}
void
Pixhawk3DWidget
::
showEvent
(
QShowEvent
*
event
)
{
Q3DWidget
::
showEvent
(
event
);
emit
visibilityChanged
(
true
);
}
void
Pixhawk3DWidget
::
hideEvent
(
QHideEvent
*
event
)
{
Q3DWidget
::
hideEvent
(
event
);
emit
visibilityChanged
(
false
);
}
void
Pixhawk3DWidget
::
mouseMoveEvent
(
QMouseEvent
*
event
)
{
...
...
@@ -908,7 +1120,7 @@ Pixhawk3DWidget::getPosition(double& x, double& y, double& z)
}
osg
::
ref_ptr
<
osg
::
Geode
>
Pixhawk3DWidget
::
createGrid
(
void
)
Pixhawk3DWidget
::
create
Local
Grid
(
void
)
{
osg
::
ref_ptr
<
osg
::
Geode
>
geode
(
new
osg
::
Geode
());
osg
::
ref_ptr
<
osg
::
Geometry
>
fineGeometry
(
new
osg
::
Geometry
());
...
...
@@ -916,16 +1128,16 @@ Pixhawk3DWidget::createGrid(void)
geode
->
addDrawable
(
fineGeometry
);
geode
->
addDrawable
(
coarseGeometry
);
float
radius
=
10
.0
f
;
float
radius
=
5
.0
f
;
float
resolution
=
0.25
f
;
osg
::
ref_ptr
<
osg
::
Vec3Array
>
fineCoords
(
new
osg
::
Vec3Array
);
osg
::
ref_ptr
<
osg
::
Vec3Array
>
coarseCoords
(
new
osg
::
Vec3Array
);
// draw a
20m x 2
0m grid with 0.25m resolution
// draw a
10m x 1
0m grid with 0.25m resolution
for
(
float
i
=
-
radius
;
i
<=
radius
;
i
+=
resolution
)
{
if
(
fabs
(
i
-
floor
(
i
+
0.5
f
))
<
0.01
f
)
if
(
fabs
(
i
/
1.0
f
-
floor
(
i
/
1.0
f
))
<
0.01
f
)
{
coarseCoords
->
push_back
(
osg
::
Vec3
(
i
,
-
radius
,
0.0
f
));
coarseCoords
->
push_back
(
osg
::
Vec3
(
i
,
radius
,
0.0
f
));
...
...
@@ -961,45 +1173,153 @@ Pixhawk3DWidget::createGrid(void)
fineLinewidth
->
setWidth
(
0.25
f
);
fineStateset
->
setAttributeAndModes
(
fineLinewidth
,
osg
::
StateAttribute
::
ON
);
fineStateset
->
setMode
(
GL_LIGHTING
,
osg
::
StateAttribute
::
OFF
);
fineStateset
->
setMode
(
GL_LINE_SMOOTH
,
osg
::
StateAttribute
::
ON
);
fineStateset
->
setMode
(
GL_BLEND
,
osg
::
StateAttribute
::
ON
);
fineGeometry
->
setStateSet
(
fineStateset
);
osg
::
ref_ptr
<
osg
::
StateSet
>
coarseStateset
(
new
osg
::
StateSet
);
osg
::
ref_ptr
<
osg
::
LineWidth
>
coarseLinewidth
(
new
osg
::
LineWidth
());
coarseLinewidth
->
setWidth
(
2
.0
f
);
coarseLinewidth
->
setWidth
(
1
.0
f
);
coarseStateset
->
setAttributeAndModes
(
coarseLinewidth
,
osg
::
StateAttribute
::
ON
);
coarseStateset
->
setMode
(
GL_LIGHTING
,
osg
::
StateAttribute
::
OFF
);
coarseStateset
->
setMode
(
GL_LINE_SMOOTH
,
osg
::
StateAttribute
::
ON
);
coarseStateset
->
setMode
(
GL_BLEND
,
osg
::
StateAttribute
::
ON
);
coarseGeometry
->
setStateSet
(
coarseStateset
);
return
geode
;
}
osg
::
ref_ptr
<
osg
::
Geode
>
Pixhawk3DWidget
::
create
Trail
(
void
)
Pixhawk3DWidget
::
create
WorldGrid
(
void
)
{
osg
::
ref_ptr
<
osg
::
Geode
>
geode
(
new
osg
::
Geode
());
trailGeometry
=
new
osg
::
Geometry
();
trailGeometry
->
setUseDisplayList
(
false
);
geode
->
addDrawable
(
trailGeometry
.
get
());
osg
::
ref_ptr
<
osg
::
Geometry
>
fineGeometry
(
new
osg
::
Geometry
());
osg
::
ref_ptr
<
osg
::
Geometry
>
coarseGeometry
(
new
osg
::
Geometry
());
osg
::
ref_ptr
<
osg
::
Geometry
>
axisGeometry
(
new
osg
::
Geometry
());
geode
->
addDrawable
(
fineGeometry
);
geode
->
addDrawable
(
coarseGeometry
);
geode
->
addDrawable
(
axisGeometry
.
get
());
float
radius
=
20.0
f
;
float
resolution
=
1.0
f
;
trailVertices
=
new
osg
::
Vec3dArray
;
trailGeometry
->
setVertexArray
(
trailVertices
);
osg
::
ref_ptr
<
osg
::
Vec3Array
>
fineCoords
(
new
osg
::
Vec3Array
);
osg
::
ref_ptr
<
osg
::
Vec3Array
>
coarseCoords
(
new
osg
::
Vec3Array
);
// draw a 40m x 40m grid with 1.0m resolution
for
(
float
i
=
-
radius
;
i
<=
radius
;
i
+=
resolution
)
{
if
(
fabs
(
i
/
5.0
f
-
floor
(
i
/
5.0
f
))
<
0.01
f
)
{
coarseCoords
->
push_back
(
osg
::
Vec3
(
i
,
-
radius
,
0.0
f
));
coarseCoords
->
push_back
(
osg
::
Vec3
(
i
,
radius
,
0.0
f
));
coarseCoords
->
push_back
(
osg
::
Vec3
(
-
radius
,
i
,
0.0
f
));
coarseCoords
->
push_back
(
osg
::
Vec3
(
radius
,
i
,
0.0
f
));
}
else
{
fineCoords
->
push_back
(
osg
::
Vec3
(
i
,
-
radius
,
0.0
f
));
fineCoords
->
push_back
(
osg
::
Vec3
(
i
,
radius
,
0.0
f
));
fineCoords
->
push_back
(
osg
::
Vec3
(
-
radius
,
i
,
0.0
f
));
fineCoords
->
push_back
(
osg
::
Vec3
(
radius
,
i
,
0.0
f
));
}
}
trailDrawArrays
=
new
osg
::
DrawArrays
(
osg
::
PrimitiveSet
::
LINE_STRIP
);
trailGeometry
->
addPrimitiveSet
(
trailDrawArray
s
);
fineGeometry
->
setVertexArray
(
fineCoords
);
coarseGeometry
->
setVertexArray
(
coarseCoord
s
);
osg
::
ref_ptr
<
osg
::
Vec4Array
>
color
(
new
osg
::
Vec4Array
);
color
->
push_back
(
osg
::
Vec4
(
1.0
f
,
0.0
f
,
0.0
f
,
1.0
f
));
trailGeometry
->
setColorArray
(
color
);
trailGeometry
->
setColorBinding
(
osg
::
Geometry
::
BIND_OVERALL
);
color
->
push_back
(
osg
::
Vec4
(
0.5
f
,
0.5
f
,
0.5
f
,
1.0
f
));
fineGeometry
->
setColorArray
(
color
);
coarseGeometry
->
setColorArray
(
color
);
fineGeometry
->
setColorBinding
(
osg
::
Geometry
::
BIND_OVERALL
);
coarseGeometry
->
setColorBinding
(
osg
::
Geometry
::
BIND_OVERALL
);
fineGeometry
->
addPrimitiveSet
(
new
osg
::
DrawArrays
(
osg
::
PrimitiveSet
::
LINES
,
0
,
fineCoords
->
size
()));
coarseGeometry
->
addPrimitiveSet
(
new
osg
::
DrawArrays
(
osg
::
PrimitiveSet
::
LINES
,
0
,
coarseCoords
->
size
()));
osg
::
ref_ptr
<
osg
::
StateSet
>
fineStateset
(
new
osg
::
StateSet
);
osg
::
ref_ptr
<
osg
::
LineWidth
>
fineLinewidth
(
new
osg
::
LineWidth
());
fineLinewidth
->
setWidth
(
0.1
f
);
fineStateset
->
setAttributeAndModes
(
fineLinewidth
,
osg
::
StateAttribute
::
ON
);
fineStateset
->
setMode
(
GL_LIGHTING
,
osg
::
StateAttribute
::
OFF
);
fineStateset
->
setMode
(
GL_LINE_SMOOTH
,
osg
::
StateAttribute
::
ON
);
fineStateset
->
setMode
(
GL_BLEND
,
osg
::
StateAttribute
::
ON
);
fineGeometry
->
setStateSet
(
fineStateset
);
osg
::
ref_ptr
<
osg
::
StateSet
>
coarseStateset
(
new
osg
::
StateSet
);
osg
::
ref_ptr
<
osg
::
LineWidth
>
coarseLinewidth
(
new
osg
::
LineWidth
());
coarseLinewidth
->
setWidth
(
2.0
f
);
coarseStateset
->
setAttributeAndModes
(
coarseLinewidth
,
osg
::
StateAttribute
::
ON
);
coarseStateset
->
setMode
(
GL_LIGHTING
,
osg
::
StateAttribute
::
OFF
);
coarseStateset
->
setMode
(
GL_LINE_SMOOTH
,
osg
::
StateAttribute
::
ON
);
coarseStateset
->
setMode
(
GL_BLEND
,
osg
::
StateAttribute
::
ON
);
coarseGeometry
->
setStateSet
(
coarseStateset
);
// add axes
osg
::
ref_ptr
<
osg
::
Vec3Array
>
coords
(
new
osg
::
Vec3Array
(
6
));
(
*
coords
)[
0
]
=
(
*
coords
)[
2
]
=
(
*
coords
)[
4
]
=
osg
::
Vec3
(
0.0
f
,
0.0
f
,
0.0
f
);
(
*
coords
)[
1
]
=
osg
::
Vec3
(
0.0
f
,
1.0
f
,
0.0
f
);
(
*
coords
)[
3
]
=
osg
::
Vec3
(
1.0
f
,
0.0
f
,
0.0
f
);
(
*
coords
)[
5
]
=
osg
::
Vec3
(
0.0
f
,
0.0
f
,
-
1.0
f
);
axisGeometry
->
setVertexArray
(
coords
);
osg
::
Vec4
redColor
(
1.0
f
,
0.0
f
,
0.0
f
,
0.0
f
);
osg
::
Vec4
greenColor
(
0.0
f
,
1.0
f
,
0.0
f
,
0.0
f
);
osg
::
Vec4
blueColor
(
0.0
f
,
0.0
f
,
1.0
f
,
0.0
f
);
osg
::
ref_ptr
<
osg
::
Vec4Array
>
axisColors
(
new
osg
::
Vec4Array
(
6
));
(
*
axisColors
)[
0
]
=
redColor
;
(
*
axisColors
)[
1
]
=
redColor
;
(
*
axisColors
)[
2
]
=
greenColor
;
(
*
axisColors
)[
3
]
=
greenColor
;
(
*
axisColors
)[
4
]
=
blueColor
;
(
*
axisColors
)[
5
]
=
blueColor
;
axisGeometry
->
setColorArray
(
axisColors
);
axisGeometry
->
setColorBinding
(
osg
::
Geometry
::
BIND_PER_VERTEX
);
axisGeometry
->
addPrimitiveSet
(
new
osg
::
DrawArrays
(
osg
::
PrimitiveSet
::
LINES
,
0
,
6
));
osg
::
ref_ptr
<
osg
::
StateSet
>
axisStateset
(
new
osg
::
StateSet
);
osg
::
ref_ptr
<
osg
::
LineWidth
>
axisLinewidth
(
new
osg
::
LineWidth
());
axisLinewidth
->
setWidth
(
4.0
f
);
axisStateset
->
setAttributeAndModes
(
axisLinewidth
,
osg
::
StateAttribute
::
ON
);
axisStateset
->
setMode
(
GL_LIGHTING
,
osg
::
StateAttribute
::
OFF
);
axisGeometry
->
setStateSet
(
axisStateset
);
return
geode
;
}
osg
::
ref_ptr
<
osg
::
Geometry
>
Pixhawk3DWidget
::
createTrail
(
const
osg
::
Vec4
&
color
)
{
osg
::
ref_ptr
<
osg
::
Geometry
>
geometry
(
new
osg
::
Geometry
());
geometry
->
setUseDisplayList
(
false
);
osg
::
ref_ptr
<
osg
::
Vec3dArray
>
vertices
(
new
osg
::
Vec3dArray
());
geometry
->
setVertexArray
(
vertices
);
osg
::
ref_ptr
<
osg
::
DrawArrays
>
drawArrays
(
new
osg
::
DrawArrays
(
osg
::
PrimitiveSet
::
LINE_STRIP
));
geometry
->
addPrimitiveSet
(
drawArrays
);
osg
::
ref_ptr
<
osg
::
Vec4Array
>
colorArray
(
new
osg
::
Vec4Array
);
colorArray
->
push_back
(
color
);
geometry
->
setColorArray
(
colorArray
);
geometry
->
setColorBinding
(
osg
::
Geometry
::
BIND_OVERALL
);
osg
::
ref_ptr
<
osg
::
StateSet
>
stateset
(
new
osg
::
StateSet
);
osg
::
ref_ptr
<
osg
::
LineWidth
>
linewidth
(
new
osg
::
LineWidth
());
linewidth
->
setWidth
(
1.0
f
);
stateset
->
setAttributeAndModes
(
linewidth
,
osg
::
StateAttribute
::
ON
);
stateset
->
setMode
(
GL_LIGHTING
,
osg
::
StateAttribute
::
OFF
);
trailG
eometry
->
setStateSet
(
stateset
);
g
eometry
->
setStateSet
(
stateset
);
return
geo
de
;
return
geo
metry
;
}
osg
::
ref_ptr
<
Imagery
>
...
...
@@ -1190,54 +1510,39 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
}
void
Pixhawk3DWidget
::
updateTrail
(
double
robotX
,
double
robotY
,
double
robotZ
)
Pixhawk3DWidget
::
updateTrail
s
(
double
robotX
,
double
robotY
,
double
robotZ
)
{
if
(
robotX
==
0.0
f
||
robotY
==
0.0
f
||
robotZ
==
0.0
f
)
{
return
;
}
QMapIterator
<
int
,
int
>
it
(
trailDrawableIdxs
);
bool
addToTrail
=
false
;
if
(
trail
.
size
()
>
0
)
{
if
(
fabs
(
robotX
-
trail
[
trail
.
size
()
-
1
].
x
())
>
0.01
f
||
fabs
(
robotY
-
trail
[
trail
.
size
()
-
1
].
y
())
>
0.01
f
||
fabs
(
robotZ
-
trail
[
trail
.
size
()
-
1
].
z
())
>
0.01
f
)
{
addToTrail
=
true
;
}
}
else
while
(
it
.
hasNext
())
{
addToTrail
=
true
;
}
it
.
next
();
if
(
addToTrail
)
{
osg
::
Vec3d
p
(
robotX
,
robotY
,
robotZ
);
if
(
trail
.
size
()
==
trail
.
capacity
())
{
memcpy
(
trail
.
data
(),
trail
.
data
()
+
1
,
(
trail
.
size
()
-
1
)
*
sizeof
(
osg
::
Vec3d
));
trail
[
trail
.
size
()
-
1
]
=
p
;
}
else
{
trail
.
append
(
p
);
}
}
osg
::
Geometry
*
geometry
=
trailNode
->
getDrawable
(
it
.
value
())
->
asGeometry
();
osg
::
DrawArrays
*
drawArrays
=
reinterpret_cast
<
osg
::
DrawArrays
*>
(
geometry
->
getPrimitiveSet
(
0
));
osg
::
ref_ptr
<
osg
::
Vec3Array
>
vertices
(
new
osg
::
Vec3Array
);
const
QVarLengthArray
<
osg
::
Vec3d
,
10000
>&
trail
=
trails
.
value
(
it
.
key
());
trailVertices
->
clear
();
for
(
int
i
=
0
;
i
<
trail
.
size
();
++
i
)
{
trailV
ertices
->
push_back
(
osg
::
Vec3d
(
trail
[
i
].
y
()
-
robotY
,
v
ertices
->
push_back
(
osg
::
Vec3d
(
trail
[
i
].
y
()
-
robotY
,
trail
[
i
].
x
()
-
robotX
,
-
(
trail
[
i
].
z
()
-
robotZ
)));
}
trailDrawArrays
->
setFirst
(
0
);
trailDrawArrays
->
setCount
(
trailVertices
->
size
());
trailGeometry
->
dirtyBound
();
geometry
->
setVertexArray
(
vertices
);
drawArrays
->
setFirst
(
0
);
drawArrays
->
setCount
(
vertices
->
size
());
geometry
->
dirtyBound
();
osg
::
PositionAttitudeTransform
*
pat
=
dynamic_cast
<
osg
::
PositionAttitudeTransform
*>
(
orientationNode
->
getChild
(
it
.
value
()));
pat
->
setPosition
(
osg
::
Vec3
(
trail
[
trail
.
size
()
-
1
].
y
()
-
robotY
,
trail
[
trail
.
size
()
-
1
].
x
()
-
robotX
,
-
(
trail
[
trail
.
size
()
-
1
].
z
()
-
robotZ
)));
}
}
void
...
...
@@ -1324,162 +1629,34 @@ Pixhawk3DWidget::updateWaypoints(void)
}
void
Pixhawk3DWidget
::
updateTarget
(
double
robotX
,
double
robotY
)
Pixhawk3DWidget
::
updateTarget
(
double
robotX
,
double
robotY
,
double
robotZ
)
{
osg
::
PositionAttitudeTransform
*
pat
=
dynamic_cast
<
osg
::
PositionAttitudeTransform
*>
(
targetNode
.
get
());
pat
->
setPosition
(
osg
::
Vec3d
(
target
.
y
()
-
robotY
,
target
.
x
()
-
robotX
,
0.0
));
pat
->
setAttitude
(
osg
::
Quat
(
target
.
z
()
-
M_PI_2
,
osg
::
Vec3d
(
1.0
f
,
0.0
f
,
0.0
f
),
pat
->
setPosition
(
osg
::
Vec3d
(
target
.
y
()
-
robotY
,
target
.
x
()
-
robotX
,
-
(
target
.
z
()
-
robotZ
)));
pat
->
setAttitude
(
osg
::
Quat
(
target
.
w
()
-
M_PI_2
,
osg
::
Vec3d
(
1.0
f
,
0.0
f
,
0.0
f
),
M_PI_2
,
osg
::
Vec3d
(
0.0
f
,
1.0
f
,
0.0
f
),
0.0
,
osg
::
Vec3d
(
0.0
f
,
0.0
f
,
1.0
f
)));
osg
::
Geode
*
geode
=
dynamic_cast
<
osg
::
Geode
*>
(
pat
->
getChild
(
0
));
osg
::
ShapeDrawable
*
sd
=
dynamic_cast
<
osg
::
ShapeDrawable
*>
(
geode
->
getDrawable
(
0
));
sd
->
setColor
(
osg
::
Vec4f
(
1.0
f
,
0.8
f
,
0.0
f
,
1.0
f
));
}
float
colormap_jet
[
128
][
3
]
=
{
{
0.0
f
,
0.0
f
,
0.53125
f
},
{
0.0
f
,
0.0
f
,
0.5625
f
},
{
0.0
f
,
0.0
f
,
0.59375
f
},
{
0.0
f
,
0.0
f
,
0.625
f
},
{
0.0
f
,
0.0
f
,
0.65625
f
},
{
0.0
f
,
0.0
f
,
0.6875
f
},
{
0.0
f
,
0.0
f
,
0.71875
f
},
{
0.0
f
,
0.0
f
,
0.75
f
},
{
0.0
f
,
0.0
f
,
0.78125
f
},
{
0.0
f
,
0.0
f
,
0.8125
f
},
{
0.0
f
,
0.0
f
,
0.84375
f
},
{
0.0
f
,
0.0
f
,
0.875
f
},
{
0.0
f
,
0.0
f
,
0.90625
f
},
{
0.0
f
,
0.0
f
,
0.9375
f
},
{
0.0
f
,
0.0
f
,
0.96875
f
},
{
0.0
f
,
0.0
f
,
1.0
f
},
{
0.0
f
,
0.03125
f
,
1.0
f
},
{
0.0
f
,
0.0625
f
,
1.0
f
},
{
0.0
f
,
0.09375
f
,
1.0
f
},
{
0.0
f
,
0.125
f
,
1.0
f
},
{
0.0
f
,
0.15625
f
,
1.0
f
},
{
0.0
f
,
0.1875
f
,
1.0
f
},
{
0.0
f
,
0.21875
f
,
1.0
f
},
{
0.0
f
,
0.25
f
,
1.0
f
},
{
0.0
f
,
0.28125
f
,
1.0
f
},
{
0.0
f
,
0.3125
f
,
1.0
f
},
{
0.0
f
,
0.34375
f
,
1.0
f
},
{
0.0
f
,
0.375
f
,
1.0
f
},
{
0.0
f
,
0.40625
f
,
1.0
f
},
{
0.0
f
,
0.4375
f
,
1.0
f
},
{
0.0
f
,
0.46875
f
,
1.0
f
},
{
0.0
f
,
0.5
f
,
1.0
f
},
{
0.0
f
,
0.53125
f
,
1.0
f
},
{
0.0
f
,
0.5625
f
,
1.0
f
},
{
0.0
f
,
0.59375
f
,
1.0
f
},
{
0.0
f
,
0.625
f
,
1.0
f
},
{
0.0
f
,
0.65625
f
,
1.0
f
},
{
0.0
f
,
0.6875
f
,
1.0
f
},
{
0.0
f
,
0.71875
f
,
1.0
f
},
{
0.0
f
,
0.75
f
,
1.0
f
},
{
0.0
f
,
0.78125
f
,
1.0
f
},
{
0.0
f
,
0.8125
f
,
1.0
f
},
{
0.0
f
,
0.84375
f
,
1.0
f
},
{
0.0
f
,
0.875
f
,
1.0
f
},
{
0.0
f
,
0.90625
f
,
1.0
f
},
{
0.0
f
,
0.9375
f
,
1.0
f
},
{
0.0
f
,
0.96875
f
,
1.0
f
},
{
0.0
f
,
1.0
f
,
1.0
f
},
{
0.03125
f
,
1.0
f
,
0.96875
f
},
{
0.0625
f
,
1.0
f
,
0.9375
f
},
{
0.09375
f
,
1.0
f
,
0.90625
f
},
{
0.125
f
,
1.0
f
,
0.875
f
},
{
0.15625
f
,
1.0
f
,
0.84375
f
},
{
0.1875
f
,
1.0
f
,
0.8125
f
},
{
0.21875
f
,
1.0
f
,
0.78125
f
},
{
0.25
f
,
1.0
f
,
0.75
f
},
{
0.28125
f
,
1.0
f
,
0.71875
f
},
{
0.3125
f
,
1.0
f
,
0.6875
f
},
{
0.34375
f
,
1.0
f
,
0.65625
f
},
{
0.375
f
,
1.0
f
,
0.625
f
},
{
0.40625
f
,
1.0
f
,
0.59375
f
},
{
0.4375
f
,
1.0
f
,
0.5625
f
},
{
0.46875
f
,
1.0
f
,
0.53125
f
},
{
0.5
f
,
1.0
f
,
0.5
f
},
{
0.53125
f
,
1.0
f
,
0.46875
f
},
{
0.5625
f
,
1.0
f
,
0.4375
f
},
{
0.59375
f
,
1.0
f
,
0.40625
f
},
{
0.625
f
,
1.0
f
,
0.375
f
},
{
0.65625
f
,
1.0
f
,
0.34375
f
},
{
0.6875
f
,
1.0
f
,
0.3125
f
},
{
0.71875
f
,
1.0
f
,
0.28125
f
},
{
0.75
f
,
1.0
f
,
0.25
f
},
{
0.78125
f
,
1.0
f
,
0.21875
f
},
{
0.8125
f
,
1.0
f
,
0.1875
f
},
{
0.84375
f
,
1.0
f
,
0.15625
f
},
{
0.875
f
,
1.0
f
,
0.125
f
},
{
0.90625
f
,
1.0
f
,
0.09375
f
},
{
0.9375
f
,
1.0
f
,
0.0625
f
},
{
0.96875
f
,
1.0
f
,
0.03125
f
},
{
1.0
f
,
1.0
f
,
0.0
f
},
{
1.0
f
,
0.96875
f
,
0.0
f
},
{
1.0
f
,
0.9375
f
,
0.0
f
},
{
1.0
f
,
0.90625
f
,
0.0
f
},
{
1.0
f
,
0.875
f
,
0.0
f
},
{
1.0
f
,
0.84375
f
,
0.0
f
},
{
1.0
f
,
0.8125
f
,
0.0
f
},
{
1.0
f
,
0.78125
f
,
0.0
f
},
{
1.0
f
,
0.75
f
,
0.0
f
},
{
1.0
f
,
0.71875
f
,
0.0
f
},
{
1.0
f
,
0.6875
f
,
0.0
f
},
{
1.0
f
,
0.65625
f
,
0.0
f
},
{
1.0
f
,
0.625
f
,
0.0
f
},
{
1.0
f
,
0.59375
f
,
0.0
f
},
{
1.0
f
,
0.5625
f
,
0.0
f
},
{
1.0
f
,
0.53125
f
,
0.0
f
},
{
1.0
f
,
0.5
f
,
0.0
f
},
{
1.0
f
,
0.46875
f
,
0.0
f
},
{
1.0
f
,
0.4375
f
,
0.0
f
},
{
1.0
f
,
0.40625
f
,
0.0
f
},
{
1.0
f
,
0.375
f
,
0.0
f
},
{
1.0
f
,
0.34375
f
,
0.0
f
},
{
1.0
f
,
0.3125
f
,
0.0
f
},
{
1.0
f
,
0.28125
f
,
0.0
f
},
{
1.0
f
,
0.25
f
,
0.0
f
},
{
1.0
f
,
0.21875
f
,
0.0
f
},
{
1.0
f
,
0.1875
f
,
0.0
f
},
{
1.0
f
,
0.15625
f
,
0.0
f
},
{
1.0
f
,
0.125
f
,
0.0
f
},
{
1.0
f
,
0.09375
f
,
0.0
f
},
{
1.0
f
,
0.0625
f
,
0.0
f
},
{
1.0
f
,
0.03125
f
,
0.0
f
},
{
1.0
f
,
0.0
f
,
0.0
f
},
{
0.96875
f
,
0.0
f
,
0.0
f
},
{
0.9375
f
,
0.0
f
,
0.0
f
},
{
0.90625
f
,
0.0
f
,
0.0
f
},
{
0.875
f
,
0.0
f
,
0.0
f
},
{
0.84375
f
,
0.0
f
,
0.0
f
},
{
0.8125
f
,
0.0
f
,
0.0
f
},
{
0.78125
f
,
0.0
f
,
0.0
f
},
{
0.75
f
,
0.0
f
,
0.0
f
},
{
0.71875
f
,
0.0
f
,
0.0
f
},
{
0.6875
f
,
0.0
f
,
0.0
f
},
{
0.65625
f
,
0.0
f
,
0.0
f
},
{
0.625
f
,
0.0
f
,
0.0
f
},
{
0.59375
f
,
0.0
f
,
0.0
f
},
{
0.5625
f
,
0.0
f
,
0.0
f
},
{
0.53125
f
,
0.0
f
,
0.0
f
},
{
0.5
f
,
0.0
f
,
0.0
f
}
};
#ifdef QGC_PROTOBUF_ENABLED
void
Pixhawk3DWidget
::
updateRGBD
(
double
robotX
,
double
robotY
,
double
robotZ
)
{
px
::
RGBDImage
rgbdImage
=
uas
->
getRGBDImage
();
px
::
PointCloudXYZRGB
pointCloud
=
uas
->
getPointCloud
();
qreal
receivedRGBDImageTimestamp
,
receivedPointCloudTimestamp
;
px
::
RGBDImage
rgbdImage
=
uas
->
getRGBDImage
(
receivedRGBDImageTimestamp
);
px
::
PointCloudXYZRGB
pointCloud
=
uas
->
getPointCloud
(
receivedPointCloudTimestamp
);
if
(
rgbdImage
.
rows
()
>
0
&&
rgbdImage
.
cols
()
>
0
)
if
(
rgbdImage
.
rows
()
>
0
&&
rgbdImage
.
cols
()
>
0
&&
QGC
::
groundTimeSeconds
()
-
receivedRGBDImageTimestamp
<
kMessageTimeout
)
{
rgbImage
->
setImage
(
rgbdImage
.
cols
(),
rgbdImage
.
rows
(),
1
,
GL_LUMINANCE
,
GL_LUMINANCE
,
GL_UNSIGNED_BYTE
,
...
...
@@ -1499,9 +1676,11 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
int
idx
=
fminf
(
depth
[
c
],
7.0
f
)
/
7.0
f
*
127.0
f
;
idx
=
127
-
idx
;
pixel
[
0
]
=
colormap_jet
[
idx
][
2
]
*
255.0
f
;
pixel
[
1
]
=
colormap_jet
[
idx
][
1
]
*
255.0
f
;
pixel
[
2
]
=
colormap_jet
[
idx
][
0
]
*
255.0
f
;
float
r
,
g
,
b
;
qgc
::
colormap
(
"jet"
,
idx
,
r
,
g
,
b
);
pixel
[
0
]
=
r
*
255.0
f
;
pixel
[
1
]
=
g
*
255.0
f
;
pixel
[
2
]
=
b
*
255.0
f
;
}
pixel
+=
3
;
...
...
@@ -1516,9 +1695,15 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
}
osg
::
Geometry
*
geometry
=
rgbd3DNode
->
getDrawable
(
0
)
->
asGeometry
();
osg
::
Vec3Array
*
vertices
=
static_cast
<
osg
::
Vec3Array
*>
(
geometry
->
getVertexArray
());
osg
::
Vec4Array
*
colors
=
static_cast
<
osg
::
Vec4Array
*>
(
geometry
->
getColorArray
());
if
(
QGC
::
groundTimeSeconds
()
-
receivedPointCloudTimestamp
>
kMessageTimeout
)
{
geometry
->
removePrimitiveSet
(
0
,
geometry
->
getNumPrimitiveSets
());
return
;
}
for
(
int
i
=
0
;
i
<
pointCloud
.
points_size
();
++
i
)
{
const
px
::
PointCloudXYZRGB_PointXYZRGB
&
p
=
pointCloud
.
points
(
i
);
...
...
@@ -1544,10 +1729,11 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
{
double
dist
=
sqrt
(
x
*
x
+
y
*
y
+
z
*
z
);
int
colorIndex
=
static_cast
<
int
>
(
fmin
(
dist
/
7.0
*
127.0
,
127.0
));
(
*
colors
)[
i
].
set
(
colormap_jet
[
colorIndex
][
0
],
colormap_jet
[
colorIndex
][
1
],
colormap_jet
[
colorIndex
][
2
],
1.0
f
);
float
r
,
g
,
b
;
qgc
::
colormap
(
"jet"
,
colorIndex
,
r
,
g
,
b
);
(
*
colors
)[
i
].
set
(
r
,
g
,
b
,
1.0
f
);
}
}
...
...
@@ -1564,9 +1750,100 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
}
void
Pixhawk3DWidget
::
updateObstacles
(
void
)
Pixhawk3DWidget
::
updateObstacles
(
double
robotX
,
double
robotY
,
double
robotZ
)
{
obstacleGroupNode
->
update
(
frame
,
uas
);
if
(
frame
==
MAV_FRAME_GLOBAL
)
{
obstacleGroupNode
->
clear
();
return
;
}
qreal
receivedTimestamp
;
px
::
ObstacleList
obstacleList
=
uas
->
getObstacleList
(
receivedTimestamp
);
if
(
QGC
::
groundTimeSeconds
()
-
receivedTimestamp
<
kMessageTimeout
)
{
obstacleGroupNode
->
update
(
robotX
,
robotY
,
robotZ
,
obstacleList
);
}
else
{
obstacleGroupNode
->
clear
();
}
}
void
Pixhawk3DWidget
::
updatePath
(
double
robotX
,
double
robotY
,
double
robotZ
)
{
qreal
receivedTimestamp
;
px
::
Path
path
=
uas
->
getPath
(
receivedTimestamp
);
osg
::
Geometry
*
geometry
=
pathNode
->
getDrawable
(
0
)
->
asGeometry
();
osg
::
DrawArrays
*
drawArrays
=
reinterpret_cast
<
osg
::
DrawArrays
*>
(
geometry
->
getPrimitiveSet
(
0
));
osg
::
Vec4Array
*
colorArray
=
reinterpret_cast
<
osg
::
Vec4Array
*>
(
geometry
->
getColorArray
());
geometry
->
setColorBinding
(
osg
::
Geometry
::
BIND_PER_VERTEX
);
osg
::
ref_ptr
<
osg
::
LineWidth
>
linewidth
(
new
osg
::
LineWidth
());
linewidth
->
setWidth
(
2.0
f
);
geometry
->
getStateSet
()
->
setAttributeAndModes
(
linewidth
,
osg
::
StateAttribute
::
ON
);
colorArray
->
clear
();
osg
::
ref_ptr
<
osg
::
Vec3Array
>
vertices
(
new
osg
::
Vec3Array
);
if
(
QGC
::
groundTimeSeconds
()
-
receivedTimestamp
<
kMessageTimeout
)
{
// find path length
float
length
=
0.0
f
;
for
(
int
i
=
0
;
i
<
path
.
waypoints_size
()
-
1
;
++
i
)
{
const
px
::
Waypoint
&
wp0
=
path
.
waypoints
(
i
);
const
px
::
Waypoint
&
wp1
=
path
.
waypoints
(
i
+
1
);
length
+=
qgc
::
hypot3f
(
wp0
.
x
()
-
wp1
.
x
(),
wp0
.
y
()
-
wp1
.
y
(),
wp0
.
z
()
-
wp1
.
z
());
}
// build path
if
(
path
.
waypoints_size
()
>
0
)
{
const
px
::
Waypoint
&
wp0
=
path
.
waypoints
(
0
);
vertices
->
push_back
(
osg
::
Vec3d
(
wp0
.
y
()
-
robotY
,
wp0
.
x
()
-
robotX
,
-
(
wp0
.
z
()
-
robotZ
)));
float
r
,
g
,
b
;
qgc
::
colormap
(
"autumn"
,
0
,
r
,
g
,
b
);
colorArray
->
push_back
(
osg
::
Vec4d
(
r
,
g
,
b
,
1.0
f
));
}
float
lengthCurrent
=
0.0
f
;
for
(
int
i
=
0
;
i
<
path
.
waypoints_size
()
-
1
;
++
i
)
{
const
px
::
Waypoint
&
wp0
=
path
.
waypoints
(
i
);
const
px
::
Waypoint
&
wp1
=
path
.
waypoints
(
i
+
1
);
lengthCurrent
+=
qgc
::
hypot3f
(
wp0
.
x
()
-
wp1
.
x
(),
wp0
.
y
()
-
wp1
.
y
(),
wp0
.
z
()
-
wp1
.
z
());
vertices
->
push_back
(
osg
::
Vec3d
(
wp1
.
y
()
-
robotY
,
wp1
.
x
()
-
robotX
,
-
(
wp1
.
z
()
-
robotZ
)));
int
colorIdx
=
lengthCurrent
/
length
*
127.0
f
;
float
r
,
g
,
b
;
qgc
::
colormap
(
"autumn"
,
colorIdx
,
r
,
g
,
b
);
colorArray
->
push_back
(
osg
::
Vec4f
(
r
,
g
,
b
,
1.0
f
));
}
}
geometry
->
setVertexArray
(
vertices
);
drawArrays
->
setFirst
(
0
);
drawArrays
->
setCount
(
vertices
->
size
());
geometry
->
dirtyBound
();
}
#endif
...
...
src/ui/map3D/Pixhawk3DWidget.h
View file @
c2a4f4d2
...
...
@@ -59,11 +59,14 @@ public:
public
slots
:
void
setActiveUAS
(
UASInterface
*
uas
);
void
addToTrails
(
UASInterface
*
uas
,
int
component
,
double
x
,
double
y
,
double
z
,
quint64
time
);
void
updateAttitude
(
UASInterface
*
uas
,
int
component
,
double
roll
,
double
pitch
,
double
yaw
,
quint64
time
);
private
slots
:
void
selectFrame
(
QString
text
);
void
showGrid
(
int
state
);
void
showTrail
(
int
state
);
void
showLocalGrid
(
int
state
);
void
showWorldGrid
(
int
state
);
void
showTrails
(
int
state
);
void
showWaypoints
(
int
state
);
void
selectMapSource
(
int
index
);
void
selectVehicleModel
(
int
index
);
...
...
@@ -87,17 +90,8 @@ protected:
virtual
void
display
(
void
);
virtual
void
keyPressEvent
(
QKeyEvent
*
event
);
virtual
void
mousePressEvent
(
QMouseEvent
*
event
);
void
showEvent
(
QShowEvent
*
event
)
{
QWidget
::
showEvent
(
event
);
emit
visibilityChanged
(
true
);
}
void
hideEvent
(
QHideEvent
*
event
)
{
QWidget
::
hideEvent
(
event
);
emit
visibilityChanged
(
false
);
}
virtual
void
showEvent
(
QShowEvent
*
event
);
virtual
void
hideEvent
(
QHideEvent
*
event
);
virtual
void
mouseMoveEvent
(
QMouseEvent
*
event
);
UASInterface
*
uas
;
...
...
@@ -115,8 +109,9 @@ private:
QString
&
utmZone
);
void
getPosition
(
double
&
x
,
double
&
y
,
double
&
z
);
osg
::
ref_ptr
<
osg
::
Geode
>
createGrid
(
void
);
osg
::
ref_ptr
<
osg
::
Geode
>
createTrail
(
void
);
osg
::
ref_ptr
<
osg
::
Geode
>
createLocalGrid
(
void
);
osg
::
ref_ptr
<
osg
::
Geode
>
createWorldGrid
(
void
);
osg
::
ref_ptr
<
osg
::
Geometry
>
createTrail
(
const
osg
::
Vec4
&
color
);
osg
::
ref_ptr
<
Imagery
>
createMap
(
void
);
osg
::
ref_ptr
<
osg
::
Geode
>
createRGBD3D
(
void
);
osg
::
ref_ptr
<
osg
::
Node
>
createTarget
(
void
);
...
...
@@ -127,14 +122,15 @@ private:
void
updateHUD
(
double
robotX
,
double
robotY
,
double
robotZ
,
double
robotRoll
,
double
robotPitch
,
double
robotYaw
,
const
QString
&
utmZone
);
void
updateTrail
(
double
robotX
,
double
robotY
,
double
robotZ
);
void
updateTrail
s
(
double
robotX
,
double
robotY
,
double
robotZ
);
void
updateImagery
(
double
originX
,
double
originY
,
double
originZ
,
const
QString
&
zone
);
void
updateWaypoints
(
void
);
void
updateTarget
(
double
robotX
,
double
robotY
);
void
updateTarget
(
double
robotX
,
double
robotY
,
double
robotZ
);
#ifdef QGC_PROTOBUF_ENABLED
void
updateRGBD
(
double
robotX
,
double
robotY
,
double
robotZ
);
void
updateObstacles
(
void
);
void
updateObstacles
(
double
robotX
,
double
robotY
,
double
robotZ
);
void
updatePath
(
double
robotX
,
double
robotY
,
double
robotZ
);
#endif
int
findWaypoint
(
const
QPoint
&
mousePos
);
...
...
@@ -142,6 +138,8 @@ private:
void
showInsertWaypointMenu
(
const
QPoint
&
cursorPos
);
void
showEditWaypointMenu
(
const
QPoint
&
cursorPos
);
const
qreal
kMessageTimeout
;
// message timeout in seconds
enum
Mode
{
DEFAULT_MODE
,
MOVE_WAYPOINT_POSITION_MODE
,
...
...
@@ -151,20 +149,22 @@ private:
Mode
mode
;
int
selectedWpIndex
;
bool
displayGrid
;
bool
displayTrail
;
bool
displayLocalGrid
;
bool
displayWorldGrid
;
bool
displayTrails
;
bool
displayImagery
;
bool
displayWaypoints
;
bool
displayRGBD2D
;
bool
displayRGBD3D
;
bool
displayObstacleList
;
bool
displayPath
;
bool
enableRGBDColor
;
bool
enableTarget
;
bool
followCamera
;
osg
::
ref_ptr
<
osg
::
Vec3dArray
>
trailVertice
s
;
Q
VarLengthArray
<
osg
::
Vec3d
,
10000
>
trail
;
QMap
<
int
,
QVarLengthArray
<
osg
::
Vec3d
,
10000
>
>
trail
s
;
Q
Map
<
int
,
int
>
trailDrawableIdxs
;
osg
::
ref_ptr
<
osg
::
Node
>
vehicleModel
;
osg
::
ref_ptr
<
osg
::
Geometry
>
hudBackgroundGeometry
;
...
...
@@ -174,22 +174,23 @@ private:
osg
::
ref_ptr
<
ImageWindowGeode
>
depth2DGeode
;
osg
::
ref_ptr
<
osg
::
Image
>
rgbImage
;
osg
::
ref_ptr
<
osg
::
Image
>
depthImage
;
osg
::
ref_ptr
<
osg
::
Geode
>
gridNode
;
osg
::
ref_ptr
<
osg
::
Geode
>
localGridNode
;
osg
::
ref_ptr
<
osg
::
Geode
>
worldGridNode
;
osg
::
ref_ptr
<
osg
::
Geode
>
trailNode
;
osg
::
ref_ptr
<
osg
::
Geometry
>
trailGeometry
;
osg
::
ref_ptr
<
osg
::
DrawArrays
>
trailDrawArrays
;
osg
::
ref_ptr
<
osg
::
Group
>
orientationNode
;
osg
::
ref_ptr
<
Imagery
>
mapNode
;
osg
::
ref_ptr
<
WaypointGroupNode
>
waypointGroupNode
;
osg
::
ref_ptr
<
osg
::
Node
>
targetNode
;
osg
::
ref_ptr
<
osg
::
Geode
>
rgbd3DNode
;
#ifdef QGC_PROTOBUF_ENABLED
osg
::
ref_ptr
<
ObstacleGroupNode
>
obstacleGroupNode
;
osg
::
ref_ptr
<
osg
::
Geode
>
pathNode
;
#endif
QVector
<
osg
::
ref_ptr
<
osg
::
Node
>
>
vehicleModels
;
MAV_FRAME
frame
;
osg
::
Vec3d
target
;
QVector4D
target
;
QPoint
cachedMousePos
;
double
lastRobotX
,
lastRobotY
,
lastRobotZ
;
};
...
...
src/ui/map3D/Q3DWidget.cc
View file @
c2a4f4d2
...
...
@@ -148,8 +148,8 @@ Q3DWidget::createRobot(void)
osg
::
ref_ptr
<
osg
::
Vec3Array
>
coords
(
new
osg
::
Vec3Array
(
6
));
(
*
coords
)[
0
]
=
(
*
coords
)[
2
]
=
(
*
coords
)[
4
]
=
osg
::
Vec3
(
0.0
f
,
0.0
f
,
0.0
f
);
(
*
coords
)[
1
]
=
osg
::
Vec3
(
0.
15
f
,
0.0
f
,
0.0
f
);
(
*
coords
)[
3
]
=
osg
::
Vec3
(
0.
0
f
,
0.3
f
,
0.0
f
);
(
*
coords
)[
1
]
=
osg
::
Vec3
(
0.
0
f
,
0.3
f
,
0.0
f
);
(
*
coords
)[
3
]
=
osg
::
Vec3
(
0.
15
f
,
0.0
f
,
0.0
f
);
(
*
coords
)[
5
]
=
osg
::
Vec3
(
0.0
f
,
0.0
f
,
-
0.15
f
);
geometry
->
setVertexArray
(
coords
);
...
...
src/ui/map3D/gpl.cc
0 → 100644
View file @
c2a4f4d2
#include "gpl.h"
namespace
qgc
{
double
hypot3
(
double
x
,
double
y
,
double
z
)
{
return
sqrt
(
square
(
x
)
+
square
(
y
)
+
square
(
z
));
}
float
hypot3f
(
float
x
,
float
y
,
float
z
)
{
return
sqrtf
(
square
(
x
)
+
square
(
y
)
+
square
(
z
));
}
double
d2r
(
double
deg
)
{
return
deg
/
180.0
*
M_PI
;
}
float
d2r
(
float
deg
)
{
return
deg
/
180.0
f
*
M_PI
;
}
double
r2d
(
double
rad
)
{
return
rad
/
M_PI
*
180.0
;
}
float
r2d
(
float
rad
)
{
return
rad
/
M_PI
*
180.0
f
;
}
float
colormapAutumn
[
128
][
3
]
=
{
{
1.0
f
,
0.
f
,
0.
f
},
{
1.0
f
,
0.007874
f
,
0.
f
},
{
1.0
f
,
0.015748
f
,
0.
f
},
{
1.0
f
,
0.023622
f
,
0.
f
},
{
1.0
f
,
0.031496
f
,
0.
f
},
{
1.0
f
,
0.03937
f
,
0.
f
},
{
1.0
f
,
0.047244
f
,
0.
f
},
{
1.0
f
,
0.055118
f
,
0.
f
},
{
1.0
f
,
0.062992
f
,
0.
f
},
{
1.0
f
,
0.070866
f
,
0.
f
},
{
1.0
f
,
0.07874
f
,
0.
f
},
{
1.0
f
,
0.086614
f
,
0.
f
},
{
1.0
f
,
0.094488
f
,
0.
f
},
{
1.0
f
,
0.10236
f
,
0.
f
},
{
1.0
f
,
0.11024
f
,
0.
f
},
{
1.0
f
,
0.11811
f
,
0.
f
},
{
1.0
f
,
0.12598
f
,
0.
f
},
{
1.0
f
,
0.13386
f
,
0.
f
},
{
1.0
f
,
0.14173
f
,
0.
f
},
{
1.0
f
,
0.14961
f
,
0.
f
},
{
1.0
f
,
0.15748
f
,
0.
f
},
{
1.0
f
,
0.16535
f
,
0.
f
},
{
1.0
f
,
0.17323
f
,
0.
f
},
{
1.0
f
,
0.1811
f
,
0.
f
},
{
1.0
f
,
0.18898
f
,
0.
f
},
{
1.0
f
,
0.19685
f
,
0.
f
},
{
1.0
f
,
0.20472
f
,
0.
f
},
{
1.0
f
,
0.2126
f
,
0.
f
},
{
1.0
f
,
0.22047
f
,
0.
f
},
{
1.0
f
,
0.22835
f
,
0.
f
},
{
1.0
f
,
0.23622
f
,
0.
f
},
{
1.0
f
,
0.24409
f
,
0.
f
},
{
1.0
f
,
0.25197
f
,
0.
f
},
{
1.0
f
,
0.25984
f
,
0.
f
},
{
1.0
f
,
0.26772
f
,
0.
f
},
{
1.0
f
,
0.27559
f
,
0.
f
},
{
1.0
f
,
0.28346
f
,
0.
f
},
{
1.0
f
,
0.29134
f
,
0.
f
},
{
1.0
f
,
0.29921
f
,
0.
f
},
{
1.0
f
,
0.30709
f
,
0.
f
},
{
1.0
f
,
0.31496
f
,
0.
f
},
{
1.0
f
,
0.32283
f
,
0.
f
},
{
1.0
f
,
0.33071
f
,
0.
f
},
{
1.0
f
,
0.33858
f
,
0.
f
},
{
1.0
f
,
0.34646
f
,
0.
f
},
{
1.0
f
,
0.35433
f
,
0.
f
},
{
1.0
f
,
0.3622
f
,
0.
f
},
{
1.0
f
,
0.37008
f
,
0.
f
},
{
1.0
f
,
0.37795
f
,
0.
f
},
{
1.0
f
,
0.38583
f
,
0.
f
},
{
1.0
f
,
0.3937
f
,
0.
f
},
{
1.0
f
,
0.40157
f
,
0.
f
},
{
1.0
f
,
0.40945
f
,
0.
f
},
{
1.0
f
,
0.41732
f
,
0.
f
},
{
1.0
f
,
0.4252
f
,
0.
f
},
{
1.0
f
,
0.43307
f
,
0.
f
},
{
1.0
f
,
0.44094
f
,
0.
f
},
{
1.0
f
,
0.44882
f
,
0.
f
},
{
1.0
f
,
0.45669
f
,
0.
f
},
{
1.0
f
,
0.46457
f
,
0.
f
},
{
1.0
f
,
0.47244
f
,
0.
f
},
{
1.0
f
,
0.48031
f
,
0.
f
},
{
1.0
f
,
0.48819
f
,
0.
f
},
{
1.0
f
,
0.49606
f
,
0.
f
},
{
1.0
f
,
0.50394
f
,
0.
f
},
{
1.0
f
,
0.51181
f
,
0.
f
},
{
1.0
f
,
0.51969
f
,
0.
f
},
{
1.0
f
,
0.52756
f
,
0.
f
},
{
1.0
f
,
0.53543
f
,
0.
f
},
{
1.0
f
,
0.54331
f
,
0.
f
},
{
1.0
f
,
0.55118
f
,
0.
f
},
{
1.0
f
,
0.55906
f
,
0.
f
},
{
1.0
f
,
0.56693
f
,
0.
f
},
{
1.0
f
,
0.5748
f
,
0.
f
},
{
1.0
f
,
0.58268
f
,
0.
f
},
{
1.0
f
,
0.59055
f
,
0.
f
},
{
1.0
f
,
0.59843
f
,
0.
f
},
{
1.0
f
,
0.6063
f
,
0.
f
},
{
1.0
f
,
0.61417
f
,
0.
f
},
{
1.0
f
,
0.62205
f
,
0.
f
},
{
1.0
f
,
0.62992
f
,
0.
f
},
{
1.0
f
,
0.6378
f
,
0.
f
},
{
1.0
f
,
0.64567
f
,
0.
f
},
{
1.0
f
,
0.65354
f
,
0.
f
},
{
1.0
f
,
0.66142
f
,
0.
f
},
{
1.0
f
,
0.66929
f
,
0.
f
},
{
1.0
f
,
0.67717
f
,
0.
f
},
{
1.0
f
,
0.68504
f
,
0.
f
},
{
1.0
f
,
0.69291
f
,
0.
f
},
{
1.0
f
,
0.70079
f
,
0.
f
},
{
1.0
f
,
0.70866
f
,
0.
f
},
{
1.0
f
,
0.71654
f
,
0.
f
},
{
1.0
f
,
0.72441
f
,
0.
f
},
{
1.0
f
,
0.73228
f
,
0.
f
},
{
1.0
f
,
0.74016
f
,
0.
f
},
{
1.0
f
,
0.74803
f
,
0.
f
},
{
1.0
f
,
0.75591
f
,
0.
f
},
{
1.0
f
,
0.76378
f
,
0.
f
},
{
1.0
f
,
0.77165
f
,
0.
f
},
{
1.0
f
,
0.77953
f
,
0.
f
},
{
1.0
f
,
0.7874
f
,
0.
f
},
{
1.0
f
,
0.79528
f
,
0.
f
},
{
1.0
f
,
0.80315
f
,
0.
f
},
{
1.0
f
,
0.81102
f
,
0.
f
},
{
1.0
f
,
0.8189
f
,
0.
f
},
{
1.0
f
,
0.82677
f
,
0.
f
},
{
1.0
f
,
0.83465
f
,
0.
f
},
{
1.0
f
,
0.84252
f
,
0.
f
},
{
1.0
f
,
0.85039
f
,
0.
f
},
{
1.0
f
,
0.85827
f
,
0.
f
},
{
1.0
f
,
0.86614
f
,
0.
f
},
{
1.0
f
,
0.87402
f
,
0.
f
},
{
1.0
f
,
0.88189
f
,
0.
f
},
{
1.0
f
,
0.88976
f
,
0.
f
},
{
1.0
f
,
0.89764
f
,
0.
f
},
{
1.0
f
,
0.90551
f
,
0.
f
},
{
1.0
f
,
0.91339
f
,
0.
f
},
{
1.0
f
,
0.92126
f
,
0.
f
},
{
1.0
f
,
0.92913
f
,
0.
f
},
{
1.0
f
,
0.93701
f
,
0.
f
},
{
1.0
f
,
0.94488
f
,
0.
f
},
{
1.0
f
,
0.95276
f
,
0.
f
},
{
1.0
f
,
0.96063
f
,
0.
f
},
{
1.0
f
,
0.9685
f
,
0.
f
},
{
1.0
f
,
0.97638
f
,
0.
f
},
{
1.0
f
,
0.98425
f
,
0.
f
},
{
1.0
f
,
0.99213
f
,
0.
f
},
{
1.0
f
,
1.0
f
,
0.0
f
}
};
float
colormapJet
[
128
][
3
]
=
{
{
0.0
f
,
0.0
f
,
0.53125
f
},
{
0.0
f
,
0.0
f
,
0.5625
f
},
{
0.0
f
,
0.0
f
,
0.59375
f
},
{
0.0
f
,
0.0
f
,
0.625
f
},
{
0.0
f
,
0.0
f
,
0.65625
f
},
{
0.0
f
,
0.0
f
,
0.6875
f
},
{
0.0
f
,
0.0
f
,
0.71875
f
},
{
0.0
f
,
0.0
f
,
0.75
f
},
{
0.0
f
,
0.0
f
,
0.78125
f
},
{
0.0
f
,
0.0
f
,
0.8125
f
},
{
0.0
f
,
0.0
f
,
0.84375
f
},
{
0.0
f
,
0.0
f
,
0.875
f
},
{
0.0
f
,
0.0
f
,
0.90625
f
},
{
0.0
f
,
0.0
f
,
0.9375
f
},
{
0.0
f
,
0.0
f
,
0.96875
f
},
{
0.0
f
,
0.0
f
,
1.0
f
},
{
0.0
f
,
0.03125
f
,
1.0
f
},
{
0.0
f
,
0.0625
f
,
1.0
f
},
{
0.0
f
,
0.09375
f
,
1.0
f
},
{
0.0
f
,
0.125
f
,
1.0
f
},
{
0.0
f
,
0.15625
f
,
1.0
f
},
{
0.0
f
,
0.1875
f
,
1.0
f
},
{
0.0
f
,
0.21875
f
,
1.0
f
},
{
0.0
f
,
0.25
f
,
1.0
f
},
{
0.0
f
,
0.28125
f
,
1.0
f
},
{
0.0
f
,
0.3125
f
,
1.0
f
},
{
0.0
f
,
0.34375
f
,
1.0
f
},
{
0.0
f
,
0.375
f
,
1.0
f
},
{
0.0
f
,
0.40625
f
,
1.0
f
},
{
0.0
f
,
0.4375
f
,
1.0
f
},
{
0.0
f
,
0.46875
f
,
1.0
f
},
{
0.0
f
,
0.5
f
,
1.0
f
},
{
0.0
f
,
0.53125
f
,
1.0
f
},
{
0.0
f
,
0.5625
f
,
1.0
f
},
{
0.0
f
,
0.59375
f
,
1.0
f
},
{
0.0
f
,
0.625
f
,
1.0
f
},
{
0.0
f
,
0.65625
f
,
1.0
f
},
{
0.0
f
,
0.6875
f
,
1.0
f
},
{
0.0
f
,
0.71875
f
,
1.0
f
},
{
0.0
f
,
0.75
f
,
1.0
f
},
{
0.0
f
,
0.78125
f
,
1.0
f
},
{
0.0
f
,
0.8125
f
,
1.0
f
},
{
0.0
f
,
0.84375
f
,
1.0
f
},
{
0.0
f
,
0.875
f
,
1.0
f
},
{
0.0
f
,
0.90625
f
,
1.0
f
},
{
0.0
f
,
0.9375
f
,
1.0
f
},
{
0.0
f
,
0.96875
f
,
1.0
f
},
{
0.0
f
,
1.0
f
,
1.0
f
},
{
0.03125
f
,
1.0
f
,
0.96875
f
},
{
0.0625
f
,
1.0
f
,
0.9375
f
},
{
0.09375
f
,
1.0
f
,
0.90625
f
},
{
0.125
f
,
1.0
f
,
0.875
f
},
{
0.15625
f
,
1.0
f
,
0.84375
f
},
{
0.1875
f
,
1.0
f
,
0.8125
f
},
{
0.21875
f
,
1.0
f
,
0.78125
f
},
{
0.25
f
,
1.0
f
,
0.75
f
},
{
0.28125
f
,
1.0
f
,
0.71875
f
},
{
0.3125
f
,
1.0
f
,
0.6875
f
},
{
0.34375
f
,
1.0
f
,
0.65625
f
},
{
0.375
f
,
1.0
f
,
0.625
f
},
{
0.40625
f
,
1.0
f
,
0.59375
f
},
{
0.4375
f
,
1.0
f
,
0.5625
f
},
{
0.46875
f
,
1.0
f
,
0.53125
f
},
{
0.5
f
,
1.0
f
,
0.5
f
},
{
0.53125
f
,
1.0
f
,
0.46875
f
},
{
0.5625
f
,
1.0
f
,
0.4375
f
},
{
0.59375
f
,
1.0
f
,
0.40625
f
},
{
0.625
f
,
1.0
f
,
0.375
f
},
{
0.65625
f
,
1.0
f
,
0.34375
f
},
{
0.6875
f
,
1.0
f
,
0.3125
f
},
{
0.71875
f
,
1.0
f
,
0.28125
f
},
{
0.75
f
,
1.0
f
,
0.25
f
},
{
0.78125
f
,
1.0
f
,
0.21875
f
},
{
0.8125
f
,
1.0
f
,
0.1875
f
},
{
0.84375
f
,
1.0
f
,
0.15625
f
},
{
0.875
f
,
1.0
f
,
0.125
f
},
{
0.90625
f
,
1.0
f
,
0.09375
f
},
{
0.9375
f
,
1.0
f
,
0.0625
f
},
{
0.96875
f
,
1.0
f
,
0.03125
f
},
{
1.0
f
,
1.0
f
,
0.0
f
},
{
1.0
f
,
0.96875
f
,
0.0
f
},
{
1.0
f
,
0.9375
f
,
0.0
f
},
{
1.0
f
,
0.90625
f
,
0.0
f
},
{
1.0
f
,
0.875
f
,
0.0
f
},
{
1.0
f
,
0.84375
f
,
0.0
f
},
{
1.0
f
,
0.8125
f
,
0.0
f
},
{
1.0
f
,
0.78125
f
,
0.0
f
},
{
1.0
f
,
0.75
f
,
0.0
f
},
{
1.0
f
,
0.71875
f
,
0.0
f
},
{
1.0
f
,
0.6875
f
,
0.0
f
},
{
1.0
f
,
0.65625
f
,
0.0
f
},
{
1.0
f
,
0.625
f
,
0.0
f
},
{
1.0
f
,
0.59375
f
,
0.0
f
},
{
1.0
f
,
0.5625
f
,
0.0
f
},
{
1.0
f
,
0.53125
f
,
0.0
f
},
{
1.0
f
,
0.5
f
,
0.0
f
},
{
1.0
f
,
0.46875
f
,
0.0
f
},
{
1.0
f
,
0.4375
f
,
0.0
f
},
{
1.0
f
,
0.40625
f
,
0.0
f
},
{
1.0
f
,
0.375
f
,
0.0
f
},
{
1.0
f
,
0.34375
f
,
0.0
f
},
{
1.0
f
,
0.3125
f
,
0.0
f
},
{
1.0
f
,
0.28125
f
,
0.0
f
},
{
1.0
f
,
0.25
f
,
0.0
f
},
{
1.0
f
,
0.21875
f
,
0.0
f
},
{
1.0
f
,
0.1875
f
,
0.0
f
},
{
1.0
f
,
0.15625
f
,
0.0
f
},
{
1.0
f
,
0.125
f
,
0.0
f
},
{
1.0
f
,
0.09375
f
,
0.0
f
},
{
1.0
f
,
0.0625
f
,
0.0
f
},
{
1.0
f
,
0.03125
f
,
0.0
f
},
{
1.0
f
,
0.0
f
,
0.0
f
},
{
0.96875
f
,
0.0
f
,
0.0
f
},
{
0.9375
f
,
0.0
f
,
0.0
f
},
{
0.90625
f
,
0.0
f
,
0.0
f
},
{
0.875
f
,
0.0
f
,
0.0
f
},
{
0.84375
f
,
0.0
f
,
0.0
f
},
{
0.8125
f
,
0.0
f
,
0.0
f
},
{
0.78125
f
,
0.0
f
,
0.0
f
},
{
0.75
f
,
0.0
f
,
0.0
f
},
{
0.71875
f
,
0.0
f
,
0.0
f
},
{
0.6875
f
,
0.0
f
,
0.0
f
},
{
0.65625
f
,
0.0
f
,
0.0
f
},
{
0.625
f
,
0.0
f
,
0.0
f
},
{
0.59375
f
,
0.0
f
,
0.0
f
},
{
0.5625
f
,
0.0
f
,
0.0
f
},
{
0.53125
f
,
0.0
f
,
0.0
f
},
{
0.5
f
,
0.0
f
,
0.0
f
}
};
bool
colormap
(
const
QString
&
name
,
unsigned
char
idx
,
float
&
r
,
float
&
g
,
float
&
b
)
{
if
(
idx
>
127
)
{
return
false
;
}
if
(
name
.
compare
(
"jet"
)
==
0
)
{
float
*
color
=
colormapJet
[
idx
];
r
=
color
[
0
];
g
=
color
[
1
];
b
=
color
[
2
];
return
true
;
}
else
if
(
name
.
compare
(
"autumn"
)
==
0
)
{
float
*
color
=
colormapAutumn
[
idx
];
r
=
color
[
0
];
g
=
color
[
1
];
b
=
color
[
2
];
return
true
;
}
return
false
;
}
}
src/ui/map3D/gpl.h
0 → 100644
View file @
c2a4f4d2
#ifndef GPL_H
#define GPL_H
#include <cmath>
#include <QString>
namespace
qgc
{
template
<
class
T
>
const
T
clamp
(
const
T
&
v
,
const
T
&
a
,
const
T
&
b
)
{
return
qMin
(
b
,
qMax
(
a
,
v
));
}
double
hypot3
(
double
x
,
double
y
,
double
z
);
float
hypot3f
(
float
x
,
float
y
,
float
z
);
template
<
class
T
>
const
T
normalizeTheta
(
const
T
&
theta
)
{
T
normTheta
=
theta
;
while
(
normTheta
<
-
M_PI
)
{
normTheta
+=
2
.
0
*
M_PI
;
}
while
(
normTheta
>
M_PI
)
{
normTheta
-=
2
.
0
*
M_PI
;
}
return
normTheta
;
}
double
d2r
(
double
deg
);
float
d2r
(
float
deg
);
double
r2d
(
double
rad
);
float
r2d
(
float
rad
);
template
<
class
T
>
const
T
square
(
const
T
&
x
)
{
return
x
*
x
;
}
bool
colormap
(
const
QString
&
name
,
unsigned
char
idx
,
float
&
r
,
float
&
g
,
float
&
b
);
}
#endif
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