Skip to content
GitLab
Commit dffd1ce9 authored by lm's avatar lm
Browse files

Added option to add local waypoints, added waypoint visualization

parent 0d2be655
Hide whitespace changes
Inline Side-by-side
src/uas/PxQuadMAV.cc
View file @ dffd1ce9
......@@ -136,8 +136,6 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit localizationChanged(this, status.position_fix);
emit visionLocalizationChanged(this, status.vision_fix);
emit gpsLocalizationChanged(this, status.gps_fix);
qDebug() << "ATT CONTROL IS" << status.control_att;
}
break;
default:
......
src/ui/HSIDisplay.cc
View file @ dffd1ce9
......@@ -234,11 +234,11 @@ void HSIDisplay::paintDisplay()
paintText(tr("E"), ringColor, 3.5f, + baseRadius + 2.0f, - 1.75f, &painter);
paintText(tr("W"), ringColor, 3.5f, - baseRadius - 5.5f, - 1.75f, &painter);
// // Just for testing
// bodyXSetCoordinate = 0.95 * bodyXSetCoordinate + 0.05 * uiXSetCoordinate;
// bodyYSetCoordinate = 0.95 * bodyYSetCoordinate + 0.05 * uiYSetCoordinate;
// bodyZSetCoordinate = 0.95 * bodyZSetCoordinate + 0.05 * uiZSetCoordinate;
// bodyYawSet = 0.95 * bodyYawSet + 0.05 * uiYawSet;
// // Just for testing
// bodyXSetCoordinate = 0.95 * bodyXSetCoordinate + 0.05 * uiXSetCoordinate;
// bodyYSetCoordinate = 0.95 * bodyYSetCoordinate + 0.05 * uiYSetCoordinate;
// bodyZSetCoordinate = 0.95 * bodyZSetCoordinate + 0.05 * uiZSetCoordinate;
// bodyYawSet = 0.95 * bodyYawSet + 0.05 * uiYawSet;
}
void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, QPainter& painter)
......@@ -507,11 +507,11 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir
bodyYawSet = yawDesired;
mavInitialized = true;
// qDebug() << "Received setpoint at x: " << x << "metric y:" << y;
// posXSet = xDesired;
// posYSet = yDesired;
// posZSet = zDesired;
// posYawSet = yawDesired;
// qDebug() << "Received setpoint at x: " << x << "metric y:" << y;
// posXSet = xDesired;
// posYSet = yDesired;
// posZSet = zDesired;
// posYawSet = yawDesired;
}
void HSIDisplay::updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec)
......@@ -632,59 +632,62 @@ void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color
void HSIDisplay::drawWaypoints(QPainter& painter)
{
QVector<Waypoint*> list = QVector<Waypoint*>();
list.append(new Waypoint(0, x, y, z, yaw, false, false, 0.5f, 2000));
list.append(new Waypoint(0, x+0.1, y+0.1, z, yaw, true, true, 0.5f, 2000));
if (uas)
{
QVector<Waypoint*> list = QVector<Waypoint*>();
list.append(new Waypoint(0, x, y, z, yaw, false, false, 0.5f, 2000));
list.append(new Waypoint(0, x+0.1, y+0.1, z, yaw, true, true, 0.5f, 2000));
QColor color;
painter.setBrush(Qt::NoBrush);
QColor color;
painter.setBrush(Qt::NoBrush);
QPointF lastWaypoint;
QPointF lastWaypoint;
for (int i = 0; i < list.size(); i++)
{
QPointF in(list.at(i)->getX(), list.at(i)->getY());
// Transform from world to body coordinates
in = metricWorldToBody(in);
// Scale from metric to screen reference coordinates
QPointF p = metricBodyToRef(in);
// Select color based on if this is the current waypoint
if (list.at(i)->getCurrent())
for (int i = 0; i < list.size(); i++)
{
color = uas->getColor().lighter().lighter();
}
else
{
color = uas->getColor();
}
QPointF in(list.at(i)->getX(), list.at(i)->getY());
// Transform from world to body coordinates
in = metricWorldToBody(in);
// Scale from metric to screen reference coordinates
QPointF p = metricBodyToRef(in);
// Select color based on if this is the current waypoint
if (list.at(i)->getCurrent())
{
color = uas->getColor().lighter().lighter();
}
else
{
color = uas->getColor();
}
// Setup pen
QPen pen(color);
pen.setWidthF(refLineWidthToPen(0.4f));
painter.setPen(pen);
// Setup pen
QPen pen(color);
pen.setWidthF(refLineWidthToPen(0.4f));
painter.setPen(pen);
// Draw line from last waypoint to this one
if (!lastWaypoint.isNull())
{
drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f), color, &painter);
// Draw line from last waypoint to this one
if (!lastWaypoint.isNull())
{
drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f), color, &painter);
}
lastWaypoint = p;
//drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter);
float waypointSize = vwidth / 20.0f * 2.0f;
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()));
drawPolygon(poly, &painter);
float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
painter.setBrush(color);
}
lastWaypoint = p;
//drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter);
float waypointSize = vwidth / 20.0f * 2.0f;
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()));
drawPolygon(poly, &painter);
float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
painter.setBrush(color);
}
}
......
src/ui/WaypointList.cc
View file @ dffd1ce9
......@@ -41,6 +41,10 @@ This file is part of the PIXHAWK project
WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
QWidget(parent),
uas(NULL),
mavX(0.0),
mavY(0.0),
mavZ(0.0),
mavYaw(0.0),
m_ui(new Ui::WaypointList)
{
m_ui->setupUi(this);
......@@ -60,6 +64,9 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
connect(m_ui->addButton, SIGNAL(clicked()), m_ui->actionAddWaypoint, SIGNAL(triggered()));
connect(m_ui->actionAddWaypoint, SIGNAL(triggered()), this, SLOT(add()));
// ADD WAYPOINT AT CURRENT POSITION
connect(m_ui->positionAddButton, SIGNAL(clicked()), this, SLOT(addCurrentPositonWaypoint()));
// SEND WAYPOINTS
connect(m_ui->transmitButton, SIGNAL(clicked()), this, SLOT(transmit()));
......@@ -89,6 +96,24 @@ void WaypointList::updateStatusLabel(const QString &string)
m_ui->statusLabel->setText(string);
}
void WaypointList::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec)
{
Q_UNUSED(uas);
Q_UNUSED(usec);
mavX = x;
mavY = y;
mavZ = z;
}
void WaypointList::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec)
{
Q_UNUSED(uas);
Q_UNUSED(usec);
Q_UNUSED(roll);
Q_UNUSED(pitch);
mavYaw = yaw;
}
void WaypointList::setUAS(UASInterface* uas)
{
if (this->uas == NULL && uas != NULL)
......@@ -98,6 +123,8 @@ void WaypointList::setUAS(UASInterface* uas)
connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &)));
connect(&uas->getWaypointManager(), SIGNAL(waypointUpdated(int,quint16,double,double,double,double,bool,bool,double,int)), this, SLOT(setWaypoint(int,quint16,double,double,double,double,bool,bool,double,int)));
connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
connect(this, SIGNAL(sendWaypoints(const QVector<Waypoint*> &)), &uas->getWaypointManager(), SLOT(sendWaypoints(const QVector<Waypoint*> &)));
connect(this, SIGNAL(requestWaypoints()), &uas->getWaypointManager(), SLOT(requestWaypoints()));
......@@ -173,11 +200,28 @@ void WaypointList::add()
}
else
{
addWaypoint(new Waypoint(waypoints.size(), 1.1, 1.1, -0.8, 0.0, true, true, 0.1, 1000));
addWaypoint(new Waypoint(waypoints.size(), 1.1, 1.1, -0.8, 0.0, true, true, 0.15, 2000));
}
}
}
void WaypointList::addCurrentPositonWaypoint()
{
if (uas)
{
if (waypoints.size() > 0)
{
Waypoint *last = waypoints.at(waypoints.size()-1);
addWaypoint(new Waypoint(waypoints.size(), mavX, mavY, mavZ, mavYaw, last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()));
}
else
{
addWaypoint(new Waypoint(waypoints.size(), mavX, mavY, mavZ, mavYaw, true, true, 0.15, 2000));
}
}
}
void WaypointList::addWaypoint(Waypoint* wp)
{
waypoints.push_back(wp);
......
src/ui/WaypointList.h
View file @ dffd1ce9
......@@ -62,7 +62,10 @@ public slots:
void loadWaypoints();
void transmit();
void read();
/** @brief Add a waypoint */
void add();
/** @brief Add a waypoint at the current MAV position */
void addCurrentPositonWaypoint();
void moveUp(Waypoint* wp);
void moveDown(Waypoint* wp);
......@@ -76,6 +79,8 @@ public slots:
void addWaypoint(Waypoint* wp);
void removeWaypoint(Waypoint* wp);
void waypointReached(UASInterface* uas, quint16 waypointId);
void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec);
void updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
signals:
void sendWaypoints(const QVector<Waypoint*> &);
......@@ -89,6 +94,10 @@ protected:
QMap<Waypoint*, WaypointView*> wpViews;
QVBoxLayout* listLayout;
UASInterface* uas;
double mavX;
double mavY;
double mavZ;
double mavYaw;
private:
Ui::WaypointList *m_ui;
......
src/ui/WaypointList.ui
View file @ dffd1ce9
......@@ -26,7 +26,7 @@
<property name="spacing">
<number>4</number>
</property>
<item row="0" column="0" colspan="8">
<item row="0" column="0" colspan="9">
<widget class="QScrollArea" name="scrollArea">
<property name="widgetResizable">
<bool>true</bool>
......@@ -37,7 +37,7 @@
<x>0</x>
<y>0</y>
<width>466</width>
<height>148</height>
<height>149</height>
</rect>
</property>
<layout class="QHBoxLayout" name="horizontalLayout">
......@@ -54,7 +54,7 @@
</widget>
</widget>
</item>
<item row="2" column="6">
<item row="2" column="7">
<widget class="QPushButton" name="readButton">
<property name="text">
<string>Read</string>
......@@ -65,7 +65,7 @@
</property>
</widget>
</item>
<item row="2" column="7">
<item row="2" column="8">
<widget class="QPushButton" name="transmitButton">
<property name="text">
<string>Write</string>
......@@ -76,14 +76,14 @@
</property>
</widget>
</item>
<item row="3" column="0" colspan="8">
<item row="3" column="0" colspan="9">
<widget class="QLabel" name="statusLabel">
<property name="text">
<string>TextLabel</string>
</property>
</widget>
</item>
<item row="2" column="5">
<item row="2" column="6">
<widget class="QToolButton" name="addButton">
<property name="text">
<string>...</string>
......@@ -121,6 +121,20 @@
</property>
</spacer>
</item>
<item row="2" column="5">
<widget class="QToolButton" name="positionAddButton">
<property name="toolTip">
<string>Set the current vehicle position as new waypoint</string>
</property>
<property name="text">
<string>...</string>
</property>
<property name="icon">
<iconset resource="../../mavground.qrc">
<normaloff>:/images/actions/go-bottom.svg</normaloff>:/images/actions/go-bottom.svg</iconset>
</property>
</widget>
</item>
</layout>
<action name="actionAddWaypoint">
<property name="icon">
......
src/ui/uas/UASInfoWidget.cc
View file @ dffd1ce9
......@@ -125,7 +125,7 @@ void UASInfoWidget::updateCPULoad(UASInterface* uas, double load)
{
if (activeUAS == uas)
{
this->load = load;
this->load = this->load * 0.8f + load * 0.2f;
}
}
......
Supports Markdown
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