Skip to content

Q
qgroundcontrol
Commit 306c978c authored by pixhawk's avatar pixhawk
Browse files
Options

Merge branch 'master' of git@pixhawk.ethz.ch:qgroundcontrol

parents 95ad5350 8b414e7d
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 60929 additions and 44 deletions
qgroundcontrol.pro
View file @ 306c978c
......@@ -43,7 +43,9 @@ DEPENDPATH += . \
INCLUDEPATH += . \
lib/QMapControl \
$$BASEDIR/../mavlink/contrib/slugs/include \
$$BASEDIR/../mavlink/include
$$BASEDIR/../mavlink/include \
/usr/include/freetype2
LIBS += /usr/lib/libftgl.so
# ../mavlink/include \
# MAVLink/include \
......@@ -89,7 +91,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 +163,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 +233,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/Core.cc
View file @ 306c978c
......@@ -123,8 +123,8 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv)
msgBox.setIcon(QMessageBox::Critical);
msgBox.setText("Could not connect UDP port. Is an instance of " + qAppName() + "already running?");
msgBox.setInformativeText("You will not be able to receive data via UDP. Please check that you're running the right executable and then re-start " + qAppName() + ". Do you want to close the application?");
msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
msgBox.setDefaultButton(QMessageBox::Cancel);
msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No);
msgBox.setDefaultButton(QMessageBox::No);
int ret = msgBox.exec();
// Close the message box shortly after the click to prevent accidental clicks
......
src/uas/UAS.cc
View file @ 306c978c
......@@ -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);
......@@ -1120,8 +1132,8 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
manualYawAngle = yaw * yawScaling;
manualThrust = thrust * thrustScaling;
if(mode == (int)MAV_MODE_MANUAL)
{
// if(mode == (int)MAV_MODE_MANUAL)
// {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
mavlink_message_t message;
mavlink_msg_manual_control_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual);
......@@ -1130,7 +1142,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow());
#endif
}
// }
}
int UAS::getSystemType()
......
src/uas/UAS.h
View file @ 306c978c
......@@ -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
View file @ 306c978c
......@@ -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
View file @ 306c978c
......@@ -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
View file @ 306c978c
......@@ -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/MainWindow.ui
View file @ 306c978c
......@@ -38,7 +38,7 @@
<x>0</x>
<y>0</y>
<width>1000</width>
<height>23</height>
<height>25</height>
</rect>
</property>
<widget class="QMenu" name="menuMGround">
......@@ -113,7 +113,7 @@
<widget class="QStatusBar" name="statusBar"/>
<action name="actionExit">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/actions/system-log-out.svg</normaloff>:/images/actions/system-log-out.svg</iconset>
</property>
<property name="text">
......@@ -123,22 +123,9 @@
<string>Ctrl+Q</string>
</property>
</action>
<action name="actionSettings">
<property name="icon">
<iconset>
<normaloff>:/images/categories/preferences-system.svg</normaloff>
<normalon>:/images/categories/preferences-system.svg</normalon>:/images/categories/preferences-system.svg</iconset>
</property>
<property name="text">
<string>Settings</string>
</property>
<property name="shortcut">
<string>Ctrl+P</string>
</property>
</action>
<action name="actionLiftoff">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/control/launch.svg</normaloff>
<normalon>:/images/control/launch.svg</normalon>:/images/control/launch.svg</iconset>
</property>
......@@ -148,7 +135,7 @@
</action>
<action name="actionLand">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/control/land.svg</normaloff>:/images/control/land.svg</iconset>
</property>
<property name="text">
......@@ -173,7 +160,7 @@
</action>
<action name="actionAdd_Link">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/actions/list-add.svg</normaloff>:/images/actions/list-add.svg</iconset>
</property>
<property name="text">
......@@ -182,7 +169,7 @@
</action>
<action name="actionConfiguration">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/categories/applications-system.svg</normaloff>:/images/categories/applications-system.svg</iconset>
</property>
<property name="text">
......@@ -194,7 +181,7 @@
</action>
<action name="actionEngineerView">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/apps/utilities-system-monitor.svg</normaloff>:/images/apps/utilities-system-monitor.svg</iconset>
</property>
<property name="text">
......@@ -203,7 +190,7 @@
</action>
<action name="actionPilotView">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/status/weather-overcast.svg</normaloff>:/images/status/weather-overcast.svg</iconset>
</property>
<property name="text">
......@@ -212,7 +199,7 @@
</action>
<action name="actionStyleConfig">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/categories/applications-internet.svg</normaloff>:/images/categories/applications-internet.svg</iconset>
</property>
<property name="text">
......@@ -221,7 +208,7 @@
</action>
<action name="actionJoystickSettings">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/devices/input-gaming.svg</normaloff>:/images/devices/input-gaming.svg</iconset>
</property>
<property name="text">
......@@ -230,7 +217,7 @@
</action>
<action name="actionOperatorView">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/status/network-wireless-encrypted.svg</normaloff>:/images/status/network-wireless-encrypted.svg</iconset>
</property>
<property name="text">
......@@ -242,7 +229,7 @@
</action>
<action name="action3DView">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/categories/preferences-system.svg</normaloff>:/images/categories/preferences-system.svg</iconset>
</property>
<property name="text">
......@@ -257,7 +244,7 @@
<bool>true</bool>
</property>
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/control/launch.svg</normaloff>:/images/control/launch.svg</iconset>
</property>
<property name="text">
......@@ -269,7 +256,7 @@
</action>
<action name="actionShow_full_view">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/status/network-transmit-receive.svg</normaloff>:/images/status/network-transmit-receive.svg</iconset>
</property>
<property name="text">
......@@ -278,7 +265,7 @@
</action>
<action name="actionShow_MAVLink_view">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/devices/network-wired.svg</normaloff>:/images/devices/network-wired.svg</iconset>
</property>
<property name="text">
......@@ -287,7 +274,7 @@
</action>
<action name="actionOnline_documentation">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/categories/applications-internet.svg</normaloff>:/images/categories/applications-internet.svg</iconset>
</property>
<property name="text">
......@@ -296,7 +283,7 @@
</action>
<action name="actionShow_data_analysis_view">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/apps/utilities-system-monitor.svg</normaloff>:/images/apps/utilities-system-monitor.svg</iconset>
</property>
<property name="text">
......@@ -305,7 +292,7 @@
</action>
<action name="actionProject_Roadmap">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/status/software-update-available.svg</normaloff>:/images/status/software-update-available.svg</iconset>
</property>
<property name="text">
......@@ -314,7 +301,7 @@
</action>
<action name="actionCredits_Developers">
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/categories/preferences-system.svg</normaloff>:/images/categories/preferences-system.svg</iconset>
</property>
<property name="text">
......@@ -323,7 +310,9 @@
</action>
</widget>
<layoutdefault spacing="6" margin="11"/>
<resources/>
<resources>
<include location="../../mavground.qrc"/>
</resources>
<connections>
<connection>
<sender>actionExit</sender>
......
src/ui/map3D/CheetahGL.cc 0 → 100755
View file @ 306c978c
This diff is collapsed.
src/ui/map3D/CheetahGL.h 0 → 100644
View file @ 306c978c
#ifndef CHEETAHGL_H_
#define CHEETAHGL_H_
#include <GL/gl.h>
GLint generatePixhawkCheetah(float red, float green, float blue);
#endif
src/ui/map3D/CheetahModel.cc 0 → 100644
View file @ 306c978c
#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 0 → 100644
View file @ 306c978c
#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.cc 0 → 100755
View file @ 306c978c
This diff is collapsed.
src/ui/map3D/Q3DWidget.h 0 → 100644
View file @ 306c978c
#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 cameraFov, float minClipRange,
float maxClipRange);
void setCameraLimit(bool onoff);
void setCameraLock(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);
void setDisplayMode2D(void);
std::pair<float,float> getPositionIn3DMode(int32_t mouseX,
int32_t mouseY);
std::pair<float,float> getPositionIn2DMode(int32_t mouseX,
int32_t mouseY);
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);
protected:
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 setDisplayMode3D(void);
float r2d(float angle);
float d2r(float angle);
private:
// 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;
bool lockCamera;
CameraParams cameraParams;
QBasicTimer timer;
void (*timerFunc)(void *);
void* timerFuncData;
};
#endif
src/ui/map3D/QMap3DWidget.cc 0 → 100644
View file @ 306c978c
#include "QMap3DWidget.h"
#include <FTGL/ftgl.h>
#include <QCheckBox>
#include <sys/time.h>
#include "CheetahModel.h"
#include "UASManager.h"
#include "UASInterface.h"
QMap3DWidget::QMap3DWidget(QWidget* parent)
: Q3DWidget(parent)
, uas(NULL)
, lastRedrawTime(0.0)
, displayGrid(false)
, displayTrail(false)
, lockCamera(false)
{
setFocusPolicy(Qt::StrongFocus);
initialize(10, 10, 1000, 900, 10.0f);
setCameraParams(0.05f, 0.5f, 0.01f, 0.5f, 30.0f, 0.01f, 400.0f);
setDisplayFunc(display, this);
addTimerFunc(100, timer, this);
buildLayout();
font.reset(new FTTextureFont("images/Vera.ttf"));
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
this, SLOT(setActiveUAS(UASInterface*)));
}
QMap3DWidget::~QMap3DWidget()
{
}
void
QMap3DWidget::buildLayout(void)
{
QCheckBox* gridCheckBox = new QCheckBox(this);
gridCheckBox->setText("Grid");
gridCheckBox->setChecked(displayGrid);
QCheckBox* trailCheckBox = new QCheckBox(this);
trailCheckBox->setText("Trail");
trailCheckBox->setChecked(displayTrail);
QPushButton* recenterButton = new QPushButton(this);
recenterButton->setText("Recenter Camera");
QCheckBox* lockCameraCheckBox = new QCheckBox(this);
lockCameraCheckBox->setText("Lock Camera");
lockCameraCheckBox->setChecked(lockCamera);
QGridLayout* layout = new QGridLayout(this);
layout->setMargin(0);
layout->setSpacing(2);
//layout->addWidget(mc, 0, 0, 1, 2);
layout->addWidget(gridCheckBox, 1, 0);
layout->addWidget(trailCheckBox, 1, 1);
layout->addWidget(recenterButton, 1, 2);
layout->addWidget(lockCameraCheckBox, 1, 3);
layout->setRowStretch(0, 100);
layout->setRowStretch(1, 1);
layout->setColumnStretch(0, 1);
layout->setColumnStretch(1, 50);
setLayout(layout);
connect(gridCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(showGrid(int)));
connect(trailCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(showTrail(int)));
connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenterCamera()));
connect(lockCameraCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(toggleLockCamera(int)));
}
void
QMap3DWidget::display(void* clientData)
{
QMap3DWidget* map3d = reinterpret_cast<QMap3DWidget *>(clientData);
map3d->displayHandler();
}
void
QMap3DWidget::displayHandler(void)
{
if (cheetahModel.data() == 0)
{
cheetahModel.reset(new CheetahModel);
cheetahModel->init(1.0f, 1.0f, 1.0f);
}
float robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f;
float robotRoll = 0.0f, robotPitch = 0.0f, robotYaw = 0.0f;
if (uas != NULL)
{
robotX = uas->getLocalX();
robotY = uas->getLocalY();
robotZ = uas->getLocalZ();
robotRoll = uas->getRoll();
robotPitch = uas->getPitch();
robotYaw = uas->getYaw();
}
setCameraLock(lockCamera);
// 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
drawPlatform(robotRoll, robotPitch, robotYaw);
if (displayGrid)
{
drawGrid();
}
if (displayTrail)
{
drawTrail(robotX, robotY, robotZ);
}
// switch to 2D
setDisplayMode2D();
// display pose information
glColor4f(0.0f, 0.0f, 0.0f, 0.5f);
glBegin(GL_POLYGON);
glVertex2f(0.0f, 0.0f);
glVertex2f(0.0f, 45.0f);
glVertex2f(getWindowWidth(), 45.0f);
glVertex2f(getWindowWidth(), 0.0f);
glEnd();
char buffer[6][255];
sprintf(buffer[0], "x = %.2f", robotX);
sprintf(buffer[1], "y = %.2f", robotY);
sprintf(buffer[2], "z = %.2f", robotZ);
sprintf(buffer[3], "r = %.2f", robotRoll);
sprintf(buffer[4], "p = %.2f", robotPitch);
sprintf(buffer[5], "y = %.2f", robotYaw);
font->FaceSize(10);
glColor3f(1.0f, 1.0f, 1.0f);
glPushMatrix();
glTranslatef(0.0f, 30.0f, 0.0f);
for (int32_t i = 0; i < 6; ++i)
{
glTranslatef(60.0f, 0.0f, 0.0f);
font->Render(buffer[i]);
}
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;
}
void
QMap3DWidget::showGrid(int32_t state)
{
if (state == Qt::Checked)
{
displayGrid = true;
}
else
{
displayGrid = false;
}
}
void
QMap3DWidget::showTrail(int32_t state)
{
if (state == Qt::Checked)
{
if (!displayTrail)
{
trail.clear();
}
displayTrail = true;
}
else
{
displayTrail = false;
}
}
void
QMap3DWidget::recenterCamera(void)
{
recenter();
}
void
QMap3DWidget::toggleLockCamera(int32_t state)
{
if (state == Qt::Checked)
{
lockCamera = true;
}
else
{
lockCamera = false;
}
}
void
QMap3DWidget::drawPlatform(float roll, float pitch, float yaw)
{
glPushMatrix();
glRotatef(yaw, 0.0f, 0.0f, 1.0f);
glRotatef(pitch, 0.0f, 1.0f, 0.0f);
glRotatef(roll, 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();
}
void
QMap3DWidget::drawGrid(void)
{
float radius = 10.0f;
float resolution = 0.25f;
glPushMatrix();
// draw a 20m x 20m grid with 0.25m resolution
glColor3f(0.5f, 0.5f, 0.5f);
for (float i = -radius; i <= radius; i += resolution)
{
if (fabsf(i - roundf(i)) < 0.01f)
{
glLineWidth(2.0f);
}
else
{
glLineWidth(0.25f);
}
glBegin(GL_LINES);
glVertex3f(i, -radius, 0.0f);
glVertex3f(i, radius, 0.0f);
glVertex3f(-radius, i, 0.0f);
glVertex3f(radius, i, 0.0f);
glEnd();
}
glPopMatrix();
}
void
QMap3DWidget::drawTrail(float x, float y, float z)
{
bool addToTrail = false;
if (trail.size() > 0)
{
if (fabsf(x - trail[trail.size() - 1].x) > 0.01f ||
fabsf(y - trail[trail.size() - 1].y) > 0.01f ||
fabsf(z - trail[trail.size() - 1].z) > 0.01f)
{
addToTrail = true;
}
}
else
{
addToTrail = true;
}
if (addToTrail)
{
Pose3D p = {x, y, z};
if (trail.size() == trail.capacity())
{
memcpy(trail.data(), trail.data() + 1,
(trail.size() - 1) * sizeof(Pose3D));
trail[trail.size() - 1] = p;
}
else
{
trail.append(p);
}
}
glColor3f(1.0f, 0.0f, 0.0f);
glLineWidth(1.0f);
glBegin(GL_LINE_STRIP);
for (int32_t i = 0; i < trail.size(); ++i)
{
glVertex3f(trail[i].x - x, trail[i].y - y, trail[i].z - z);
}
glEnd();
}
src/ui/map3D/QMap3DWidget.h 0 → 100644
View file @ 306c978c
#ifndef QMAP3DWIDGET_H
#define QMAP3DWIDGET_H
#include "Q3DWidget.h"
class CheetahModel;
class FTTextureFont;
class UASInterface;
class QMap3DWidget : public Q3DWidget
{
Q_OBJECT
public:
explicit QMap3DWidget(QWidget* parent);
~QMap3DWidget();
void buildLayout(void);
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);
private slots:
void showGrid(int state);
void showTrail(int state);
void recenterCamera(void);
void toggleLockCamera(int state);
protected:
UASInterface* uas;
private:
void drawPlatform(float roll, float pitch, float yaw);
void drawGrid(void);
void drawTrail(float x, float y, float z);
double lastRedrawTime;
bool displayGrid;
bool displayTrail;
bool lockCamera;
typedef struct
{
float x;
float y;
float z;
} Pose3D;
QVarLengthArray<Pose3D, 10000> trail;
QScopedPointer<CheetahModel> cheetahModel;
QScopedPointer<FTTextureFont> font;
};
#endif // QMAP3DWIDGET_H
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment