Added support for more generic waypoints / mission items

qgroundcontrol.pro

@@ -155,7 +155,8 @@ FORMS += src/ui/MainWindow.ui \
     src/ui/designer/QGCParamSlider.ui \
     src/ui/designer/QGCActionButton.ui \
     src/ui/QGCMAVLinkLogPlayer.ui \
-    src/ui/QGCWaypointListMulti.ui
+    src/ui/QGCWaypointListMulti.ui \
+    src/ui/mission/QGCCustomWaypointAction.ui
 
 INCLUDEPATH += src \
     src/ui \

src/Waypoint.cc

@@ -42,8 +42,10 @@ Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _yaw, bo
     action(_action),
     autocontinue(_autocontinue),
     current(_current),
-    orbit(_orbit),
-    holdTime(_holdTime),
+    orbit(0),
+    orbitDirection(0),
+    param1(_orbit),
+    param2(_holdTime),
     name(QString("WP%1").arg(id, 2, 10, QChar('0')))
 {
 }
@@ -60,7 +62,7 @@ void Waypoint::save(QTextStream &saveStream)
     position = position.arg(y, 0, 'g', 18);
     position = position.arg(z, 0, 'g', 18);
     position = position.arg(yaw, 0, 'g', 8);
-    saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t"  << this->getOrbit() << "\t" << /*Orbit Direction*/ 0 << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\t" << this->getCurrent() << "\t" << position  << "\t" << this->getAutoContinue() << "\r\n";
+    saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t"  << orbit << "\t" << orbitDirection << "\t" << param1 << "\t" << param2 << "\t" << this->getCurrent() << "\t" << position  << "\t" << this->getAutoContinue() << "\r\n";
 }
 
 bool Waypoint::load(QTextStream &loadStream)
@@ -72,9 +74,9 @@ bool Waypoint::load(QTextStream &loadStream)
         this->frame = (MAV_FRAME) wpParams[1].toInt();
         this->action = (MAV_ACTION) wpParams[2].toInt();
         this->orbit = wpParams[3].toDouble();
-        //TODO: orbit direction
-        //TODO: param1
-        this->holdTime = wpParams[6].toInt();
+        this->orbitDirection = wpParams[4].toInt();
+        this->param1 = wpParams[5].toDouble();
+        this->param2 = wpParams[6].toDouble();
         this->current = (wpParams[7].toInt() == 1 ? true : false);
         this->x = wpParams[8].toDouble();
         this->y = wpParams[9].toDouble();
@@ -130,6 +132,15 @@ void Waypoint::setYaw(double yaw)
     }
 }
 
+void Waypoint::setAction(int action)
+{
+    if (this->action != (MAV_ACTION)action)
+    {
+        this->action = (MAV_ACTION)action;
+        emit changed(this);
+    }
+}
+
 void Waypoint::setAction(MAV_ACTION action)
 {
     if (this->action != action)
@@ -166,7 +177,52 @@ void Waypoint::setCurrent(bool current)
     }
 }
 
