Added 3D visualization from Lionel

qgroundcontrol.pro

@@ -89,7 +89,8 @@ INCLUDEPATH += src \
     src/lib/qmapcontrol \
     src/ui/mavlink \
     src/ui/param \
-    src/ui/watchdog
+    src/ui/watchdog \
+    src/ui/map3D
 HEADERS += src/MG.h \
     src/Core.h \
     src/uas/UASInterface.h \
@@ -160,7 +161,11 @@ HEADERS += src/MG.h \
     src/ui/map/QGC2DIcon.h \
     src/ui/QGCRemoteControlView.h \
     src/WaypointGlobal.h \
-    src/ui/WaypointGlobalView.h
+    src/ui/WaypointGlobalView.h \
+    src/ui/map3D/Q3DWidget.h \
+    src/ui/map3D/CheetahModel.h \
+    src/ui/map3D/CheetahGL.h \
+    src/ui/map3D/QMap3DWidget.h
 SOURCES += src/main.cc \
     src/Core.cc \
     src/uas/UASManager.cc \
@@ -226,7 +231,11 @@ SOURCES += src/main.cc \
     src/ui/map/QGC2DIcon.cc \
     src/ui/QGCRemoteControlView.cc \
     src/WaypointGlobal.cpp \
-    src/ui/WaypointGlobalView.cpp
+    src/ui/WaypointGlobalView.cpp \
+    src/ui/map3D/Q3DWidget.cc \
+    src/ui/map3D/CheetahModel.cc \
+    src/ui/map3D/CheetahGL.cc \
+    src/ui/map3D/QMap3DWidget.cc
 RESOURCES = mavground.qrc
 
 # Include RT-LAB Library

src/uas/UAS.cc

@@ -73,6 +73,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
         sendDropRate(0),
         lowBattAlarm(false),
         positionLock(false),
+        localX(0),
+        localY(0),
+        localZ(0),
+        roll(0),
+        pitch(0),
+        yaw(0),
         statusTimeout(new QTimer(this))
 {
     color = UASInterface::getNextColor();
@@ -281,6 +287,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 mavlink_attitude_t attitude;
                 mavlink_msg_attitude_decode(&message, &attitude);
                 quint64 time = getUnixTime(attitude.usec);
+                roll = attitude.roll;
+                pitch = attitude.pitch;
+                yaw = attitude.yaw;
                 emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time);
                 emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time);
                 emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time);
@@ -300,6 +309,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 mavlink_local_position_t pos;
                 mavlink_msg_local_position_decode(&message, &pos);
                 quint64 time = getUnixTime(pos.usec);
+                localX = pos.x;
+                localY = pos.y;
+                localZ = pos.z;
                 emit valueChanged(uasId, "x", pos.x, time);
                 emit valueChanged(uasId, "y", pos.y, time);
                 emit valueChanged(uasId, "z", pos.z, time);

src/uas/UAS.h

@@ -79,6 +79,15 @@ public:
     /** @brief Get the links associated with this robot */
     QList<LinkInterface*>* getLinks();
 
+    double getLocalX() const { return localX; };
+    double getLocalY() const { return localY; };
+    double getLocalZ() const { return localZ; };
+
+    double getRoll() const { return roll; };
+    double getPitch() const { return pitch; };
+    double getYaw() const { return yaw; };
+
+
 friend class UASWaypointManager;
 protected:
     int uasId;                    ///< Unique system ID
@@ -127,6 +136,12 @@ protected:
     float sendDropRate;         ///< Percentage of packets that were not received from the MAV by the GCS
     bool lowBattAlarm;          ///< Switch if battery is low
     bool positionLock;          ///< Status if position information is available or not
+    double localX;
+    double localY;
+    double localZ;
+    double roll;
+    double pitch;
+    double yaw;
     QTimer* statusTimeout;      ///< Timer for various status timeouts
 
     /** @brief Set the current battery type */

src/uas/UASInterface.h

@@ -64,6 +64,14 @@ public:
     /** @brief Get the status flag for the communication **/
     virtual int getCommunicationStatus() const = 0;
 
+    virtual double getLocalX() const = 0;
+    virtual double getLocalY() const = 0;
+    virtual double getLocalZ() const = 0;
+
+    virtual double getRoll() const = 0;
+    virtual double getPitch() const = 0;
+    virtual double getYaw() const = 0;
+
     /** @brief Get reference to the waypoint manager **/
     virtual UASWaypointManager &getWaypointManager(void) = 0;
 

src/ui/MainWindow.cc

@@ -45,6 +45,7 @@ This file is part of the QGROUNDCONTROL project
 #include "MainWindow.h"
 #include "JoystickWidget.h"
 #include "GAudioOutput.h"
