diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 1fee5ee7d01aab520236f92fa30d559a3efdafd2..be2348b9fe1d17a10300ea8a9e0704f13faad295 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -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: diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 2d71385d08115d59357feb71307e25ee7e103e0a..bdccbf302d3f0476ecf7be54c960ef86a058df9b 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -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 list = QVector(); - 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 list = QVector(); + 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); } } diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 11e1a0ee3fcca5b8b37c115e9705cf3c9fae0bce..ff0acf160ac50997190cf26b36699f046776fe49 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -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 &)), &uas->getWaypointManager(), SLOT(sendWaypoints(const QVector &))); 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); diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index 1164b9f00ee2d4bde320c7827c8fe78a1379ee9b..1494edfdd9f855c2d63f2b30b28495772d1f4d19 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -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 &); @@ -89,6 +94,10 @@ protected: QMap wpViews; QVBoxLayout* listLayout; UASInterface* uas; + double mavX; + double mavY; + double mavZ; + double mavYaw; private: Ui::WaypointList *m_ui; diff --git a/src/ui/WaypointList.ui b/src/ui/WaypointList.ui index c70a5e79b4129004b7259809d315fd0a421742dc..8ada605ccf5e8502e3c28406817897b4e932a231 100644 --- a/src/ui/WaypointList.ui +++ b/src/ui/WaypointList.ui @@ -26,7 +26,7 @@ 4 - + true @@ -37,7 +37,7 @@ 0 0 466 - 148 + 149 @@ -54,7 +54,7 @@ - + Read @@ -65,7 +65,7 @@ - + Write @@ -76,14 +76,14 @@ - + TextLabel - + ... @@ -121,6 +121,20 @@ + + + + Set the current vehicle position as new waypoint + + + ... + + + + :/images/actions/go-bottom.svg:/images/actions/go-bottom.svg + + + diff --git a/src/ui/uas/UASInfoWidget.cc b/src/ui/uas/UASInfoWidget.cc index c835c5f64daa52f7f9734ef94e2e1e118ef8aaa3..0b926de03c1c572994f0c3fadd115651962af44f 100644 --- a/src/ui/uas/UASInfoWidget.cc +++ b/src/ui/uas/UASInfoWidget.cc @@ -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; } }