-void Waypoint::setOrbit(double orbit)
+void Waypoint::setAcceptanceRadius(double radius)
+{
+    if (this->param1 != radius)
+    {
+        this->param1 = radius;
+        emit changed(this);
+    }
+}
+
+void Waypoint::setParam1(double param1)
+{
+    if (this->param1 != param1)
+    {
+        this->param1 = param1;
+        emit changed(this);
+    }
+}
+
+void Waypoint::setParam2(double param2)
+{
+    if (this->param2 != param2)
+    {
+        this->param2 = param2;
+        emit changed(this);
+    }
+}
+
+void Waypoint::setParam3(double param3)
+{
+    if (this->x != param3)
+    {
+        this->x = param3;
+        emit changed(this);
+    }
+}
+
+void Waypoint::setParam4(double param4)
+{
+    if (this->y != param4)
+    {
+        this->y = param4;
+        emit changed(this);
+    }
+}
+
+void Waypoint::setLoiterOrbit(double orbit)
 {
     if (this->orbit != orbit)
     {
@@ -177,9 +233,18 @@ void Waypoint::setOrbit(double orbit)
 
 void Waypoint::setHoldTime(int holdTime)
 {
-    if (this->holdTime != holdTime)
+    if (this->param2 != holdTime)
+    {
+        this->param2 = holdTime;
+        emit changed(this);
+    }
+}
+
+void Waypoint::setTurns(int turns)
+{
+    if (this->param1 != turns)
     {
-        this->holdTime = holdTime;
+        this->param1 = turns;
         emit changed(this);
     }
 }

src/Waypoint.h

@@ -55,8 +55,14 @@ public:
     double getYaw() const { return yaw; }
     bool getAutoContinue() const { return autocontinue; }
     bool getCurrent() const { return current; }
-    double getOrbit() const { return orbit; }
-    double getHoldTime() const { return holdTime; }
+    double getLoiterOrbit() const { return orbit; }
+    double getAcceptanceRadius() const { return param1; }
+    double getHoldTime() const { return param2; }
+    double getParam1() const { return param1; }
+    double getParam2() const { return param2; }
+    double getParam3() const { return x; }
+    double getParam4() const { return y; }
+    int getTurns() const { return param1; }
     MAV_FRAME getFrame() const { return frame; }
     MAV_ACTION getAction() const { return action; }
     const QString& getName() const { return name; }
@@ -76,7 +82,10 @@ protected:
     bool autocontinue;
     bool current;
     double orbit;
-    int holdTime;
+    int orbitDirection;
+    double param1;
+    double param2;
+    int turns;
     QString name;
 
 public slots:
@@ -85,20 +94,21 @@ public slots:
     void setY(double y);
     void setZ(double z);
     void setYaw(double yaw);
+    /** @brief Set the waypoint action */
+    void setAction(int action);
     void setAction(MAV_ACTION action);
     void setFrame(MAV_FRAME frame);
     void setAutocontinue(bool autoContinue);
     void setCurrent(bool current);
-    void setOrbit(double orbit);
+    void setLoiterOrbit(double orbit);
+    void setParam1(double param1);
+    void setParam2(double param2);
+    void setParam3(double param3);
+    void setParam4(double param4);
+    void setAcceptanceRadius(double radius);
     void setHoldTime(int holdTime);
-
-
-//    //for QDoubleSpin
-//    void setX(double x);
-//    void setY(double y);
-//    void setZ(double z);
-//    void setYaw(double yaw);
-//    void setOrbit(double orbit);
+    /** @brief Number of turns for loiter waypoints */
+    void setTurns(int turns);
 
 signals:
     /** @brief Announces a change to the waypoint data */

src/comm/MAVLinkSimulationMAV.cc

@@ -265,7 +265,7 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
             {
                 switch (action.action)
                 {
-                case MAV_ACTION_LAUNCH:
+                case MAV_ACTION_TAKEOFF:
                     flying = true;
                     break;
                 default:

src/uas/UAS.cc

@@ -1480,7 +1480,7 @@ void UAS::launch()
 {
     mavlink_message_t msg;
     // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_LAUNCH);
+    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_TAKEOFF);
     // Send message twice to increase chance of reception
     sendMessage(msg);
     sendMessage(msg);
@@ -1494,7 +1494,7 @@ void UAS::enable_motors()
 {
     mavlink_message_t msg;
     // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START);
+    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START);
     // Send message twice to increase chance of reception
     sendMessage(msg);
     sendMessage(msg);
@@ -1508,7 +1508,7 @@ void UAS::disable_motors()
 {
     mavlink_message_t msg;
     // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP);
+    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP);
     // Send message twice to increase chance of reception
     sendMessage(msg);
     sendMessage(msg);

src/uas/UASWaypointManager.cc

@@ -555,10 +555,10 @@ void UASWaypointManager::writeWaypoints()
 
                 cur_d->autocontinue = cur_s->getAutoContinue();
                 cur_d->current = cur_s->getCurrent() & noCurrent;   //make sure only one current waypoint is selected, the first selected will be chosen
-                cur_d->orbit = 0;
+                cur_d->orbit = cur_s->getLoiterOrbit();
                 cur_d->orbit_direction = 0;
-                cur_d->param1 = cur_s->getOrbit();
-                cur_d->param2 = cur_s->getHoldTime();
+                cur_d->param1 = cur_s->getParam1();
+                cur_d->param2 = cur_s->getParam2();
                 cur_d->frame = cur_s->getFrame();
                 cur_d->action = cur_s->getAction();
                 cur_d->seq = i;     // don't read out the sequence number of the waypoint class

src/ui/HSIDisplay.cc

@@ -41,6 +41,7 @@ This file is part of the QGROUNDCONTROL project
 #include "Waypoint.h"
 #include "UASWaypointManager.h"
 #include "Waypoint2DIcon.h"
+#include "MAV2DIcon.h"
 
 #include <QDebug>
 
@@ -229,11 +230,21 @@ void HSIDisplay::renderOverlay()
     painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
 
     // Draw center indicator
-    QPolygonF p(3);
-    p.replace(0, QPointF(xCenterPos, yCenterPos-4.0f));
-    p.replace(1, QPointF(xCenterPos-4.0f, yCenterPos+3.5f));
-    p.replace(2, QPointF(xCenterPos+4.0f, yCenterPos+3.5f));
-    drawPolygon(p, &painter);
+//    QPolygonF p(3);
+//    p.replace(0, QPointF(xCenterPos, yCenterPos-4.0f));
+//    p.replace(1, QPointF(xCenterPos-4.0f, yCenterPos+3.5f));
+//    p.replace(2, QPointF(xCenterPos+4.0f, yCenterPos+3.5f));
+//    drawPolygon(p, &painter);
+
+    if (uas)
+    {
+        // Translate to center
+        painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
+        QColor uasColor = uas->getColor();
+        MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast<int>((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f);
+        // Translate back
+        painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
+    }
 
     // ----------------------
 

src/ui/MapWidget.cc

@@ -450,6 +450,18 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
     qDebug() << "UPDATING WP" << wp->getId() << wp <<  __FILE__ << __LINE__;
     if (uas == this->mav->getUASID())
     {
+        // TODO The map widget is not yet aware of non-global, non-navigation WPs
+        if (wp->getFrame() == MAV_FRAME_GLOBAL && wp->getAction() == MAV_ACTION_NAVIGATE)
+        {
+            // We're good, this is a global waypoint
+            // This is where the code below should be executed in
+        }
+        else
+        {
+            // We're screwed, this is not a global waypoint
+            qDebug() << "WARNING: The map widget is not prepared yet for non-navigation WPs" << __FILE__ << __LINE__;
+        }
+
         int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getIndexOf(wp);
         if (wpindex == -1) return;
         // Create waypoint name
@@ -467,8 +479,6 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
             // Waypoint exists, update it
             if(!waypointIsDrag)
             {
-                qDebug() <<"indice WP= "<< wpindex <<"\n";
-
                 QPointF coordinate;
                 coordinate.setX(wp->getX());
                 coordinate.setY(wp->getY());
@@ -482,6 +492,7 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
                     // Now update icon position
                     //mc->layer("Waypoints")->removeGeometry(wpIcons.at(wpindex));
                     wpIcons.at(wpindex)->setCoordinate(coordinate);
+                    wpIcons.at(wpindex)->setPen(mavPens.value(uas));
                     //mc->layer("Waypoints")->addGeometry(wpIcons.at(wpindex));
                     // Then waypoint line coordinate
                     Point* linesegment = NULL;
@@ -505,12 +516,29 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
                 }
             }
         }
+        // Set unvisible
+        if (wp->getFrame() != MAV_FRAME_GLOBAL || !(wp->getAction() == MAV_ACTION_NAVIGATE
+                                                 || wp->getAction() == MAV_ACTION_LOITER
+                                                 || wp->getAction() == MAV_ACTION_LOITER_MAX_TIME
+                                                 || wp->getAction() == MAV_ACTION_LOITER_MAX_TURNS
+                                                 || wp->getAction() == MAV_ACTION_TAKEOFF
+                                                 || wp->getAction() == MAV_ACTION_LAND
+                                                 || wp->getAction() == MAV_ACTION_RETURN))
+        {
+            wpIcons.at(wpindex)->setVisible(false);
+            waypointPath->points().at(wpindex)->setVisible(false);
+        }
+        else
+        {
+            wpIcons.at(wpindex)->setVisible(true);
+            waypointPath->points().at(wpindex)->setVisible(true);
+        }
     }
 }
 
 void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate)
 {
-    if (!wpExists(coordinate))
+    //if (!wpExists(coordinate))
     {
         // Create waypoint name
         QString str;
@@ -820,7 +848,7 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
         // Icon
         //QPen* pointpen = new QPen(uasColor);
         qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__;
-        p = new MAV2DIcon(uas, 50, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
+        p = new MAV2DIcon(uas, 68, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
         uasIcons.insert(uas->getUASID(), p);
         mc->layer("Waypoints")->addGeometry(p);
 

src/ui/WaypointList.cc

@@ -203,7 +203,7 @@ void WaypointList::add()
             {
                 // Create waypoint with last frame
                 Waypoint *last = waypoints.at(waypoints.size()-1);
-                wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(),
+                wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getAcceptanceRadius(),
                                             last->getHoldTime(), last->getFrame(), last->getAction());
                 uas->getWaypointManager()->addWaypoint(wp);
             }

src/ui/WaypointView.cc

@@ -14,35 +14,58 @@
 #include <QDoubleSpinBox>
 #include <QDebug>
 
-#include <cmath>    //M_PI
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
+#include <cmath>
+#include <qmath.h>
 
 #include "WaypointView.h"
 #include "ui_WaypointView.h"
+#include "ui_QGCCustomWaypointAction.h"
 
 WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
         QWidget(parent),
+        customCommand(new Ui_QGCCustomWaypointAction),
         m_ui(new Ui::WaypointView)
 {
     m_ui->setupUi(this);
 
     this->wp = wp;
 
+    // CUSTOM COMMAND WIDGET
+    customCommand->setupUi(m_ui->customActionWidget);
+
     // add actions
-    m_ui->comboBox_action->addItem("Navigate",MAV_ACTION_NAVIGATE);
-    m_ui->comboBox_action->addItem("TakeOff",MAV_ACTION_TAKEOFF);
-    m_ui->comboBox_action->addItem("Land",MAV_ACTION_LAND);
-    m_ui->comboBox_action->addItem("Loiter",MAV_ACTION_LOITER);
+    m_ui->comboBox_action->addItem(tr("Navigate"),MAV_ACTION_NAVIGATE);
+    m_ui->comboBox_action->addItem(tr("TakeOff"),MAV_ACTION_TAKEOFF);
+    m_ui->comboBox_action->addItem(tr("Loiter Unlim."),MAV_ACTION_LOITER);
+    m_ui->comboBox_action->addItem(tr("Loiter Time"),MAV_ACTION_LOITER_MAX_TIME);
+    m_ui->comboBox_action->addItem(tr("Loiter Turns"),MAV_ACTION_LOITER_MAX_TURNS);
+    m_ui->comboBox_action->addItem(tr("Return to Launch"),MAV_ACTION_RETURN);
+    m_ui->comboBox_action->addItem(tr("Land"),MAV_ACTION_LAND);
+    m_ui->comboBox_action->addItem(tr("Other"), MAV_ACTION_NB);
+    //    m_ui->comboBox_action->addItem(tr("Delay"), MAV_ACTION_DELAY_BEFORE_COMMAND);
+    //    m_ui->comboBox_action->addItem(tr("Ascend/Descent"), MAV_ACTION_ASCEND_AT_RATE);
+    //    m_ui->comboBox_action->addItem(tr("Change Mode"), MAV_ACTION_CHANGE_MODE);
+    //    m_ui->comboBox_action->addItem(tr("Relay ON"), MAV_ACTION_RELAY_ON);
+    //    m_ui->comboBox_action->addItem(tr("Relay OFF"), MAV_ACTION_RELAY_OFF);
 
     // add frames 
     m_ui->comboBox_frame->addItem("Global",MAV_FRAME_GLOBAL);
     m_ui->comboBox_frame->addItem("Local",MAV_FRAME_LOCAL);
+    m_ui->comboBox_frame->addItem("Mission",MAV_FRAME_MISSION);
+
+    // Initialize view correctly
+    updateActionView(wp->getAction());
+    updateFrameView(wp->getFrame());
 
     // Read values and set user interface
     updateValues();
 
+    // Check for mission frame
+    if (wp->getFrame() == MAV_FRAME_MISSION)
+    {
+        m_ui->comboBox_action->setCurrentIndex(m_ui->comboBox_action->count()-1);
+    }
+
     // defaults
     //changedAction(wp->getAction());
     //changedFrame(wp->getFrame());
@@ -68,8 +91,20 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
     connect(m_ui->comboBox_action, SIGNAL(activated(int)), this, SLOT(changedAction(int)));
     connect(m_ui->comboBox_frame, SIGNAL(activated(int)), this, SLOT(changedFrame(int)));
 
-    connect(m_ui->orbitSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setOrbit(double)));
+    connect(m_ui->orbitSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setLoiterOrbit(double)));
+    connect(m_ui->acceptanceSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setAcceptanceRadius(double)));
     connect(m_ui->holdTimeSpinBox, SIGNAL(valueChanged(int)), wp, SLOT(setHoldTime(int)));
+    connect(m_ui->turnsSpinBox, SIGNAL(valueChanged(int)), wp, SLOT(setTurns(int)));
+
+    // Connect actions
+    connect(customCommand->commandSpinBox, SIGNAL(valueChanged(int)),   wp, SLOT(setAction(int)));
+    connect(customCommand->param1SpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam1(double)));
+    connect(customCommand->param2SpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam2(double)));
+    connect(customCommand->param3SpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam3(double)));
+    connect(customCommand->param4SpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam4(double)));
+
+    // MISSION ELEMENT WIDGET
+    // TODO
 }
 
 void WaypointView::setYaw(int yawDegree)
@@ -101,48 +136,133 @@ void WaypointView::changedAutoContinue(int state)
         wp->setAutocontinue(true);
 }
 
-void WaypointView::changedAction(int index)
+void WaypointView::updateActionView(int action)
 {
-    // set action for waypoint
-
-
-    // hide everything to start
-    m_ui->orbitSpinBox->hide();
-    m_ui->takeOffAngleSpinBox->hide();
-    m_ui->autoContinue->hide();
-    m_ui->holdTimeSpinBox->hide();
-
-    // set waypoint action
-    MAV_ACTION action = (MAV_ACTION) m_ui->comboBox_action->itemData(index).toUInt();
-    wp->setAction(action);
-
     // expose ui based on action
+
     switch(action)
     {
     case MAV_ACTION_TAKEOFF:
+        m_ui->orbitSpinBox->hide();
+        m_ui->yawSpinBox->hide();
+        m_ui->turnsSpinBox->hide();
+        m_ui->autoContinue->hide();
+        m_ui->holdTimeSpinBox->hide();
+        m_ui->acceptanceSpinBox->hide();
+        m_ui->customActionWidget->hide();
         m_ui->takeOffAngleSpinBox->show();
         break;
     case MAV_ACTION_LAND:
+        m_ui->orbitSpinBox->hide();
+        m_ui->takeOffAngleSpinBox->hide();
+        m_ui->yawSpinBox->hide();
+        m_ui->turnsSpinBox->hide();
+        m_ui->autoContinue->hide();
+        m_ui->holdTimeSpinBox->hide();
+        m_ui->acceptanceSpinBox->hide();
+        m_ui->customActionWidget->hide();
+        break;
+    case MAV_ACTION_RETURN:
+        m_ui->orbitSpinBox->hide();
+        m_ui->takeOffAngleSpinBox->hide();
+        m_ui->yawSpinBox->hide();
+        m_ui->turnsSpinBox->hide();
+        m_ui->autoContinue->hide();
+        m_ui->holdTimeSpinBox->hide();
+        m_ui->acceptanceSpinBox->hide();
+        m_ui->customActionWidget->hide();
         break;
     case MAV_ACTION_NAVIGATE:
+        m_ui->orbitSpinBox->hide();
+        m_ui->takeOffAngleSpinBox->hide();
+        m_ui->turnsSpinBox->hide();
+        m_ui->holdTimeSpinBox->hide();
+        m_ui->customActionWidget->hide();
+
         m_ui->autoContinue->show();
-        m_ui->orbitSpinBox->show();
+        m_ui->acceptanceSpinBox->show();
+        m_ui->yawSpinBox->show();
         break;
     case MAV_ACTION_LOITER:
+        m_ui->takeOffAngleSpinBox->hide();
+        m_ui->yawSpinBox->hide();
+        m_ui->turnsSpinBox->hide();
+        m_ui->autoContinue->hide();
+        m_ui->holdTimeSpinBox->hide();
+        m_ui->acceptanceSpinBox->hide();
+        m_ui->customActionWidget->hide();
+        m_ui->orbitSpinBox->show();
+        break;
+    case MAV_ACTION_LOITER_MAX_TURNS:
+        m_ui->takeOffAngleSpinBox->hide();
+        m_ui->yawSpinBox->hide();
+        m_ui->autoContinue->hide();
+        m_ui->holdTimeSpinBox->hide();
+        m_ui->acceptanceSpinBox->hide();
+        m_ui->customActionWidget->hide();
+        m_ui->orbitSpinBox->show();
+        m_ui->turnsSpinBox->show();
+        break;
+    case MAV_ACTION_LOITER_MAX_TIME:
+        m_ui->takeOffAngleSpinBox->hide();
+        m_ui->yawSpinBox->hide();
+        m_ui->turnsSpinBox->hide();
+        m_ui->autoContinue->hide();
+        m_ui->acceptanceSpinBox->hide();
+        m_ui->customActionWidget->hide();
         m_ui->orbitSpinBox->show();
         m_ui->holdTimeSpinBox->show();
         break;
     default:
-        std::cerr << "unknown action" << std::endl;
+        break;
     }
 }
 
