Skip to content

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

Merged

parents e4ea6b31 788ba064
Expand all Hide 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
This diff is collapsed.
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
......@@ -71,6 +71,9 @@ void
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);
......@@ -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