Merge branch 'master' of pixhawk.ethz.ch:qgroundcontrol

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

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

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:

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<Waypoint*> &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<Waypoint*> &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();

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<Waypoint *> &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<Waypoint *> &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<Waypoint *> waypoints;                  ///< local waypoint list
     QVector<mavlink_waypoint_t *> waypoint_buffer;  ///< communication buffer for waypoints
     QTimer protocol_timer;                          ///< Timer to catch timeouts
 };

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 <QDebug>
 
@@ -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<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);
+
+        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)

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:

src/ui/UASControl.ui

@@ -6,14 +6,20 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>380</width>
-    <height>190</height>
+    <width>280</width>
+    <height>164</height>
    </rect>
   </property>
+  <property name="minimumSize">
+   <size>
+    <width>280</width>
+    <height>130</height>
+   </size>
+  </property>
   <property name="windowTitle">
    <string>Form</string>
   </property>
-  <layout class="QGridLayout" name="gridLayout" rowminimumheight="0,0,0,0,0,0,100">
+  <layout class="QGridLayout" name="gridLayout" rowminimumheight="5,10,10,0,10,5,0">
    <property name="leftMargin">
     <number>6</number>
    </property>
@@ -147,10 +153,13 @@
      <property name="orientation">
       <enum>Qt::Vertical</enum>
      </property>
+     <property name="sizeType">
+      <enum>QSizePolicy::Expanding</enum>
+     </property>
      <property name="sizeHint" stdset="0">
       <size>
-       <width>20</width>
-       <height>0</height>
+       <width>0</width>
+       <height>1</height>
       </size>
      </property>
     </spacer>

src/ui/UASInfo.ui

@@ -287,7 +287,7 @@
    <item row="3" column="0">
     <widget class="QLabel" name="label_3">
      <property name="text">
-      <string>CPU Load</string>
+      <string>MCU Load</string>
      </property>
     </widget>
    </item>
@@ -370,14 +370,14 @@
      </property>
     </widget>
    </item>
-   <item row="4" column="0" colspan="6">
+   <item row="5" column="0" colspan="6">
     <widget class="Line" name="line">
      <property name="orientation">
       <enum>Qt::Horizontal</enum>
      </property>
     </widget>
    </item>
-   <item row="6" column="0" colspan="7">
+   <item row="7" column="0" colspan="7">
     <spacer name="verticalSpacer">
      <property name="orientation">
       <enum>Qt::Vertical</enum>
@@ -393,13 +393,69 @@
      </property>
     </spacer>
    </item>
-   <item row="5" column="0" colspan="6">
+   <item row="6" column="0" colspan="6">
     <widget class="QLabel" name="errorLabel">
      <property name="text">
       <string>No error status received yet</string>
      </property>
     </widget>
    </item>
+   <item row="4" column="0">
+    <widget class="QLabel" name="label_4">
+     <property name="text">
+      <string>CPU Load</string>
+     </property>
+    </widget>
+   </item>
+   <item row="4" column="2">
+    <widget class="QLabel" name="label_5">
+     <property name="text">
+      <string>0</string>
+     </property>
+     <property name="alignment">
+      <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+     </property>
+    </widget>
+   </item>
+   <item row="4" column="3">
+    <widget class="QLabel" name="label_10">
+     <property name="text">
+      <string>%</string>
+     </property>
+    </widget>
+   </item>
+   <item row="4" column="4" colspan="2">
+    <widget class="QProgressBar" name="progressBar">
+     <property name="maximumSize">
+      <size>
+       <width>16777215</width>
+       <height>20</height>
+      </size>
+     </property>
+     <property name="baseSize">
+      <size>
+       <width>0</width>
+       <height>18</height>
+      </size>
+     </property>
+     <property name="value">
+      <number>24</number>
+     </property>
+    </widget>
+   </item>
+   <item row="4" column="1">
+    <spacer name="horizontalSpacer">
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>13</width>
+       <height>15</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
   </layout>
  </widget>
  <resources/>

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<Waypoint*> &)),    &uas->getWaypointManager(), SLOT(sendWaypoints(const QVector<Waypoint*> &)));
-        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<Waypoint *> &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<Waypoint *> &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<Waypoint *> &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<Waypoint *> &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<Waypoint*,WaypointView*> 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<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
+
+        // Clear list layout
+        if (!wpViews.empty())
         {
-            listLayout->addWidget(wpViews.value(waypoints[i]));
+            QMapIterator<Waypoint*,WaypointView*> 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<Waypoint *> &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<Waypoint *> &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<Waypoint *> &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<Waypoint *> &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<Waypoint *> &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();
 }
 

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<Waypoint*> &);
+    void sendWaypoints();
     void requestWaypoints();
     void clearWaypointList();
 
 protected:
     virtual void changeEvent(QEvent *e);
 
-    QVector<Waypoint*> waypoints;
     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

@@ -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/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"));

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;
     }
 }
 

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);

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 */

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

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

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