-void WaypointView::changedFrame(int index)
+/**
+ * @param index The index of the combo box of the action entry, NOT the action ID
+ */
+void WaypointView::changedAction(int index)
 {
     // set waypoint action
-    MAV_FRAME frame = (MAV_FRAME)m_ui->comboBox_frame->itemData(index).toUInt();
-    wp->setFrame(frame);
+    int actionIndex = m_ui->comboBox_action->itemData(index).toUInt();
+    if (actionIndex < MAV_ACTION_NB && actionIndex >= 0)
+    {
+        MAV_ACTION action = (MAV_ACTION) actionIndex;
+        wp->setAction(action);
+    }
+
+    // Expose ui based on action
+    // Change to mission frame
+    // if action is unknown
+
+    switch(actionIndex)
+    {
+    case MAV_ACTION_TAKEOFF:
+    case MAV_ACTION_LAND:
+    case MAV_ACTION_RETURN:
+    case MAV_ACTION_NAVIGATE:
+    case MAV_ACTION_LOITER:
+    case MAV_ACTION_LOITER_MAX_TURNS:
+    case MAV_ACTION_LOITER_MAX_TIME:
+    case MAV_ACTION_DELAY_BEFORE_COMMAND:
+    case MAV_ACTION_CHANGE_MODE:
+    case MAV_ACTION_SET_ORIGIN:
+    case MAV_ACTION_RELAY_ON:
+    case MAV_ACTION_RELAY_OFF:
+        // Back to global frame
+        if (wp->getFrame() == MAV_FRAME_MISSION) changedFrame(0);
+        break;
+    case MAV_ACTION_NB:
+    default:
+        // Switch to mission frame
+        changedFrame(MAV_FRAME_MISSION);
+        wp->setFrame(MAV_FRAME_MISSION);
+        break;
+    }
+}
 
