diff --git a/parameters_alpha.txt b/parameters_alpha.txt index 949fd543c351efbf1af98b6c8558935529b031a2..e5134d1a89a0885db312ec0a6571daddb04a52c8 100644 --- a/parameters_alpha.txt +++ b/parameters_alpha.txt @@ -31,10 +31,12 @@ 42 200 PID_ATT_AWU 0.3 42 200 PID_ATT_D 30 42 200 PID_ATT_I 60 +42 200 PID_ATT_LIM 100 42 200 PID_ATT_P 90 42 200 PID_POS_AWU 5 42 200 PID_POS_D 2 42 200 PID_POS_I 0.4 +42 200 PID_POS_LIM 0.2 42 200 PID_POS_P 1.8 42 200 PID_POS_Z_AWU 3 42 200 PID_POS_Z_D 0.2 @@ -44,17 +46,19 @@ 42 200 PID_YAWPOS_AWU 1 42 200 PID_YAWPOS_D 1 42 200 PID_YAWPOS_I 0.1 +42 200 PID_YAWPOS_LIM 2 42 200 PID_YAWPOS_P 5 42 200 PID_YAWSPEED_D 0 -42 200 PID_YAWSPEED_I 10 -42 200 PID_YAWSPEED_P 30 +42 200 PID_YAWSPEED_I 5 +42 200 PID_YAWSPEED_P 15 42 200 PID_YAWSPE_AWU 1 +42 200 PID_YAWSPE_LIM 50 42 200 POS_SP_ACCEPT 1 -42 200 POS_SP_X 3.7 -42 200 POS_SP_Y 0.3 -42 200 POS_SP_YAW 0 -42 200 POS_SP_Z 0 -42 200 POS_TIMEOUT 2e+06 +42 200 POS_SP_X 1.09 +42 200 POS_SP_Y 0.527403 +42 200 POS_SP_YAW 1.57763 +42 200 POS_SP_Z -0.7 +42 200 POS_TIMEOUT 1e+06 42 200 RC_NICK_CHAN 1 42 200 RC_ROLL_CHAN 2 42 200 RC_SAFETY_CHAN 5 diff --git a/parameters_bravo.txt b/parameters_bravo.txt index 58bb861b97da81ca4aa7e99137fc3cd1ddee93cd..029850bfeb745ae6573bca01a14704bcc3a74d00 100644 --- a/parameters_bravo.txt +++ b/parameters_bravo.txt @@ -20,41 +20,45 @@ 42 200 DEBUG_4 0 42 200 DEBUG_5 0 42 200 DEBUG_6 0 -42 200 GYRO_OFFSET_X 29830 -42 200 GYRO_OFFSET_Y 29990 -42 200 GYRO_OFFSET_Z 29440 +42 200 GYRO_OFFSET_X 29884 +42 200 GYRO_OFFSET_Y 29943 +42 200 GYRO_OFFSET_Z 29479 42 200 MIX_OFFSET 0 42 200 MIX_POSITION 0 42 200 MIX_POS_YAW 0 42 200 MIX_REMOTE 0 42 200 MIX_Z_POSITION 0 42 200 PID_ATT_AWU 0.3 -42 200 PID_ATT_D 20 -42 200 PID_ATT_I 40 -42 200 PID_ATT_P 60 +42 200 PID_ATT_D 10 +42 200 PID_ATT_I 20 +42 200 PID_ATT_LIM 100 +42 200 PID_ATT_P 55 42 200 PID_POS_AWU 5 42 200 PID_POS_D 2 42 200 PID_POS_I 0.25 +42 200 PID_POS_LIM 0.2 42 200 PID_POS_P 1.7 42 200 PID_POS_Z_AWU 3 42 200 PID_POS_Z_D 0.2 42 200 PID_POS_Z_I 0.2 42 200 PID_POS_Z_LIM 0.3 -42 200 PID_POS_Z_P 0.5 +42 200 PID_POS_Z_P 0.3 42 200 PID_YAWPOS_AWU 1 42 200 PID_YAWPOS_D 1 42 200 PID_YAWPOS_I 0.1 +42 200 PID_YAWPOS_LIM 2 42 200 PID_YAWPOS_P 5 42 200 PID_YAWSPEED_D 0 -42 200 PID_YAWSPEED_I 10 -42 200 PID_YAWSPEED_P 30 +42 200 PID_YAWSPEED_I 5 +42 200 PID_YAWSPEED_P 15 42 200 PID_YAWSPE_AWU 1 -42 200 POS_SP_ACCEPT 0 -42 200 POS_SP_X 3.7 -42 200 POS_SP_Y 0.3 +42 200 PID_YAWSPE_LIM 50 +42 200 POS_SP_ACCEPT 1 +42 200 POS_SP_X 1.1 +42 200 POS_SP_Y 1.1 42 200 POS_SP_YAW 0 42 200 POS_SP_Z -0.8 -42 200 POS_TIMEOUT 500000 +42 200 POS_TIMEOUT 1e+06 42 200 RC_NICK_CHAN 1 42 200 RC_ROLL_CHAN 2 42 200 RC_SAFETY_CHAN 5 diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 92e33634aadaf51a785a8cd611bb584a5ea31e7e..695561b08cc444096bd9b0e12f43f63abf283e02 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -138,8 +138,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/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 05523810cfc7c5cc9dbe2db5613e7ebe6dff284b..9a54fcf786e5d0bbddd85b1d33df39c7ae8487b9 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -94,7 +94,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ if(wp->seq == current_wp_id) { //update the UI FIXME - emit waypointUpdated(uas.getUASID(), wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->orbit, wp->hold_time); + emit waypointUpdated(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->orbit, wp->hold_time); //get next waypoint current_wp_id++; @@ -206,15 +206,15 @@ void UASWaypointManager::requestWaypoints() } } -void UASWaypointManager::sendWaypoints(const QVector &list) +void UASWaypointManager::sendWaypoints() { if (current_state == WP_IDLE) { - if (list.count() > 0) + if (waypoints.count() > 0) { protocol_timer.start(PROTOCOL_TIMEOUT_MS); - current_count = list.count(); + current_count = waypoints.count(); current_state = WP_SENDLIST; current_wp_id = 0; current_partner_systemid = uas.getUASID(); @@ -233,7 +233,7 @@ void UASWaypointManager::sendWaypoints(const QVector &list) waypoint_buffer.push_back(new mavlink_waypoint_t); mavlink_waypoint_t *cur_d = waypoint_buffer.back(); memset(cur_d, 0, sizeof(mavlink_waypoint_t)); //initialize with zeros - const Waypoint *cur_s = list.at(i); + const Waypoint *cur_s = waypoints.at(i); cur_d->autocontinue = cur_s->getAutoContinue(); cur_d->current = cur_s->getCurrent(); diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 8e7e7458453b96c9cee4c9537b3813a9448af043..f8432d31bb35a1bc99d8dfe5b19d9faac17744c7 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -60,6 +60,8 @@ public: void handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr); void handleWaypointSetCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_set_current_t *wpr); + QVector &getWaypointList(void) { return waypoints; } + private: void sendWaypointRequest(quint16 seq); void sendWaypoint(quint16 seq); @@ -68,12 +70,12 @@ public slots: void timeout(); void clearWaypointList(); void requestWaypoints(); - void sendWaypoints(const QVector &list); + void sendWaypoints(); signals: - void waypointUpdated(int,quint16,double,double,double,double,bool,bool,double,int); ///< Adds a waypoint to the waypoint list widget - void currentWaypointChanged(quint16); ///< emits the new current waypoint sequence number - void updateStatusString(const QString &); ///< emits the current status string + void waypointUpdated(quint16,double,double,double,double,bool,bool,double,int); ///< Adds a waypoint to the waypoint list widget + void currentWaypointChanged(quint16); ///< emits the new current waypoint sequence number + void updateStatusString(const QString &); ///< emits the current status string private: UAS &uas; ///< Reference to the corresponding UAS @@ -83,6 +85,7 @@ private: quint8 current_partner_systemid; ///< The current protocol communication target system quint8 current_partner_compid; ///< The current protocol communication target component + QVector waypoints; ///< local waypoint list QVector waypoint_buffer; ///< communication buffer for waypoints QTimer protocol_timer; ///< Timer to catch timeouts }; diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 870ca033948ea0eceb3ae6df419a37406c15bb71..645d2832d3bdc6dffa7482e6ba90c31f80554df5 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -36,6 +36,7 @@ This file is part of the PIXHAWK project #include "HSIDisplay.h" #include "MG.h" #include "QGC.h" +#include "Waypoint.h" #include @@ -79,7 +80,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) : uiYSetCoordinate(0.0f), uiZSetCoordinate(0.0f), uiYawSet(0.0f), - metricWidth(2.0f), + metricWidth(4.0f), positionLock(false), attControlEnabled(false), xyControlEnabled(false), @@ -207,9 +208,12 @@ void HSIDisplay::paintDisplay() drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, QGC::colorCyan, &painter); } + // Draw waypoints + drawWaypoints(painter); + // Labels on outer part and bottom - //if (localAvailable > 0) + if (localAvailable > 0) { // Position QString str; @@ -230,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) @@ -503,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) @@ -628,25 +632,64 @@ void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color void HSIDisplay::drawWaypoints(QPainter& painter) { - QColor color = uas->getColor(); - float x = 1.1; - float y = 1.1; - float radius = vwidth / 20.0f; - QPen pen(color); - pen.setWidthF(refLineWidthToPen(0.4f)); - pen.setColor(color); - painter.setPen(pen); - painter.setBrush(Qt::NoBrush); - QPointF in(x, y); - // Transform from body to world coordinates - in = metricWorldToBody(in); - // Scale from metric to screen reference coordinates - QPointF p = metricBodyToRef(in); - drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter); - radius *= 0.8; - drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * radius, refLineWidthToPen(0.4f), color, &painter); - painter.setBrush(color); - drawCircle(p.x(), p.y(), radius * 0.1f, 0.1f, color, &painter); + 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); + + 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()) + { + color = uas->getColor().lighter().lighter(); + } + else + { + color = uas->getColor(); + } + + // Setup pen + QPen pen(color); + pen.setWidthF(refLineWidthToPen(0.4f)); + painter.setPen(pen); + painter.setBrush(Qt::NoBrush); + + // 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); + } + } } void HSIDisplay::drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter) diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 17524f09d50cbed1bdc901ee9d066d5ed9ce42e9..630db5dffaa2033347bf2cee982f6e703bf593bb 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -99,7 +99,7 @@ protected slots: void drawWaypoints(QPainter& painter); /** @brief Draw the limiting safety area */ void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter); - + /** @brief Receive mouse clicks */ void mouseDoubleClickEvent(QMouseEvent* event); protected: diff --git a/src/ui/UASControl.ui b/src/ui/UASControl.ui index c1a095749e0cb2cde8db34ff6ce149fb01b79d85..bc34fd146b929d2281b9ca6962fe1b74888364c3 100644 --- a/src/ui/UASControl.ui +++ b/src/ui/UASControl.ui @@ -6,14 +6,20 @@ 0 0 - 380 - 190 + 280 + 164 + + + 280 + 130 + + Form - + 6 @@ -147,10 +153,13 @@ Qt::Vertical + + QSizePolicy::Expanding + - 20 - 0 + 0 + 1 diff --git a/src/ui/UASInfo.ui b/src/ui/UASInfo.ui index 029e796c19b97702463ec9d0556510d933d2d09c..a86c9a2623c27ad2c2a54915c61e67acdba7933f 100644 --- a/src/ui/UASInfo.ui +++ b/src/ui/UASInfo.ui @@ -287,7 +287,7 @@ - CPU Load + MCU Load @@ -370,14 +370,14 @@ - + Qt::Horizontal - + Qt::Vertical @@ -393,13 +393,69 @@ - + No error status received yet + + + + CPU Load + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + % + + + + + + + + 16777215 + 20 + + + + + 0 + 18 + + + + 24 + + + + + + + Qt::Horizontal + + + + 13 + 15 + + + + diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 11e1a0ee3fcca5b8b37c115e9705cf3c9fae0bce..42c8cd5edb6322c1ac89bd34417f9b1edc95ad98 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())); @@ -72,11 +79,11 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); - // STATUS LABEL - updateStatusLabel(""); - // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED setUAS(uas); + + // STATUS LABEL + updateStatusLabel(""); } WaypointList::~WaypointList() @@ -86,7 +93,28 @@ WaypointList::~WaypointList() void WaypointList::updateStatusLabel(const QString &string) { - m_ui->statusLabel->setText(string); + if (this->uas) + { + 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) @@ -95,69 +123,81 @@ void WaypointList::setUAS(UASInterface* uas) { this->uas = 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->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &))); + connect(&uas->getWaypointManager(), SIGNAL(waypointUpdated(quint16,double,double,double,double,bool,bool,double,int)), this, SLOT(setWaypoint(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())); - connect(this, SIGNAL(clearWaypointList()), &uas->getWaypointManager(), SLOT(clearWaypointList())); + connect(this, SIGNAL(sendWaypoints()), &uas->getWaypointManager(), SLOT(sendWaypoints())); + connect(this, SIGNAL(requestWaypoints()), &uas->getWaypointManager(), SLOT(requestWaypoints())); + connect(this, SIGNAL(clearWaypointList()), &uas->getWaypointManager(), SLOT(clearWaypointList())); } } -void WaypointList::setWaypoint(int uasId, quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime) +void WaypointList::setWaypoint(quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime) { - if (uasId == this->uas->getUASID()) + if (this->uas) { Waypoint* wp = new Waypoint(id, x, y, z, yaw, autocontinue, current, orbit, holdTime); addWaypoint(wp); } } -void WaypointList::waypointReached(UASInterface* uas, quint16 waypointId) +void WaypointList::waypointReached(quint16 waypointId) { - Q_UNUSED(uas); - qDebug() << "Waypoint reached: " << waypointId; - - updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId)); + if (this->uas) + { + updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId)); + } } void WaypointList::currentWaypointChanged(quint16 seq) { - if (seq < waypoints.size()) + if (this->uas) { - for(int i = 0; i < waypoints.size(); i++) - { - WaypointView* widget = wpViews.find(waypoints[i]).value(); + QVector &waypoints = uas->getWaypointManager().getWaypointList(); - if (waypoints[i]->getId() == seq) - { - waypoints[i]->setCurrent(true); - widget->setCurrent(true); - } - else + if (seq < waypoints.size()) + { + for(int i = 0; i < waypoints.size(); i++) { - waypoints[i]->setCurrent(false); - widget->setCurrent(false); + WaypointView* widget = wpViews.find(waypoints[i]).value(); + + if (waypoints[i]->getId() == seq) + { + waypoints[i]->setCurrent(true); + widget->setCurrent(true); + } + else + { + waypoints[i]->setCurrent(false); + widget->setCurrent(false); + } } + redrawList(); } - redrawList(); } } void WaypointList::read() { - while(waypoints.size()>0) + if (uas) { - removeWaypoint(waypoints[0]); - } + QVector &waypoints = uas->getWaypointManager().getWaypointList(); - emit requestWaypoints(); + while(waypoints.size()>0) + { + removeWaypoint(waypoints[0]); + } + + emit requestWaypoints(); + } } void WaypointList::transmit() { - emit sendWaypoints(waypoints); + emit sendWaypoints(); //emit requestWaypoints(); FIXME } @@ -166,6 +206,8 @@ void WaypointList::add() // Only add waypoints if UAS is present if (uas) { + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + if (waypoints.size() > 0) { Waypoint *last = waypoints.at(waypoints.size()-1); @@ -173,111 +215,153 @@ 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::addWaypoint(Waypoint* wp) +void WaypointList::addCurrentPositonWaypoint() { - waypoints.push_back(wp); - if (!wpViews.contains(wp)) + if (uas) { - WaypointView* wpview = new WaypointView(wp, this); - wpViews.insert(wp, wpview); - listLayout->addWidget(wpViews.value(wp)); - connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); - connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); - connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); - connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + 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::redrawList() +void WaypointList::addWaypoint(Waypoint* wp) { - // Clear list layout - if (!wpViews.empty()) + if (uas) { - QMapIterator viewIt(wpViews); - viewIt.toFront(); - while(viewIt.hasNext()) + uas->getWaypointManager().getWaypointList().push_back(wp); + if (!wpViews.contains(wp)) { - viewIt.next(); - listLayout->removeWidget(viewIt.value()); + WaypointView* wpview = new WaypointView(wp, this); + wpViews.insert(wp, wpview); + listLayout->addWidget(wpViews.value(wp)); + connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); + connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); + connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); + connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); } - // Re-add waypoints - for(int i = 0; i < waypoints.size(); i++) + } +} + +void WaypointList::redrawList() +{ + if (uas) + { + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + // Clear list layout + if (!wpViews.empty()) { - listLayout->addWidget(wpViews.value(waypoints[i])); + QMapIterator viewIt(wpViews); + viewIt.toFront(); + while(viewIt.hasNext()) + { + viewIt.next(); + listLayout->removeWidget(viewIt.value()); + } + // Re-add waypoints + for(int i = 0; i < waypoints.size(); i++) + { + listLayout->addWidget(wpViews.value(waypoints[i])); + } } } } void WaypointList::moveUp(Waypoint* wp) { - int id = wp->getId(); - if (waypoints.size() > 1 && waypoints.size() > id) + if (uas) { - Waypoint* temp = waypoints[id]; - if (id > 0) - { - waypoints[id] = waypoints[id-1]; - waypoints[id-1] = temp; - waypoints[id-1]->setId(id-1); - waypoints[id]->setId(id); - } - else + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + int id = wp->getId(); + if (waypoints.size() > 1 && waypoints.size() > id) { - waypoints[id] = waypoints[waypoints.size()-1]; - waypoints[waypoints.size()-1] = temp; - waypoints[waypoints.size()-1]->setId(waypoints.size()-1); - waypoints[id]->setId(id); + Waypoint* temp = waypoints[id]; + if (id > 0) + { + waypoints[id] = waypoints[id-1]; + waypoints[id-1] = temp; + waypoints[id-1]->setId(id-1); + waypoints[id]->setId(id); + } + else + { + waypoints[id] = waypoints[waypoints.size()-1]; + waypoints[waypoints.size()-1] = temp; + waypoints[waypoints.size()-1]->setId(waypoints.size()-1); + waypoints[id]->setId(id); + } + redrawList(); } - redrawList(); } } void WaypointList::moveDown(Waypoint* wp) { - int id = wp->getId(); - if (waypoints.size() > 1 && waypoints.size() > id) + if (uas) { - Waypoint* temp = waypoints[id]; - if (id != waypoints.size()-1) - { - waypoints[id] = waypoints[id+1]; - waypoints[id+1] = temp; - waypoints[id+1]->setId(id+1); - waypoints[id]->setId(id); - } - else + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + int id = wp->getId(); + if (waypoints.size() > 1 && waypoints.size() > id) { - waypoints[id] = waypoints[0]; - waypoints[0] = temp; - waypoints[0]->setId(0); - waypoints[id]->setId(id); + Waypoint* temp = waypoints[id]; + if (id != waypoints.size()-1) + { + waypoints[id] = waypoints[id+1]; + waypoints[id+1] = temp; + waypoints[id+1]->setId(id+1); + waypoints[id]->setId(id); + } + else + { + waypoints[id] = waypoints[0]; + waypoints[0] = temp; + waypoints[0]->setId(0); + waypoints[id]->setId(id); + } + redrawList(); } - redrawList(); } } void WaypointList::removeWaypoint(Waypoint* wp) { - // Delete from list - if (wp != NULL) + if (uas) { - waypoints.remove(wp->getId()); - for(int i = wp->getId(); i < waypoints.size(); i++) + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + // Delete from list + if (wp != NULL) { - waypoints[i]->setId(i); - } + waypoints.remove(wp->getId()); + for(int i = wp->getId(); i < waypoints.size(); i++) + { + waypoints[i]->setId(i); + } - // Remove from view - WaypointView* widget = wpViews.find(wp).value(); - wpViews.remove(wp); - widget->hide(); - listLayout->removeWidget(widget); - delete wp; + // Remove from view + WaypointView* widget = wpViews.find(wp).value(); + wpViews.remove(wp); + widget->hide(); + listLayout->removeWidget(widget); + delete wp; + } } } @@ -294,40 +378,50 @@ void WaypointList::changeEvent(QEvent *e) void WaypointList::saveWaypoints() { - QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)")); - QFile file(fileName); - if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) - return; - - QTextStream in(&file); - for (int i = 0; i < waypoints.size(); i++) + if (uas) { - Waypoint* wp = waypoints[i]; - in << "\t" << wp->getId() << "\t" << wp->getX() << "\t" << wp->getY() << "\t" << wp->getZ() << "\t" << wp->getYaw() << "\t" << wp->getAutoContinue() << "\t" << wp->getCurrent() << "\t" << wp->getOrbit() << "\t" << wp->getHoldTime() << "\n"; - in.flush(); + QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)")); + QFile file(fileName); + if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) + return; + + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + QTextStream in(&file); + for (int i = 0; i < waypoints.size(); i++) + { + Waypoint* wp = waypoints[i]; + in << "\t" << wp->getId() << "\t" << wp->getX() << "\t" << wp->getY() << "\t" << wp->getZ() << "\t" << wp->getYaw() << "\t" << wp->getAutoContinue() << "\t" << wp->getCurrent() << "\t" << wp->getOrbit() << "\t" << wp->getHoldTime() << "\n"; + in.flush(); + } + file.close(); } - file.close(); } void WaypointList::loadWaypoints() { - QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); - QFile file(fileName); - if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) - return; - - while(waypoints.size()>0) + if (uas) { - removeWaypoint(waypoints[0]); - } + QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); + QFile file(fileName); + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) + return; - QTextStream in(&file); - while (!in.atEnd()) - { - QStringList wpParams = in.readLine().split("\t"); - if (wpParams.size() == 10) - addWaypoint(new Waypoint(wpParams[1].toInt(), wpParams[2].toDouble(), wpParams[3].toDouble(), wpParams[4].toDouble(), wpParams[5].toDouble(), (wpParams[6].toInt() == 1 ? true : false), (wpParams[7].toInt() == 1 ? true : false), wpParams[8].toDouble(), wpParams[9].toInt())); + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + while(waypoints.size()>0) + { + removeWaypoint(waypoints[0]); + } + + QTextStream in(&file); + while (!in.atEnd()) + { + QStringList wpParams = in.readLine().split("\t"); + if (wpParams.size() == 10) + addWaypoint(new Waypoint(wpParams[1].toInt(), wpParams[2].toDouble(), wpParams[3].toDouble(), wpParams[4].toDouble(), wpParams[5].toDouble(), (wpParams[6].toInt() == 1 ? true : false), (wpParams[7].toInt() == 1 ? true : false), wpParams[8].toDouble(), wpParams[9].toInt())); + } + file.close(); } - file.close(); } diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index 1164b9f00ee2d4bde320c7827c8fe78a1379ee9b..111f7a8867801a297e2bb499da36f820787f2f40 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); @@ -70,25 +73,28 @@ public slots: void updateStatusLabel(const QString &string); void currentWaypointChanged(quint16 seq); - - //To be moved to UASWaypointManager (?) - void setWaypoint(int uasId, quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime); + void setWaypoint(quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime); void addWaypoint(Waypoint* wp); void removeWaypoint(Waypoint* wp); - void waypointReached(UASInterface* uas, quint16 waypointId); + void waypointReached(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 &); + void sendWaypoints(); void requestWaypoints(); void clearWaypointList(); protected: virtual void changeEvent(QEvent *e); - QVector waypoints; 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/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc index 31fbc7df8d61d04ad57b420b5cb249969ae539de..0ac7a740a5b6da061d302b96b52b647bd8a8e53d 100644 --- a/src/ui/uas/UASControlWidget.cc +++ b/src/ui/uas/UASControlWidget.cc @@ -201,16 +201,14 @@ void UASControlWidget::cycleContextButton() if (!engineOn) { ui.controlButton->setText(tr("Stop Engine")); - mav->setMode(MAV_MODE_MANUAL); mav->enable_motors(); - ui.lastActionLabel->setText(QString("Enabled motors on %1").arg(uas->getUASName())); + ui.lastActionLabel->setText(QString("Attempted to enable motors on %1").arg(uas->getUASName())); } else { ui.controlButton->setText(tr("Activate Engine")); - mav->setMode(MAV_MODE_LOCKED); mav->disable_motors(); - ui.lastActionLabel->setText(QString("Disabled motors on %1").arg(uas->getUASName())); + ui.lastActionLabel->setText(QString("Attempted to disable motors on %1").arg(uas->getUASName())); } //ui.controlButton->setText(tr("Force Landing")); //ui.controlButton->setText(tr("KILL VEHICLE")); 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; } } diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index defc81c479a48ee3069beb6351d6fc6114a342e3..18ff0086c799796d934be83df476a10bf768f043 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -36,6 +36,7 @@ This file is part of the PIXHAWK project #include "MG.h" #include "UASManager.h" #include "UASView.h" +#include "UASWaypointManager.h" #include "ui_UASView.h" UASView::UASView(UASInterface* uas, QWidget *parent) : @@ -75,6 +76,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double))); //connect(uas, SIGNAL(waypointUpdated(int,int,double,double,double,double,bool,bool)), this, SLOT(setWaypoint(int,int,double,double,double,double,bool,bool))); connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int,int))); + connect(&(uas->getWaypointManager()), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16))); connect(uas, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint))); // Setup UAS selection @@ -251,6 +253,11 @@ void UASView::updateSpeed(UASInterface*, double x, double y, double z, quint64 u totalSpeed = sqrt((pow(x, 2) + pow(y, 2) + pow(z, 2))); } +void UASView::currentWaypointUpdated(quint16 waypoint) +{ + m_ui->waypointLabel->setText(tr("WP") + QString::number(waypoint)); +} + void UASView::setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current) { Q_UNUSED(x); diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h index 928423e35d2589e5583765583c353b50eba1c681..7f57f808460b4734af3fd5cbfbd4889af2ccba2f 100644 --- a/src/ui/uas/UASView.h +++ b/src/ui/uas/UASView.h @@ -63,6 +63,8 @@ public slots: void refresh(); /** @brief Receive new waypoint information */ void setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current); + /** @brief Update the current active waypoint */ + void currentWaypointUpdated(quint16 waypoint); /** @brief Set waypoint as current target */ void selectWaypoint(int uasId, int id); /** @brief Set the current system type */ diff --git a/waypoints-final-pole-racing.txt b/waypoints-final-pole-racing.txt new file mode 100644 index 0000000000000000000000000000000000000000..6a4b4ba550021b4f5bf796eea555be2d5ef69fe6 --- /dev/null +++ b/waypoints-final-pole-racing.txt @@ -0,0 +1,9 @@ + 0 1.60066 0.497923 -0.7 -0.156858 1 1 0.15 2000 + 1 0.973377 0.691139 -0.7 -0.0664193 1 0 0.15 2000 + 2 0.646637 1.57225 -0.7 6.26573 1 0 0.15 2000 + 3 0.659852 2.39951 -0.7 -0.0549587 1 0 0.15 2000 + 4 1.41441 2.8804 -0.7 -0.0258336 1 0 0.15 2000 + 5 2.33563 2.90308 -0.7 -0.0621037 1 0 0.15 2000 + 6 2.76383 2.04368 -0.7 -0.0381553 1 0 0.15 2000 + 7 2.73091 1.05595 -0.7 -0.0520457 1 0 0.15 2000 + 8 1.73409 0.599043 0 6.26573 1 0 0.15 2000 diff --git a/waypoints_A0_circle.txt b/waypoints_A0_circle.txt index 776ec04deac9a3d22d4d79f55db31e2d6f603348..433e05d9218e7ae1aed37a90724d26d60ac7a9ff 100644 --- a/waypoints_A0_circle.txt +++ b/waypoints_A0_circle.txt @@ -1,11 +1,11 @@ - 0 1.75 0.5 -0.8 0 0 1 0.1 2000 - 1 1.75 0.5 -0.8 0 1 0 0.1 2000 - 2 0.6 0.5 -0.8 0 0 0 0.1 2000 - 3 0.5 1.5 -0.8 0 1 0 0.1 2000 - 4 0.5 2.8 -0.8 0 1 0 0.1 2000 - 5 1.75 2.8 -0.8 0 1 0 0.1 2000 - 6 2.8 2.8 -0.8 0 1 0 0.1 2000 - 7 2.8 1.75 -0.8 0 1 0 0.1 2000 - 8 2.8 0.5 -0.8 0 1 0 0.1 2000 - 9 1.75 0.5 -0.8 0 1 0 0.1 2000 - 10 1.75 0.5 0 0 1 0 0.1 2000 + 0 1.75 0.5 -0.7 0 1 0 0.3 1000 + 1 1.75 0.5 -0.7 0 1 0 0.3 1000 + 2 0.95 0.5 -0.7 0 1 0 0.3 1000 + 3 0.5 1.5 -0.7 0 1 1 0.3 1000 + 4 0.5 2.8 -0.7 0 1 0 0.3 1000 + 5 1.75 2.8 -0.7 0 1 0 0.3 1000 + 6 3 2.8 -0.7 0 1 0 0.3 1000 + 7 2.8 1.75 -0.7 0 1 0 0.3 1000 + 8 2.8 0.7 -0.7 0 1 0 0.3 1000 + 9 1.75 0.5 -0.7 0 1 0 0.3 1000 + 10 1.75 0.5 0 0 0 0 0.3 1000 diff --git a/waypoints_A0_circlev2.txt b/waypoints_A0_circlev2.txt new file mode 100644 index 0000000000000000000000000000000000000000..e2e684c3ab462e83229b8b38ea21677fd02492d9 --- /dev/null +++ b/waypoints_A0_circlev2.txt @@ -0,0 +1,10 @@ + 0 1.75 0.5 -0.7 0 1 1 0.15 10000 + 1 0.95 0.5 -0.7 0 1 0 0.15 2000 + 2 0.5 1.5 -0.7 0 1 0 0.15 2000 + 3 0.5 2.8 -0.7 0 1 0 0.15 2000 + 4 1.75 2.8 -0.7 0 1 0 0.15 2000 + 5 2.8 2.8 -0.7 0 1 0 0.15 2000 + 6 2.8 1.75 -0.7 0 1 0 0.15 2000 + 7 2.8 0.7 -0.7 0 1 0 0.15 2000 + 8 1.75 0.5 -0.7 0 1 0 0.15 2000 + 9 1.75 0.5 0 0 0 0 0.15 2000