Skip to content

Q
qgroundcontrol
Commit 0f18c7c5 authored by pixhawk's avatar pixhawk
Browse files
Options

Merged

parents e4ea6b31 788ba064
Show whitespace changes
Inline Side-by-side
Showing with 406 additions and 103 deletions
src/comm/MAVLinkSimulationLink.cc
View file @ 0f18c7c5
......@@ -99,12 +99,6 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
// Open packet log
mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName());
mavlinkLogFile->open(QIODevice::ReadOnly);
// position at Pixhawk lab @ ETHZ
x = 5247273.0f;
y = 465955.0f;
z = -0.2f;
yaw = 0;
}
MAVLinkSimulationLink::~MAVLinkSimulationLink()
......@@ -383,15 +377,14 @@ void MAVLinkSimulationLink::mainloop()
x = x*0.93f + 0.07f*(x+sin(static_cast<float>(QGC::groundTimeUsecs()) * 0.08f));
y = y*0.93f + 0.07f*(y+sin(static_cast<float>(QGC::groundTimeUsecs()) * 0.5f));
z = z*0.93f + 0.07f*(z+sin(static_cast<float>(QGC::groundTimeUsecs()*0.001f)) * 0.1f);
x = 5247273.0f;
y = 465955.0f;
// x = (x > 5.0f) ? 5.0f : x;
// y = (y > 5.0f) ? 5.0f : y;
// z = (z > 3.0f) ? 3.0f : z;
//
// x = (x < -5.0f) ? -5.0f : x;
// y = (y < -5.0f) ? -5.0f : y;
// z = (z < -3.0f) ? -3.0f : z;
x = (x > 5.0f) ? 5.0f : x;
y = (y > 5.0f) ? 5.0f : y;
z = (z > 3.0f) ? 3.0f : z;
x = (x < -5.0f) ? -5.0f : x;
y = (y < -5.0f) ? -5.0f : y;
z = (z < -3.0f) ? -3.0f : z;
// Send back new setpoint
mavlink_message_t ret;
......@@ -409,14 +402,14 @@ void MAVLinkSimulationLink::mainloop()
streampointer += bufferlength;
// GPS RAW
mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.001), 8.548103+(y*0.001), z, 0, 0, 2.5f, 0.1f);
mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.00001), 8.548103+(y*0.00001), z, 0, 0, 2.5f, 0.1f);
bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength;
// GLOBAL POSITION
mavlink_msg_global_position_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.001), 8.548103+(y*0.001), z, 0, 0);
mavlink_msg_global_position_pack(systemId, componentId, &ret, 0, 47.378028137103+(x*0.00001), 8.54899892510421+(y*0.00001), z, 0, 0, 0);
bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
......
src/input/Freenect.cc
View file @ 0f18c7c5
/*=====================================================================
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 Freenect.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#include "Freenect.h"
#include <cmath>
......
src/input/Freenect.h
View file @ 0f18c7c5
/*=====================================================================
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 Freenect.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#ifndef FREENECT_H
#define FREENECT_H
......
src/uas/UAS.cc
View file @ 0f18c7c5
......@@ -73,15 +73,15 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
sendDropRate(0),
lowBattAlarm(false),
positionLock(false),
localX(0),
localY(0),
localZ(0),
latitude(0),
longitude(0),
altitude(0),
roll(0),
pitch(0),
yaw(0),
localX(0.0),
localY(0.0),
localZ(0.0),
latitude(0.0),
longitude(0.0),
altitude(0.0),
roll(0.0),
pitch(0.0),
yaw(0.0),
statusTimeout(new QTimer(this))
{
color = UASInterface::getNextColor();
......@@ -345,6 +345,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
mavlink_global_position_t pos;
mavlink_msg_global_position_decode(&message, &pos);
quint64 time = getUnixTime(pos.usec);
latitude = pos.lat;
longitude = pos.lon;
altitude = pos.alt;
emit valueChanged(uasId, "lat", pos.lat, time);
emit valueChanged(uasId, "lon", pos.lon, time);
emit valueChanged(uasId, "alt", pos.alt, time);
......
src/uas/UAS.h
View file @ 0f18c7c5
......@@ -82,9 +82,9 @@ public:
double getLocalX() const { return localX; };
double getLocalY() const { return localY; };
double getLocalZ() const { return localZ; };
double getLat() const { return latitude; };
double getLon() const { return longitude; };
double getAlt() const { return altitude; };
double getLatitude() const { return latitude; };
double getLongitude() const { return longitude; };
double getAltitude() const { return altitude; };
double getRoll() const { return roll; };
double getPitch() const { return pitch; };
......
src/uas/UASInterface.h
View file @ 0f18c7c5
......@@ -70,9 +70,9 @@ public:
virtual double getLocalY() const = 0;
virtual double getLocalZ() const = 0;
virtual double getLat() const = 0;
virtual double getLon() const = 0;
virtual double getAlt() const = 0;
virtual double getLatitude() const = 0;
virtual double getLongitude() const = 0;
virtual double getAltitude() const = 0;
virtual double getRoll() const = 0;
virtual double getPitch() const = 0;
......
src/ui/map3D/Imagery.cc
View file @ 0f18c7c5
......@@ -118,6 +118,7 @@ Imagery::prefetch2D(double windowWidth, double windowHeight,
void
Imagery::draw2D(double windowWidth, double windowHeight,
double zoom, double xOrigin, double yOrigin,
double xOffset, double yOffset, double zOffset,
const QString& utmZone)
{
if (getNumDrawables() > 0)
......@@ -171,10 +172,11 @@ Imagery::draw2D(double windowWidth, double windowHeight,
TexturePtr t = textureCache->get(tileURL);
if (!t.isNull())
{
addDrawable(t->draw(y1, x1,
y2, x2,
y3, x3,
y4, x4,
addDrawable(t->draw(y1 - yOffset, x1 - xOffset,
y2 - yOffset, x2 - xOffset,
y3 - yOffset, x3 - xOffset,
y4 - yOffset, x4 - xOffset,
zOffset,
true));
}
}
......@@ -213,6 +215,7 @@ Imagery::prefetch3D(double radius, double tileResolution,
void
Imagery::draw3D(double radius, double tileResolution,
double xOrigin, double yOrigin,
double xOffset, double yOffset, double zOffset,
const QString& utmZone)
{
if (getNumDrawables() > 0)
......@@ -246,10 +249,11 @@ Imagery::draw3D(double radius, double tileResolution,
if (!t.isNull())
{
addDrawable(t->draw(y1, x1,
y2, x2,
y3, x3,
y4, x4,
addDrawable(t->draw(y1 - yOffset, x1 - xOffset,
y2 - yOffset, x2 - xOffset,
y3 - yOffset, x3 - xOffset,
y4 - yOffset, x4 - xOffset,
zOffset,
true));
}
}
......@@ -397,7 +401,7 @@ Imagery::UTMtoTile(double northing, double easting, const QString& utmZone,
}
QChar
Imagery::UTMLetterDesignator(double latitude) const
Imagery::UTMLetterDesignator(double latitude)
{
// This routine determines the correct UTM letter designator for the given latitude
// returns 'Z' if latitude is outside the UTM limits of 84N to 80S
......@@ -432,7 +436,7 @@ Imagery::UTMLetterDesignator(double latitude) const
void
Imagery::LLtoUTM(double latitude, double longitude,
double& utmNorthing, double& utmEasting,
QString& utmZone) const
QString& utmZone)
{
// converts lat/long to UTM coords. Equations from USGS Bulletin 1532
// East Longitudes are positive, West longitudes are negative.
......@@ -513,7 +517,7 @@ Imagery::LLtoUTM(double latitude, double longitude,
void
Imagery::UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
double& latitude, double& longitude) const
double& latitude, double& longitude)
{
// converts UTM coords to lat/long. Equations from USGS Bulletin 1532
// East Longitudes are positive, West longitudes are negative.
......
src/ui/map3D/Imagery.h
View file @ 0f18c7c5
......@@ -60,6 +60,7 @@ public:
const QString& utmZone);
void draw2D(double windowWidth, double windowHeight,
double zoom, double xOrigin, double yOrigin,
double xOffset, double yOffset, double zOffset,
const QString& utmZone);
void prefetch3D(double radius, double tileResolution,
......@@ -67,10 +68,17 @@ public:
const QString& utmZone);
void draw3D(double radius, double tileResolution,
double xOrigin, double yOrigin,
double xOffset, double yOffset, double zOffset,
const QString& utmZone);
bool update(void);
static void LLtoUTM(double latitude, double longitude,
double& utmNorthing, double& utmEasting,
QString& utmZone);
static void UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
double& latitude, double& longitude);
private:
void imageBounds(int tileX, int tileY, double tileResolution,
double& x1, double& y1, double& x2, double& y2,
......@@ -90,13 +98,7 @@ private:
void UTMtoTile(double northing, double easting, const QString& utmZone,
double tileResolution, int& tileX, int& tileY,
int& zoomLevel) const;
QChar UTMLetterDesignator(double latitude) const;
void LLtoUTM(double latitude, double longitude,
double& utmNorthing, double& utmEasting,
QString& utmZone) const;
void UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
double& latitude, double& longitude) const;
static QChar UTMLetterDesignator(double latitude);
QString getTileLocation(int tileX, int tileY, int zoomLevel,
double tileResolution) const;
......
src/ui/map3D/Pixhawk3DWidget.cc
View file @ 0f18c7c5
......@@ -47,6 +47,8 @@
Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
: Q3DWidget(parent)
, uas(NULL)
, mode(DEFAULT_MODE)
, selectedWpIndex(-1)
, displayGrid(true)
, displayTrail(false)
, displayImagery(true)
......@@ -78,7 +80,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
// generate map model
mapNode = createMap();
allocentricMap->addChild(mapNode);
rollingMap->addChild(mapNode);
// generate target model
allocentricMap->addChild(createTarget());
......@@ -203,9 +205,13 @@ Pixhawk3DWidget::recenter(void)
double robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f;
if (uas != NULL)
{
robotX = uas->getLocalX();
robotY = uas->getLocalY();
robotZ = uas->getLocalZ();
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
robotZ = -altitude;
}
recenterCamera(robotY, robotX, -robotZ);
......@@ -224,6 +230,88 @@ Pixhawk3DWidget::toggleFollowCamera(int32_t state)
}
}
void
Pixhawk3DWidget::insertWaypoint(void)
{
if (uas)
{
double altitude = uas->getAltitude();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
Waypoint* wp = new Waypoint(0,
cursorWorldCoords.first,
cursorWorldCoords.second,
-altitude);
uas->getWaypointManager().addWaypoint(wp);
}
}
void
Pixhawk3DWidget::moveWaypoint(void)
{
mode = MOVE_WAYPOINT_MODE;
}
void
Pixhawk3DWidget::setWaypoint(void)
{
if (uas)
{
double altitude = uas->getAltitude();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
const QVector<Waypoint *> waypoints =
uas->getWaypointManager().getWaypointList();
Waypoint* waypoint = waypoints.at(selectedWpIndex);
waypoint->setX(cursorWorldCoords.first);
waypoint->setY(cursorWorldCoords.second);
waypoint->setZ(-altitude);
}
}
void
Pixhawk3DWidget::deleteWaypoint(void)
{
if (uas)
{
uas->getWaypointManager().removeWaypoint(selectedWpIndex);
}
}
void
Pixhawk3DWidget::setWaypointAltitude(void)
{
if (uas)
{
const QVector<Waypoint *> waypoints =
uas->getWaypointManager().getWaypointList();
// waypoints.at(selectedWpIndex)->setZ(0.0);
}
}
void
Pixhawk3DWidget::clearAllWaypoints(void)
{
if (uas)
{
double altitude = uas->getAltitude();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
const QVector<Waypoint *> waypoints =
uas->getWaypointManager().getWaypointList();
for (int i = waypoints.size() - 1; i >= 0; --i)
{
uas->getWaypointManager().removeWaypoint(i);
}
}
}
QVector< osg::ref_ptr<osg::Node> >
Pixhawk3DWidget::findVehicleModels(void)
{
......@@ -297,18 +385,13 @@ Pixhawk3DWidget::buildLayout(void)
mapComboBox->addItem("Map (Google)");
mapComboBox->addItem("Satellite (Google)");
QLabel* modelLabel = new QLabel("Vehicle Model", this);
QLabel* modelLabel = new QLabel("Vehicle", this);
QComboBox* modelComboBox = new QComboBox(this);
for (int i = 0; i < vehicleModels.size(); ++i)
{
modelComboBox->addItem(vehicleModels[i]->getName().c_str());
}
targetButton = new QPushButton(this);
targetButton->setCheckable(true);
targetButton->setChecked(false);
targetButton->setIcon(QIcon(QString::fromUtf8(":/images/status/weather-clear.svg")));
QPushButton* recenterButton = new QPushButton(this);
recenterButton->setText("Recenter Camera");
......@@ -327,10 +410,9 @@ Pixhawk3DWidget::buildLayout(void)
layout->addWidget(mapComboBox, 1, 5);
layout->addWidget(modelLabel, 1, 6);
layout->addWidget(modelComboBox, 1, 7);
layout->addWidget(targetButton, 1, 8);
layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 9);
layout->addWidget(recenterButton, 1, 10);
layout->addWidget(followCameraCheckBox, 1, 11);
layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 8);
layout->addWidget(recenterButton, 1, 9);
layout->addWidget(followCameraCheckBox, 1, 10);
layout->setRowStretch(0, 100);
layout->setRowStretch(1, 1);
setLayout(layout);
......@@ -358,9 +440,16 @@ Pixhawk3DWidget::display(void)
return;
}
double robotX = uas->getLocalX();
double robotY = uas->getLocalY();
double robotZ = uas->getLocalZ();
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
double robotX;
double robotY;
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
double robotZ = -altitude;
double robotRoll = uas->getRoll();
double robotPitch = uas->getPitch();
double robotYaw = uas->getYaw();
......@@ -397,7 +486,7 @@ Pixhawk3DWidget::display(void)
if (displayImagery)
{
updateImagery();
updateImagery(robotX, robotY, robotZ, utmZone);
}
if (displayTarget)
......@@ -422,7 +511,7 @@ Pixhawk3DWidget::display(void)
rollingMap->setChildValue(gridNode, displayGrid);
rollingMap->setChildValue(trailNode, displayTrail);
allocentricMap->setChildValue(mapNode, displayImagery);
rollingMap->setChildValue(mapNode, displayImagery);
rollingMap->setChildValue(targetNode, displayTarget);
rollingMap->setChildValue(waypointsNode, displayWaypoints);
if (enableFreenect)
......@@ -462,9 +551,30 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
void
Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
{
if (event->button() == Qt::LeftButton && targetButton->isChecked())
if (event->button() == Qt::LeftButton)
{
if (mode == MOVE_WAYPOINT_MODE)
{
setWaypoint();
mode = DEFAULT_MODE;
return;
}
if (event->modifiers() == Qt::ShiftModifier)
{
selectedWpIndex = findWaypoint(event->x(), event->y());
if (selectedWpIndex == -1)
{
showInsertWaypointMenu(event->globalPos());
}
else
{
markTarget();
showEditWaypointMenu(event->globalPos());
}
return;
}
}
Q3DWidget::mousePressEvent(event);
......@@ -779,15 +889,14 @@ Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ)
}
void
Pixhawk3DWidget::updateImagery(void)
Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
const QString& zone)
{
if (mapNode->getImageryType() == Imagery::BLANK_MAP)
{
return;
}
char zone[5] = "32T";
double viewingRadius = cameraManipulator->getDistance() * 10.0;
if (viewingRadius < 100.0)
{
......@@ -828,6 +937,9 @@ Pixhawk3DWidget::updateImagery(void)
resolution,
cameraManipulator->getCenter().y(),
cameraManipulator->getCenter().x(),
originX,
originY,
originZ,
zone);
// prefetch map tiles
......@@ -894,6 +1006,14 @@ Pixhawk3DWidget::updateWaypoints(void)
{
if (uas)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double robotX, robotY;
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
double robotZ = -uas->getAltitude();
if (waypointsNode->getNumChildren() > 0)
{
waypointsNode->removeChild(0, waypointsNode->getNumChildren());
......@@ -903,29 +1023,63 @@ Pixhawk3DWidget::updateWaypoints(void)
for (int i = 0; i < list.size(); i++)
{
Waypoint* wp = list.at(i);
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere;
sphere->setRadius(0.2);
sd->setShape(sphere);
osg::ref_ptr<osg::Cylinder> cylinder =
new osg::Cylinder(osg::Vec3d(0.0, 0.0, - wp->getZ() / 2.0),
wp->getOrbit(),
fabs(wp->getZ()));
sd->setShape(cylinder);
sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
if (list.at(i)->getCurrent())
if (wp->getCurrent())
{
sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 1.0f));
sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
}
else
{
sd->setColor(osg::Vec4(0.0f, 1.0f, 1.0f, 1.0f));
sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
}
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
geode->addDrawable(sd);
char wpLabel[10];
sprintf(wpLabel, "wp%d", i);
geode->setName(wpLabel);
if (i < list.size() - 1)
{
osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
osg::ref_ptr<osg::Vec3dArray> vertices = new osg::Vec3dArray;
vertices->push_back(osg::Vec3d(0.0, 0.0, -wp->getZ()));
vertices->push_back(osg::Vec3d(list.at(i+1)->getY() - wp->getY(),
list.at(i+1)->getX() - wp->getX(),
-list.at(i+1)->getZ()));
geometry->setVertexArray(vertices);
osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
colors->push_back(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
geometry->setColorArray(colors);
geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
linewidth->setWidth(2.0f);
geometry->getOrCreateStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
geode->addDrawable(geometry);
}
osg::ref_ptr<osg::PositionAttitudeTransform> pat =
new osg::PositionAttitudeTransform;
pat->setPosition(osg::Vec3d(list.at(i)->getY() - uas->getLocalY(),
list.at(i)->getX() - uas->getLocalX(),
0.0));
pat->setPosition(osg::Vec3d(wp->getY() - robotY,
wp->getX() - robotX,
robotZ));
waypointsNode->addChild(pat);
pat->addChild(geode);
......@@ -1142,3 +1296,59 @@ Pixhawk3DWidget::markTarget(void)
targetButton->setChecked(false);
}
int
Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
{
if (getSceneData() != NULL)
{
osgUtil::LineSegmentIntersector::Intersections intersections;
if (computeIntersections(mouseX, height() - mouseY, intersections))
{
for (osgUtil::LineSegmentIntersector::Intersections::iterator
it = intersections.begin(); it != intersections.end(); it++)
{
for (uint i = 0 ; i < it->nodePath.size(); ++i)
{
std::string nodeName = it->nodePath[i]->getName();
if (nodeName.substr(0, 2).compare("wp") == 0)
{
qDebug() << nodeName.c_str() << "Got!!";
return atoi(nodeName.substr(2).c_str());
}
}
}
}
}
return -1;
}
void
Pixhawk3DWidget::showInsertWaypointMenu(const QPoint &cursorPos)
{
QMenu menu;
menu.addAction("Insert new waypoint", this, SLOT(insertWaypoint()));
menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints()));
menu.exec(cursorPos);
}
void
Pixhawk3DWidget::showEditWaypointMenu(const QPoint &cursorPos)
{
QMenu menu;
QString text;
text = QString("Move waypoint %1").arg(QString::number(selectedWpIndex));
menu.addAction(text, this, SLOT(moveWaypoint()));
text = QString("Change altitude of waypoint %1").arg(QString::number(selectedWpIndex));
menu.addAction(text, this, SLOT(setWaypointAltitude()));
text = QString("Delete waypoint %1").arg(QString::number(selectedWpIndex));
menu.addAction(text, this, SLOT(deleteWaypoint()));
menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints()));
menu.exec(cursorPos);
}
src/ui/map3D/Pixhawk3DWidget.h
View file @ 0f18c7c5
......@@ -68,6 +68,13 @@ private slots:
void recenter(void);
void toggleFollowCamera(int state);
void insertWaypoint(void);
void moveWaypoint(void);
void setWaypoint(void);
void deleteWaypoint(void);
void setWaypointAltitude(void);
void clearAllWaypoints(void);
protected:
QVector< osg::ref_ptr<osg::Node> > findVehicleModels(void);
void buildLayout(void);
......@@ -91,7 +98,8 @@ private:
void updateHUD(double robotX, double robotY, double robotZ,
double robotRoll, double robotPitch, double robotYaw);
void updateTrail(double robotX, double robotY, double robotZ);
void updateImagery(void);
void updateImagery(double originX, double originY, double originZ,
const QString& zone);
void updateTarget(void);
void updateWaypoints(void);
#ifdef QGC_LIBFREENECT_ENABLED
......@@ -100,6 +108,17 @@ private:
void markTarget(void);
int findWaypoint(int mouseX, int mouseY);
void showInsertWaypointMenu(const QPoint& cursorPos);
void showEditWaypointMenu(const QPoint& cursorPos);
enum Mode {
DEFAULT_MODE,
MOVE_WAYPOINT_MODE
};
Mode mode;
int selectedWpIndex;
bool displayGrid;
bool displayTrail;
bool displayImagery;
......
src/ui/map3D/Q3DWidget.cc
View file @ 0f18c7c5
......@@ -72,6 +72,9 @@ Q3DWidget::init(float fps)
{
getCamera()->setGraphicsContext(osgGW);
// manually specify near and far clip planes
getCamera()->setComputeNearFarMode(osg::CullSettings::DO_NOT_COMPUTE_NEAR_FAR);
setLightingMode(osg::View::SKY_LIGHT);
// set up various maps
......@@ -149,8 +152,9 @@ Q3DWidget::createRobot(void)
osg::ref_ptr<osg::Node>
Q3DWidget::createHUD(void)
{
hudProjectionMatrix->setMatrix(osg::Matrix::ortho2D(0, width(),
0, height()));
hudProjectionMatrix->setMatrix(osg::Matrix::ortho(0.0, width(),
0.0, height(),
-10.0, 10.0));
osg::ref_ptr<osg::MatrixTransform> hudModelViewMatrix(
new osg::MatrixTransform);
......@@ -256,8 +260,9 @@ Q3DWidget::getMouseY(void)
void
Q3DWidget::resizeGL(int width, int height)
{
hudProjectionMatrix->setMatrix(osg::Matrix::ortho2D(0, width,
0, height));
hudProjectionMatrix->setMatrix(osg::Matrix::ortho(0.0, width,
0.0, height,
-10.0, 10.0));
osgGW->getEventQueue()->windowResize(0, 0, width, height);
osgGW->resized(0 , 0, width, height);
......
src/ui/map3D/QGCGoogleEarthView.cc
View file @ 0f18c7c5
......@@ -76,24 +76,25 @@ void QGCGoogleEarthView::updateState()
if (mav)
{
uasId = mav->getUASID();
lat = mav->getLat();
lon = mav->getLon();
alt = mav->getAlt();
lat = mav->getLatitude();
lon = mav->getLongitude();
alt = mav->getAltitude();
roll = mav->getRoll();
pitch = mav->getPitch();
yaw = mav->getYaw();
}
// ui->webView->page()->currentFrame()->evaluateJavaScript(QString("setAircraftPosition(%1, %2, %3, %4);")
// .arg(uasId)
// .arg(lat)
// .arg(lon)
// .arg(alt));
// //ui->webView->page()->currentFrame()->evaluateJavaScript(QString("drawAndCenter(%1, %2, %3, %4, '%5', %6, %7, %8, %9, %10, %11);").arg(lat).arg(lon).arg(alt).arg("true").arg("ff0000ff").arg("1").arg("true").arg("true").arg(yaw).arg(pitch).arg(roll));
ui->webView->page()->currentFrame()->evaluateJavaScript(QString("setAircraftPositionAttitude(%1, %2, %3, %4, %6, %7, %8);")
.arg(uasId)
.arg(lat)
.arg(lon)
.arg(alt+500)
.arg(roll)
.arg(pitch)
.arg(yaw));
if (followCamera)
{
ui->webView->page()->currentFrame()->evaluateJavaScript(QString("updateFollowAircraft()"));
//ui->webView->page()->currentFrame()->evaluateJavaScript(QString("followAircraft(%1);").arg(mav->getUASID()));
}
}
}
......
src/ui/map3D/Texture.cc
View file @ 0f18c7c5
......@@ -113,22 +113,24 @@ Texture::sync(const WebImagePtr& image)
osg::ref_ptr<osg::Geometry>
Texture::draw(double x1, double y1, double x2, double y2,
double z,
bool smoothInterpolation) const
{
return draw(x1, y1, x2, y1, x2, y2, x1, y2, smoothInterpolation);
return draw(x1, y1, x2, y1, x2, y2, x1, y2, z, smoothInterpolation);
}
osg::ref_ptr<osg::Geometry>
Texture::draw(double x1, double y1, double x2, double y2,
double x3, double y3, double x4, double y4,
double z,
bool smoothInterpolation) const
{
osg::Vec3dArray* vertices =
static_cast<osg::Vec3dArray*>(geometry->getVertexArray());
(*vertices)[0].set(x1, y1, -0.1);
(*vertices)[1].set(x2, y2, -0.1);
(*vertices)[2].set(x3, y3, -0.1);
(*vertices)[3].set(x4, y4, -0.1);
(*vertices)[0].set(x1, y1, z - 0.1);
(*vertices)[1].set(x2, y2, z - 0.1);
(*vertices)[2].set(x3, y3, z - 0.1);
(*vertices)[3].set(x4, y4, z - 0.1);
osg::DrawArrays* drawarrays =
static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
......
src/ui/map3D/Texture.h
View file @ 0f18c7c5
......@@ -52,9 +52,11 @@ public:
void sync(const WebImagePtr& image);
osg::ref_ptr<osg::Geometry> draw(double x1, double y1, double x2, double y2,
double z,
bool smoothInterpolation) const;
osg::ref_ptr<osg::Geometry> draw(double x1, double y1, double x2, double y2,
double x3, double y3, double x4, double y4,
double z,
bool smoothInterpolation) const;
private:
......
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