+#include "QMap3DWidget.h"
 
 // FIXME Move
 #include "PxQuadMAV.h"
@@ -131,7 +132,7 @@ void MainWindow::buildWidgets()
     mapWidget         = new MapWidget(this);
     protocolWidget    = new XMLCommProtocolWidget(this);
     dataplotWidget    = new QGCDataPlot2D(this);
-    map3DWidget       = new QWidget(this); // FIXME Lionel, insert visualizer here
+    map3DWidget       = new QMap3DWidget(this);
 
     // Dock widgets
     controlDockWidget = new QDockWidget(tr("Control"), this);
@@ -216,6 +217,7 @@ void MainWindow::arrangeCenterStack()
     if (linechartWidget) centerStack->addWidget(linechartWidget);
     if (protocolWidget) centerStack->addWidget(protocolWidget);
     if (mapWidget) centerStack->addWidget(mapWidget);
+    if (map3DWidget) centerStack->addWidget(map3DWidget);
     if (hudWidget) centerStack->addWidget(hudWidget);
     if (dataplotWidget) centerStack->addWidget(dataplotWidget);
 

src/ui/MainWindow.h

@@ -63,6 +63,7 @@ This file is part of the QGROUNDCONTROL project
 #include "HSIDisplay.h"
 #include "QGCDataPlot2D.h"
 #include "QGCRemoteControlView.h"
+#include "QMap3DWidget.h"
 
 #include "LogCompressor.h"
 
@@ -160,7 +161,7 @@ protected:
     QPointer<MapWidget> mapWidget;
     QPointer<XMLCommProtocolWidget> protocolWidget;
     QPointer<QGCDataPlot2D> dataplotWidget;
-    QPointer<QWidget> map3DWidget;
+    QPointer<QMap3DWidget> map3DWidget;
     // Dock widgets
     QPointer<QDockWidget> controlDockWidget;
     QPointer<QDockWidget> infoDockWidget;

src/ui/map3D/CheetahGL.h

+#ifndef CHEETAHGL_H_
+#define CHEETAHGL_H_
+
+#include <GL/gl.h>
+
+GLint generatePixhawkCheetah(float red, float green, float blue);
+
+#endif

src/ui/map3D/CheetahModel.cc

+#include "CheetahModel.h"
+
+#include "CheetahGL.h"
+
+CheetahModel::CheetahModel()
+ : cheetah_dl(-1)
+{
+
+}
+
+void
+CheetahModel::init(float red, float green, float blue)
+{
+	cheetah_dl = generatePixhawkCheetah(red, green, blue);
+}
+
+void
+CheetahModel::draw(void)
+{
+	glPushMatrix();
+        glScalef(0.5f, 0.5f, -0.5f);
+	glCallList(cheetah_dl);
+	glPopMatrix();
+}

src/ui/map3D/CheetahModel.h

+#ifndef CHEETAHMODEL_H_
+#define CHEETAHMODEL_H_
+
+#include <GL/gl.h>
+
+class CheetahModel
+{
+public:
+        CheetahModel();
+
+        void init(float red, float green, float blue);
+	void draw(void);
+
+private:
+	GLint cheetah_dl;
+};
+
+#endif /* CHEETAHMODEL_H_ */

src/ui/map3D/Q3DWidget.h