+void WaypointView::updateFrameView(int frame)
+{
     switch(frame)
     {
     case MAV_FRAME_GLOBAL:
@@ -152,6 +272,9 @@ void WaypointView::changedFrame(int index)
         m_ui->lonSpinBox->show();
         m_ui->latSpinBox->show();
         m_ui->altSpinBox->show();
+        // Coordinate frame
+        m_ui->comboBox_frame->show();
+        m_ui->customActionWidget->hide();
         break;
     case MAV_FRAME_LOCAL:
         m_ui->lonSpinBox->hide();
@@ -160,12 +283,50 @@ void WaypointView::changedFrame(int index)
         m_ui->posNSpinBox->show();
         m_ui->posESpinBox->show();
         m_ui->posDSpinBox->show();
+        // Coordinate frame
+        m_ui->comboBox_frame->show();
+        m_ui->customActionWidget->hide();
+        break;
+    case MAV_FRAME_MISSION:
+        // Hide almost everything
+        m_ui->orbitSpinBox->hide();
+        m_ui->takeOffAngleSpinBox->hide();
+        m_ui->yawSpinBox->hide();
+        m_ui->turnsSpinBox->hide();
+        m_ui->holdTimeSpinBox->hide();
+        m_ui->acceptanceSpinBox->hide();
+        m_ui->posDSpinBox->hide();
+        m_ui->posESpinBox->hide();
+        m_ui->posNSpinBox->hide();
+        m_ui->latSpinBox->hide();
+        m_ui->lonSpinBox->hide();
+        m_ui->altSpinBox->hide();
+        m_ui->comboBox_frame->hide();
+
+        // Show action widget
+        if (!m_ui->customActionWidget->isVisible())
+        {
+            m_ui->customActionWidget->show();
+        }
+        if (!m_ui->autoContinue->isVisible())
+        {
+            m_ui->autoContinue->show();
+        }
         break;
     default:
         std::cerr << "unknown frame" << std::endl;
     }
 }
 
