diff --git a/files/pixhawk/hexrotor/widgets/mavconn.qgw b/files/pixhawk/hexarotor/widgets/mavconn.qgw
similarity index 57%
rename from files/pixhawk/hexrotor/widgets/mavconn.qgw
rename to files/pixhawk/hexarotor/widgets/mavconn.qgw
index e7b6e644f1ec9a866d50d7bec4aedd9fcf7e4d94..ff33f3786baccbb9cf2237611ab28dc946c90b8a 100644
--- a/files/pixhawk/hexrotor/widgets/mavconn.qgw
+++ b/files/pixhawk/hexarotor/widgets/mavconn.qgw
@@ -1,6 +1,6 @@
[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
diff --git a/images/status/colorbars.png b/images/status/colorbars.png
new file mode 100644
index 0000000000000000000000000000000000000000..29320d193256ade753ccf033b080928161140a2a
Binary files /dev/null and b/images/status/colorbars.png differ
diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri
index d82552af43bc939f429ef852e264a9e11d5e9a84..897b3b4e2443776db52af6ec254a65c76e18c1a6 100644
--- a/qgroundcontrol.pri
+++ b/qgroundcontrol.pri
@@ -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.
diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index dec0ea3d4803a7b452f5deb604643ef6ad8db06a..c2d8fcf4e1c8d2c6a28a561296e16365964b6e8a 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -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
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 524119c4dbd7e53d721f4fc758ef01d54c966738..1c7619edab995e054b62a874c3ee88ebf66071d9 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -75,6 +75,7 @@
images/status/audio-volume-medium.svg
images/status/audio-volume-low.svg
images/status/audio-volume-high.svg
+ images/status/colorbars.png
images/style-mission.css
images/splash.png
audio/alert.wav
diff --git a/src/QGC.cc b/src/QGC.cc
index c30209f83b0ceb8ea992d4d465dd9f385838f53c..50eb1c01754cac4f0018f8438ed6e68eb58901da 100644
--- a/src/QGC.cc
+++ b/src/QGC.cc
@@ -47,6 +47,15 @@ quint64 groundTimeMilliseconds()
return static_cast(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(seconds + (time.time().msec() / 1000.0));
+}
+
float limitAngleToPMPIf(float angle)
{
if (angle > -20*M_PI && angle < 20*M_PI)
diff --git a/src/QGC.h b/src/QGC.h
index 7f17bd143cb4ef2a6641e6120d56f3aec214252e..15391e315dddd7db2c76b04fc3c8decbb68853cb 100644
--- a/src/QGC.h
+++ b/src/QGC.h
@@ -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 */
diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index 917fa67109c49d60f52bdb6b06c19df51b8a9004..b12a82c7aebbb30c7e9d1112d11776a2174bb9f0 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -31,6 +31,10 @@
#include "QGCMAVLinkUASFactory.h"
#include "QGC.h"
+#ifdef QGC_PROTOBUF_ENABLED
+#include
+#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;
@@ -219,7 +224,42 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
if (protobufManager.getMessage(protobuf_msg))
{
- emit extendedMessageReceived(link, 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);
+ }
}
}
diff --git a/src/uas/QGCUASParamManager.h b/src/uas/QGCUASParamManager.h
index db991dcb7c6e32941643df0898e996921489095e..d3fedc04f5c171a3b0caa7cd9a50c57d8d5c2ef3 100644
--- a/src/uas/QGCUASParamManager.h
+++ b/src/uas/QGCUASParamManager.h
@@ -20,8 +20,20 @@ public:
QList 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;
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 4125518db4834f1e36c2fa185d2ec5faf979111f..6d77db8baab28cf7c20f2939f90fa16d08571420 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -27,6 +27,10 @@
#include "LinkManager.h"
#include "SerialLink.h"
+#ifdef QGC_PROTOBUF_ENABLED
+#include
+#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,19 +500,39 @@ 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);
- localX = pos.x;
- localY = pos.y;
- localZ = pos.z;
- emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
- emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
- // Set internal state
- if (!positionLock) {
- // If position was not locked before, notify positive
- GAudioOutput::instance()->notifyPositive();
+ // 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);
+
+ // Set internal state
+ if (!positionLock) {
+ // If position was not locked before, notify positive
+ GAudioOutput::instance()->notifyPositive();
+ }
+ positionLock = true;
+ isLocalPositionKnown = true;
}
- 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:
@@ -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 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
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 5216711a8f31e9fe17f036516c9973b6d7d8b331..e74af17f7180131df2e445229b64710faf28696d 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -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* > 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);
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index e3cbacfe27acc07b8b2f69d6d52d7b67d329c62e..cec880cb1109ac04774bd06e9c221e3c2ace8465 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -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 */
diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc
index 6119f43aa29d4b5cbe6f419bf2bfd916e532dc2d..91c65cd376ca17a7c00ff25bab4e3c70acdee864 100644
--- a/src/uas/UASWaypointManager.cc
+++ b/src/uas/UASWaypointManager.cc
@@ -61,6 +61,11 @@ UASWaypointManager::UASWaypointManager(UAS* _uas)
}
}
+UASWaypointManager::~UASWaypointManager()
+{
+
+}
+
void UASWaypointManager::timeout()
{
if (current_retries > 0) {
diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h
index d195fcd4976f5dc0c791565f3478e69b51fc3767..207d11ec429c24dd4667e02e5ff305a3bc989806 100644
--- a/src/uas/UASWaypointManager.h
+++ b/src/uas/UASWaypointManager.h
@@ -65,6 +65,7 @@ private:
public:
UASWaypointManager(UAS* uas=NULL); ///< Standard constructor
+ ~UASWaypointManager();
/** @name Received message handlers */
/*@{*/
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index 0e260bf1af20886478237cad56b3ae7cfa1641a4..20b8c09e7db0652c98e0cfb576a70172906eb313 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -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);
diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h
index cdce6e7d911a7fae2324a4797f9e0fef4d18f9d4..fd815e705f935c0daf3f15f3bc7490818a58dc96 100644
--- a/src/ui/HSIDisplay.h
+++ b/src/ui/HSIDisplay.h
@@ -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);
diff --git a/src/ui/HUD.h b/src/ui/HUD.h
index 1f5eebdbdacc5f74b92a0fc9f04cc6ebef81feb1..5239808d1265615f77d08fe3a8acbdea05d2894a 100644
--- a/src/ui/HUD.h
+++ b/src/ui/HUD.h
@@ -61,7 +61,7 @@ public slots:
//void paintGL();
/** @brief Set the currently monitored UAS */
- void setActiveUAS(UASInterface* uas);
+ virtual void 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);
diff --git a/src/ui/QGCRGBDView.cc b/src/ui/QGCRGBDView.cc
index ded4a4edac8b2a3808e8d99f0d8e364d36232f8b..cfa24beac5d0e7d5cd1cf859771b8cf8db5487c0 100644
--- a/src/ui/QGCRGBDView.cc
+++ b/src/ui/QGCRGBDView.cc
@@ -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::setActiveUAS(UASInterface* uas)
+{
+ 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::addUAS(UASInterface *uas)
+void QGCRGBDView::clearData(void)
{
- // TODO Enable multi-uas support
- connect(uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*)));
+ 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.0f) / 7.0f * 127.0f;
+ int idx = fminf(depth[c], 10.0f) / 10.0f * 127.0f;
idx = 127 - idx;
pixel[0] = colormapJet[idx][2] * 255.0f;
diff --git a/src/ui/QGCRGBDView.h b/src/ui/QGCRGBDView.h
index 586bba7623d5670846d5d397c2b2709119e5b7a2..2ca599feb77e7a4307138c2408d2e02db28df0df 100644
--- a/src/ui/QGCRGBDView.h
+++ b/src/ui/QGCRGBDView.h
@@ -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);
diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc
index 842a9ba8db35d9a8c7d3e5daebeb9bc95518205a..1c9bdb4453c744be8e080b031d56a66903514a68 100644
--- a/src/ui/WaypointList.cc
+++ b/src/ui/WaypointList.cc
@@ -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;
diff --git a/src/ui/map3D/ObstacleGroupNode.cc b/src/ui/map3D/ObstacleGroupNode.cc
index 72120fcea2d58a73c79272beeb396bdd1ef6e7d0..94c208f7a8a83204ff6f3ca862e0165fd101bfaa 100644
--- a/src/ui/map3D/ObstacleGroupNode.cc
+++ b/src/ui/map3D/ObstacleGroupNode.cc
@@ -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 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 geode = new osg::Geode;
for (int i = 0; i < obstacleList.obstacles_size(); ++i)
{
diff --git a/src/ui/map3D/ObstacleGroupNode.h b/src/ui/map3D/ObstacleGroupNode.h
index 0c965cef0d94ee150b1d14ec3715dbae3255ebef..8a0715ebe566fa67ff97e4e298c724e1b1cdd6df 100644
--- a/src/ui/map3D/ObstacleGroupNode.h
+++ b/src/ui/map3D/ObstacleGroupNode.h
@@ -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
diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc
index e8fdf5355b93974133f2b4524902e5ecb9b9c806..22af254d8748d1c7b02ec1152a548fadec240d3c 100644
--- a/src/ui/map3D/Pixhawk3DWidget.cc
+++ b/src/ui/map3D/Pixhawk3DWidget.cc
@@ -43,6 +43,7 @@
#include "UASManager.h"
#include "QGC.h"
+#include "gpl.h"
#ifdef QGC_PROTOBUF_ENABLED
#include
@@ -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.0f, 0.8f, 0.0f, 1.0f)));
+ 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();
+ 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 group = new osg::Group;
+
+ // cone indicates waypoint orientation
+ osg::ref_ptr sd = new osg::ShapeDrawable;
+ double coneRadius = radius / 2.0;
+ osg::ref_ptr 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 geode = new osg::Geode;
+ geode->addDrawable(sd);
+
+ osg::ref_ptr pat =
+ new osg::PositionAttitudeTransform;
+ pat->addChild(geode);
+ pat->setAttitude(osg::Quat(- M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
+ M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
+ 0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));
+ group->addChild(pat);
+
+ // cylinder indicates waypoint position
+ sd = new osg::ShapeDrawable;
+ osg::ref_ptr 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& trail = trails[component];
+
+ bool addToTrail = false;
+ if (trail.size() > 0)
+ {
+ if (fabs(x - trail[trail.size() - 1].x()) > 0.01f ||
+ fabs(y - trail[trail.size() - 1].y()) > 0.01f ||
+ fabs(z - trail[trail.size() - 1].z()) > 0.01f)
+ {
+ 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(orientationNode->getChild(idx));
+
+ pat->setAttitude(osg::Quat(-yaw, osg::Vec3d(0.0f, 0.0f, 1.0f),
+ 0.0, osg::Vec3d(1.0f, 0.0f, 0.0f),
+ 0.0, osg::Vec3d(0.0f, 1.0f, 0.0f)));
+}
+
void
Pixhawk3DWidget::selectFrame(QString text)
{
@@ -159,33 +307,46 @@ Pixhawk3DWidget::selectFrame(QString text)
}
void
-Pixhawk3DWidget::showGrid(int32_t state)
+Pixhawk3DWidget::showLocalGrid(int32_t state)
+{
+ if (state == Qt::Checked)
+ {
+ displayLocalGrid = true;
+ }
+ else
+ {
+ displayLocalGrid = false;
+ }
+}
+
+void
+Pixhawk3DWidget::showWorldGrid(int32_t state)
{
if (state == Qt::Checked)
{
- displayGrid = true;
+ displayWorldGrid = true;
}
else
{
- displayGrid = false;
+ displayWorldGrid = false;
}
}
void
-Pixhawk3DWidget::showTrail(int32_t state)
+Pixhawk3DWidget::showTrails(int32_t state)
{
if (state == Qt::Checked)
{
- if (!displayTrail)
+ if (!displayTrails)
{
- trail.clear();
+ trails.clear();
}
- displayTrail = true;
+ displayTrails = true;
}
else
{
- displayTrail = false;
+ displayTrails = 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 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("Trails");
+ trailCheckBox->setChecked(displayTrails);
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(showTrails(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.0f, 0.0f, 0.0f),
robotRoll, osg::Vec3d(0.0f, 1.0f, 0.0f)));
- if (displayTrail)
+ if (displayTrails)
{
- updateTrail(robotX, robotY, robotZ);
+ updateTrails(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
-Pixhawk3DWidget::createGrid(void)
+Pixhawk3DWidget::createLocalGrid(void)
{
osg::ref_ptr geode(new osg::Geode());
osg::ref_ptr fineGeometry(new osg::Geometry());
@@ -916,16 +1128,16 @@ Pixhawk3DWidget::createGrid(void)
geode->addDrawable(fineGeometry);
geode->addDrawable(coarseGeometry);
- float radius = 10.0f;
+ float radius = 5.0f;
float resolution = 0.25f;
osg::ref_ptr fineCoords(new osg::Vec3Array);
osg::ref_ptr coarseCoords(new osg::Vec3Array);
- // draw a 20m x 20m grid with 0.25m resolution
+ // draw a 10m x 10m grid with 0.25m resolution
for (float i = -radius; i <= radius; i += resolution)
{
- if (fabs(i - floor(i + 0.5f)) < 0.01f)
+ if (fabs(i / 1.0f - floor(i / 1.0f)) < 0.01f)
{
coarseCoords->push_back(osg::Vec3(i, -radius, 0.0f));
coarseCoords->push_back(osg::Vec3(i, radius, 0.0f));
@@ -961,45 +1173,153 @@ Pixhawk3DWidget::createGrid(void)
fineLinewidth->setWidth(0.25f);
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 coarseStateset(new osg::StateSet);
osg::ref_ptr coarseLinewidth(new osg::LineWidth());
- coarseLinewidth->setWidth(2.0f);
+ coarseLinewidth->setWidth(1.0f);
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
-Pixhawk3DWidget::createTrail(void)
+Pixhawk3DWidget::createWorldGrid(void)
{
osg::ref_ptr geode(new osg::Geode());
- trailGeometry = new osg::Geometry();
- trailGeometry->setUseDisplayList(false);
- geode->addDrawable(trailGeometry.get());
+ osg::ref_ptr fineGeometry(new osg::Geometry());
+ osg::ref_ptr coarseGeometry(new osg::Geometry());
+ osg::ref_ptr axisGeometry(new osg::Geometry());
+ geode->addDrawable(fineGeometry);
+ geode->addDrawable(coarseGeometry);
+ geode->addDrawable(axisGeometry.get());
+
+ float radius = 20.0f;
+ float resolution = 1.0f;
- trailVertices = new osg::Vec3dArray;
- trailGeometry->setVertexArray(trailVertices);
+ osg::ref_ptr fineCoords(new osg::Vec3Array);
+ osg::ref_ptr 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.0f - floor(i / 5.0f)) < 0.01f)
+ {
+ coarseCoords->push_back(osg::Vec3(i, -radius, 0.0f));
+ coarseCoords->push_back(osg::Vec3(i, radius, 0.0f));
+ coarseCoords->push_back(osg::Vec3(-radius, i, 0.0f));
+ coarseCoords->push_back(osg::Vec3(radius, i, 0.0f));
+ }
+ else
+ {
+ fineCoords->push_back(osg::Vec3(i, -radius, 0.0f));
+ fineCoords->push_back(osg::Vec3(i, radius, 0.0f));
+ fineCoords->push_back(osg::Vec3(-radius, i, 0.0f));
+ fineCoords->push_back(osg::Vec3(radius, i, 0.0f));
+ }
+ }
- trailDrawArrays = new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP);
- trailGeometry->addPrimitiveSet(trailDrawArrays);
+ fineGeometry->setVertexArray(fineCoords);
+ coarseGeometry->setVertexArray(coarseCoords);
osg::ref_ptr color(new osg::Vec4Array);
- color->push_back(osg::Vec4(1.0f, 0.0f, 0.0f, 1.0f));
- trailGeometry->setColorArray(color);
- trailGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+ color->push_back(osg::Vec4(0.5f, 0.5f, 0.5f, 1.0f));
+ 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 fineStateset(new osg::StateSet);
+ osg::ref_ptr fineLinewidth(new osg::LineWidth());
+ fineLinewidth->setWidth(0.1f);
+ 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 coarseStateset(new osg::StateSet);
+ osg::ref_ptr coarseLinewidth(new osg::LineWidth());
+ coarseLinewidth->setWidth(2.0f);
+ 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 coords(new osg::Vec3Array(6));
+ (*coords)[0] = (*coords)[2] = (*coords)[4] =
+ osg::Vec3(0.0f, 0.0f, 0.0f);
+ (*coords)[1] = osg::Vec3(0.0f, 1.0f, 0.0f);
+ (*coords)[3] = osg::Vec3(1.0f, 0.0f, 0.0f);
+ (*coords)[5] = osg::Vec3(0.0f, 0.0f, -1.0f);
+
+ axisGeometry->setVertexArray(coords);
+
+ osg::Vec4 redColor(1.0f, 0.0f, 0.0f, 0.0f);
+ osg::Vec4 greenColor(0.0f, 1.0f, 0.0f, 0.0f);
+ osg::Vec4 blueColor(0.0f, 0.0f, 1.0f, 0.0f);
+
+ osg::ref_ptr 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 axisStateset(new osg::StateSet);
+ osg::ref_ptr axisLinewidth(new osg::LineWidth());
+ axisLinewidth->setWidth(4.0f);
+ axisStateset->setAttributeAndModes(axisLinewidth, osg::StateAttribute::ON);
+ axisStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+ axisGeometry->setStateSet(axisStateset);
+
+ return geode;
+}
+
+osg::ref_ptr
+Pixhawk3DWidget::createTrail(const osg::Vec4& color)
+{
+ osg::ref_ptr geometry(new osg::Geometry());
+ geometry->setUseDisplayList(false);
+
+ osg::ref_ptr vertices(new osg::Vec3dArray());
+ geometry->setVertexArray(vertices);
+
+ osg::ref_ptr drawArrays(new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP));
+ geometry->addPrimitiveSet(drawArrays);
+
+ osg::ref_ptr colorArray(new osg::Vec4Array);
+ colorArray->push_back(color);
+ geometry->setColorArray(colorArray);
+ geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
osg::ref_ptr stateset(new osg::StateSet);
osg::ref_ptr linewidth(new osg::LineWidth());
linewidth->setWidth(1.0f);
stateset->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
- trailGeometry->setStateSet(stateset);
+ geometry->setStateSet(stateset);
- return geode;
+ return geometry;
}
osg::ref_ptr
@@ -1190,54 +1510,39 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
}
void
-Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ)
+Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ)
{
- if (robotX == 0.0f || robotY == 0.0f || robotZ == 0.0f)
- {
- return;
- }
+ QMapIterator it(trailDrawableIdxs);
- bool addToTrail = false;
- if (trail.size() > 0)
- {
- if (fabs(robotX - trail[trail.size() - 1].x()) > 0.01f ||
- fabs(robotY - trail[trail.size() - 1].y()) > 0.01f ||
- fabs(robotZ - trail[trail.size() - 1].z()) > 0.01f)
- {
- 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
+ osg::Geometry* geometry = trailNode->getDrawable(it.value())->asGeometry();
+ osg::DrawArrays* drawArrays = reinterpret_cast(geometry->getPrimitiveSet(0));
+
+ osg::ref_ptr vertices(new osg::Vec3Array);
+
+ const QVarLengthArray& trail = trails.value(it.key());
+
+ for (int i = 0; i < trail.size(); ++i)
{
- trail.append(p);
+ vertices->push_back(osg::Vec3d(trail[i].y() - robotY,
+ trail[i].x() - robotX,
+ -(trail[i].z() - robotZ)));
}
- }
- trailVertices->clear();
- for (int i = 0; i < trail.size(); ++i)
- {
- trailVertices->push_back(osg::Vec3d(trail[i].y() - robotY,
- trail[i].x() - robotX,
- -(trail[i].z() - robotZ)));
- }
+ geometry->setVertexArray(vertices);
+ drawArrays->setFirst(0);
+ drawArrays->setCount(vertices->size());
+ geometry->dirtyBound();
- trailDrawArrays->setFirst(0);
- trailDrawArrays->setCount(trailVertices->size());
- trailGeometry->dirtyBound();
+ osg::PositionAttitudeTransform* pat =
+ dynamic_cast(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(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.0f, 0.0f, 0.0f),
+ 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.0f, 0.0f, 0.0f),
M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));
osg::Geode* geode = dynamic_cast(pat->getChild(0));
osg::ShapeDrawable* sd = dynamic_cast(geode->getDrawable(0));
-
sd->setColor(osg::Vec4f(1.0f, 0.8f, 0.0f, 1.0f));
}
-float colormap_jet[128][3] = {
- {0.0f,0.0f,0.53125f},
- {0.0f,0.0f,0.5625f},
- {0.0f,0.0f,0.59375f},
- {0.0f,0.0f,0.625f},
- {0.0f,0.0f,0.65625f},
- {0.0f,0.0f,0.6875f},
- {0.0f,0.0f,0.71875f},
- {0.0f,0.0f,0.75f},
- {0.0f,0.0f,0.78125f},
- {0.0f,0.0f,0.8125f},
- {0.0f,0.0f,0.84375f},
- {0.0f,0.0f,0.875f},
- {0.0f,0.0f,0.90625f},
- {0.0f,0.0f,0.9375f},
- {0.0f,0.0f,0.96875f},
- {0.0f,0.0f,1.0f},
- {0.0f,0.03125f,1.0f},
- {0.0f,0.0625f,1.0f},
- {0.0f,0.09375f,1.0f},
- {0.0f,0.125f,1.0f},
- {0.0f,0.15625f,1.0f},
- {0.0f,0.1875f,1.0f},
- {0.0f,0.21875f,1.0f},
- {0.0f,0.25f,1.0f},
- {0.0f,0.28125f,1.0f},
- {0.0f,0.3125f,1.0f},
- {0.0f,0.34375f,1.0f},
- {0.0f,0.375f,1.0f},
- {0.0f,0.40625f,1.0f},
- {0.0f,0.4375f,1.0f},
- {0.0f,0.46875f,1.0f},
- {0.0f,0.5f,1.0f},
- {0.0f,0.53125f,1.0f},
- {0.0f,0.5625f,1.0f},
- {0.0f,0.59375f,1.0f},
- {0.0f,0.625f,1.0f},
- {0.0f,0.65625f,1.0f},
- {0.0f,0.6875f,1.0f},
- {0.0f,0.71875f,1.0f},
- {0.0f,0.75f,1.0f},
- {0.0f,0.78125f,1.0f},
- {0.0f,0.8125f,1.0f},
- {0.0f,0.84375f,1.0f},
- {0.0f,0.875f,1.0f},
- {0.0f,0.90625f,1.0f},
- {0.0f,0.9375f,1.0f},
- {0.0f,0.96875f,1.0f},
- {0.0f,1.0f,1.0f},
- {0.03125f,1.0f,0.96875f},
- {0.0625f,1.0f,0.9375f},
- {0.09375f,1.0f,0.90625f},
- {0.125f,1.0f,0.875f},
- {0.15625f,1.0f,0.84375f},
- {0.1875f,1.0f,0.8125f},
- {0.21875f,1.0f,0.78125f},
- {0.25f,1.0f,0.75f},
- {0.28125f,1.0f,0.71875f},
- {0.3125f,1.0f,0.6875f},
- {0.34375f,1.0f,0.65625f},
- {0.375f,1.0f,0.625f},
- {0.40625f,1.0f,0.59375f},
- {0.4375f,1.0f,0.5625f},
- {0.46875f,1.0f,0.53125f},
- {0.5f,1.0f,0.5f},
- {0.53125f,1.0f,0.46875f},
- {0.5625f,1.0f,0.4375f},
- {0.59375f,1.0f,0.40625f},
- {0.625f,1.0f,0.375f},
- {0.65625f,1.0f,0.34375f},
- {0.6875f,1.0f,0.3125f},
- {0.71875f,1.0f,0.28125f},
- {0.75f,1.0f,0.25f},
- {0.78125f,1.0f,0.21875f},
- {0.8125f,1.0f,0.1875f},
- {0.84375f,1.0f,0.15625f},
- {0.875f,1.0f,0.125f},
- {0.90625f,1.0f,0.09375f},
- {0.9375f,1.0f,0.0625f},
- {0.96875f,1.0f,0.03125f},
- {1.0f,1.0f,0.0f},
- {1.0f,0.96875f,0.0f},
- {1.0f,0.9375f,0.0f},
- {1.0f,0.90625f,0.0f},
- {1.0f,0.875f,0.0f},
- {1.0f,0.84375f,0.0f},
- {1.0f,0.8125f,0.0f},
- {1.0f,0.78125f,0.0f},
- {1.0f,0.75f,0.0f},
- {1.0f,0.71875f,0.0f},
- {1.0f,0.6875f,0.0f},
- {1.0f,0.65625f,0.0f},
- {1.0f,0.625f,0.0f},
- {1.0f,0.59375f,0.0f},
- {1.0f,0.5625f,0.0f},
- {1.0f,0.53125f,0.0f},
- {1.0f,0.5f,0.0f},
- {1.0f,0.46875f,0.0f},
- {1.0f,0.4375f,0.0f},
- {1.0f,0.40625f,0.0f},
- {1.0f,0.375f,0.0f},
- {1.0f,0.34375f,0.0f},
- {1.0f,0.3125f,0.0f},
- {1.0f,0.28125f,0.0f},
- {1.0f,0.25f,0.0f},
- {1.0f,0.21875f,0.0f},
- {1.0f,0.1875f,0.0f},
- {1.0f,0.15625f,0.0f},
- {1.0f,0.125f,0.0f},
- {1.0f,0.09375f,0.0f},
- {1.0f,0.0625f,0.0f},
- {1.0f,0.03125f,0.0f},
- {1.0f,0.0f,0.0f},
- {0.96875f,0.0f,0.0f},
- {0.9375f,0.0f,0.0f},
- {0.90625f,0.0f,0.0f},
- {0.875f,0.0f,0.0f},
- {0.84375f,0.0f,0.0f},
- {0.8125f,0.0f,0.0f},
- {0.78125f,0.0f,0.0f},
- {0.75f,0.0f,0.0f},
- {0.71875f,0.0f,0.0f},
- {0.6875f,0.0f,0.0f},
- {0.65625f,0.0f,0.0f},
- {0.625f,0.0f,0.0f},
- {0.59375f,0.0f,0.0f},
- {0.5625f,0.0f,0.0f},
- {0.53125f,0.0f,0.0f},
- {0.5f,0.0f,0.0f}
-};
-
#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.0f) / 7.0f * 127.0f;
idx = 127 - idx;
- pixel[0] = colormap_jet[idx][2] * 255.0f;
- pixel[1] = colormap_jet[idx][1] * 255.0f;
- pixel[2] = colormap_jet[idx][0] * 255.0f;
+ float r, g, b;
+ qgc::colormap("jet", idx, r, g, b);
+ pixel[0] = r * 255.0f;
+ pixel[1] = g * 255.0f;
+ pixel[2] = b * 255.0f;
}
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(geometry->getVertexArray());
osg::Vec4Array* colors = static_cast(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(fmin(dist / 7.0 * 127.0, 127.0));
- (*colors)[i].set(colormap_jet[colorIndex][0],
- colormap_jet[colorIndex][1],
- colormap_jet[colorIndex][2],
- 1.0f);
+
+ float r, g, b;
+ qgc::colormap("jet", colorIndex, r, g, b);
+
+ (*colors)[i].set(r, g, b, 1.0f);
}
}
@@ -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(geometry->getPrimitiveSet(0));
+ osg::Vec4Array* colorArray = reinterpret_cast(geometry->getColorArray());
+
+ geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
+ osg::ref_ptr linewidth(new osg::LineWidth());
+ linewidth->setWidth(2.0f);
+ geometry->getStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
+
+ colorArray->clear();
+
+ osg::ref_ptr vertices(new osg::Vec3Array);
+
+ if (QGC::groundTimeSeconds() - receivedTimestamp < kMessageTimeout)
+ {
+ // find path length
+ float length = 0.0f;
+ 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.0f));
+ }
+
+ float lengthCurrent = 0.0f;
+ 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.0f;
+
+ float r, g, b;
+ qgc::colormap("autumn", colorIdx, r, g, b);
+ colorArray->push_back(osg::Vec4f(r, g, b, 1.0f));
+ }
+ }
+
+ geometry->setVertexArray(vertices);
+ drawArrays->setFirst(0);
+ drawArrays->setCount(vertices->size());
+ geometry->dirtyBound();
}
#endif
diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h
index fe8f255f25fb6844c8f02459cc4915ba4945e3ba..60537da14cd1354cd6e8fcb57812530b3c5e43d8 100644
--- a/src/ui/map3D/Pixhawk3DWidget.h
+++ b/src/ui/map3D/Pixhawk3DWidget.h
@@ -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 createGrid(void);
- osg::ref_ptr createTrail(void);
+ osg::ref_ptr createLocalGrid(void);
+ osg::ref_ptr createWorldGrid(void);
+ osg::ref_ptr createTrail(const osg::Vec4& color);
osg::ref_ptr createMap(void);
osg::ref_ptr createRGBD3D(void);
osg::ref_ptr 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 updateTrails(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 trailVertices;
- QVarLengthArray trail;
+ QMap > trails;
+ QMap trailDrawableIdxs;
osg::ref_ptr vehicleModel;
osg::ref_ptr hudBackgroundGeometry;
@@ -174,22 +174,23 @@ private:
osg::ref_ptr depth2DGeode;
osg::ref_ptr rgbImage;
osg::ref_ptr depthImage;
- osg::ref_ptr gridNode;
+ osg::ref_ptr localGridNode;
+ osg::ref_ptr worldGridNode;
osg::ref_ptr trailNode;
- osg::ref_ptr trailGeometry;
- osg::ref_ptr trailDrawArrays;
+ osg::ref_ptr orientationNode;
osg::ref_ptr mapNode;
osg::ref_ptr waypointGroupNode;
osg::ref_ptr targetNode;
osg::ref_ptr rgbd3DNode;
#ifdef QGC_PROTOBUF_ENABLED
osg::ref_ptr obstacleGroupNode;
+ osg::ref_ptr pathNode;
#endif
QVector< osg::ref_ptr > vehicleModels;
MAV_FRAME frame;
- osg::Vec3d target;
+ QVector4D target;
QPoint cachedMousePos;
double lastRobotX, lastRobotY, lastRobotZ;
};
diff --git a/src/ui/map3D/Q3DWidget.cc b/src/ui/map3D/Q3DWidget.cc
index 1e3f7f079855d43d1084676c444bd687226e462b..25119cfc7d2a41b093334e7dd508a2a1645c73f7 100644
--- a/src/ui/map3D/Q3DWidget.cc
+++ b/src/ui/map3D/Q3DWidget.cc
@@ -148,8 +148,8 @@ Q3DWidget::createRobot(void)
osg::ref_ptr coords(new osg::Vec3Array(6));
(*coords)[0] = (*coords)[2] = (*coords)[4] =
osg::Vec3(0.0f, 0.0f, 0.0f);
- (*coords)[1] = osg::Vec3(0.15f, 0.0f, 0.0f);
- (*coords)[3] = osg::Vec3(0.0f, 0.3f, 0.0f);
+ (*coords)[1] = osg::Vec3(0.0f, 0.3f, 0.0f);
+ (*coords)[3] = osg::Vec3(0.15f, 0.0f, 0.0f);
(*coords)[5] = osg::Vec3(0.0f, 0.0f, -0.15f);
geometry->setVertexArray(coords);
diff --git a/src/ui/map3D/gpl.cc b/src/ui/map3D/gpl.cc
new file mode 100644
index 0000000000000000000000000000000000000000..ae0240c64bac8d2743bea775d9a01caa4ee42da8
--- /dev/null
+++ b/src/ui/map3D/gpl.cc
@@ -0,0 +1,333 @@
+#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.0f * M_PI;
+}
+
+double r2d(double rad)
+{
+ return rad / M_PI * 180.0;
+}
+
+float r2d(float rad)
+{
+ return rad / M_PI * 180.0f;
+}
+
+float colormapAutumn[128][3] =
+{
+ {1.0f,0.f,0.f},
+ {1.0f,0.007874f,0.f},
+ {1.0f,0.015748f,0.f},
+ {1.0f,0.023622f,0.f},
+ {1.0f,0.031496f,0.f},
+ {1.0f,0.03937f,0.f},
+ {1.0f,0.047244f,0.f},
+ {1.0f,0.055118f,0.f},
+ {1.0f,0.062992f,0.f},
+ {1.0f,0.070866f,0.f},
+ {1.0f,0.07874f,0.f},
+ {1.0f,0.086614f,0.f},
+ {1.0f,0.094488f,0.f},
+ {1.0f,0.10236f,0.f},
+ {1.0f,0.11024f,0.f},
+ {1.0f,0.11811f,0.f},
+ {1.0f,0.12598f,0.f},
+ {1.0f,0.13386f,0.f},
+ {1.0f,0.14173f,0.f},
+ {1.0f,0.14961f,0.f},
+ {1.0f,0.15748f,0.f},
+ {1.0f,0.16535f,0.f},
+ {1.0f,0.17323f,0.f},
+ {1.0f,0.1811f,0.f},
+ {1.0f,0.18898f,0.f},
+ {1.0f,0.19685f,0.f},
+ {1.0f,0.20472f,0.f},
+ {1.0f,0.2126f,0.f},
+ {1.0f,0.22047f,0.f},
+ {1.0f,0.22835f,0.f},
+ {1.0f,0.23622f,0.f},
+ {1.0f,0.24409f,0.f},
+ {1.0f,0.25197f,0.f},
+ {1.0f,0.25984f,0.f},
+ {1.0f,0.26772f,0.f},
+ {1.0f,0.27559f,0.f},
+ {1.0f,0.28346f,0.f},
+ {1.0f,0.29134f,0.f},
+ {1.0f,0.29921f,0.f},
+ {1.0f,0.30709f,0.f},
+ {1.0f,0.31496f,0.f},
+ {1.0f,0.32283f,0.f},
+ {1.0f,0.33071f,0.f},
+ {1.0f,0.33858f,0.f},
+ {1.0f,0.34646f,0.f},
+ {1.0f,0.35433f,0.f},
+ {1.0f,0.3622f,0.f},
+ {1.0f,0.37008f,0.f},
+ {1.0f,0.37795f,0.f},
+ {1.0f,0.38583f,0.f},
+ {1.0f,0.3937f,0.f},
+ {1.0f,0.40157f,0.f},
+ {1.0f,0.40945f,0.f},
+ {1.0f,0.41732f,0.f},
+ {1.0f,0.4252f,0.f},
+ {1.0f,0.43307f,0.f},
+ {1.0f,0.44094f,0.f},
+ {1.0f,0.44882f,0.f},
+ {1.0f,0.45669f,0.f},
+ {1.0f,0.46457f,0.f},
+ {1.0f,0.47244f,0.f},
+ {1.0f,0.48031f,0.f},
+ {1.0f,0.48819f,0.f},
+ {1.0f,0.49606f,0.f},
+ {1.0f,0.50394f,0.f},
+ {1.0f,0.51181f,0.f},
+ {1.0f,0.51969f,0.f},
+ {1.0f,0.52756f,0.f},
+ {1.0f,0.53543f,0.f},
+ {1.0f,0.54331f,0.f},
+ {1.0f,0.55118f,0.f},
+ {1.0f,0.55906f,0.f},
+ {1.0f,0.56693f,0.f},
+ {1.0f,0.5748f,0.f},
+ {1.0f,0.58268f,0.f},
+ {1.0f,0.59055f,0.f},
+ {1.0f,0.59843f,0.f},
+ {1.0f,0.6063f,0.f},
+ {1.0f,0.61417f,0.f},
+ {1.0f,0.62205f,0.f},
+ {1.0f,0.62992f,0.f},
+ {1.0f,0.6378f,0.f},
+ {1.0f,0.64567f,0.f},
+ {1.0f,0.65354f,0.f},
+ {1.0f,0.66142f,0.f},
+ {1.0f,0.66929f,0.f},
+ {1.0f,0.67717f,0.f},
+ {1.0f,0.68504f,0.f},
+ {1.0f,0.69291f,0.f},
+ {1.0f,0.70079f,0.f},
+ {1.0f,0.70866f,0.f},
+ {1.0f,0.71654f,0.f},
+ {1.0f,0.72441f,0.f},
+ {1.0f,0.73228f,0.f},
+ {1.0f,0.74016f,0.f},
+ {1.0f,0.74803f,0.f},
+ {1.0f,0.75591f,0.f},
+ {1.0f,0.76378f,0.f},
+ {1.0f,0.77165f,0.f},
+ {1.0f,0.77953f,0.f},
+ {1.0f,0.7874f,0.f},
+ {1.0f,0.79528f,0.f},
+ {1.0f,0.80315f,0.f},
+ {1.0f,0.81102f,0.f},
+ {1.0f,0.8189f,0.f},
+ {1.0f,0.82677f,0.f},
+ {1.0f,0.83465f,0.f},
+ {1.0f,0.84252f,0.f},
+ {1.0f,0.85039f,0.f},
+ {1.0f,0.85827f,0.f},
+ {1.0f,0.86614f,0.f},
+ {1.0f,0.87402f,0.f},
+ {1.0f,0.88189f,0.f},
+ {1.0f,0.88976f,0.f},
+ {1.0f,0.89764f,0.f},
+ {1.0f,0.90551f,0.f},
+ {1.0f,0.91339f,0.f},
+ {1.0f,0.92126f,0.f},
+ {1.0f,0.92913f,0.f},
+ {1.0f,0.93701f,0.f},
+ {1.0f,0.94488f,0.f},
+ {1.0f,0.95276f,0.f},
+ {1.0f,0.96063f,0.f},
+ {1.0f,0.9685f,0.f},
+ {1.0f,0.97638f,0.f},
+ {1.0f,0.98425f,0.f},
+ {1.0f,0.99213f,0.f},
+ {1.0f,1.0f,0.0f}
+};
+
+
+float colormapJet[128][3] =
+{
+ {0.0f,0.0f,0.53125f},
+ {0.0f,0.0f,0.5625f},
+ {0.0f,0.0f,0.59375f},
+ {0.0f,0.0f,0.625f},
+ {0.0f,0.0f,0.65625f},
+ {0.0f,0.0f,0.6875f},
+ {0.0f,0.0f,0.71875f},
+ {0.0f,0.0f,0.75f},
+ {0.0f,0.0f,0.78125f},
+ {0.0f,0.0f,0.8125f},
+ {0.0f,0.0f,0.84375f},
+ {0.0f,0.0f,0.875f},
+ {0.0f,0.0f,0.90625f},
+ {0.0f,0.0f,0.9375f},
+ {0.0f,0.0f,0.96875f},
+ {0.0f,0.0f,1.0f},
+ {0.0f,0.03125f,1.0f},
+ {0.0f,0.0625f,1.0f},
+ {0.0f,0.09375f,1.0f},
+ {0.0f,0.125f,1.0f},
+ {0.0f,0.15625f,1.0f},
+ {0.0f,0.1875f,1.0f},
+ {0.0f,0.21875f,1.0f},
+ {0.0f,0.25f,1.0f},
+ {0.0f,0.28125f,1.0f},
+ {0.0f,0.3125f,1.0f},
+ {0.0f,0.34375f,1.0f},
+ {0.0f,0.375f,1.0f},
+ {0.0f,0.40625f,1.0f},
+ {0.0f,0.4375f,1.0f},
+ {0.0f,0.46875f,1.0f},
+ {0.0f,0.5f,1.0f},
+ {0.0f,0.53125f,1.0f},
+ {0.0f,0.5625f,1.0f},
+ {0.0f,0.59375f,1.0f},
+ {0.0f,0.625f,1.0f},
+ {0.0f,0.65625f,1.0f},
+ {0.0f,0.6875f,1.0f},
+ {0.0f,0.71875f,1.0f},
+ {0.0f,0.75f,1.0f},
+ {0.0f,0.78125f,1.0f},
+ {0.0f,0.8125f,1.0f},
+ {0.0f,0.84375f,1.0f},
+ {0.0f,0.875f,1.0f},
+ {0.0f,0.90625f,1.0f},
+ {0.0f,0.9375f,1.0f},
+ {0.0f,0.96875f,1.0f},
+ {0.0f,1.0f,1.0f},
+ {0.03125f,1.0f,0.96875f},
+ {0.0625f,1.0f,0.9375f},
+ {0.09375f,1.0f,0.90625f},
+ {0.125f,1.0f,0.875f},
+ {0.15625f,1.0f,0.84375f},
+ {0.1875f,1.0f,0.8125f},
+ {0.21875f,1.0f,0.78125f},
+ {0.25f,1.0f,0.75f},
+ {0.28125f,1.0f,0.71875f},
+ {0.3125f,1.0f,0.6875f},
+ {0.34375f,1.0f,0.65625f},
+ {0.375f,1.0f,0.625f},
+ {0.40625f,1.0f,0.59375f},
+ {0.4375f,1.0f,0.5625f},
+ {0.46875f,1.0f,0.53125f},
+ {0.5f,1.0f,0.5f},
+ {0.53125f,1.0f,0.46875f},
+ {0.5625f,1.0f,0.4375f},
+ {0.59375f,1.0f,0.40625f},
+ {0.625f,1.0f,0.375f},
+ {0.65625f,1.0f,0.34375f},
+ {0.6875f,1.0f,0.3125f},
+ {0.71875f,1.0f,0.28125f},
+ {0.75f,1.0f,0.25f},
+ {0.78125f,1.0f,0.21875f},
+ {0.8125f,1.0f,0.1875f},
+ {0.84375f,1.0f,0.15625f},
+ {0.875f,1.0f,0.125f},
+ {0.90625f,1.0f,0.09375f},
+ {0.9375f,1.0f,0.0625f},
+ {0.96875f,1.0f,0.03125f},
+ {1.0f,1.0f,0.0f},
+ {1.0f,0.96875f,0.0f},
+ {1.0f,0.9375f,0.0f},
+ {1.0f,0.90625f,0.0f},
+ {1.0f,0.875f,0.0f},
+ {1.0f,0.84375f,0.0f},
+ {1.0f,0.8125f,0.0f},
+ {1.0f,0.78125f,0.0f},
+ {1.0f,0.75f,0.0f},
+ {1.0f,0.71875f,0.0f},
+ {1.0f,0.6875f,0.0f},
+ {1.0f,0.65625f,0.0f},
+ {1.0f,0.625f,0.0f},
+ {1.0f,0.59375f,0.0f},
+ {1.0f,0.5625f,0.0f},
+ {1.0f,0.53125f,0.0f},
+ {1.0f,0.5f,0.0f},
+ {1.0f,0.46875f,0.0f},
+ {1.0f,0.4375f,0.0f},
+ {1.0f,0.40625f,0.0f},
+ {1.0f,0.375f,0.0f},
+ {1.0f,0.34375f,0.0f},
+ {1.0f,0.3125f,0.0f},
+ {1.0f,0.28125f,0.0f},
+ {1.0f,0.25f,0.0f},
+ {1.0f,0.21875f,0.0f},
+ {1.0f,0.1875f,0.0f},
+ {1.0f,0.15625f,0.0f},
+ {1.0f,0.125f,0.0f},
+ {1.0f,0.09375f,0.0f},
+ {1.0f,0.0625f,0.0f},
+ {1.0f,0.03125f,0.0f},
+ {1.0f,0.0f,0.0f},
+ {0.96875f,0.0f,0.0f},
+ {0.9375f,0.0f,0.0f},
+ {0.90625f,0.0f,0.0f},
+ {0.875f,0.0f,0.0f},
+ {0.84375f,0.0f,0.0f},
+ {0.8125f,0.0f,0.0f},
+ {0.78125f,0.0f,0.0f},
+ {0.75f,0.0f,0.0f},
+ {0.71875f,0.0f,0.0f},
+ {0.6875f,0.0f,0.0f},
+ {0.65625f,0.0f,0.0f},
+ {0.625f,0.0f,0.0f},
+ {0.59375f,0.0f,0.0f},
+ {0.5625f,0.0f,0.0f},
+ {0.53125f,0.0f,0.0f},
+ {0.5f,0.0f,0.0f}
+};
+
+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;
+}
+
+}
diff --git a/src/ui/map3D/gpl.h b/src/ui/map3D/gpl.h
new file mode 100644
index 0000000000000000000000000000000000000000..d5e9ce61aa89f4bcef81e1a7bf4352b5dc079fd2
--- /dev/null
+++ b/src/ui/map3D/gpl.h
@@ -0,0 +1,52 @@
+#ifndef GPL_H
+#define GPL_H
+
+#include
+#include
+
+namespace qgc
+{
+
+template
+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
+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
+const T square(const T& x)
+{
+ return x * x;
+}
+
+bool colormap(const QString& name, unsigned char idx,
+ float& r, float& g, float& b);
+
+}
+
+#endif