diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 7ec8a814b37fe8c9777c8f98ee7bac1154c43243..d06df2778da67cf5ca8a920707f89428be014e5a 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -45,7 +45,7 @@ INCLUDEPATH += . \ $$BASEDIR/../mavlink/contrib/slugs/include \ $$BASEDIR/../mavlink/include \ /usr/include/freetype2 -LIBS += /usr/lib/libftgl.so +LIBS += /usr/lib/libftgl.so /usr/lib/libglut.so # ../mavlink/include \ # MAVLink/include \ diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 9d561b094a2266e4b8e908199b07dd3e3b861f3c..bf075116a54750fa0b29d3e6af960290ec390838 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1336,6 +1336,16 @@ void UAS::shutdown() } } +void UAS::setTargetPosition(float x, float y, float z, float yaw) +{ + mavlink_message_t msg; + mavlink_msg_position_target_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, x, y, z, yaw); + + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + /** * @return The name of this system as string in human-readable form */ diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 1d20d60f974736d0f404f4f3a6cbd0557c5f9bdb..e3d3e7359bfeea44445599939fc913659b04cb8a 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -179,6 +179,9 @@ public slots: /** @brief Shut the system cleanly down. Will shut down any onboard computers **/ void shutdown(); + /** @brief Set the target position for the robot to navigate to. */ + void setTargetPosition(float x, float y, float z, float yaw); + void startLowBattAlarm(); void stopLowBattAlarm(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 4782397b2f549020266a451e1a757976f58c7032..6a112aca1f6e1de1216c2f4afd12c63613be07f6 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -177,6 +177,13 @@ public slots: * Works only if already landed and will cleanly shut down all onboard computers. */ virtual void shutdown() = 0; + /** @brief Set the target position for the robot to navigate to. + * @param x x-coordinate of the target position + * @param y y-coordinate of the target position + * @param z z-coordinate of the target position + * @param yaw heading of the target position + */ + virtual void setTargetPosition(float x, float y, float z, float yaw) = 0; /** @brief Request the list of stored waypoints from the robot */ //virtual void requestWaypoints() = 0; /** @brief Clear all existing waypoints on the robot */ diff --git a/src/ui/map3D/CheetahGL.cc b/src/ui/map3D/CheetahGL.cc index 1a7a0c846b5245f80fae3dc257be608b730b3a07..83b50f68b70007c9a935713561a8695b1ef16b7f 100755 --- a/src/ui/map3D/CheetahGL.cc +++ b/src/ui/map3D/CheetahGL.cc @@ -1,3 +1,34 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Generates a display list which renders a Pixhawk Cheetah MAV. + * + * @author Lionel Heng + * + */ + /* This file was produced by Deep Exploration Plugin: CPP Export filter. diff --git a/src/ui/map3D/CheetahGL.h b/src/ui/map3D/CheetahGL.h index 10f67b9d77e662e64b8d7d395272dd4f16b1b329..4dd8800d2544e51526e59cdc2fa4d6a7cac8836e 100644 --- a/src/ui/map3D/CheetahGL.h +++ b/src/ui/map3D/CheetahGL.h @@ -1,8 +1,47 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Generates a display list which renders a Pixhawk Cheetah MAV. + * + * @author Lionel Heng + * + */ + #ifndef CHEETAHGL_H_ #define CHEETAHGL_H_ #include +/** + * @brief Creates a display list which renders a Pixhawk Cheetah MAV. + * @param red Red intensity of the MAV model + * @param green Green intensity of the MAV model + * @param blue Blue intensity of the MAV model + * + * @return The index of the display list. + **/ GLint generatePixhawkCheetah(float red, float green, float blue); #endif diff --git a/src/ui/map3D/CheetahModel.cc b/src/ui/map3D/CheetahModel.cc index b0f2aa5f99271f9b7b80b5b252b5aebf33991d73..665323fb0c70f29a3f7eae5116055bed21f9bd83 100644 --- a/src/ui/map3D/CheetahModel.cc +++ b/src/ui/map3D/CheetahModel.cc @@ -1,3 +1,34 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of the class CheetahModel. + * + * @author Lionel Heng + * + */ + #include "CheetahModel.h" #include "CheetahGL.h" @@ -11,14 +42,14 @@ CheetahModel::CheetahModel() void CheetahModel::init(float red, float green, float blue) { - cheetah_dl = generatePixhawkCheetah(red, green, blue); + cheetah_dl = generatePixhawkCheetah(red, green, blue); } void CheetahModel::draw(void) { - glPushMatrix(); - glScalef(0.5f, 0.5f, -0.5f); - glCallList(cheetah_dl); - glPopMatrix(); + glPushMatrix(); + glScalef(0.5f, 0.5f, -0.5f); + glCallList(cheetah_dl); + glPopMatrix(); } diff --git a/src/ui/map3D/CheetahModel.h b/src/ui/map3D/CheetahModel.h index 8673396cdb7896a476e7eb0454cba411ce20a2f5..a1398748c0efc8220dbc2779c1627bf25d90e8f2 100644 --- a/src/ui/map3D/CheetahModel.h +++ b/src/ui/map3D/CheetahModel.h @@ -1,18 +1,62 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of the class CheetahModel. + * + * @author Lionel Heng + * + */ + #ifndef CHEETAHMODEL_H_ #define CHEETAHMODEL_H_ #include +/** + * @brief Container for display list which renders the Pixhawk Cheetah MAV. + **/ class CheetahModel { public: - CheetahModel(); + CheetahModel(); + + /** + * @brief Initializes the display list. + * @param red Red intensity of the MAV model + * @param green Green intensity of the MAV model + * @param blue Blue intensity of the MAV model + **/ + void init(float red, float green, float blue); - void init(float red, float green, float blue); - void draw(void); + /** + * @brief Executes the display list. + **/ + void draw(void); private: - GLint cheetah_dl; + GLint cheetah_dl; }; #endif /* CHEETAHMODEL_H_ */ diff --git a/src/ui/map3D/Q3DWidget.cc b/src/ui/map3D/Q3DWidget.cc index 4ba5ca893c6e68ce0e61c180e2ddac285c7506d6..04ee667966fe9343355de3712673befba66269b1 100755 --- a/src/ui/map3D/Q3DWidget.cc +++ b/src/ui/map3D/Q3DWidget.cc @@ -1,3 +1,34 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of the class Q3DWidget. + * + * @author Lionel Heng + * + */ + #include "Q3DWidget.h" #include @@ -28,7 +59,6 @@ Q3DWidget::Q3DWidget(QWidget* parent) , _forceRedraw(false) , allow2DRotation(true) , limitCamera(false) - , lockCamera(false) , timerFunc(NULL) , timerFuncData(NULL) { @@ -105,12 +135,6 @@ Q3DWidget::setCameraLimit(bool onoff) limitCamera = onoff; } -void -Q3DWidget::setCameraLock(bool onoff) -{ - lockCamera = onoff; -} - void Q3DWidget::set2DCameraParams(float zoomSensitivity2D, float rotateSensitivity2D, @@ -338,21 +362,18 @@ Q3DWidget::getMouseY(void) void Q3DWidget::rotateCamera(float dx, float dy) { - if (!lockCamera) + cameraPose.pan += dx * cameraParams.rotateSensitivity; + cameraPose.tilt += dy * cameraParams.rotateSensitivity; + if (limitCamera) { - cameraPose.pan += dx * cameraParams.rotateSensitivity; - cameraPose.tilt += dy * cameraParams.rotateSensitivity; - if (limitCamera) - { - if (cameraPose.tilt < 180.5f) - { - cameraPose.tilt = 180.5f; - } - else if (cameraPose.tilt > 269.5f) - { - cameraPose.tilt = 269.5f; - } - } + if (cameraPose.tilt < 180.5f) + { + cameraPose.tilt = 180.5f; + } + else if (cameraPose.tilt > 269.5f) + { + cameraPose.tilt = 269.5f; + } } } @@ -495,8 +516,8 @@ Q3DWidget::paintGL(void) setDisplayMode2D(); // do camera control glTranslatef(static_cast(windowWidth) / 2.0f, - static_cast(windowHeight) / 2.0f, - 0.0f); + static_cast(windowHeight) / 2.0f, + 0.0f); glScalef(cameraPose.zoom, cameraPose.zoom, 1.0f); glRotatef(r2d(cameraPose.rotation2D), 0.0f, 0.0f, 1.0f); glScalef(cameraPose.warpX, cameraPose.warpY, 1.0f); @@ -540,303 +561,301 @@ Q3DWidget::resizeGL(int32_t width, int32_t height) void Q3DWidget::keyPressEvent(QKeyEvent* event) { - float dx = 0.0f, dy = 0.0f; - - Qt::KeyboardModifiers modifiers = event->modifiers(); - if (_is3D) - { - if (modifiers & Qt::ControlModifier) - { - switch (event->key()) - { - case Qt::Key_Left: - dx = -KEY_ROTATE_AMOUNT; - dy = 0.0f; - break; - case Qt::Key_Right: - dx = KEY_ROTATE_AMOUNT; - dy = 0.0f; - break; - case Qt::Key_Up: - dx = 0.0f; - dy = KEY_ROTATE_AMOUNT; - break; - case Qt::Key_Down: - dx = 0.0f; - dy = -KEY_ROTATE_AMOUNT; - break; - default: - QWidget::keyPressEvent(event); - } - if (dx != 0.0f || dy != 0.0f) - { - rotateCamera(dx, dy); - } - } - else if (modifiers & Qt::AltModifier) - { - switch (event->key()) - { - case Qt::Key_Up: - dy = KEY_ZOOM_AMOUNT; - break; - case Qt::Key_Down: - dy = -KEY_ZOOM_AMOUNT; - break; - default: - QWidget::keyPressEvent(event); - } - if (dy != 0.0f) - { - zoomCamera(dy); - } - } - else - { - switch (event->key()) - { - case Qt::Key_Left: - dx = KEY_MOVE_AMOUNT; - dy = 0.0f; - break; - case Qt::Key_Right: - dx = -KEY_MOVE_AMOUNT; - dy = 0.0f; - break; - case Qt::Key_Up: - dx = 0.0f; - dy = -KEY_MOVE_AMOUNT; - break; - case Qt::Key_Down: - dx = 0.0f; - dy = KEY_MOVE_AMOUNT; - break; - default: - QWidget::keyPressEvent(event); - } - if (dx != 0.0f || dy != 0.0f) - { - moveCamera(dx, dy); - } - } - } - else { - if (modifiers & Qt::ControlModifier) - { - switch (event->key()) - { - case Qt::Key_Left: - dx = KEY_ROTATE_AMOUNT; - dy = 0.0f; - break; - case Qt::Key_Right: - dx = -KEY_ROTATE_AMOUNT; - dy = 0.0f; - break; - default: - QWidget::keyPressEvent(event); - } - if (dx != 0.0f) - { - rotateCamera2D(dx); - } - } - else if (modifiers & Qt::AltModifier) - { - switch (event->key()) - { - case Qt::Key_Up: - dy = KEY_ZOOM_AMOUNT; - break; - case Qt::Key_Down: - dy = -KEY_ZOOM_AMOUNT; - break; - default: - QWidget::keyPressEvent(event); - } - if (dy != 0.0f) - { - zoomCamera2D(dy); - } - } - else { - switch (event->key()) - { - case Qt::Key_Left: - dx = KEY_MOVE_AMOUNT; - dy = 0.0f; - break; - case Qt::Key_Right: - dx = -KEY_MOVE_AMOUNT; - dy = 0.0f; - break; - case Qt::Key_Up: - dx = 0.0f; - dy = KEY_MOVE_AMOUNT; - break; - case Qt::Key_Down: - dx = 0.0f; - dy = -KEY_MOVE_AMOUNT; - break; - default: - QWidget::keyPressEvent(event); - } - if (dx != 0.0f || dy != 0.0f) - { - moveCamera2D(dx, dy); - } - } - } - - _forceRedraw = true; - - if (userKeyboardFunc) - { - if (event->text().isEmpty()) - { - userKeyboardFunc(0, userKeyboardFuncData); - } - else - { - userKeyboardFunc(event->text().at(0).toAscii(), - userKeyboardFuncData); - } - } + float dx = 0.0f, dy = 0.0f; + + Qt::KeyboardModifiers modifiers = event->modifiers(); + if (_is3D) + { + if (modifiers & Qt::ControlModifier) + { + switch (event->key()) + { + case Qt::Key_Left: + dx = -KEY_ROTATE_AMOUNT; + dy = 0.0f; + break; + case Qt::Key_Right: + dx = KEY_ROTATE_AMOUNT; + dy = 0.0f; + break; + case Qt::Key_Up: + dx = 0.0f; + dy = KEY_ROTATE_AMOUNT; + break; + case Qt::Key_Down: + dx = 0.0f; + dy = -KEY_ROTATE_AMOUNT; + break; + default: + QWidget::keyPressEvent(event); + } + if (dx != 0.0f || dy != 0.0f) + { + rotateCamera(dx, dy); + } + } + else if (modifiers & Qt::AltModifier) + { + switch (event->key()) + { + case Qt::Key_Up: + dy = KEY_ZOOM_AMOUNT; + break; + case Qt::Key_Down: + dy = -KEY_ZOOM_AMOUNT; + break; + default: + QWidget::keyPressEvent(event); + } + if (dy != 0.0f) + { + zoomCamera(dy); + } + } + else + { + switch (event->key()) + { + case Qt::Key_Left: + dx = KEY_MOVE_AMOUNT; + dy = 0.0f; + break; + case Qt::Key_Right: + dx = -KEY_MOVE_AMOUNT; + dy = 0.0f; + break; + case Qt::Key_Up: + dx = 0.0f; + dy = -KEY_MOVE_AMOUNT; + break; + case Qt::Key_Down: + dx = 0.0f; + dy = KEY_MOVE_AMOUNT; + break; + default: + QWidget::keyPressEvent(event); + } + if (dx != 0.0f || dy != 0.0f) + { + moveCamera(dx, dy); + } + } + } + else + { + if (modifiers & Qt::ControlModifier) + { + switch (event->key()) + { + case Qt::Key_Left: + dx = KEY_ROTATE_AMOUNT; + dy = 0.0f; + break; + case Qt::Key_Right: + dx = -KEY_ROTATE_AMOUNT; + dy = 0.0f; + break; + default: + QWidget::keyPressEvent(event); + } + if (dx != 0.0f) + { + rotateCamera2D(dx); + } + } + else if (modifiers & Qt::AltModifier) + { + switch (event->key()) + { + case Qt::Key_Up: + dy = KEY_ZOOM_AMOUNT; + break; + case Qt::Key_Down: + dy = -KEY_ZOOM_AMOUNT; + break; + default: + QWidget::keyPressEvent(event); + } + if (dy != 0.0f) + { + zoomCamera2D(dy); + } + } + else + { + switch (event->key()) + { + case Qt::Key_Left: + dx = KEY_MOVE_AMOUNT; + dy = 0.0f; + break; + case Qt::Key_Right: + dx = -KEY_MOVE_AMOUNT; + dy = 0.0f; + break; + case Qt::Key_Up: + dx = 0.0f; + dy = KEY_MOVE_AMOUNT; + break; + case Qt::Key_Down: + dx = 0.0f; + dy = -KEY_MOVE_AMOUNT; + break; + default: + QWidget::keyPressEvent(event); + } + if (dx != 0.0f || dy != 0.0f) + { + moveCamera2D(dx, dy); + } + } + } + + _forceRedraw = true; + + if (userKeyboardFunc) + { + if (event->text().isEmpty()) + { + userKeyboardFunc(0, userKeyboardFuncData); + } + else + { + userKeyboardFunc(event->text().at(0).toAscii(), + userKeyboardFuncData); + } + } } void Q3DWidget::mousePressEvent(QMouseEvent* event) { - Qt::KeyboardModifiers modifiers = event->modifiers(); + Qt::KeyboardModifiers modifiers = event->modifiers(); - if (!(modifiers & (Qt::ControlModifier | Qt::AltModifier))) - { - lastMouseX = event->x(); - lastMouseY = event->y(); - if (event->button() == Qt::LeftButton) - { - cameraPose.state = ROTATING; - } - else if (event->button() == Qt::MidButton) - { - cameraPose.state = MOVING; - } - else if (event->button() == Qt::RightButton) - { - cameraPose.state = ZOOMING; - } - } + if (!(modifiers & (Qt::ControlModifier | Qt::AltModifier))) + { + lastMouseX = event->x(); + lastMouseY = event->y(); + if (event->button() == Qt::LeftButton) + { + cameraPose.state = ROTATING; + } + else if (event->button() == Qt::MidButton) + { + cameraPose.state = MOVING; + } + } - _forceRedraw = true; + _forceRedraw = true; - if (userMouseFunc) - { - userMouseFunc(event->button(), MOUSE_STATE_DOWN, event->x(), event->y(), - userMouseFuncData); - } + if (userMouseFunc) + { + userMouseFunc(event->button(), MOUSE_STATE_DOWN, event->x(), event->y(), + userMouseFuncData); + } } void Q3DWidget::mouseReleaseEvent(QMouseEvent* event) { - Qt::KeyboardModifiers modifiers = event->modifiers(); + Qt::KeyboardModifiers modifiers = event->modifiers(); - if (!(modifiers & (Qt::ControlModifier | Qt::AltModifier))) - { - cameraPose.state = IDLE; - } + if (!(modifiers & (Qt::ControlModifier | Qt::AltModifier))) + { + cameraPose.state = IDLE; + } - _forceRedraw = true; + _forceRedraw = true; - if (userMouseFunc) - { - userMouseFunc(event->button(), MOUSE_STATE_UP, event->x(), event->y(), - userMouseFuncData); - } + if (userMouseFunc) + { + userMouseFunc(event->button(), MOUSE_STATE_UP, event->x(), event->y(), + userMouseFuncData); + } } void Q3DWidget::mouseMoveEvent(QMouseEvent* event) { - int32_t dx = event->x() - lastMouseX; - int32_t dy = event->y() - lastMouseY; - - if (_is3D) - { - if (cameraPose.state == ROTATING) - { - rotateCamera(static_cast(dx), static_cast(dy)); - } - else if (cameraPose.state == MOVING) - { - moveCamera(static_cast(dx), static_cast(dy)); - } - else if (cameraPose.state == ZOOMING) - { - zoomCamera(static_cast(dy)); - } - } - else - { - if (cameraPose.state == ROTATING) - { - if (event->x() > windowWidth / 2) - { - dy *= -1; - } - - rotateCamera2D(static_cast(dx)); - } - else if (cameraPose.state == MOVING) - { - moveCamera2D(static_cast(dx), static_cast(dy)); - } - else if (cameraPose.state == ZOOMING) - { - zoomCamera2D(static_cast(dy)); - } - } - - lastMouseX = event->x(); - lastMouseY = event->y(); - _forceRedraw = true; - - if (userMotionFunc) - { - userMotionFunc(event->x(), event->y(), userMotionFuncData); - } + int32_t dx = event->x() - lastMouseX; + int32_t dy = event->y() - lastMouseY; + + if (_is3D) + { + if (cameraPose.state == ROTATING) + { + rotateCamera(static_cast(dx), static_cast(dy)); + } + else if (cameraPose.state == MOVING) + { + moveCamera(static_cast(dx), static_cast(dy)); + } + else if (cameraPose.state == ZOOMING) + { + zoomCamera(static_cast(dy)); + } + } + else + { + if (cameraPose.state == ROTATING) + { + if (event->x() > windowWidth / 2) + { + dy *= -1; + } + + rotateCamera2D(static_cast(dx)); + } + else if (cameraPose.state == MOVING) + { + moveCamera2D(static_cast(dx), static_cast(dy)); + } + else if (cameraPose.state == ZOOMING) + { + zoomCamera2D(static_cast(dy)); + } + } + + lastMouseX = event->x(); + lastMouseY = event->y(); + _forceRedraw = true; + + if (userMotionFunc) + { + userMotionFunc(event->x(), event->y(), userMotionFuncData); + } } void Q3DWidget::wheelEvent(QWheelEvent* event) { - if (_is3D) - { - zoomCamera(static_cast(event->delta()) / 40.0f); - } - else - { - zoomCamera2D(static_cast(event->delta()) / 40.0f); - } + if (_is3D) + { + zoomCamera(static_cast(event->delta()) / 40.0f); + } + else + { + zoomCamera2D(static_cast(event->delta()) / 40.0f); + } - _forceRedraw = true; + _forceRedraw = true; } void Q3DWidget::timerEvent(QTimerEvent* event) { - if (event->timerId() == timer.timerId()) - { - if (_forceRedraw) - { - updateGL(); - _forceRedraw = false; - } - } - else - { - QObject::timerEvent(event); - } + if (event->timerId() == timer.timerId()) + { + if (_forceRedraw) + { + updateGL(); + _forceRedraw = false; + } + } + else + { + QObject::timerEvent(event); + } } void diff --git a/src/ui/map3D/Q3DWidget.h b/src/ui/map3D/Q3DWidget.h index e3414c40bb110aa75e352675b46fe1a1c475ca8b..e964b687cbfbf2168f5262574b21d73bf46677cf 100644 --- a/src/ui/map3D/Q3DWidget.h +++ b/src/ui/map3D/Q3DWidget.h @@ -1,3 +1,34 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of the class Q3DWidget. + * + * @author Lionel Heng + * + */ + #ifndef Q3DWIDGET_H_ #define Q3DWIDGET_H_ @@ -50,6 +81,9 @@ typedef void (*KeyboardFunc)(char, void *); typedef void (*MouseFunc)(Qt::MouseButton, MouseState, int32_t, int32_t, void *); typedef void (*MotionFunc)(int32_t, int32_t, void *); +/** + * @brief A base 3D widget which executes OpenGL commands. + **/ class Q3DWidget: public QGLWidget { Q_OBJECT @@ -67,7 +101,6 @@ public: float maxClipRange); void setCameraLimit(bool onoff); - void setCameraLock(bool onoff); void set2DCameraParams(float zoomSensitivity, float rotateSensitivity, @@ -166,7 +199,6 @@ private: bool _forceRedraw; bool allow2DRotation; bool limitCamera; - bool lockCamera; CameraParams cameraParams; diff --git a/src/ui/map3D/QMap3DWidget.cc b/src/ui/map3D/QMap3DWidget.cc index 7cd86df200e0065d5662ee7bc843abc36b0ef406..f255deead1e67f8e924a7b564328034214c3d270 100644 --- a/src/ui/map3D/QMap3DWidget.cc +++ b/src/ui/map3D/QMap3DWidget.cc @@ -1,6 +1,38 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of the class QMap3DWidget. + * + * @author Lionel Heng + * + */ + #include "QMap3DWidget.h" #include +#include #include #include @@ -14,14 +46,20 @@ QMap3DWidget::QMap3DWidget(QWidget* parent) , lastRedrawTime(0.0) , displayGrid(false) , displayTrail(false) - , lockCamera(false) + , lockCamera(true) + , updateLastUnlockedPose(true) + , displayTarget(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); + int32_t argc = 0; + glutInit(&argc, NULL); + setDisplayFunc(display, this); + setMouseFunc(mouse, this); addTimerFunc(100, timer, this); buildLayout(); @@ -58,7 +96,6 @@ QMap3DWidget::buildLayout(void) 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); @@ -106,7 +143,25 @@ QMap3DWidget::displayHandler(void) robotYaw = uas->getYaw(); } - setCameraLock(lockCamera); + if (updateLastUnlockedPose) + { + lastUnlockedPose.x = robotX; + lastUnlockedPose.y = robotY; + lastUnlockedPose.z = robotZ; + + camOffset.x = 0.0f; + camOffset.y = 0.0f; + camOffset.z = 0.0f; + + updateLastUnlockedPose = false; + } + + if (!lockCamera) + { + camOffset.x = robotX - lastUnlockedPose.x; + camOffset.y = robotY - lastUnlockedPose.y; + camOffset.z = robotZ - lastUnlockedPose.z; + } // turn on smooth lines glEnable(GL_LINE_SMOOTH); @@ -118,6 +173,9 @@ QMap3DWidget::displayHandler(void) glClearColor(0.0f, 0.0f, 0.0f, 1.0f); glClear(GL_COLOR_BUFFER_BIT); + glPushMatrix(); + glTranslatef(camOffset.x, camOffset.y, camOffset.z); + // draw Cheetah model drawPlatform(robotRoll, robotPitch, robotYaw); @@ -131,6 +189,13 @@ QMap3DWidget::displayHandler(void) drawTrail(robotX, robotY, robotZ); } + if (displayTarget) + { + drawTarget(robotX, robotY, robotZ); + } + + glPopMatrix(); + // switch to 2D setDisplayMode2D(); @@ -143,7 +208,7 @@ QMap3DWidget::displayHandler(void) glVertex2f(getWindowWidth(), 0.0f); glEnd(); - char buffer[6][255]; + char buffer[7][255]; sprintf(buffer[0], "x = %.2f", robotX); sprintf(buffer[1], "y = %.2f", robotY); @@ -152,12 +217,17 @@ QMap3DWidget::displayHandler(void) sprintf(buffer[4], "p = %.2f", robotPitch); sprintf(buffer[5], "y = %.2f", robotYaw); + std::pair mouseWorldCoords = + getPositionIn3DMode(getMouseX(), getMouseY()); + sprintf(buffer[6], "Cursor [%.2f %.2f]", + mouseWorldCoords.first + robotX, mouseWorldCoords.second + robotY); + 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) + for (int32_t i = 0; i < 7; ++i) { glTranslatef(60.0f, 0.0f, 0.0f); font->Render(buffer[i]); @@ -165,19 +235,25 @@ QMap3DWidget::displayHandler(void) glPopMatrix(); } -/** - * - * @param uas the UAS/MAV to monitor/display with the HUD - */ -void QMap3DWidget::setActiveUAS(UASInterface* uas) +void +QMap3DWidget::mouse(Qt::MouseButton button, MouseState state, + int32_t x, int32_t y, void* clientData) { - if (this->uas != NULL && this->uas != uas) + QMap3DWidget* map3d = reinterpret_cast(clientData); + map3d->mouseHandler(button, state, x, y); +} + +void +QMap3DWidget::mouseHandler(Qt::MouseButton button, MouseState state, + int32_t x, int32_t y) +{ + if (button == Qt::RightButton && state == MOUSE_STATE_DOWN) { - // Disconnect any previously connected active MAV - //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); + QMenu menu(this); + QAction* targetAction = menu.addAction("Mark as Target"); + connect(targetAction, SIGNAL(triggered()), this, SLOT(markTarget())); + menu.exec(mapToGlobal(QPoint(x, y))); } - - this->uas = uas; } void @@ -210,6 +286,45 @@ QMap3DWidget::getTime(void) const static_cast(tv.tv_usec) / 1000000.0; } +/** + * + * @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::markTarget(void) +{ + std::pair mouseWorldCoords = + getPositionIn3DMode(getLastMouseX(), getLastMouseY()); + + float robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f; + if (uas != NULL) + { + robotX = uas->getLocalX(); + robotY = uas->getLocalY(); + robotZ = uas->getLocalZ(); + } + + targetPosition.x = mouseWorldCoords.first + robotX; + targetPosition.y = mouseWorldCoords.second + robotY; + targetPosition.z = robotZ; + + displayTarget = true; + + uas->setTargetPosition(targetPosition.x, targetPosition.y, + targetPosition.z, 0.0f); +} + void QMap3DWidget::showGrid(int32_t state) { @@ -244,6 +359,8 @@ QMap3DWidget::showTrail(int32_t state) void QMap3DWidget::recenterCamera(void) { + updateLastUnlockedPose = true; + recenter(); } @@ -355,3 +472,36 @@ QMap3DWidget::drawTrail(float x, float y, float z) } glEnd(); } + +void +QMap3DWidget::drawTarget(float x, float y, float z) +{ + static double radius = 0.2; + static bool expand = true; + + if (radius < 0.1) + { + expand = true; + } + else if (radius > 0.25) + { + expand = false; + } + + glPushMatrix(); + glTranslatef(targetPosition.x - x, targetPosition.y - y, 0.0f); + glColor3f(0.0f, 0.7f, 1.0f); + glLineWidth(1.0f); + glutWireSphere(radius, 10, 10); + + if (expand) + { + radius += 0.02; + } + else + { + radius -= 0.02; + } + + glPopMatrix(); +} diff --git a/src/ui/map3D/QMap3DWidget.h b/src/ui/map3D/QMap3DWidget.h index 2fccec74d908ffb1e80ddc841e696d3bd1ea9cd4..f171151f67e84ff87c98f056fd06f8021c553de7 100644 --- a/src/ui/map3D/QMap3DWidget.h +++ b/src/ui/map3D/QMap3DWidget.h @@ -1,3 +1,34 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of the class QMap3DWidget. + * + * @author Lionel Heng + * + */ + #ifndef QMAP3DWIDGET_H #define QMAP3DWIDGET_H @@ -7,6 +38,9 @@ class CheetahModel; class FTTextureFont; class UASInterface; +/** + * @brief A 3D View widget which displays vehicle-centric information. + **/ class QMap3DWidget : public Q3DWidget { Q_OBJECT @@ -20,6 +54,11 @@ public: static void display(void* clientData); void displayHandler(void); + static void mouse(Qt::MouseButton button, MouseState state, + int32_t x, int32_t y, void* clientData); + void mouseHandler(Qt::MouseButton button, MouseState state, + int32_t x, int32_t y); + static void timer(void* clientData); void timerHandler(void); @@ -27,6 +66,7 @@ public: public slots: void setActiveUAS(UASInterface* uas); + void markTarget(void); private slots: void showGrid(int state); @@ -41,12 +81,12 @@ private: void drawPlatform(float roll, float pitch, float yaw); void drawGrid(void); void drawTrail(float x, float y, float z); + void drawTarget(float x, float y, float z); double lastRedrawTime; bool displayGrid; bool displayTrail; - bool lockCamera; typedef struct { @@ -54,8 +94,17 @@ private: float y; float z; } Pose3D; + + bool lockCamera; + Pose3D lastUnlockedPose; + bool updateLastUnlockedPose; + Pose3D camOffset; + QVarLengthArray trail; + bool displayTarget; + Pose3D targetPosition; + QScopedPointer cheetahModel; QScopedPointer font; };