+void WaypointView::changedFrame(int index)
+{
+    // set waypoint action
+    MAV_FRAME frame = (MAV_FRAME)m_ui->comboBox_frame->itemData(index).toUInt();
+    wp->setFrame(frame);
+
+    updateFrameView(frame);
+}
+
 void WaypointView::changedCurrent(int state)
 {
     if (state == 0)
@@ -191,6 +352,7 @@ void WaypointView::updateValues()
     if (m_ui->comboBox_frame->currentIndex() != frame_index)
     {
         m_ui->comboBox_frame->setCurrentIndex(frame_index);
+        updateFrameView(frame);
     }
     switch(frame)
     {
@@ -226,15 +388,30 @@ void WaypointView::updateValues()
             }
         }
         break;
+    case (MAV_FRAME_MISSION):
+        {
+            // TODO Change to mission view
+        }
+        break;
     }
-    changedFrame(frame_index);
 
-    // update action
+    // Update action
     MAV_ACTION action = wp->getAction();
     int action_index = m_ui->comboBox_action->findData(action);
+    // Set to "Other" action if it was -1
+    if (action_index == -1)
+    {
+        action_index = m_ui->comboBox_action->findData(MAV_ACTION_NB);
+    }
+    // Only update if changed
     if (m_ui->comboBox_action->currentIndex() != action_index)
     {
-        m_ui->comboBox_action->setCurrentIndex(action_index);
+        // TODO Action and frame should not be coupled
+        if (wp->getFrame() != MAV_FRAME_MISSION)
+        {
+            m_ui->comboBox_action->setCurrentIndex(action_index);
+            updateActionView(action);
+        }
     }
     switch(action)
     {
@@ -249,7 +426,6 @@ void WaypointView::updateValues()
     default:
         std::cerr << "unknown action" << std::endl;
     }
-    changedAction(action_index);
 
     if (m_ui->yawSpinBox->value() != wp->getYaw()/M_PI*180.)
     {
@@ -264,14 +440,49 @@ void WaypointView::updateValues()
         m_ui->autoContinue->setChecked(wp->getAutoContinue());
     }
     m_ui->idLabel->setText(QString("%1").arg(wp->getId()));
-    if (m_ui->orbitSpinBox->value() != wp->getOrbit())
+    if (m_ui->orbitSpinBox->value() != wp->getLoiterOrbit())
     {
-        m_ui->orbitSpinBox->setValue(wp->getOrbit());
+        m_ui->orbitSpinBox->setValue(wp->getLoiterOrbit());
+    }
+    if (m_ui->acceptanceSpinBox->value() != wp->getAcceptanceRadius())
+    {
+        m_ui->acceptanceSpinBox->setValue(wp->getAcceptanceRadius());
     }
     if (m_ui->holdTimeSpinBox->value() != wp->getHoldTime())
     {
         m_ui->holdTimeSpinBox->setValue(wp->getHoldTime());
     }