+#ifndef Q3DWIDGET_H_
+#define Q3DWIDGET_H_
+
+#include <boost/scoped_ptr.hpp>
+#include <inttypes.h>
+#include <string>
+#include <QtOpenGL>
+#include <QtGui>
+
+enum CameraState
+{
+	IDLE = 0,
+	ROTATING = 1,
+	MOVING = 2,
+	ZOOMING = 3
+};
+
+struct CameraPose
+{
+        CameraState state;
+	float pan, tilt, distance;
+	float xOffset, yOffset, zOffset;
+	float xOffset2D, yOffset2D, rotation2D, zoom, warpX, warpY;
+};
+
+struct CameraParams
+{
+	float zoomSensitivity;
+	float rotateSensitivity;
+	float moveSensitivity;
+
+	float minZoomRange;
+	float cameraFov;
+	float minClipRange;
+	float maxClipRange;
+
+	float zoomSensitivity2D;
+	float rotateSensitivity2D;
+	float moveSensitivity2D;
+};
+
+enum MouseState
+{
+	MOUSE_STATE_UP = 0,
+	MOUSE_STATE_DOWN = 1
+};
+
+typedef void (*DisplayFunc)(void *);
+typedef void (*KeyboardFunc)(char, void *);
+typedef void (*MouseFunc)(Qt::MouseButton, MouseState, int32_t, int32_t, void *);
+typedef void (*MotionFunc)(int32_t, int32_t, void *);
+
+class Q3DWidget: public QGLWidget
+{
+	Q_OBJECT
+
+public:
+        explicit Q3DWidget(QWidget* parent);
+        ~Q3DWidget();
+
+        void initialize(int32_t windowX, int32_t windowY,
+                        int32_t windowWidth, int32_t windowHeight, float fps);
+
+	void setCameraParams(float zoomSensitivity, float rotateSensitivity,
+						 float moveSensitivity, float minZoomRange,
+						 float camera_fov, float minClipRange,
+						 float maxClipRange);
+
+	void setCameraLimit(bool onoff);
+
+	void set2DCameraParams(float zoomSensitivity,
+						   float rotateSensitivity,
+						   float moveSensitivity);
+
+	void set3D(bool onoff);
+	bool is3D(void) const;
+
+	void setInitialCameraPos(float pan, float tilt, float range,
+							 float xOffset, float yOffset, float zOffset);
+	void setInitial2DCameraPos(float xOffset, float yOffset,
+							   float rotation, float zoom);
+        void setCameraPose(const CameraPose& cameraPose);
+        CameraPose getCameraPose(void) const;
+
+        void setDisplayFunc(DisplayFunc func, void* clientData);
+        void setKeyboardFunc(KeyboardFunc func, void* clientData);
+        void setMouseFunc(MouseFunc func, void* clientData);
+        void setMotionFunc(MotionFunc func, void* clientData);
+	void addTimerFunc(uint32_t msecs, void(*func)(void *),
+					  void* clientData);
+
+	void forceRedraw(void);
+
+	void set2DWarping(float warpX, float warpY);
+
+	void recenter(void);
+	void recenter2D(void);
+
+	void set2DRotation(bool onoff);
+
+	std::pair<float,float> pickPoint(int32_t mouseX, int32_t mouseY);
+
+	std::pair<float,float> get2DPosition(int32_t x, int32_t y);
+
+	int32_t getWindowWidth(void);
+	int32_t getWindowHeight(void);
+	int32_t getLastMouseX(void);
+	int32_t getLastMouseY(void);
+	int32_t getMouseX(void);
+	int32_t getMouseY(void);
+
+private Q_SLOTS:
+	void userTimer(void);
+
+private:
+	void rotateCamera(float dx, float dy);
+	void zoomCamera(float dy);
+	void moveCamera(float dx, float dy);
+	void rotateCamera2D(float dx);
+	void zoomCamera2D(float dx);
+	void moveCamera2D(float dx, float dy);
+
+	void switchTo3DMode(void);
+	void setDisplayMode2D(int32_t width, int32_t height);
+	void setDisplayMode3D(int32_t width, int32_t height);
+
+	float r2d(float angle);
+	float d2r(float angle);
+
+	// QGLWidget events
+	void initializeGL(void);
+	void paintGL(void);
+	void resizeGL(int32_t width, int32_t height);
+
+	// Qt events
+	void keyPressEvent(QKeyEvent* event);
+	void mousePressEvent(QMouseEvent* event);
+	void mouseReleaseEvent(QMouseEvent* event);
+	void mouseMoveEvent(QMouseEvent* event);
+	void wheelEvent(QWheelEvent *wheel);
+	void timerEvent(QTimerEvent* event);
+	void closeEvent(QCloseEvent* event);
+
+        DisplayFunc userDisplayFunc;
+        KeyboardFunc userKeyboardFunc;
+        MouseFunc userMouseFunc;
+        MotionFunc userMotionFunc;
+
+	void* userDisplayFuncData;
+	void* userKeyboardFuncData;
+	void* userMouseFuncData;
+	void* userMotionFuncData;
+
+	int32_t windowWidth, windowHeight;
+	float requestedFps;
+        CameraPose cameraPose;
+	int32_t lastMouseX, lastMouseY;
+
+	bool _is3D;
+
+	bool _forceRedraw;
+	bool allow2DRotation;
+	bool limitCamera;
+
+        CameraParams cameraParams;
+
+	QBasicTimer timer;
+
+	void (*timerFunc)(void *);
+	void* timerFuncData;
+};
+
+#endif

src/ui/map3D/QMap3DWidget.cc

