Commit ab3a1ec0 authored by hengli's avatar hengli

Added command to select target position for robot - command can be accessed...

Added command to select target position for robot - command can be accessed via a right-click in the 3D View perspective --Lionel
parents 57207de3 804d81cf
......@@ -43,9 +43,8 @@ DEPENDPATH += . \
INCLUDEPATH += . \
lib/QMapControl \
$$BASEDIR/../mavlink/contrib/slugs/include \
$$BASEDIR/../mavlink/include \
/usr/include/freetype2
LIBS += /usr/lib/libftgl.so /usr/lib/libglut.so
$$BASEDIR/../mavlink/include
LIBS += -lglut
# ../mavlink/include \
# MAVLink/include \
......@@ -160,7 +159,6 @@ HEADERS += src/MG.h \
src/ui/linechart/IncrementalPlot.h \
src/ui/map/Waypoint2DIcon.h \
src/ui/map/MAV2DIcon.h \
src/ui/map/QGC2DIcon.h \
src/ui/QGCRemoteControlView.h \
src/WaypointGlobal.h \
src/ui/WaypointGlobalView.h \
......@@ -230,7 +228,6 @@ SOURCES += src/main.cc \
src/ui/linechart/IncrementalPlot.cc \
src/ui/map/Waypoint2DIcon.cc \
src/ui/map/MAV2DIcon.cc \
src/ui/map/QGC2DIcon.cc \
src/ui/QGCRemoteControlView.cc \
src/WaypointGlobal.cpp \
src/ui/WaypointGlobalView.cpp \
......
......@@ -405,14 +405,14 @@ void MAVLinkSimulationLink::mainloop()
streampointer += bufferlength;
// GPS RAW
mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+x*0.001f, 8.548103+y*0.001f, z, 0, 0, 2.5f, 0.1f);
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);
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.001f, 8.548103+y*0.001f, z, 0, 0);
mavlink_msg_global_position_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.001), 8.548103+(y*0.001), z, 0, 0);
bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
......
This diff is collapsed.
......@@ -246,6 +246,29 @@ void OpalLink::getSignals()
values[OpalRT::B_W_2]
);
receiveMessage(bias);
/* send radio outputs */
mavlink_message_t rc;
mavlink_msg_rc_channels_pack(systemID, componentID, &rc,
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_1]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_2]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_3]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_4]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_5]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_6]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_7]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_8]),
static_cast<uint8_t>(values[OpalRT::NORM_CHANNEL_1]*255),
static_cast<uint8_t>(values[OpalRT::NORM_CHANNEL_2]*255),
static_cast<uint8_t>(values[OpalRT::NORM_CHANNEL_3]*255),
static_cast<uint8_t>(values[OpalRT::NORM_CHANNEL_4]*255),
static_cast<uint8_t>(values[OpalRT::NORM_CHANNEL_5]*255),
static_cast<uint8_t>(values[OpalRT::NORM_CHANNEL_6]*255),
static_cast<uint8_t>(values[OpalRT::NORM_CHANNEL_7]*255),
static_cast<uint8_t>(values[OpalRT::NORM_CHANNEL_8]*255),
0 //rssi unused
);
receiveMessage(rc);
}
else if (returnVal != EAGAIN) // if returnVal == EAGAIN => data just wasn't ready
{
......@@ -294,6 +317,12 @@ bool OpalLink::isConnected() {
return connectState;
}
uint16_t OpalLink::duty2PulseMicros(double duty)
{
/* duty cycle assumed to be of a signal at 70 Hz */
return static_cast<uint16_t>(duty/70*1000000);
}
......
......@@ -151,6 +151,8 @@ protected:
OpalRT::ParameterList *params;
unsigned short opalInstID;
uint16_t duty2PulseMicros(double duty);
};
#endif // OPALLINK_H
......@@ -41,7 +41,7 @@ namespace OpalRT
Configuration info for the model
*/
const unsigned short NUM_OUTPUT_SIGNALS=39;
const unsigned short NUM_OUTPUT_SIGNALS=57;
/* ------------------------------ Outputs ------------------------------
*
......@@ -85,7 +85,25 @@ namespace OpalRT
B_F_2,
B_W_0,
B_W_1,
B_W_2
B_W_2,
RAW_CHANNEL_1 = 39,
RAW_CHANNEL_2,
RAW_CHANNEL_3,
RAW_CHANNEL_4,
RAW_CHANNEL_5,
RAW_CHANNEL_6,
RAW_CHANNEL_7,
RAW_CHANNEL_8,
NORM_CHANNEL_1,
NORM_CHANNEL_2,
NORM_CHANNEL_3,
NORM_CHANNEL_4,
NORM_CHANNEL_5,
NORM_CHANNEL_6,
NORM_CHANNEL_7,
NORM_CHANNEL_8,
CONTROLLER_AILERON,
CONTROLLER_ELEVATOR
};
/** Component IDs of the parameters. Currently they are all 1 becuase there is no advantage
......
......@@ -365,28 +365,34 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// only accept values in a realistic range
// quint64 time = getUnixTime(pos.usec);
quint64 time = MG::TIME::getGroundTimeNow();
emit valueChanged(uasId, "lat", pos.lat, time);
emit valueChanged(uasId, "lon", pos.lon, time);
// Check for NaN
int alt = pos.alt;
if (alt != alt)
{
alt = 0;
emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE");
}
emit valueChanged(uasId, "alt", pos.alt, time);
// Smaller than threshold and not NaN
if (pos.v < 1000000 && pos.v == pos.v)
{
emit valueChanged(uasId, "speed", pos.v, time);
//qDebug() << "GOT GPS RAW";
emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
}
else
if (pos.fix_type > 0)
{
emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v));
emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
// Check for NaN
int alt = pos.alt;
if (alt != alt)
{
alt = 0;
emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE");
}
emit valueChanged(uasId, "alt", pos.alt, time);
// Smaller than threshold and not NaN
if (pos.v < 1000000 && pos.v == pos.v)
{
emit valueChanged(uasId, "speed", pos.v, time);
//qDebug() << "GOT GPS RAW";
emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
}
else
{
emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v));
}
}
emit globalPositionChanged(this, pos.lon, pos.lat, alt, time);
}
break;
case MAVLINK_MSG_ID_GPS_STATUS:
......
......@@ -100,7 +100,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
topMargin(3.0f)
{
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
refreshTimer->setInterval(60);
refreshTimer->setInterval(120);
// this->setScene(new QGraphicsScene(-metricWidth/2.0f, -metricWidth/2.0f, metricWidth, metricWidth, this));
......
......@@ -448,7 +448,7 @@ void HUD::paintText(QString text, QColor color, float fontSize, float refX, floa
QFont font("Bitstream Vera Sans");
// Enforce minimum font size of 5 pixels
int fSize = qMax(1, (int)(fontSize*scalingFactor*1.26f));
int fSize = qMax(5, (int)(fontSize*scalingFactor*1.26f));
font.setPixelSize(fSize);
QFontMetrics metrics = QFontMetrics(font);
......
......@@ -657,14 +657,14 @@ void MainWindow::loadPixhawkView()
clearView();
// Engineer view, used in EMAV2009
// LINE CHART
if (linechartWidget)
// 3D map
if (map3DWidget)
{
QStackedWidget *centerStack = dynamic_cast<QStackedWidget*>(centralWidget());
if (centerStack)
{
linechartWidget->setActive(true);
centerStack->setCurrentWidget(linechartWidget);
//map3DWidget->setActive(true);
centerStack->setCurrentWidget(map3DWidget);
}
}
......@@ -675,6 +675,18 @@ void MainWindow::loadPixhawkView()
controlDockWidget->show();
}
// HORIZONTAL SITUATION INDICATOR
if (hsiDockWidget)
{
HSIDisplay* hsi = dynamic_cast<HSIDisplay*>( hsiDockWidget->widget() );
if (hsi)
{
hsi->start();
addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget);
hsiDockWidget->show();
}
}
// UAS LIST
if (listDockWidget)
{
......@@ -696,18 +708,6 @@ void MainWindow::loadPixhawkView()
waypointsDockWidget->show();
}
// HORIZONTAL SITUATION INDICATOR
if (hsiDockWidget)
{
HSIDisplay* hsi = dynamic_cast<HSIDisplay*>( hsiDockWidget->widget() );
if (hsi)
{
hsi->start();
addDockWidget(Qt::BottomDockWidgetArea, hsiDockWidget);
hsiDockWidget->show();
}
}
// DEBUG CONSOLE
if (debugConsoleDockWidget)
{
......@@ -715,13 +715,6 @@ void MainWindow::loadPixhawkView()
debugConsoleDockWidget->show();
}
// RADIO CONTROL VIEW
if (rcViewDockWidget)
{
addDockWidget(Qt::BottomDockWidgetArea, rcViewDockWidget);
rcViewDockWidget->show();
}
// ONBOARD PARAMETERS
if (parametersDockWidget)
{
......@@ -981,6 +974,13 @@ void MainWindow::loadEngineerView()
parametersDockWidget->show();
}
// RADIO CONTROL VIEW
if (rcViewDockWidget)
{
addDockWidget(Qt::BottomDockWidgetArea, rcViewDockWidget);
rcViewDockWidget->show();
}
this->show();
}
......
......@@ -37,6 +37,8 @@ This file is part of the QGROUNDCONTROL project
#include "ui_MapWidget.h"
#include "UASInterface.h"
#include "UASManager.h"
#include "MAV2DIcon.h"
#include "Waypoint2DIcon.h"
#include "MG.h"
......@@ -53,7 +55,7 @@ MapWidget::MapWidget(QWidget *parent) :
this->setFocusPolicy(Qt::StrongFocus);
// create MapControl
mc = new MapControl(QSize(320, 240));
mc = new qmapcontrol::MapControl(QSize(320, 240));
mc->showScale(true);
mc->showCoord(true);
mc->enablePersistentCache();
......@@ -62,21 +64,21 @@ MapWidget::MapWidget(QWidget *parent) :
// create MapAdapter to get maps from
//TileMapAdapter* osmAdapter = new TileMapAdapter("tile.openstreetmap.org", "/%1/%2/%3.png", 256, 0, 17);
MapAdapter* mapadapter_overlay = new YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=2.2&t=h&s=256&x=%2&y=%3&z=%1");
qmapcontrol::MapAdapter* mapadapter_overlay = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=2.2&t=h&s=256&x=%2&y=%3&z=%1");
// MAP BACKGROUND
mapadapter = new GoogleSatMapAdapter();
l = new MapLayer("Google Satellite", mapadapter);
mapadapter = new qmapcontrol::GoogleSatMapAdapter();
l = new qmapcontrol::MapLayer("Google Satellite", mapadapter);
mc->addLayer(l);
// STREET OVERLAY
overlay = new MapLayer("Overlay", mapadapter_overlay);
overlay = new qmapcontrol::MapLayer("Overlay", mapadapter_overlay);
overlay->setVisible(false);
mc->addLayer(overlay);
// WAYPOINT LAYER
// create a layer with the mapadapter and type GeometryLayer (for waypoints)
geomLayer = new GeometryLayer("Waypoints", mapadapter);
geomLayer = new qmapcontrol::GeometryLayer("Waypoints", mapadapter);
mc->addLayer(geomLayer);
//
......@@ -188,6 +190,7 @@ MapWidget::MapWidget(QWidget *parent) :
connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)),
this, SLOT(addUAS(UASInterface*)));
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUASSet(UASInterface*)));
connect(mc, SIGNAL(mouseEventCoordinate(const QMouseEvent*, const QPointF)),
this, SLOT(captureMapClick(const QMouseEvent*, const QPointF)));
......@@ -208,7 +211,7 @@ MapWidget::MapWidget(QWidget *parent) :
pointPen = new QPen(QColor(0, 255,0));
pointPen->setWidth(3);
path = new LineString (wps, "UAV Path", pointPen);
path = new qmapcontrol::LineString (wps, "UAV Path", pointPen);
mc->layer("Waypoints")->addGeometry(path);
this->setVisible(false);
......@@ -224,7 +227,7 @@ void MapWidget::mapproviderSelected(QAction* action)
int zoom = mapadapter->adaptedZoom();
mc->setZoom(0);
mapadapter = new OSMMapAdapter();
mapadapter = new qmapcontrol::OSMMapAdapter();
l->setMapAdapter(mapadapter);
geomLayer->setMapAdapter(mapadapter);
......@@ -240,7 +243,7 @@ void MapWidget::mapproviderSelected(QAction* action)
int zoom = mapadapter->adaptedZoom();
mc->setZoom(0);
mapadapter = new YahooMapAdapter();
mapadapter = new qmapcontrol::YahooMapAdapter();
l->setMapAdapter(mapadapter);
geomLayer->setMapAdapter(mapadapter);
......@@ -256,7 +259,7 @@ void MapWidget::mapproviderSelected(QAction* action)
QPointF a = mc->currentCoordinate();
mc->setZoom(0);
mapadapter = new YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=1.7&t=a&s=256&x=%2&y=%3&z=%1");
mapadapter = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=1.7&t=a&s=256&x=%2&y=%3&z=%1");
l->setMapAdapter(mapadapter);
mc->updateRequestNew();
......@@ -267,7 +270,7 @@ void MapWidget::mapproviderSelected(QAction* action)
{
int zoom = mapadapter->adaptedZoom();
mc->setZoom(0);
mapadapter = new GoogleMapAdapter();
mapadapter = new qmapcontrol::GoogleMapAdapter();
l->setMapAdapter(mapadapter);
geomLayer->setMapAdapter(mapadapter);
......@@ -281,7 +284,7 @@ void MapWidget::mapproviderSelected(QAction* action)
{
int zoom = mapadapter->adaptedZoom();
mc->setZoom(0);
mapadapter = new GoogleSatMapAdapter();
mapadapter = new qmapcontrol::GoogleSatMapAdapter();
l->setMapAdapter(mapadapter);
geomLayer->setMapAdapter(mapadapter);
......@@ -342,10 +345,19 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina
str = QString("%1").arg(path->numberOfPoints());
// create the WP and set everything in the LineString to display the path
CirclePoint* tempCirclePoint = new CirclePoint(coordinate.x(), coordinate.y(), 10, str);
Waypoint2DIcon* tempCirclePoint;
if (mav)
{
tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, new QPen(mav->getColor()));
}
else
{
tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
}
mc->layer("Waypoints")->addGeometry(tempCirclePoint);
Point* tempPoint = new Point(coordinate.x(), coordinate.y(),str);
qmapcontrol::Point* tempPoint = new qmapcontrol::Point(coordinate.x(), coordinate.y(),str);
wps.append(tempPoint);
path->addPoint(tempPoint);
......@@ -360,7 +372,7 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina
}
}
void MapWidget::captureGeometryClick(Geometry* geom, QPoint point){
void MapWidget::captureGeometryClick(qmapcontrol::Geometry* geom, QPoint point){
Q_UNUSED(geom);
Q_UNUSED(point);
......@@ -369,29 +381,35 @@ void MapWidget::captureGeometryClick(Geometry* geom, QPoint point){
}
void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate){
void MapWidget::captureGeometryDrag(qmapcontrol::Geometry* geom, QPointF coordinate){
Q_UNUSED(coordinate);
// Refresh the screen
mc->updateRequestNew();
int temp = 0;
Point* point2Find;
qmapcontrol::Point* point2Find;
point2Find = wpIndex[geom->name()];
point2Find->setCoordinate(coordinate);
point2Find = dynamic_cast <Point*> (geom);
point2Find->setCoordinate(coordinate);
if (point2Find)
{
point2Find->setCoordinate(coordinate);
// qDebug() << geom->name();
temp = geom->get_myIndex();
//qDebug() << temp;
emit sendGeometryEndDrag(coordinate,temp);
point2Find = dynamic_cast <qmapcontrol::Point*> (geom);
if (point2Find)
{
point2Find->setCoordinate(coordinate);
// qDebug() << geom->name();
temp = geom->get_myIndex();
//qDebug() << temp;
emit sendGeometryEndDrag(coordinate,temp);
}
}
}
void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate)
void MapWidget::captureGeometryEndDrag(qmapcontrol::Geometry* geom, QPointF coordinate)
{
mc->setMouseMode(qmapcontrol::MapControl::Panning);
......@@ -411,10 +429,18 @@ MapWidget::~MapWidget()
*/
void MapWidget::addUAS(UASInterface* uas)
{
mav = uas;
connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
}
void MapWidget::activeUASSet(UASInterface* uas)
{
if (uas)
{
mav = uas;
path->setPen(new QPen(mav->getColor()));
}
}
/**
* Updates the global position of one MAV and append the last movement to the trail
*
......@@ -446,19 +472,19 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
// Icon
QPen* pointpen = new QPen(uasColor);
CirclePoint* p = new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen);
MAV2DIcon* p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, pointpen);
uasIcons.insert(uas->getUASID(), p);
geomLayer->addGeometry(p);
// Line
// A QPen also can use transparency
QList<Point*> points;
points.append(new Point(lat, lon, QString("lat: %1 lon: %2").arg(lat, lon)));
QList<qmapcontrol::Point*> points;
points.append(new qmapcontrol::Point(lat, lon, QString("lat: %1 lon: %2").arg(lat, lon)));
QPen* linepen = new QPen(uasColor.darker());
linepen->setWidth(2);
// Add the Points and the QPen to a LineString
LineString* ls = new LineString(points, uas->getUASName(), linepen);
qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, uas->getUASName(), linepen);
uasTrails.insert(uas->getUASID(), ls);
// Add the LineString to the layer
......@@ -466,10 +492,14 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
}
else
{
CirclePoint* p = uasIcons.value(uas->getUASID());
p->setCoordinate(QPointF(lat, lon));
MAV2DIcon* p = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID()));
if (p)
{
p->setCoordinate(QPointF(lat, lon));
p->setYaw(uas->getYaw());
}
// Extend trail
uasTrails.value(uas->getUASID())->addPoint(new Point(lat, lon, QString("lat: %1 lon: %2").arg(lat, lon)));
uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(lat, lon, QString("lat: %1 lon: %2").arg(lat, lon)));
}
// points.append(new CirclePoint(8.275145, 50.016992, 15, "Wiesbaden-Mainz-Kastel, Johannes-Goßner-Straße", Point::Middle, pointpen));
......
......@@ -44,8 +44,6 @@ namespace Ui {
class MapWidget;
}
using namespace qmapcontrol;
/**
* @brief 2D Moving map
*
......@@ -60,6 +58,7 @@ public:
public slots:
void addUAS(UASInterface* uas);
void activeUASSet(UASInterface* uas);
void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec);
void updatePosition(float time, double lat, double lon);
......@@ -86,8 +85,8 @@ protected:
QMenu* mapMenu;
QPushButton* mapButton;
MapControl* mc; ///< QMapControl widget
MapAdapter* mapadapter; ///< Adapter to load the map data
qmapcontrol::MapControl* mc; ///< QMapControl widget
qmapcontrol::MapAdapter* mapadapter; ///< Adapter to load the map data
qmapcontrol::Layer* l; ///< Current map layer (background)
qmapcontrol::Layer* overlay; ///< Street overlay (foreground)
qmapcontrol::GeometryLayer* geomLayer; ///< Layer for waypoints
......@@ -98,18 +97,18 @@ protected:
//Layer* gSatLayer;
QMap<int, CirclePoint*> uasIcons;
QMap<int, LineString*> uasTrails;
QMap<int, qmapcontrol::Point*> uasIcons;
QMap<int, qmapcontrol::LineString*> uasTrails;
UASInterface* mav;
quint64 lastUpdate;
protected slots:
void captureMapClick (const QMouseEvent* event, const QPointF coordinate);
void createPathButtonClicked(bool checked);
void captureGeometryClick(Geometry*, QPoint);
void captureGeometryClick(qmapcontrol::Geometry*, QPoint);
void mapproviderSelected(QAction* action);
void captureGeometryDrag(Geometry* geom, QPointF coordinate);
void captureGeometryEndDrag(Geometry* geom, QPointF coordinate);
void captureGeometryDrag(qmapcontrol::Geometry* geom, QPointF coordinate);
void captureGeometryEndDrag(qmapcontrol::Geometry* geom, QPointF coordinate);
......@@ -122,9 +121,9 @@ protected:
private:
Ui::MapWidget *m_ui;
QList<Point*> wps;
QHash <QString, Point*> wpIndex;
LineString* path;
QList<qmapcontrol::Point*> wps;
QHash <QString, qmapcontrol::Point*> wpIndex;
qmapcontrol::LineString* path;
QPen* pointPen;
};
......
......@@ -13,7 +13,16 @@
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<layout class="QGridLayout" name="gridLayout" rowstretch="2,10,5">
<property name="horizontalSpacing">
<number>5</number>
</property>
<property name="verticalSpacing">
<number>2</number>
</property>
<property name="margin">
<number>6</number>
</property>
<item row="0" column="0">
<widget class="QLabel" name="label">
<property name="text">
......@@ -30,6 +39,9 @@
<string>Onboard Parameters</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout">
<property name="margin">
<number>3</number>
</property>
<item>
<widget class="QStackedWidget" name="stackedWidget">
<property name="currentIndex">
......
......@@ -45,6 +45,7 @@ QGCRemoteControlView::QGCRemoteControlView(QWidget *parent) :
channelLayout(new QVBoxLayout()),
ui(new Ui::QGCRemoteControlView)
{
//ui->setupUi(this);
QGridLayout* layout = new QGridLayout(this);
layout->addLayout(channelLayout, 1, 0, 1, 2);
......
......@@ -6,135 +6,152 @@
<rect>
<x>0</x>
<y>0</y>
<width>350</width>
<height>545</height>
<width>319</width>
<height>221</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QGridLayout" name="gridLayout_4">
<layout class="QGridLayout" name="gridLayout_4" rowstretch="10,0">
<property name="margin">
<number>0</number>
</property>
<item row="0" column="0">
<widget class="QGroupBox" name="groupBox">
<item row="1" column="0">
<widget class="QGroupBox" name="groupBox_3">
<property name="title">
<string>Activate Extended Output</string>
<string>Calibration Wizards</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
<property name="margin">
<layout class="QGridLayout" name="gridLayout" columnstretch="5,5">
<property name="leftMargin">
<number>6</number>
</property>
<property name="topMargin">
<number>2</number>
</property>
<property name="rightMargin">
<number>6</number>
</property>
<property name="bottomMargin">
<number>2</number>
</property>
<property name="horizontalSpacing">
<number>12</number>
</property>
<property name="verticalSpacing">
<number>6</number>
</property>
<item row="0" column="0">
<widget class="QCheckBox" name="sendRawCheckBox">
<widget class="QPushButton" name="rcCalButton">
<property name="text">
<string>Send RAW Sensor data</string>
<string>RC Calibration</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="sendExtendedCheckBox">
<item row="0" column="1">
<widget class="QPushButton" name="magCalButton">
<property name="text">
<string>Send extended status</string>
<string>Mag. Calibration</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="sendRCCheckBox">
<item row="1" column="1">
<widget class="QPushButton" name="gyroCalButton">
<property name="text">
<string>Send RC-values</string>
<string>Gyro Calibration</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="sendControllerCheckBox">
<item row="1" column="0">
<widget class="QPushButton" name="pressureCalButton">
<property name="text">
<string>Send raw controller outputs</string>
<string>Pressure Calibration</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="sendPositionCheckBox">
</layout>
</widget>
</item>
<item row="0" column="0">
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>Activate Extended Output</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
<property name="leftMargin">
<number>6</number>
</property>
<property name="topMargin">
<number>2</number>
</property>
<property name="rightMargin">
<number>6</number>
</property>
<property name="bottomMargin">
<number>2</number>
</property>
<property name="horizontalSpacing">
<number>5</number>
</property>
<property name="verticalSpacing">
<number>2</number>
</property>
<item row="0" column="0">
<widget class="QCheckBox" name="sendRawCheckBox">
<property name="text">
<string>Send position setpoint and estimate</string>
<string>RAW Sensor Data</string>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="sendExtra1CheckBox">
<item row="0" column="1">
<widget class="QCheckBox" name="sendRCCheckBox">
<property name="text">
<string>Send Extra1</string>
<string>RC Values</string>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="sendExtra2CheckBox">
<item row="4" column="0">
<widget class="QCheckBox" name="sendPositionCheckBox">
<property name="text">
<string>Send Extra2</string>
<string>Position setpoint</string>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="sendExtra3CheckBox">
<item row="5" column="0">
<widget class="QCheckBox" name="sendControllerCheckBox">
<property name="text">
<string>Send Extra3</string>
<string>Raw Controller</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="1" column="0">
<widget class="QGroupBox" name="groupBox_3">
<property name="title">
<string>Calibration Wizards</string>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,50">
<property name="margin">
<number>6</number>
</property>
<item row="1" column="0">
<widget class="QPushButton" name="magCalButton">
<item row="1" column="1">
<widget class="QCheckBox" name="sendExtra1CheckBox">
<property name="text">
<string>Start Mag. Calibration</string>
<string>Send Extra1</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QPushButton" name="gyroCalButton">
<item row="1" column="0">
<widget class="QCheckBox" name="sendExtendedCheckBox">
<property name="text">
<string>Start Gyro Calibration</string>
<string>Attitude</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QPushButton" name="rcCalButton">
<item row="4" column="1">
<widget class="QCheckBox" name="sendExtra2CheckBox">
<property name="text">
<string>Start RC Calibration</string>
<string>Send Extra2</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QPushButton" name="pressureCalButton">
<item row="5" column="1">
<widget class="QCheckBox" name="sendExtra3CheckBox">
<property name="text">
<string>Start Pressure Calibration</string>
<string>Send Extra3</string>
</property>
</widget>
</item>
<item row="0" column="1">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
......
......@@ -6,12 +6,12 @@
<rect>
<x>0</x>
<y>0</y>
<width>869</width>
<height>60</height>
<width>774</width>
<height>184</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Maximum" vsizetype="Fixed">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
......@@ -144,6 +144,15 @@ QProgressBar::chunk#thrustBar {
}</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<property name="sizeConstraint">
<enum>QLayout::SetDefaultConstraint</enum>
</property>
<property name="margin">
<number>0</number>
</property>
<property name="spacing">
<number>0</number>
</property>
<item row="0" column="0">
<widget class="QGroupBox" name="groupBox_2">
<property name="sizePolicy">
......@@ -155,7 +164,13 @@ QProgressBar::chunk#thrustBar {
<property name="title">
<string/>
</property>
<layout class="QHBoxLayout" name="horizontalLayout">
<layout class="QHBoxLayout" name="horizontalLayout" stretch="20,0,0,50,0,0,50,10,0,20,20,20">
<property name="spacing">
<number>2</number>
</property>
<property name="margin">
<number>2</number>
</property>
<item>
<widget class="QLabel" name="idWP_label">
<property name="text">
......@@ -168,10 +183,13 @@ QProgressBar::chunk#thrustBar {
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::MinimumExpanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>135</width>
<height>13</height>
<width>2</width>
<height>0</height>
</size>
</property>
</spacer>
......@@ -184,26 +202,7 @@ QProgressBar::chunk#thrustBar {
</widget>
</item>
<item>
<widget class="QTextEdit" name="m_latitudtextEdit">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>120</width>
<height>25</height>
</size>
</property>
<property name="verticalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
<widget class="QLineEdit" name="m_latitudtextEdit"/>
</item>
<item>
<spacer name="horizontalSpacer_2">
......@@ -212,8 +211,8 @@ QProgressBar::chunk#thrustBar {
</property>
<property name="sizeHint" stdset="0">
<size>
<width>50</width>
<height>30</height>
<width>2</width>
<height>0</height>
</size>
</property>
</spacer>
......@@ -226,30 +225,20 @@ QProgressBar::chunk#thrustBar {
</widget>
</item>
<item>
<widget class="QTextEdit" name="m_longitudtextEdit">
<property name="maximumSize">
<size>
<width>120</width>
<height>25</height>
</size>
</property>
<property name="verticalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
<widget class="QLineEdit" name="m_longitudtextEdit"/>
</item>
<item>
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::MinimumExpanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>50</width>
<height>30</height>
<width>2</width>
<height>0</height>
</size>
</property>
</spacer>
......
......@@ -160,7 +160,7 @@ QProgressBar::chunk#thrustBar {
<number>2</number>
</property>
<property name="margin">
<number>5</number>
<number>2</number>
</property>
<item>
<widget class="QCheckBox" name="selectedBox">
......@@ -205,7 +205,7 @@ QProgressBar::chunk#thrustBar {
<property name="sizeHint" stdset="0">
<size>
<width>25</width>
<height>12</height>
<height>0</height>
</size>
</property>
</spacer>
......@@ -355,7 +355,7 @@ QProgressBar::chunk#thrustBar {
<string/>
</property>
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/actions/go-up.svg</normaloff>:/images/actions/go-up.svg</iconset>
</property>
</widget>
......@@ -375,7 +375,7 @@ QProgressBar::chunk#thrustBar {
<string/>
</property>
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/actions/go-down.svg</normaloff>:/images/actions/go-down.svg</iconset>
</property>
</widget>
......@@ -395,7 +395,7 @@ QProgressBar::chunk#thrustBar {
<string/>
</property>
<property name="icon">
<iconset>
<iconset resource="../../mavground.qrc">
<normaloff>:/images/actions/list-remove.svg</normaloff>:/images/actions/list-remove.svg</iconset>
</property>
</widget>
......@@ -405,6 +405,8 @@ QProgressBar::chunk#thrustBar {
</item>
</layout>
</widget>
<resources/>
<resources>
<include location="../../mavground.qrc"/>
</resources>
<connections/>
</ui>
......@@ -213,8 +213,8 @@ public:
static const int SCALE_BEST_FIT = 1;
static const int SCALE_LOGARITHMIC = 2;
static const int DEFAULT_REFRESH_RATE = 40; ///< The default refresh rate is 25 Hz / every 100 ms
static const int DEFAULT_PLOT_INTERVAL = 1000 * 15; ///< The default plot interval is 15 seconds
static const int DEFAULT_REFRESH_RATE = 50; ///< The default refresh rate is 25 Hz / every 100 ms
static const int DEFAULT_PLOT_INTERVAL = 1000 * 12; ///< The default plot interval is 15 seconds
static const int DEFAULT_SCALE_INTERVAL = 1000 * 5;
public slots:
......
......@@ -90,7 +90,7 @@ updateTimer(new QTimer())
connect(this, SIGNAL(plotWindowPositionUpdated(int)), scrollbar, SLOT(setValue(int)));
connect(scrollbar, SIGNAL(sliderMoved(int)), this, SLOT(setPlotWindowPosition(int)));
updateTimer->setInterval(100);
updateTimer->setInterval(300);
connect(updateTimer, SIGNAL(timeout()), this, SLOT(refresh()));
updateTimer->start();
}
......@@ -247,13 +247,13 @@ void LinechartWidget::refresh()
str.sprintf("%+.2f", activePlot->getMean(j.key()));
j.value()->setText(str);
}
QMap<QString, QLabel*>::iterator k;
for (k = curveMedians->begin(); k != curveMedians->end(); ++k)
{
// Median
str.sprintf("%+.2f", activePlot->getMedian(k.key()));
k.value()->setText(str);
}
// QMap<QString, QLabel*>::iterator k;
// for (k = curveMedians->begin(); k != curveMedians->end(); ++k)
// {
// // Median
// str.sprintf("%+.2f", activePlot->getMedian(k.key()));
// k.value()->setText(str);
// }
}
......@@ -397,11 +397,11 @@ QWidget* LinechartWidget::createCurveItem(QString curve)
curveMeans->insert(curve, mean);
horizontalLayout->addWidget(mean);
// Median
median = new QLabel(form);
value->setNum(0.00);
curveMedians->insert(curve, median);
horizontalLayout->addWidget(median);
// // Median
// median = new QLabel(form);
// value->setNum(0.00);
// curveMedians->insert(curve, median);
// horizontalLayout->addWidget(median);
/* Color picker
QColor color = QColorDialog::getColor(Qt::green, this);
......@@ -418,7 +418,7 @@ QWidget* LinechartWidget::createCurveItem(QString curve)
horizontalLayout->setStretchFactor(label, 80);
horizontalLayout->setStretchFactor(value, 50);
horizontalLayout->setStretchFactor(mean, 50);
horizontalLayout->setStretchFactor(median, 50);
// horizontalLayout->setStretchFactor(median, 50);
// Connect actions
QObject::connect(checkBox, SIGNAL(clicked(bool)), this, SLOT(takeButtonClick(bool)));
......
#include "MAV2DIcon.h"
#include <QPainter>
MAV2DIcon::MAV2DIcon(QGraphicsItem* parent) :
QGC2DIcon(parent)
MAV2DIcon::MAV2DIcon(qreal x, qreal y, int radius, QString name, Alignment alignment, QPen* pen)
: Point(x, y, name, alignment),
yaw(0.0f)
{
size = QSize(radius, radius);
drawIcon(pen);
}
/**
* @return the bounding rectangle of the icon
*/
QRectF MAV2DIcon::boundingRect() const
MAV2DIcon::MAV2DIcon(qreal x, qreal y, QString name, Alignment alignment, QPen* pen)
: Point(x, y, name, alignment)
{
int radius = 10;
size = QSize(radius, radius);
drawIcon(pen);
}
MAV2DIcon::~MAV2DIcon()
{
delete mypixmap;
}
void MAV2DIcon::setPen(QPen* pen)
{
qreal penWidth = 1;
return QRectF(-10 - penWidth / 2, -10 - penWidth / 2,
20 + penWidth, 20 + penWidth);
mypen = pen;
drawIcon(pen);
}
/**
* @param painter QPainter to draw with
* @param option Visual style
* @param widget Parent widget
* @param yaw in radians, 0 = north, positive = clockwise
*/
void MAV2DIcon::paint(QPainter* painter, const QStyleOptionGraphicsItem* option, QWidget* widget)
void MAV2DIcon::setYaw(float yaw)
{
painter->drawRoundedRect(-10, -10, 20, 20, 5, 5);
this->yaw = yaw;
}
void MAV2DIcon::drawIcon(QPen* pen)
{
mypixmap = new QPixmap(radius+1, radius+1);
mypixmap->fill(Qt::transparent);
QPainter painter(mypixmap);
// DRAW MICRO AIR VEHICLE
QPointF p(radius/2, radius/2);
float waypointSize = radius;
QPolygonF poly(3);
// Top point
poly.replace(0, QPointF(p.x(), p.y()+waypointSize/2.0f));
// Right point
poly.replace(1, QPointF(p.x()-waypointSize/2.0f, p.y()-waypointSize/2.0f));
// Left point
poly.replace(2, QPointF(p.x()+waypointSize/2.0f, p.y() + waypointSize/2.0f));
// // Select color based on if this is the current waypoint
// if (list.at(i)->getCurrent())
// {
// color = QGC::colorCyan;//uas->getColor();
// pen.setWidthF(refLineWidthToPen(0.8f));
// }
// else
// {
// color = uas->getColor();
// pen.setWidthF(refLineWidthToPen(0.4f));
// }
//pen.setColor(color);
if (pen)
{
pen->setWidthF(2);
painter.setPen(*pen);
}
else
{
QPen pen2(Qt::yellow);
pen2.setWidth(2);
painter.setPen(pen2);
}
painter.setBrush(Qt::NoBrush);
painter.drawPolygon(poly);
}
#ifndef MAV2DICON_H
#define MAV2DICON_H
#include "QGC2DIcon.h"
#include "qmapcontrol.h"
class MAV2DIcon : public QGC2DIcon
class MAV2DIcon : public qmapcontrol::Point
{
public:
explicit MAV2DIcon(QGraphicsItem* parent = 0);
/*!
*
* @param x longitude
* @param y latitude
* @param name name of the circle point
* @param alignment alignment (Middle or TopLeft)
* @param pen QPen for drawing
*/
MAV2DIcon(qreal x, qreal y, QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
//!
/*!
*
* @param x longitude
* @param y latitude
* @param radius the radius of the circle
* @param name name of the circle point
* @param alignment alignment (Middle or TopLeft)
* @param pen QPen for drawing
*/
MAV2DIcon(qreal x, qreal y, int radius = 10, QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
virtual ~MAV2DIcon();
//! sets the QPen which is used for drawing the circle
/*!
* A QPen can be used to modify the look of the drawn circle
* @param pen the QPen which should be used for drawing
* @see http://doc.trolltech.com/4.3/qpen.html
*/
virtual void setPen(QPen* pen);
void setYaw(float yaw);
void drawIcon(QPen* pen);
protected:
float yaw; ///< Yaw angle of the MAV
int radius; ///< Maximum dimension of the MAV icon
QRectF boundingRect() const;
void paint(QPainter*, const QStyleOptionGraphicsItem*, QWidget*);
};
......
#include "QGC2DIcon.h"
QGC2DIcon::QGC2DIcon(QPointF localOriginInGlobalCoords, bool onlyLocal, QGraphicsItem* parent) :
QGraphicsItem(parent),
localOriginInGlobalCoords(localOriginInGlobalCoords),
local(onlyLocal)
{
}
QGC2DIcon::QGC2DIcon(bool onlyLocal, QGraphicsItem* parent) :
QGraphicsItem(parent),
localOriginInGlobalCoords(QPointF(0, 0)),
local(onlyLocal)
{
}
QGC2DIcon::QGC2DIcon(QGraphicsItem* parent) :
QGraphicsItem(parent),
localOriginInGlobalCoords(QPointF(0, 0)),
local(false)
{
}
QGC2DIcon::~QGC2DIcon()
{
}
QPointF QGC2DIcon::getGlobalPosition()
{
}
QPointF QGC2DIcon::getLocalPosition()
{
}
void QGC2DIcon::setGlobalPosition(QPointF pos)
{
}
void QGC2DIcon::setLocalPosition(QPointF pos)
{
}
void QGC2DIcon::setLocalPosition(float x, float y)
{
}
bool QGC2DIcon::isLocal()
{
return local;
}
#ifndef QGC2DICON_H
#define QGC2DICON_H
#include <QGraphicsItem>
#include <QPointF>
class QGC2DIcon : public QGraphicsItem
{
public:
QGC2DIcon(QPointF localOriginInGlobalCoords, bool onlyLocal=false, QGraphicsItem* parent = 0);
QGC2DIcon(bool onlyLocal=false, QGraphicsItem* parent = 0);
explicit QGC2DIcon(QGraphicsItem* parent = 0);
~QGC2DIcon();
QPointF getGlobalPosition();
QPointF getLocalPosition();
void setGlobalPosition(QPointF pos);
void setLocalPosition(QPointF pos);
void setLocalPosition(float x, float y);
bool isLocal();
virtual QRectF boundingRect() const = 0;
virtual void paint(QPainter*, const QStyleOptionGraphicsItem*, QWidget*) = 0;
signals:
public slots:
protected:
QPointF localOriginInGlobalCoords;
QPointF globalPosition;
QPointF localPosition;
bool local;
};
#endif // QGC2DICON_H
#include "Waypoint2DIcon.h"
#include <QPainter>
#include <QDebug>
Waypoint2DIcon::Waypoint2DIcon(qreal x, qreal y, int radius, QString name, Alignment alignment, QPen* pen)
: Point(x, y, name, alignment),
yaw(0.0f),
radius(radius)
{
size = QSize(radius, radius);
drawIcon(pen);
}
Waypoint2DIcon::Waypoint2DIcon(QGraphicsItem* parent) :
QGC2DIcon(parent)
Waypoint2DIcon::Waypoint2DIcon(qreal x, qreal y, QString name, Alignment alignment, QPen* pen)
: Point(x, y, name, alignment)
{
int radius = 10;
size = QSize(radius, radius);
drawIcon(pen);
}
/**
* @return the bounding rectangle of the icon
*/
QRectF Waypoint2DIcon::boundingRect() const
Waypoint2DIcon::~Waypoint2DIcon()
{
qreal penWidth = 1;
return QRectF(-10 - penWidth / 2, -10 - penWidth / 2,
20 + penWidth, 20 + penWidth);
delete mypixmap;
}
void Waypoint2DIcon::setPen(QPen* pen)
{
mypen = pen;
drawIcon(pen);
}
/**
* @param painter QPainter to draw with
* @param option Visual style
* @param widget Parent widget
* @param yaw in radians, 0 = north, positive = clockwise
*/
void Waypoint2DIcon::paint(QPainter* painter, const QStyleOptionGraphicsItem* option, QWidget* widget)
void Waypoint2DIcon::setYaw(float yaw)
{
qDebug() << __FILE__ << __LINE__ << "DRAWING";
painter->setPen(QPen(Qt::red));
painter->drawRoundedRect(-10, -10, 20, 20, 5, 5);
this->yaw = yaw;
}
void Waypoint2DIcon::drawIcon(QPen* pen)
{
mypixmap = new QPixmap(radius+1, radius+1);
mypixmap->fill(Qt::transparent);
QPainter painter(mypixmap);
// DRAW WAYPOINT
QPointF p(radius/2, radius/2);
float waypointSize = radius;
QPolygonF poly(4);
// Top point
poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
// Right point
poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
// Bottom point
poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
// // Select color based on if this is the current waypoint
// if (list.at(i)->getCurrent())
// {
// color = QGC::colorCyan;//uas->getColor();
// pen.setWidthF(refLineWidthToPen(0.8f));
// }
// else
// {
// color = uas->getColor();
// pen.setWidthF(refLineWidthToPen(0.4f));
// }
//pen.setColor(color);
if (pen)
{
pen->setWidthF(2);
painter.setPen(*pen);
}
else
{
QPen pen2(Qt::red);
pen2.setWidth(2);
painter.setPen(pen2);
}
painter.setBrush(Qt::NoBrush);
float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
painter.drawPolygon(poly);
}
......@@ -2,16 +2,49 @@
#define WAYPOINT2DICON_H
#include <QGraphicsItem>
#include "QGC2DIcon.h"
#include "qmapcontrol.h"
class Waypoint2DIcon : public QGC2DIcon
class Waypoint2DIcon : public qmapcontrol::Point
{
public:
explicit Waypoint2DIcon(QGraphicsItem* parent = 0);
/*!
*
* @param x longitude
* @param y latitude
* @param name name of the circle point
* @param alignment alignment (Middle or TopLeft)
* @param pen QPen for drawing
*/
Waypoint2DIcon(qreal x, qreal y, QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
QRectF boundingRect() const;
void paint(QPainter*, const QStyleOptionGraphicsItem*, QWidget*);
//!
/*!
*
* @param x longitude
* @param y latitude
* @param radius the radius of the circle
* @param name name of the circle point
* @param alignment alignment (Middle or TopLeft)
* @param pen QPen for drawing
*/
Waypoint2DIcon(qreal x, qreal y, int radius = 10, QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
virtual ~Waypoint2DIcon();
//! sets the QPen which is used for drawing the circle
/*!
* A QPen can be used to modify the look of the drawn circle
* @param pen the QPen which should be used for drawing
* @see http://doc.trolltech.com/4.3/qpen.html
*/
virtual void setPen(QPen* pen);
void setYaw(float yaw);
void drawIcon(QPen* pen);
protected:
float yaw; ///< Yaw angle of the MAV
int radius; ///<
};
......
This diff is collapsed.
......@@ -32,7 +32,11 @@ This file is part of the QGROUNDCONTROL project
#ifndef CHEETAHGL_H_
#define CHEETAHGL_H_
#if (defined __APPLE__) & (defined __MACH__)
#include <OpenGL/gl.h>
#else
#include <GL/gl.h>
#endif
/**
* @brief Creates a display list which renders a Pixhawk Cheetah MAV.
......
......@@ -32,7 +32,11 @@ This file is part of the QGROUNDCONTROL project
#ifndef CHEETAHMODEL_H_
#define CHEETAHMODEL_H_
#if (defined __APPLE__) & (defined __MACH__)
#include <OpenGL/gl.h>
#else
#include <GL/gl.h>
#endif
/**
* @brief Container for display list which renders the Pixhawk Cheetah MAV.
......
......@@ -32,8 +32,9 @@ This file is part of the QGROUNDCONTROL project
#include "Q3DWidget.h"
#include <cmath>
#include <GL/gl.h>
#include <GL/glu.h>
//#include <GL/gl.h>
//#include <GL/glu.h>
static const float KEY_ROTATE_AMOUNT = 5.0f;
static const float KEY_MOVE_AMOUNT = 10.0f;
......@@ -63,9 +64,9 @@ Q3DWidget::Q3DWidget(QWidget* parent)
, timerFuncData(NULL)
{
cameraPose.state = IDLE;
cameraPose.pan = 0.0f;
cameraPose.tilt = 180.0f;
cameraPose.distance = 10.0f;
cameraPose.pan = 50.0f;
cameraPose.tilt = 200.0f;
cameraPose.distance = 5.0f;
cameraPose.xOffset = 0.0f;
cameraPose.yOffset = 0.0f;
cameraPose.zOffset = 0.0f;
......
......@@ -32,7 +32,6 @@ This file is part of the QGROUNDCONTROL project
#ifndef Q3DWIDGET_H_
#define Q3DWIDGET_H_
#include <boost/scoped_ptr.hpp>
#include <inttypes.h>
#include <string>
#include <QtOpenGL>
......
......@@ -31,7 +31,6 @@ This file is part of the QGROUNDCONTROL project
#include "QMap3DWidget.h"
#include <FTGL/ftgl.h>
#include <GL/glut.h>
#include <QCheckBox>
#include <sys/time.h>
......@@ -44,7 +43,7 @@ QMap3DWidget::QMap3DWidget(QWidget* parent)
: Q3DWidget(parent)
, uas(NULL)
, lastRedrawTime(0.0)
, displayGrid(false)
, displayGrid(true)
, displayTrail(false)
, lockCamera(true)
, updateLastUnlockedPose(true)
......@@ -52,7 +51,7 @@ QMap3DWidget::QMap3DWidget(QWidget* parent)
{
setFocusPolicy(Qt::StrongFocus);
initialize(10, 10, 1000, 900, 10.0f);
initialize(10, 10, 1000, 900, 15.0f);
setCameraParams(0.05f, 0.5f, 0.01f, 0.5f, 30.0f, 0.01f, 400.0f);
int32_t argc = 0;
......@@ -64,7 +63,7 @@ QMap3DWidget::QMap3DWidget(QWidget* parent)
buildLayout();
font.reset(new FTTextureFont("images/Vera.ttf"));
//font.reset(new FTTextureFont("images/Vera.ttf"));
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
this, SLOT(setActiveUAS(UASInterface*)));
......@@ -93,17 +92,21 @@ QMap3DWidget::buildLayout(void)
lockCameraCheckBox->setText("Lock Camera");
lockCameraCheckBox->setChecked(lockCamera);
//positionLabel = new QLabel(this);
//positionLabel->setText(tr("Waiting for first position update.. "));
QGridLayout* layout = new QGridLayout(this);
layout->setMargin(0);
layout->setSpacing(2);
layout->addWidget(gridCheckBox, 1, 0);
layout->addWidget(trailCheckBox, 1, 1);
layout->addWidget(recenterButton, 1, 2);
layout->addWidget(lockCameraCheckBox, 1, 3);
layout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 2);
layout->addWidget(recenterButton, 1, 3);
layout->addWidget(lockCameraCheckBox, 1, 4);
layout->setRowStretch(0, 100);
layout->setRowStretch(1, 1);
layout->setColumnStretch(0, 1);
layout->setColumnStretch(1, 50);
//layout->setColumnStretch(0, 1);
layout->setColumnStretch(2, 50);
setLayout(layout);
connect(gridCheckBox, SIGNAL(stateChanged(int)),
......@@ -208,31 +211,46 @@ QMap3DWidget::displayHandler(void)
glVertex2f(getWindowWidth(), 0.0f);
glEnd();
char buffer[7][255];
sprintf(buffer[0], "x = %.2f", robotX);
sprintf(buffer[1], "y = %.2f", robotY);
sprintf(buffer[2], "z = %.2f", robotZ);
sprintf(buffer[3], "r = %.2f", robotRoll);
sprintf(buffer[4], "p = %.2f", robotPitch);
sprintf(buffer[5], "y = %.2f", robotYaw);
std::pair<float,float> 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();
// QT QPAINTER OPENGL PAINTING
QPainter painter;
painter.begin(this);
painter.setRenderHint(QPainter::Antialiasing, true);
painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
paintText(QString("x = %1 y = %2 z = %3 r = %4 p = %5 y = %6 Cursor [%7 %8]").arg(robotX, 0, 'f', 2).arg(robotY, 0, 'f', 2).arg(robotZ, 0, 'f', 2).arg(robotRoll, 0, 'f', 2).arg(robotPitch, 0, 'f', 2).arg(robotYaw, 0, 'f', 2).arg( mouseWorldCoords.first + robotX, 0, 'f', 2).arg( mouseWorldCoords.second + robotY, 0, 'f', 2),
QColor(255, 255, 255),
12,
5,
5,
&painter);
}
glTranslatef(0.0f, 30.0f, 0.0f);
for (int32_t i = 0; i < 7; ++i)
{
glTranslatef(60.0f, 0.0f, 0.0f);
font->Render(buffer[i]);
}
glPopMatrix();
void QMap3DWidget::paintText(QString text, QColor color, float fontSize, float refX, float refY, QPainter* painter)
{
QPen prevPen = painter->pen();
float pPositionX = refX;
float pPositionY = refY;
QFont font("Bitstream Vera Sans");
// Enforce minimum font size of 5 pixels
int fSize = qMax(5, (int)(fontSize));
font.setPixelSize(fSize);
QFontMetrics metrics = QFontMetrics(font);
int border = qMax(4, metrics.leading());
QRect rect = metrics.boundingRect(0, 0, width() - 2*border, int(height()*0.125),
Qt::AlignLeft | Qt::TextWordWrap, text);
painter->setPen(color);
painter->setFont(font);
painter->setRenderHint(QPainter::TextAntialiasing);
painter->drawText(pPositionX, pPositionY,
rect.width(), rect.height(),
Qt::AlignCenter | Qt::TextWordWrap, text);
painter->setPen(prevPen);
}
void
......@@ -382,17 +400,33 @@ QMap3DWidget::drawPlatform(float roll, float pitch, float yaw)
{
glPushMatrix();
glRotatef(yaw, 0.0f, 0.0f, 1.0f);
glRotatef(pitch, 0.0f, 1.0f, 0.0f);
glRotatef(roll, 1.0f, 0.0f, 0.0f);
glRotatef((yaw*180.0f)/M_PI, 0.0f, 0.0f, 1.0f);
glRotatef((pitch*180.0f)/M_PI, 0.0f, 1.0f, 0.0f);
glRotatef((roll*180.0f)/M_PI, 1.0f, 0.0f, 0.0f);
glLineWidth(3.0f);
glColor3f(0.0f, 1.0f, 0.0f);
// X AXIS
glColor3f(1.0f, 0.0f, 0.0f);
glBegin(GL_LINES);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.3f, 0.0f, 0.0f);
glEnd();
// Y AXIS
glColor3f(0.0f, 1.0f, 0.0f);
glBegin(GL_LINES);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, 0.15f, 0.0f);
glEnd();
// Z AXIS
glColor3f(0.0f, 0.0f, 1.0f);
glBegin(GL_LINES);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, 0.0f, 0.15f);
glEnd();
cheetahModel->draw();
glPopMatrix();
......
......@@ -32,10 +32,11 @@ This file is part of the QGROUNDCONTROL project
#ifndef QMAP3DWIDGET_H
#define QMAP3DWIDGET_H
#include <QLabel>
#include "Q3DWidget.h"
class CheetahModel;
class FTTextureFont;
class UASInterface;
/**
......@@ -76,6 +77,7 @@ private slots:
protected:
UASInterface* uas;
void paintText(QString text, QColor color, float fontSize, float refX, float refY, QPainter* painter);
private:
void drawPlatform(float roll, float pitch, float yaw);
......@@ -106,7 +108,6 @@ private:
Pose3D targetPosition;
QScopedPointer<CheetahModel> cheetahModel;
QScopedPointer<FTTextureFont> font;
};
#endif // QMAP3DWIDGET_H
......@@ -109,7 +109,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
// Heartbeat fade
refreshTimer = new QTimer(this);
connect(refreshTimer, SIGNAL(timeout()), this, SLOT(refresh()));
refreshTimer->start(180);
refreshTimer->start(updateInterval);
}
UASView::~UASView()
......@@ -195,7 +195,7 @@ void UASView::receiveHeartbeat(UASInterface* uas)
m_ui->heartbeatIcon->setStyleSheet(colorstyle);
m_ui->heartbeatIcon->setAutoFillBackground(true);
refreshTimer->stop();
refreshTimer->start(100);
refreshTimer->start(updateInterval);
}
}
......@@ -330,6 +330,21 @@ void UASView::updateLoad(UASInterface* uas, double load)
void UASView::refresh()
{
//setUpdatesEnabled(false);
//setUpdatesEnabled(true);
//repaint();
static quint64 lastupdate = 0;
qDebug() << "UASVIEW update diff: " << MG::TIME::getGroundTimeNow() - lastupdate;
lastupdate = MG::TIME::getGroundTimeNow();
// FIXME
static int generalUpdateCount = 0;
if (generalUpdateCount == 4)
{
generalUpdateCount = 0;
qDebug() << "UPDATING EVERYTHING";
// State
m_ui->stateLabel->setText(state);
m_ui->statusTextLabel->setText(stateDesc);
......@@ -411,6 +426,8 @@ void UASView::refresh()
QString timeText;
timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec);
m_ui->timeElapsedLabel->setText(timeText);
}
generalUpdateCount++;
// Fade heartbeat icon
// Make color darker
......@@ -421,6 +438,9 @@ void UASView::refresh()
heartbeatColor.red(), heartbeatColor.green(), heartbeatColor.blue());
m_ui->heartbeatIcon->setStyleSheet(colorstyle);
m_ui->heartbeatIcon->setAutoFillBackground(true);
//setUpdatesEnabled(true);
//setUpdatesEnabled(false);
}
void UASView::changeEvent(QEvent *e)
......
......@@ -99,6 +99,7 @@ protected:
float lon;
float alt;
float groundDistance;
static const int updateInterval = 300;
void mouseDoubleClickEvent (QMouseEvent * event);
......
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