+
+    // UPDATE CUSTOM ACTION WIDGET
+
+    qDebug() << "UPDATING CUSTOM ACTION WIDGET";
+
+    if (customCommand->commandSpinBox->value() != wp->getAction())
+    {
+        customCommand->commandSpinBox->setValue(wp->getAction());
+        qDebug() << "Changed action";
+    }
+    // Param 1
+    if (customCommand->param1SpinBox->value() != wp->getParam1())
+    {
+        customCommand->param1SpinBox->setValue(wp->getParam1());
+    }
+    // Param 2
+    if (customCommand->param2SpinBox->value() != wp->getParam2())
+    {
+        customCommand->param2SpinBox->setValue(wp->getParam2());
+    }
+    // Param 3
+    if (customCommand->param3SpinBox->value() != wp->getParam3())
+    {
+        customCommand->param3SpinBox->setValue(wp->getParam3());
+    }
+    // Param 4
+    if (customCommand->param4SpinBox->value() != wp->getParam4())
+    {
+        customCommand->param4SpinBox->setValue(wp->getParam4());
+    }
+
     wp->blockSignals(false);
 }
 

src/ui/WaypointView.h

@@ -41,6 +41,7 @@ This file is part of the QGROUNDCONTROL project
 namespace Ui {
     class WaypointView;
 }
+class Ui_QGCCustomWaypointAction;
 
 class WaypointView : public QWidget {
     Q_OBJECT
@@ -57,7 +58,9 @@ public slots:
     void moveDown();
     void remove();
     void changedAutoContinue(int);
+    void updateFrameView(int frame);
     void changedFrame(int state);
+    void updateActionView(int action);
     void changedAction(int state);
     void changedCurrent(int);
     void updateValues(void);
@@ -67,6 +70,9 @@ public slots:
 protected:
     virtual void changeEvent(QEvent *e);
     Waypoint* wp;
+    // Special widgets extendending the
+    // waypoint view to mission capabilities
+    Ui_QGCCustomWaypointAction* customCommand;
 
 private:
     Ui::WaypointView *m_ui;

src/ui/WaypointView.ui

@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>1389</width>
+    <width>2208</width>
     <height>39</height>
    </rect>
   </property>
@@ -161,7 +161,7 @@ QProgressBar::chunk#thrustBar {
      <property name="title">
       <string/>
      </property>
-     <layout class="QHBoxLayout" name="horizontalLayout" stretch="0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0">
+     <layout class="QHBoxLayout" name="horizontalLayout" stretch="1,1,2,2,10,10,10,10,10,10,10,10,10,10,10,10,10,1,1,1,1">
       <property name="spacing">
        <number>2</number>
       </property>
@@ -202,7 +202,7 @@ QProgressBar::chunk#thrustBar {
          <string>Waypoint Sequence Number</string>
         </property>
         <property name="text">
-         <string>TextLabel</string>
+         <string>ID</string>
         </property>
         <property name="alignment">
          <set>Qt::AlignCenter</set>
@@ -223,6 +223,9 @@ QProgressBar::chunk#thrustBar {
         <property name="statusTip">
          <string>Action at Waypoint</string>
         </property>
+        <property name="sizeAdjustPolicy">
+         <enum>QComboBox::AdjustToContentsOnFirstShow</enum>
+        </property>
        </widget>
       </item>
       <item>
@@ -239,6 +242,9 @@ QProgressBar::chunk#thrustBar {
         <property name="statusTip">
          <string>Coordinate frame</string>
         </property>
+        <property name="sizeAdjustPolicy">
+         <enum>QComboBox::AdjustToContentsOnFirstShow</enum>
+        </property>
        </widget>
       </item>
       <item>
@@ -528,6 +534,16 @@ QProgressBar::chunk#thrustBar {
         </property>
        </widget>
       </item>
+      <item>
+       <widget class="QDoubleSpinBox" name="acceptanceSpinBox">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="value">
+         <double>0.200000000000000</double>
+        </property>
+       </widget>
+      </item>
       <item>
        <widget class="QSpinBox" name="holdTimeSpinBox">
         <property name="sizePolicy">
@@ -559,6 +575,19 @@ QProgressBar::chunk#thrustBar {
         </property>
        </widget>
       </item>
+      <item>
+       <widget class="QSpinBox" name="turnsSpinBox">
+        <property name="toolTip">
+         <string>Number of turns to loiter</string>
+        </property>
+        <property name="statusTip">
+         <string>Number of turns to loiter</string>
+        </property>
+        <property name="suffix">
+         <string> turns</string>
+        </property>
+       </widget>
+      </item>
       <item>
        <widget class="QDoubleSpinBox" name="takeOffAngleSpinBox">
         <property name="sizePolicy">
@@ -584,6 +613,16 @@ QProgressBar::chunk#thrustBar {
         </property>
        </widget>
       </item>
+      <item>
+       <widget class="QWidget" name="customActionWidget" native="true">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+       </widget>
+      </item>
       <item>
        <widget class="QCheckBox" name="autoContinue">
         <property name="toolTip">

src/ui/map/MAV2DIcon.cc

@@ -99,7 +99,11 @@ void MAV2DIcon::drawIcon(QPen* pen)
         //qDebug() << "Painting ellipse" << radius/2-width << width;
         //selPen->deleteLater();
     }
