Commit 5a0a9297 authored by Bryant Mairs's avatar Bryant Mairs

Merge remote-tracking branch 'remotes/upstream/master'

parents 6dbf6dab 1592061b
......@@ -246,11 +246,11 @@ message("Compiling for linux 32")
-losgGA \
-losgDB \
-losgText \
-losgQt \
-lOpenThreads
#-losgQt \
DEFINES += QGC_OSG_ENABLED
DEFINES += QGC_OSG_QT_ENABLED
}
exists(/usr/local/include/google/protobuf) {
......@@ -330,11 +330,11 @@ linux-g++-64 {
-losgGA \
-losgDB \
-losgText \
-losgQt \
-lOpenThreads
# -losgQt \
DEFINES += QGC_OSG_ENABLED
DEFINES += QGC_OSG_QT_ENABLED
}
exists(/usr/local/include/google/protobuf) {
......
......@@ -387,7 +387,8 @@ contains(DEPENDENCIES_PRESENT, protobuf):contains(MAVLINK_CONF, pixhawk) {
message("Including headers for Protocol Buffers")
# Enable only if protobuf is available
HEADERS += ../mavlink/include/pixhawk/pixhawk.pb.h
HEADERS += ../mavlink/include/pixhawk/pixhawk.pb.h \
src/ui/map3D/ObstacleGroupNode.h
}
contains(DEPENDENCIES_PRESENT, libfreenect) {
message("Including headers for libfreenect")
......@@ -527,7 +528,8 @@ contains(DEPENDENCIES_PRESENT, protobuf):contains(MAVLINK_CONF, pixhawk) {
message("Including sources for Protocol Buffers")
# Enable only if protobuf is available
SOURCES += ../mavlink/src/pixhawk/pixhawk.pb.cc
SOURCES += ../mavlink/src/pixhawk/pixhawk.pb.cc \
src/ui/map3D/ObstacleGroupNode.cc
}
contains(DEPENDENCIES_PRESENT, libfreenect) {
message("Including sources for libfreenect")
......
......@@ -993,6 +993,11 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
rgbdImage.CopyFrom(*message);
emit rgbdImageChanged(this);
}
else if (message->GetTypeName() == obstacleList.GetTypeName())
{
obstacleList.CopyFrom(*message);
emit obstacleListChanged(this);
}
}
#endif
......@@ -1906,25 +1911,6 @@ void UAS::executeCommand(MAV_CMD command)
sendMessage(msg);
}
void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component)
{
mavlink_message_t msg;
mavlink_command_long_t cmd;
cmd.command = (uint8_t)command;
cmd.confirmation = confirmation;
cmd.param1 = param1;
cmd.param2 = param2;
cmd.param3 = param3;
cmd.param4 = param4;
cmd.param5 = 0.0f;
cmd.param6 = 0.0f;
cmd.param7 = 0.0f;
cmd.target_system = uasId;
cmd.target_component = component;
mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
sendMessage(msg);
}
void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component)
{
mavlink_message_t msg;
......@@ -2191,7 +2177,7 @@ void UAS::shutdown()
void UAS::setTargetPosition(float x, float y, float z, float yaw)
{
mavlink_message_t msg;
mavlink_msg_set_local_position_setpoint_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_FRAME_LOCAL_NED, x, y, z, yaw);
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 2, 3, 0, yaw, x, y, z);
sendMessage(msg);
}
......
......@@ -142,6 +142,10 @@ public:
px::RGBDImage getRGBDImage() const {
return rgbdImage;
}
px::ObstacleList getObstacleList() const {
return obstacleList;
}
#endif
friend class UASWaypointManager;
......@@ -230,6 +234,7 @@ protected: //COMMENTS FOR TEST UNIT
#ifdef QGC_PROTOBUF_ENABLED
px::PointCloudXYZRGB pointCloud;
px::RGBDImage rgbdImage;
px::ObstacleList obstacleList;
#endif
QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters
......@@ -406,8 +411,6 @@ public slots:
void setUASName(const QString& name);
/** @brief Executes a command **/
void executeCommand(MAV_CMD command);
/** @brief Executes a command **/
void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component);
/** @brief Executes a command with 7 params */
void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component);
/** @brief Set the current battery type and voltages */
......@@ -563,10 +566,14 @@ signals:
void imageStarted(quint64 timestamp);
/** @brief A new camera image has arrived */
void imageReady(UASInterface* uas);
#ifdef QGC_PROTOBUF_ENABLED
/** @brief Point cloud data has been changed */
void pointCloudChanged(UASInterface* uas);
/** @brief RGBD image data has been changed */
void rgbdImageChanged(UASInterface* uas);
/** @brief Obstacle list data has been changed */
void obstacleListChanged(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);
......
......@@ -46,7 +46,7 @@ This file is part of the QGROUNDCONTROL project
#ifdef QGC_PROTOBUF_ENABLED
#include <tr1/memory>
#include <pixhawk.pb.h>
#include <pixhawk/pixhawk.pb.h>
#endif
/**
......@@ -97,6 +97,7 @@ public:
#ifdef QGC_PROTOBUF_ENABLED
virtual px::PointCloudXYZRGB getPointCloud() const = 0;
virtual px::RGBDImage getRGBDImage() const = 0;
virtual px::ObstacleList getObstacleList() const = 0;
#endif
virtual bool isArmed() const = 0;
......@@ -214,7 +215,7 @@ public slots:
/** @brief Execute command immediately **/
virtual void executeCommand(MAV_CMD command) = 0;
/** @brief Executes a command **/
virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component) = 0;
virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) = 0;
/** @brief Selects the airframe */
virtual void setAirframe(int airframe) = 0;
......
......@@ -1431,10 +1431,13 @@ void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int s
void HUD::copyImage()
{
if (isVisible())
{
qDebug() << "HUD::copyImage()";
UAS* u = dynamic_cast<UAS*>(this->uas);
if (u)
{
this->glImage = QGLWidget::convertToGLFormat(u->getImage());
}
}
}
......@@ -27,8 +27,9 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
messageFilter.insert(MAVLINK_MSG_ID_MISSION_ITEM, false);
messageFilter.insert(MAVLINK_MSG_ID_MISSION_COUNT, false);
messageFilter.insert(MAVLINK_MSG_ID_MISSION_ACK, false);
#ifdef MAVLINK_ENABLED_PIXHAWK
messageFilter.insert(MAVLINK_MSG_ID_DATA_STREAM, false);
#ifdef MAVLINK_ENABLED_PIXHAWK
messageFilter.insert(MAVLINK_MSG_ID_ENCAPSULATED_DATA, false);
messageFilter.insert(MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, false);
#endif
messageFilter.insert(MAVLINK_MSG_ID_EXTENDED_MESSAGE, false);
......
......@@ -17,9 +17,23 @@ public:
protected:
void changeEvent(QEvent *e);
void showEvent(QShowEvent* event)
{
QWidget::showEvent(event);
emit visibilityChanged(true);
}
void hideEvent(QHideEvent* event)
{
QWidget::hideEvent(event);
emit visibilityChanged(false);
}
private:
Ui::QGCFirmwareUpdate *ui;
signals:
void visibilityChanged(bool visible);
};
#endif // QGCFIRMWAREUPDATE_H
......@@ -392,6 +392,6 @@ void QGCToolBar::receiveTextMessage(int uasid, int componentid, int severity, QS
QGCToolBar::~QGCToolBar()
{
delete toggleLoggingAction;
delete logReplayAction;
if (toggleLoggingAction) toggleLoggingAction->deleteLater();
if (logReplayAction) logReplayAction->deleteLater();
}
......@@ -79,7 +79,6 @@ QGCCommandButton::~QGCCommandButton()
void QGCCommandButton::sendCommand()
{
if (QGCToolWidgetItem::uas) {
// FIXME
int index = ui->editCommandComboBox->itemData(ui->editCommandComboBox->currentIndex()).toInt();
MAV_CMD command = static_cast<MAV_CMD>(index);
int confirm = (ui->editConfirmationCheckBox->isChecked()) ? 1 : 0;
......@@ -87,9 +86,12 @@ void QGCCommandButton::sendCommand()
float param2 = ui->editParam2SpinBox->value();
float param3 = ui->editParam3SpinBox->value();
float param4 = ui->editParam4SpinBox->value();
float param5 = ui->editParam5SpinBox->value();
float param6 = ui->editParam6SpinBox->value();
float param7 = ui->editParam7SpinBox->value();
int component = ui->editComponentSpinBox->value();
QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, component);
QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, param5, param6, param7, component);
qDebug() << __FILE__ << __LINE__ << "SENDING COMMAND" << index;
} else {
qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING";
......
......@@ -284,6 +284,68 @@ void QGCMapWidget::updateGlobalPosition()
}
}
void QGCMapWidget::updateLocalPosition()
{
QList<UASInterface*> systems = UASManager::instance()->getUASList();
foreach (UASInterface* system, systems)
{
// Get reference to graphic UAV item
mapcontrol::UAVItem* uav = GetUAV(system->getUASID());
// Check if reference is valid, else create a new one
if (uav == NULL)
{
MAV2DIcon* newUAV = new MAV2DIcon(map, this, system);
AddUAV(system->getUASID(), newUAV);
uav = newUAV;
uav->SetTrailTime(1);
uav->SetTrailDistance(5);
uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
}
// Set new lat/lon position of UAV icon
double latitude = UASManager::instance()->getHomeLatitude();
double longitude = UASManager::instance()->getHomeLongitude();
double altitude = UASManager::instance()->getHomeAltitude();
internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
uav->SetUAVPos(pos_lat_lon, system->getAltitude());
// Follow status
if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
// Convert from radians to degrees and apply
uav->SetUAVHeading((system->getYaw()/M_PI)*180.0f);
}
}
void QGCMapWidget::updateLocalPositionEstimates()
{
QList<UASInterface*> systems = UASManager::instance()->getUASList();
foreach (UASInterface* system, systems)
{
// Get reference to graphic UAV item
mapcontrol::UAVItem* uav = GetUAV(system->getUASID());
// Check if reference is valid, else create a new one
if (uav == NULL)
{
MAV2DIcon* newUAV = new MAV2DIcon(map, this, system);
AddUAV(system->getUASID(), newUAV);
uav = newUAV;
uav->SetTrailTime(1);
uav->SetTrailDistance(5);
uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
}
// Set new lat/lon position of UAV icon
double latitude = UASManager::instance()->getHomeLatitude();
double longitude = UASManager::instance()->getHomeLongitude();
double altitude = UASManager::instance()->getHomeAltitude();
internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
uav->SetUAVPos(pos_lat_lon, system->getAltitude());
// Follow status
if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
// Convert from radians to degrees and apply
uav->SetUAVHeading((system->getYaw()/M_PI)*180.0f);
}
}
void QGCMapWidget::updateSystemSpecs(int uas)
{
......
......@@ -46,6 +46,10 @@ public slots:
void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec);
/** @brief Update the global position of all systems */
void updateGlobalPosition();
/** @brief Update the local position and draw it converted to GPS reference */
void updateLocalPosition();
/** @brief Update the local position estimates (individual sensors) and draw it converted to GPS reference */
void updateLocalPositionEstimates();
/** @brief Update the type, size, etc. of this system */
void updateSystemSpecs(int uas);
/** @brief Change current system in focus / editing */
......
......@@ -40,7 +40,7 @@ HUDScaleGeode::HUDScaleGeode()
}
void
HUDScaleGeode::init(void)
HUDScaleGeode::init(osg::ref_ptr<osgText::Font>& font)
{
osg::ref_ptr<osg::Vec2Array> outlineVertices(new osg::Vec2Array);
outlineVertices->push_back(osg::Vec2(20.0f, 50.0f));
......@@ -96,7 +96,7 @@ HUDScaleGeode::init(void)
text = new osgText::Text;
text->setCharacterSize(11);
text->setFont("images/Vera.ttf");
text->setFont(font);
text->setAxisAlignment(osgText::Text::SCREEN);
text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
text->setPosition(osg::Vec3(40.0f, 45.0f, -1.5f));
......
......@@ -41,7 +41,7 @@ class HUDScaleGeode : public osg::Geode
public:
HUDScaleGeode();
void init(void);
void init(osg::ref_ptr<osgText::Font>& font);
void update(int windowHeight, float cameraFov, float cameraDistance,
bool darkBackground);
......
......@@ -31,11 +31,17 @@
#include "ImageWindowGeode.h"
ImageWindowGeode::ImageWindowGeode(const QString& caption,
const osg::Vec4& backgroundColor,
osg::ref_ptr<osg::Image>& image)
ImageWindowGeode::ImageWindowGeode()
: border(5)
{
}
void
ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor,
osg::ref_ptr<osg::Image>& image,
osg::ref_ptr<osgText::Font>& font)
{
// image
osg::ref_ptr<osg::Geometry> imageGeometry = new osg::Geometry;
imageVertices = new osg::Vec3Array(4);
......@@ -82,7 +88,7 @@ ImageWindowGeode::ImageWindowGeode(const QString& caption,
text = new osgText::Text;
text->setText(caption.toStdString().c_str());
text->setCharacterSize(11);
text->setFont("images/Vera.ttf");
text->setFont(font);
text->setAxisAlignment(osgText::Text::SCREEN);
text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
......
......@@ -40,8 +40,10 @@
class ImageWindowGeode : public osg::Geode
{
public:
ImageWindowGeode(const QString& caption, const osg::Vec4& backgroundColor,
osg::ref_ptr<osg::Image>& image);
ImageWindowGeode();
void init(const QString& caption, const osg::Vec4& backgroundColor,
osg::ref_ptr<osg::Image>& image,
osg::ref_ptr<osgText::Font>& font);
void setAttributes(int x, int y, int width, int height);
......
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class ObstacleGroupNode.
*
* @author Lionel Heng <hengli@inf.ethz.ch>
*
*/
#include "ObstacleGroupNode.h"
#include <osg/PositionAttitudeTransform>
#include <osg/ShapeDrawable>
#include "Imagery.h"
ObstacleGroupNode::ObstacleGroupNode()
{
}
void
ObstacleGroupNode::init(void)
{
}
void
ObstacleGroupNode::update(MAV_FRAME frame, UASInterface *uas)
{
if (!uas || frame == MAV_FRAME_GLOBAL)
{
return;
}
double robotX = uas->getLocalX();
double robotY = uas->getLocalY();
double robotZ = uas->getLocalZ();
if (getNumChildren() > 0)
{
removeChild(0, getNumChildren());
}
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
px::ObstacleList obstacleList = uas->getObstacleList();
for (int i = 0; i < obstacleList.obstacles_size(); ++i)
{
const px::Obstacle& obs = obstacleList.obstacles(i);
osg::Vec3d obsPos(obs.y() - robotY, obs.x() - robotX, robotZ - obs.z());
osg::ref_ptr<osg::Box> box =
new osg::Box(obsPos, obs.width(), obs.width(), obs.height());
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(box);
sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
sd->setColor(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f));
geode->addDrawable(sd);
}
addChild(geode);
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class ObstacleGroupNode.
*
* @author Lionel Heng <hengli@inf.ethz.ch>
*
*/
#ifndef OBSTACLEGROUPNODE_H
#define OBSTACLEGROUPNODE_H
#include <osg/Group>
#include "UASInterface.h"
class ObstacleGroupNode : public osg::Group
{
public:
ObstacleGroupNode();
void init(void);
void update(MAV_FRAME frame, UASInterface* uas);
};
#endif // OBSTACLEGROUPNODE_H
This diff is collapsed.
......@@ -25,7 +25,7 @@
* @file
* @brief Definition of the class Pixhawk3DWidget.
*
* @author Lionel Heng <hengli@student.ethz.ch>
* @author Lionel Heng <hengli@inf.ethz.ch>
*
*/
......@@ -38,6 +38,9 @@
#include "Imagery.h"
#include "ImageWindowGeode.h"
#include "WaypointGroupNode.h"
#ifdef QGC_PROTOBUF_ENABLED
#include "ObstacleGroupNode.h"
#endif
#include "Q3DWidget.h"
......@@ -67,10 +70,12 @@ private slots:
void recenter(void);
void toggleFollowCamera(int state);
void selectTargetHeading(void);
void selectTarget(void);
void setTarget(void);
void insertWaypoint(void);
void moveWaypoint(void);
void setWaypoint(void);
void moveWaypointPosition(void);
void moveWaypointHeading(void);
void deleteWaypoint(void);
void setWaypointAltitude(void);
void clearAllWaypoints(void);
......@@ -78,12 +83,28 @@ private slots:
protected:
QVector< osg::ref_ptr<osg::Node> > findVehicleModels(void);
void buildLayout(void);
virtual void resizeGL(int width, int height);
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 mouseMoveEvent(QMouseEvent* event);
UASInterface* uas;
signals:
void visibilityChanged(bool visible);
private:
void getPose(double& x, double& y, double& z,
double& roll, double& pitch, double& yaw,
......@@ -113,16 +134,19 @@ private:
void updateTarget(double robotX, double robotY);
#ifdef QGC_PROTOBUF_ENABLED
void updateRGBD(double robotX, double robotY, double robotZ);
void updateObstacles(void);
#endif
int findWaypoint(int mouseX, int mouseY);
int findWaypoint(const QPoint& mousePos);
bool findTarget(int mouseX, int mouseY);
void showInsertWaypointMenu(const QPoint& cursorPos);
void showEditWaypointMenu(const QPoint& cursorPos);
enum Mode {
DEFAULT_MODE,
MOVE_WAYPOINT_MODE
MOVE_WAYPOINT_POSITION_MODE,
MOVE_WAYPOINT_HEADING_MODE,
SELECT_TARGET_HEADING_MODE
};
Mode mode;
int selectedWpIndex;
......@@ -133,6 +157,7 @@ private:
bool displayWaypoints;
bool displayRGBD2D;
bool displayRGBD3D;
bool displayObstacleList;
bool enableRGBDColor;
bool enableTarget;
......@@ -157,11 +182,15 @@ private:
osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
osg::ref_ptr<osg::Node> targetNode;
osg::ref_ptr<osg::Geode> rgbd3DNode;
#ifdef QGC_PROTOBUF_ENABLED
osg::ref_ptr<ObstacleGroupNode> obstacleGroupNode;
#endif
QVector< osg::ref_ptr<osg::Node> > vehicleModels;
MAV_FRAME frame;
osg::Vec2d target;
osg::Vec3d target;
QPoint cachedMousePos;
double lastRobotX, lastRobotY, lastRobotZ;
};
......
......@@ -35,6 +35,9 @@ This file is part of the QGROUNDCONTROL project
#include <osg/Geometry>
#include <osg/LineWidth>
#include <osg/MatrixTransform>
#ifdef QGC_OSG_QT_ENABLED
#include <osgQt/QFontImplementation>
#endif
#ifdef Q_OS_MACX
#include <Carbon/Carbon.h>
#endif
......@@ -56,12 +59,21 @@ Q3DWidget::Q3DWidget(QWidget* parent)
cameraParams.cameraFov = 30.0f;
cameraParams.minClipRange = 1.0f;
cameraParams.maxClipRange = 10000.0f;
#ifdef QGC_OSG_QT_ENABLED
osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
fontImpl = new osgQt::QFontImplementation(QFont(":/general/vera.ttf"));
#else
osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
fontImpl = 0;//new osgText::Font::Font("images/Vera.ttf");
#endif
font = new osgText::Font(fontImpl);
osgGW = new osgViewer::GraphicsWindowEmbedded(0, 0, width(), height());
setThreadingModel(osgViewer::Viewer::SingleThreaded);
setFocusPolicy(Qt::StrongFocus);
setMouseTracking(true);
}
Q3DWidget::~Q3DWidget()
......
......@@ -38,6 +38,7 @@ This file is part of the QGROUNDCONTROL project
#include <osg/LineSegment>
#include <osg/PositionAttitudeTransform>
#include <osgGA/TrackballManipulator>
#include <osgText/Font>
#include <osgViewer/Viewer>
#include "GCManipulator.h"
......@@ -259,6 +260,8 @@ protected:
CameraParams cameraParams; /**< Struct representing camera parameters. */
float fps;
osg::ref_ptr<osgText::Font> font;
};
#endif // Q3DWIDGET_H
......@@ -25,7 +25,7 @@ This file is part of the QGROUNDCONTROL project
* @file
* @brief Definition of the class WaypointGroupNode.
*
* @author Lionel Heng <hengli@student.ethz.ch>
* @author Lionel Heng <hengli@inf.ethz.ch>
*
*/
......@@ -51,11 +51,16 @@ WaypointGroupNode::init(void)
void
WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
{
if (uas) {
if (!uas)
{
return;
}
double robotX = 0.0;
double robotY = 0.0;
double robotZ = 0.0;
if (frame == MAV_FRAME_GLOBAL) {
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
......@@ -63,25 +68,63 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
robotZ = -altitude;
} else if (frame == MAV_FRAME_LOCAL_NED) {
}
else if (frame == MAV_FRAME_LOCAL_NED)
{
robotX = uas->getLocalX();
robotY = uas->getLocalY();
robotZ = uas->getLocalZ();
}
if (getNumChildren() > 0) {
if (getNumChildren() > 0)
{
removeChild(0, getNumChildren());
}
const QVector<Waypoint *>& list = uas->getWaypointManager()->getWaypointEditableList();
for (int i = 0; i < list.size(); i++) {
for (int i = 0; i < list.size(); i++)
{
Waypoint* wp = list.at(i);
double wpX, wpY, wpZ;
getPosition(wp, wpX, wpY, wpZ);
double wpYaw = osg::DegreesToRadians(wp->getYaw());
osg::ref_ptr<osg::Group> group = new osg::Group;
// cone indicates waypoint orientation
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
double coneRadius = wp->getAcceptanceRadius() / 2.0;
osg::ref_ptr<osg::Cone> cone =
new osg::Cone(osg::Vec3d(wpZ, 0.0, 0.0),
coneRadius, wp->getAcceptanceRadius() * 2.0);
sd->setShape(cone);
sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
if (wp->getCurrent())
{
sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
}
else
{
sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
}
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
geode->addDrawable(sd);
osg::ref_ptr<osg::PositionAttitudeTransform> pat =
new osg::PositionAttitudeTransform;
pat->addChild(geode);
pat->setAttitude(osg::Quat(wpYaw - 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<osg::Cylinder> cylinder =
new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
wp->getAcceptanceRadius(),
......@@ -90,20 +133,25 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
sd->setShape(cylinder);
sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
if (wp->getCurrent()) {
if (wp->getCurrent())
{
sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
} else {
}
else
{
sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
}
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
geode = new osg::Geode;
geode->addDrawable(sd);
group->addChild(geode);
char wpLabel[10];
sprintf(wpLabel, "wp%d", i);
geode->setName(wpLabel);
group->setName(wpLabel);
if (i < list.size() - 1) {
if (i < list.size() - 1)
{
double nextWpX, nextWpY, nextWpZ;
getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
......@@ -130,23 +178,21 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
geode->addDrawable(geometry);
}
osg::ref_ptr<osg::PositionAttitudeTransform> pat =
new osg::PositionAttitudeTransform;
pat = new osg::PositionAttitudeTransform;
pat->setPosition(osg::Vec3d(wpY - robotY,
wpX - robotX,
robotZ));
addChild(pat);
pat->addChild(geode);
}
pat->addChild(group);
}
}
void
WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
{
if (wp->getFrame() == MAV_FRAME_GLOBAL) {
if (wp->getFrame() == MAV_FRAME_GLOBAL)
{
double latitude = wp->getY();
double longitude = wp->getX();
double altitude = wp->getZ();
......@@ -154,7 +200,9 @@ WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
z = -altitude;
} else if (wp->getFrame() == MAV_FRAME_LOCAL_NED) {
}
else if (wp->getFrame() == MAV_FRAME_LOCAL_NED)
{
x = wp->getX();
y = wp->getY();
z = wp->getZ();
......
......@@ -25,7 +25,7 @@ This file is part of the QGROUNDCONTROL project
* @file
* @brief Definition of the class WaypointGroupNode.
*
* @author Lionel Heng <hengli@student.ethz.ch>
* @author Lionel Heng <hengli@inf.ethz.ch>
*
*/
......
......@@ -42,6 +42,18 @@ public:
registerType(msg);
}
// register ObstacleList
{
std::tr1::shared_ptr<px::ObstacleList> msg(new px::ObstacleList);
registerType(msg);
}
// register ObstacleMap
{
std::tr1::shared_ptr<px::ObstacleMap> msg(new px::ObstacleMap);
registerType(msg);
}
srand(time(NULL));
mStreamID = rand() + 1;
}
......@@ -186,6 +198,11 @@ public:
if (offset == 0)
{
queue.push_back(msg);
if ((flags & 0x1) != 0x1)
{
reassemble = true;
}
}
else
{
......
......@@ -15,10 +15,21 @@
#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
#if (defined __GNUC__) || (defined _MSC_VER) || (defined __MINGW32__) || (defined __WATCOMC__) || (defined __CMB__) || (defined __BORLANDC__) || (defined __clang__)
#define MAVLINK_EXTENDED_MESSAGES_ENABLED
#endif
// EXTENDED message definition - the extended message set is compatible to standard MAVLink message passing
// but does NOT have to be supported by the platform. The extended message set will NOT consume
// any memory if the messages are not explicitely used
#ifdef MAVLINK_EXTENDED_MESSAGES_ENABLED
#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
#define MAVLINK_EXTENDED_HEADER_LEN 14
#define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507
#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
#endif
typedef struct param_union {
union {
......@@ -51,12 +62,13 @@ typedef struct __mavlink_message {
uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
} mavlink_message_t;
#ifdef MAVLINK_EXTENDED_MESSAGES_ENABLED
typedef struct __mavlink_extended_message {
mavlink_message_t base_msg;
int32_t extended_payload_len; ///< Length of extended payload if any
uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
} mavlink_extended_message_t;
#endif
typedef enum {
......
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