+#include "QMap3DWidget.h"
+#include <QPushButton>
+
+#include <sys/time.h>
+
+#include "UASManager.h"
+#include "UASInterface.h"
+
+QMap3DWidget::QMap3DWidget(QWidget* parent)
+     : Q3DWidget(parent),
+     uas(NULL),
+     lastRedrawTime(0.0)
+{
+    setFocusPolicy(Qt::StrongFocus);
+
+    initialize(10, 10, 1000, 900, 10.0f);
+    setCameraParams(0.05f, 0.5f, 0.001f, 0.5f, 30.0f, 0.01f, 400.0f);
+
+    setDisplayFunc(display, this);
+    addTimerFunc(100, timer, this);
+
+    QPushButton* mapButton = new QPushButton(this);
+    mapButton->setText("Grid");
+
+    // display the MapControl in the application
+    QGridLayout* layout = new QGridLayout(this);
+    layout->setMargin(0);
+    layout->setSpacing(2);
+    //layout->addWidget(mc, 0, 0, 1, 2);
+    layout->addWidget(mapButton, 1, 0);
+    layout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 1);
+    layout->setRowStretch(0, 100);
+    layout->setRowStretch(1, 1);
+    layout->setColumnStretch(0, 1);
+    layout->setColumnStretch(1, 50);
+    setLayout(layout);
+
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
+            this, SLOT(setActiveUAS(UASInterface*)));
+}
+
+QMap3DWidget::~QMap3DWidget()
+{
+
+}
+
+void
+QMap3DWidget::display(void* clientData)
+{
+    QMap3DWidget* map3d = reinterpret_cast<QMap3DWidget *>(clientData);
+    map3d->displayHandler();
+}
+
+void
+QMap3DWidget::displayHandler(void)
+{
+    if (cheetahModel.get() == 0)
+    {
+        cheetahModel.reset(new CheetahModel);
+        cheetahModel->init(1.0f, 1.0f, 1.0f);
+    }
+
+    if (uas == NULL)
+    {
+        return;
+    }
+
+    // turn on smooth lines
+    glEnable(GL_LINE_SMOOTH);
+    glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    // clear window
+    glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
+    glClear(GL_COLOR_BUFFER_BIT);
+
+    // draw Cheetah model
+    glPushMatrix();
+
+    glRotatef(uas->getYaw(), 0.0f, 0.0f, 1.0f);
+    glRotatef(uas->getPitch(), 0.0f, 1.0f, 0.0f);
+    glRotatef(uas->getRoll(), 1.0f, 0.0f, 0.0f);
+
+    glLineWidth(3.0f);
+    glColor3f(0.0f, 1.0f, 0.0f);
+    glBegin(GL_LINES);
+    glVertex3f(0.0f, 0.0f, 0.0f);
+    glVertex3f(0.3f, 0.0f, 0.0f);
+    glEnd();
+
+    cheetahModel->draw();
+
+    glPopMatrix();
+}
+
+/**
+ *
+ * @param uas the UAS/MAV to monitor/display with the HUD
+ */
+void QMap3DWidget::setActiveUAS(UASInterface* uas)
+{
+    if (this->uas != NULL && this->uas != uas)
+    {
+        // Disconnect any previously connected active MAV
+        //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
+    }
+
+    this->uas = uas;
+}
+
+void
+QMap3DWidget::timer(void* clientData)
+{
+    QMap3DWidget* map3d = reinterpret_cast<QMap3DWidget *>(clientData);
+    map3d->timerHandler();
+}
+
+void
+QMap3DWidget::timerHandler(void)
+{
+    double timeLapsed = getTime() - lastRedrawTime;
+    if (timeLapsed > 0.1)
+    {
+        forceRedraw();
+        lastRedrawTime = getTime();
+    }
+    addTimerFunc(100, timer, this);
+}
+
+double
+QMap3DWidget::getTime(void) const
+{
+     struct timeval tv;
+
+     gettimeofday(&tv, NULL);
+
+     return static_cast<double>(tv.tv_sec) +
+             static_cast<double>(tv.tv_usec) / 1000000.0;
+}

src/ui/map3D/QMap3DWidget.h

+#ifndef QMAP3DWIDGET_H
+#define QMAP3DWIDGET_H
+
+#include "Q3DWidget.h"
+#include "CheetahModel.h"
+
+class UASInterface;
+
+class QMap3DWidget : public Q3DWidget
+{
+    Q_OBJECT
+
+public:
+    explicit QMap3DWidget(QWidget* parent);
+    ~QMap3DWidget();
+
+    static void display(void* clientData);
+    void displayHandler(void);
+
+    static void timer(void* clientData);
+    void timerHandler(void);
+
+    double getTime(void) const;
+
+public slots:
+    void setActiveUAS(UASInterface* uas);
+
+protected:
+    UASInterface* uas;
+
+private:
+    double lastRedrawTime;
+
+    boost::scoped_ptr<CheetahModel> cheetahModel;
+};
+
+#endif // QMAP3DWIDGET_H