+    drawAirframePolygon(airframe, painter, radius, iconColor, yaw);
+}
 
+void MAV2DIcon::drawAirframePolygon(int airframe, QPainter& painter, int radius, QColor& iconColor, float yaw)
+{
     switch (airframe)
     {
     case UASInterface::QGC_AIRFRAME_PREDATOR:
@@ -147,6 +151,7 @@ void MAV2DIcon::drawIcon(QPen* pen)
             painter.setPen(iconPen);
 
             painter.drawPolygon(poly);
+            painter.rotate(-yawRotate);
         }
         break;
     case UASInterface::QGC_AIRFRAME_MIKROKOPTER:
@@ -186,6 +191,7 @@ void MAV2DIcon::drawIcon(QPen* pen)
 
             painter.setBrush(Qt::red);
             painter.drawEllipse(front, radius/4/2, radius/4/2);
+            painter.rotate(-yawRotate);
         }
         break;
     case UASInterface::QGC_AIRFRAME_EASYSTAR:
@@ -243,6 +249,7 @@ void MAV2DIcon::drawIcon(QPen* pen)
             painter.setPen(iconPen);
 
             painter.drawPolygon(poly);
+            painter.rotate(-yawRotate);
         }
         break;
         case UASInterface::QGC_AIRFRAME_GENERIC:
@@ -268,6 +275,7 @@ void MAV2DIcon::drawIcon(QPen* pen)
             painter.setPen(iconPen);
 
             painter.drawPolygon(poly);
+            painter.rotate(-yawRotate);
         }
         break;
     }

src/ui/map/MAV2DIcon.h

@@ -61,6 +61,7 @@ public:
 
     void drawIcon(QPen* pen);
     void drawIcon() { drawIcon(mypen); }
+    static void drawAirframePolygon(int airframe, QPainter& painter, int radius, QColor& iconColor, float yaw);
 
 protected:
     float yaw;      ///< Yaw angle of the MAV

src/ui/map3D/WaypointGroupNode.cc

@@ -90,7 +90,7 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
             osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
             osg::ref_ptr<osg::Cylinder> cylinder =
                     new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
-                                      wp->getOrbit(),
+                                      wp->getAcceptanceRadius(),
                                       fabs(wpZ));
 
             sd->setShape(cylinder);

src/ui/mission/QGCCustomWaypointAction.ui

+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>QGCCustomWaypointAction</class>
+ <widget class="QWidget" name="QGCCustomWaypointAction">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>791</width>
+    <height>25</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+  <layout class="QHBoxLayout" name="horizontalLayout">
+   <property name="spacing">
+    <number>5</number>
+   </property>
+   <property name="margin">
+    <number>0</number>
+   </property>
+   <item>
+    <widget class="QSpinBox" name="commandSpinBox">
+     <property name="prefix">
+      <string>CMD </string>
+     </property>
+     <property name="minimum">
+      <number>0</number>
+     </property>
+     <property name="maximum">
+      <number>255</number>
+     </property>
+     <property name="value">
+      <number>0</number>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QDoubleSpinBox" name="param1SpinBox">
+     <property name="prefix">
+      <string>P1 </string>
+     </property>
+     <property name="minimum">
+      <double>-2147483647.000000000000000</double>
+     </property>
+     <property name="maximum">
+      <double>2147483647.000000000000000</double>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QDoubleSpinBox" name="param2SpinBox">
+     <property name="prefix">
+      <string>P2 </string>
+     </property>
+     <property name="minimum">
+      <double>-2147483647.000000000000000</double>
+     </property>
+     <property name="maximum">
+      <double>2147483647.000000000000000</double>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QDoubleSpinBox" name="param3SpinBox">
+     <property name="prefix">
+      <string>P3 </string>
+     </property>
+     <property name="minimum">
+      <double>-2147483647.000000000000000</double>
+     </property>
+     <property name="maximum">
+      <double>2147483647.000000000000000</double>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QDoubleSpinBox" name="param4SpinBox">
+     <property name="prefix">
+      <string>P4 </string>
+     </property>
+     <property name="minimum">
+      <double>-2147483647.000000000000000</double>
+     </property>
+     <property name="maximum">
+      <double>2147483647.000000000000000</double>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>