Added option to add local waypoints, added waypoint visualization

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:

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,6 +632,8 @@ void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color
 
 void HSIDisplay::drawWaypoints(QPainter& 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));
@@ -686,6 +688,7 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
             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/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<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

@@ -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

@@ -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

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