diff --git a/src/Waypoint.cc b/src/Waypoint.cc
index d4e2746913170278a17fb2cdbf438f1b342500a4..b3d7ca4bff0b176e1c5b687d80ceb5d6b8b96137 100644
--- a/src/Waypoint.cc
+++ b/src/Waypoint.cc
@@ -32,15 +32,17 @@ This file is part of the PIXHAWK project
 
 #include "Waypoint.h"
 
-Waypoint::Waypoint(quint16 id, float x, float y, float z, float yaw, bool autocontinue, bool current)
+Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _autocontinue, bool _current, float _orbit, int _holdTime)
+: id(_id),
+  x(_x),
+  y(_y),
+  z(_z),
+  yaw(_yaw),
+  autocontinue(_autocontinue),
+  current(_current),
+  orbit(_orbit),
+  holdTime(_holdTime)
 {
-    this->id = id;
-    this->x = x;
-    this->y = y;
-    this->z = z;
-    this->yaw = yaw;
-    this->autocontinue = autocontinue;
-    this->current = current;
 }
 
 Waypoint::~Waypoint()
@@ -83,6 +85,16 @@ void Waypoint::setCurrent(bool current)
     this->current = current;
 }
 
+void Waypoint::setOrbit(float orbit)
+{
+    this->orbit = orbit;
+}
+
+void Waypoint::setHoldTime(int holdTime)
+{
+    this->holdTime = holdTime;
+}
+
 void Waypoint::setX(double x)
 {
     this->x = x;
@@ -102,3 +114,8 @@ void Waypoint::setYaw(double yaw)
 {
     this->yaw = yaw;
 }
+
+void Waypoint::setOrbit(double orbit)
+{
+    this->orbit = orbit;
+}
diff --git a/src/Waypoint.h b/src/Waypoint.h
index d736af3dfd1c8e2ead32f8f97b63ec6eb83cab34..e5c4ae1bdbe12afb932c029e27ac7da4df41c9d0 100644
--- a/src/Waypoint.h
+++ b/src/Waypoint.h
@@ -41,7 +41,7 @@ class Waypoint : public QObject
     Q_OBJECT
 
 public:
-    Waypoint(quint16 id = 0, float x = 0.0f, float y = 0.0f, float z = 0.0f, float yaw = 0.0f, bool autocontinue = false, bool current = false);
+    Waypoint(quint16 id = 0, float x = 0.0f, float y = 0.0f, float z = 0.0f, float yaw = 0.0f, bool autocontinue = false, bool current = false, float orbit = 0.1f, int holdTime = 2000);
     ~Waypoint();
 
     quint16 getId() const { return id; }
@@ -51,6 +51,8 @@ public:
     float getYaw() const { return yaw; }
     bool getAutoContinue() const { return autocontinue; }
     bool getCurrent() const { return current; }
+    float getOrbit() const { return orbit; }
+    float getHoldTime() const { return holdTime; }
 
 private:
     quint16 id;
@@ -60,6 +62,8 @@ private:
     float yaw;
     bool autocontinue;
     bool current;
+    float orbit;
+    int holdTime;
 
 public slots:
     void setId(quint16 id);
@@ -69,12 +73,15 @@ public slots:
     void setYaw(float yaw);
     void setAutocontinue(bool autoContinue);
     void setCurrent(bool current);
+    void setOrbit(float orbit);
+    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);
 };
 
 #endif // WAYPOINT_H
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 8185d21e6daccf3fa4362f521522201d2c1fe9af..2dee93140b8f79b5eae01c7d86bda73d7dee9b24 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -661,10 +661,20 @@ void UAS::requestParameters()
     sendMessage(msg);
 }
 
-void UAS::writeParameters()
+void UAS::writeParametersToStorage()
 {
-    //mavlink_message_t msg;
-    qDebug() << __FILE__ << __LINE__ << __func__ << "IS NOT IMPLEMENTED!";
+    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_STORAGE_WRITE);
+    sendMessage(msg);
+}
+
+void UAS::readParametersFromStorage()
+{
+    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_STORAGE_READ);
+    sendMessage(msg);
 }
 
 void UAS::enableAllDataTransmission(bool enabled)
@@ -929,7 +939,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(),(uint8_t)MAV_ACTION_LAUNCH);
+    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_LAUNCH);
     // Send message twice to increase chance of reception
     sendMessage(msg);
     sendMessage(msg);
@@ -943,7 +953,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(),(uint8_t)MAV_ACTION_MOTORS_START);
+    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &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);
@@ -957,7 +967,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(),(uint8_t)MAV_ACTION_MOTORS_STOP);
+    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &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);
@@ -1069,7 +1079,7 @@ void UAS::halt()
 {
     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(), (int)MAV_ACTION_HALT);
+    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_HALT);
     // Send message twice to increase chance of reception
     sendMessage(msg);
     sendMessage(msg);
@@ -1079,7 +1089,7 @@ void UAS::go()
 {
     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(), (int)MAV_ACTION_CONTINUE);
+    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,  (int)MAV_ACTION_CONTINUE);
     // Send message twice to increase chance of reception
     sendMessage(msg);
     sendMessage(msg);
@@ -1090,7 +1100,7 @@ void UAS::home()
 {
     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(), (int)MAV_ACTION_RETURN);
+    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,  (int)MAV_ACTION_RETURN);
     // Send message twice to increase chance of reception
     sendMessage(msg);
     sendMessage(msg);
@@ -1104,7 +1114,7 @@ void UAS::emergencySTOP()
 {
     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(), (int)MAV_ACTION_EMCY_LAND);
+    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND);
     // Send message twice to increase chance of reception
     sendMessage(msg);
     sendMessage(msg);
@@ -1135,7 +1145,7 @@ bool UAS::emergencyKILL()
     {
         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(), (int)MAV_ACTION_EMCY_KILL);
+        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL);
         // Send message twice to increase chance of reception
         sendMessage(msg);
         sendMessage(msg);
@@ -1164,7 +1174,7 @@ void UAS::shutdown()
         // If the active UAS is set, execute command
         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(), (int)MAV_ACTION_SHUTDOWN);
+        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_SHUTDOWN);
         // Send message twice to increase chance of reception
         sendMessage(msg);
         sendMessage(msg);
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 88cb4027b1709a413eb1784bc58d01d270c14b9a..37e82e1173ad5a34531f387b895874e2799f0cd7 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -200,7 +200,9 @@ public slots:
     void setParameter(int component, QString id, float value);
 
     /** @brief Write parameters to permanent storage */
-    void writeParameters();
+    void writeParametersToStorage();
+    /** @brief Read parameters from permanent storage */
+    void readParametersFromStorage();
 
     void enableAllDataTransmission(bool enabled);
     void enableRawSensorDataTransmission(bool enabled);
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index 6c4d54e570120324138f8d9343107830bf3c90c1..3ebcbdb2ce7196039456dc497ebb0525e167d5c8 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -173,7 +173,9 @@ public slots:
     /** @brief Request all onboard parameters of all components */
     virtual void requestParameters() = 0;
     /** @brief Write parameter to permanent storage */
-    virtual void writeParameters() = 0;
+    virtual void writeParametersToStorage() = 0;
+    /** @brief Read parameter from permanent storage */
+    virtual void readParametersFromStorage() = 0;
     /** @brief Set a system parameter
      * @param component ID of the system component to write the parameter to
      * @param id String identifying the parameter
diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc
index 4f5b9d94ffe4feff80cc9a404761eedb5b92c76c..05523810cfc7c5cc9dbe2db5613e7ebe6dff284b 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);
+            emit waypointUpdated(uas.getUASID(), 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++;
@@ -237,7 +237,8 @@ void UASWaypointManager::sendWaypoints(const QVector<Waypoint*> &list)
 
                 cur_d->autocontinue = cur_s->getAutoContinue();
                 cur_d->current = cur_s->getCurrent();
-                cur_d->orbit = 0.1f;    //FIXME
+                cur_d->orbit = cur_s->getOrbit();
+                cur_d->hold_time = cur_s->getHoldTime();
                 cur_d->type = 1;        //FIXME
                 cur_d->seq = i;
                 cur_d->x = cur_s->getX();
diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h
index 247feacc1d8079d3e61745752177306bec22327a..8e7e7458453b96c9cee4c9537b3813a9448af043 100644
--- a/src/uas/UASWaypointManager.h
+++ b/src/uas/UASWaypointManager.h
@@ -71,7 +71,7 @@ public slots:
     void sendWaypoints(const QVector<Waypoint *> &list);
 
 signals:
-    void waypointUpdated(int,quint16,double,double,double,double,bool,bool);    ///< Adds a waypoint to the waypoint list widget
+    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
 
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index a54c2dea17bcf47d28f04b6ffc097af78355c300..c758e24fbe47a70725bbbacff437e35b431a6492 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -504,7 +504,7 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir
     bodyYawSet = yawDesired;
     mavInitialized = true;
 
-    qDebug() << "Received setpoint at x: " << x << "metric y:" << y;
+//    qDebug() << "Received setpoint at x: " << x << "metric y:" << y;
 //    posXSet = xDesired;
 //    posYSet = yDesired;
 //    posZSet = zDesired;
diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc
index 58c9617bd03ab8dacac4fee111da286b71951efb..aaf08d03a33e48689eab26432e04156ef0e0dadd 100644
--- a/src/ui/QGCParamWidget.cc
+++ b/src/ui/QGCParamWidget.cc
@@ -31,6 +31,8 @@ This file is part of the QGROUNDCONTROL project
 
 #include <QGridLayout>
 #include <QPushButton>
+#include <QFileDialog>
+#include <QFile>
 
 #include "QGCParamWidget.h"
 #include "UASInterface.h"
@@ -46,7 +48,8 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) :
         mav(uas),
         components(new QMap<int, QTreeWidgetItem*>()),
         paramGroups(),
-        changedValues()
+        changedValues(),
+        parameters()
 {
     // Create tree widget
     tree = new QTreeWidget(this);
@@ -61,18 +64,30 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) :
     horizontalLayout->setSizeConstraint(QLayout::SetMinimumSize);
 
     horizontalLayout->addWidget(tree, 0, 0, 1, 3);
-    QPushButton* readButton = new QPushButton(tr("Read"));
-    connect(readButton, SIGNAL(clicked()), this, SLOT(requestParameterList()));
-    horizontalLayout->addWidget(readButton, 1, 0);
+    QPushButton* refreshButton = new QPushButton(tr("Refresh"));
+    connect(refreshButton, SIGNAL(clicked()), this, SLOT(requestParameterList()));
+    horizontalLayout->addWidget(refreshButton, 1, 0);
 
-    QPushButton* setButton = new QPushButton(tr("Set (RAM)"));
+    QPushButton* setButton = new QPushButton(tr("Transmit"));
     connect(setButton, SIGNAL(clicked()), this, SLOT(setParameters()));
     horizontalLayout->addWidget(setButton, 1, 1);
 
-    QPushButton* writeButton = new QPushButton(tr("Write (Disk)"));
+    QPushButton* writeButton = new QPushButton(tr("Write (ROM)"));
     connect(writeButton, SIGNAL(clicked()), this, SLOT(writeParameters()));
     horizontalLayout->addWidget(writeButton, 1, 2);
 
+    QPushButton* readButton = new QPushButton(tr("Read (ROM)"));
+    connect(readButton, SIGNAL(clicked()), this, SLOT(readParameters()));
+    horizontalLayout->addWidget(readButton, 2, 2);
+
+    QPushButton* loadFileButton = new QPushButton(tr("Load File"));
+    connect(loadFileButton, SIGNAL(clicked()), this, SLOT(loadParameters()));
+    horizontalLayout->addWidget(loadFileButton, 2, 0);
+
+    QPushButton* saveFileButton = new QPushButton(tr("Save File"));
+    connect(saveFileButton, SIGNAL(clicked()), this, SLOT(saveParameters()));
+    horizontalLayout->addWidget(saveFileButton, 2, 1);
+
     // Set layout
     this->setLayout(horizontalLayout);
 
@@ -128,6 +143,16 @@ void QGCParamWidget::addComponent(int uas, int component, QString componentName)
         paramGroups.insert(component, new QMap<QString, QTreeWidgetItem*>());
         tree->addTopLevelItem(comp);
         tree->update();
+        // Create map in parameters
+        if (!parameters.contains(component))
+        {
+            parameters.insert(component, new QMap<QString, float>());
+        }
+        // Create map in changed parameters
+        if (!changedValues.contains(component))
+        {
+            changedValues.insert(component, new QMap<QString, float>());
+        }
     }
 }
 
@@ -148,6 +173,13 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName,
         addComponent(uas, component, "Component #" + QString::number(component));
     }
 
+    // Replace value in map
+
+    // FIXME
+    if (parameters.value(component)->contains(parameterName)) parameters.value(component)->remove(parameterName);
+    parameters.value(component)->insert(parameterName, value);
+
+
     QString splitToken = "_";
     // Check if auto-grouping can work
     if (parameterName.contains(splitToken))
@@ -267,16 +299,101 @@ void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column)
                 if (ok)
                 {
                     qDebug() << "PARAM CHANGED: COMP:" << key << "KEY:" << str << "VALUE:" << value;
+                    // Changed values list
                     if (map->contains(str)) map->remove(str);
                     map->insert(str, value);
-                    //current->setBackground(0, QBrush(QColor(QGC::colorGreen)));
-                    //current->setBackground(1, QBrush(QColor(QGC::colorGreen)));
+
+                    // Check if the value was numerically changed
+                    if (!parameters.value(key)->contains(str) || parameters.value(key)->value(str, 0.0f) != value)
+                    {
+                        current->setBackground(0, QBrush(QColor(QGC::colorGreen)));
+                        current->setBackground(1, QBrush(QColor(QGC::colorGreen)));
+                    }
+
+                    // All parameters list
+                    if (parameters.value(key)->contains(str)) parameters.value(key)->remove(str);
+                    parameters.value(key)->insert(str, value);
                 }
             }
         }
     }
 }
 
+void QGCParamWidget::saveParameters()
+{    
+    QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./parameters.txt", tr("Parameter File (*.txt)"));
+    QFile file(fileName);
+    if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
+    {
+        return;
+    }
+
+    QTextStream in(&file);
+
+    in << "# Onboard parameters for system " << mav->getUASName() << "\n";
+    in << "#\n";
+    in << "# MAV ID  COMPONENT ID  PARAM NAME  VALUE (FLOAT)\n";
+
+    // Iterate through all components, through all parameters and emit them
+    QMap<int, QMap<QString, float>*>::iterator i;
+    for (i = parameters.begin(); i != parameters.end(); ++i)
+    {
+        // Iterate through the parameters of the component
+        int compid = i.key();
+        QMap<QString, float>* comp = i.value();
+        {
+            QMap<QString, float>::iterator j;
+            for (j = comp->begin(); j != comp->end(); ++j)
+            {
+                in << mav->getUASID() << "\t" << compid << "\t" << j.key() << "\t" << j.value() << "\n";
+                in.flush();
+            }
+        }
+    }
+    file.close();
+}
+
+void QGCParamWidget::loadParameters()
+{
+    QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Parameter file (*.txt)"));
+    QFile file(fileName);
+    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
+        return;
+
+    // Clear list
+    clear();
+
+    QTextStream in(&file);
+    while (!in.atEnd())
+    {
+        QString line = in.readLine();
+        if (!line.startsWith("#"))
+        {
+            QStringList wpParams = line.split("\t");
+            if (wpParams.size() == 4)
+            {
+                // Only load parameters for right mav
+                if (mav->getUASID() == wpParams.at(0).toInt())
+                {
+                    addParameter(wpParams.at(0).toInt(), wpParams.at(1).toInt(), wpParams.at(2), wpParams.at(3).toDouble());
+                    if (changedValues.contains(wpParams.at(1).toInt()))
+                    {
+                        if (changedValues.value(wpParams.at(1).toInt())->contains(wpParams.at(1)))
+                        {
+                            changedValues.value(wpParams.at(1).toInt())->remove(wpParams.at(1));
+                        }
+
+                        changedValues.value(wpParams.at(1).toInt())->insert(wpParams.at(1), (float)wpParams.at(2).toDouble());
+                    }
+                }
+            }
+        }
+    }
+    file.close();
+
+}
+
+
 /**
  * @param component the subsystem which has the parameter
  * @param parameterName name of the parameter, as delivered by the system
@@ -319,7 +436,12 @@ void QGCParamWidget::setParameters()
  */
 void QGCParamWidget::writeParameters()
 {
-    mav->writeParameters();
+    mav->writeParametersToStorage();
+}
+
+void QGCParamWidget::readParameters()
+{
+    mav->readParametersFromStorage();
 }
 
 /**
diff --git a/src/ui/QGCParamWidget.h b/src/ui/QGCParamWidget.h
index 55dfd441b9ac6f62fc89dea9c2e7a743c60a77da..c688216e60b2aa52ca14b6d9866045dac3edd44e 100644
--- a/src/ui/QGCParamWidget.h
+++ b/src/ui/QGCParamWidget.h
@@ -65,16 +65,24 @@ public slots:
     void setParameters();
     /** @brief Write the current parameters to permanent storage (EEPROM/HDD) */
     void writeParameters();
+    /** @brief Read the parameters from permanent storage to RAM */
+    void readParameters();
     /** @brief Clear the parameter list */
     void clear();
     /** @brief Update when user changes parameters */
     void parameterItemChanged(QTreeWidgetItem* prev, int column);
+
+    /** @brief Store parameters to a file */
+    void saveParameters();
+    /** @brief Load parameters from a file */
+    void loadParameters();
 protected:
     UASInterface* mav;  ///< The MAV this widget is controlling
     QTreeWidget* tree;  ///< The parameter tree
     QMap<int, QTreeWidgetItem*>* components; ///< The list of components
     QMap<int, QMap<QString, QTreeWidgetItem*>* > paramGroups; ///< Parameter groups
     QMap<int, QMap<QString, float>* > changedValues; ///< Changed values
+    QMap<int, QMap<QString, float>* > parameters; ///< All parameters
 
 };
 
diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc
index 622b67549afe40ae18d750f5c7ce5fe2048abf40..39fe08554fa593124fd498ddaa76b4e3ceaac723 100644
--- a/src/ui/WaypointList.cc
+++ b/src/ui/WaypointList.cc
@@ -96,7 +96,7 @@ 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)), this, SLOT(setWaypoint(int,quint16,double,double,double,double,bool,bool)));
+        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(this, SIGNAL(sendWaypoints(const QVector<Waypoint*> &)),    &uas->getWaypointManager(), SLOT(sendWaypoints(const QVector<Waypoint*> &)));
@@ -105,11 +105,11 @@ void WaypointList::setUAS(UASInterface* uas)
     }
 }
 
-void WaypointList::setWaypoint(int uasId, quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current)
+void WaypointList::setWaypoint(int uasId, quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime)
 {
     if (uasId == this->uas->getUASID())
     {
-        Waypoint* wp = new Waypoint(id, x, y, z, yaw, autocontinue, current);
+        Waypoint* wp = new Waypoint(id, x, y, z, yaw, autocontinue, current, orbit, holdTime);
         addWaypoint(wp);
     }
 }
@@ -168,11 +168,11 @@ void WaypointList::add()
     {
         if (waypoints.size() > 0)
         {
-            addWaypoint(new Waypoint(waypoints.size(), 1.1, 1.1, -0.6, 0.0, true, false));
+            addWaypoint(new Waypoint(waypoints.size(), 1.1, 1.1, -0.6, 0.0, true, false, 0.1, 2000));
         }
         else
         {
-            addWaypoint(new Waypoint(waypoints.size(), 0.0, 0.0, -0.0, 0.0, true, true));
+            addWaypoint(new Waypoint(waypoints.size(), 1.1, 1.1, -0.6, 0.0, true, true, 0.1, 2000));
         }
     }
 }
@@ -302,7 +302,7 @@ void WaypointList::saveWaypoints()
     for (int i = 0; i < waypoints.size(); i++)
     {
         Waypoint* wp = waypoints[i];
-        in << "~" << wp->getId() << "~" << wp->getX() << "~" << wp->getY()  << "~" << wp->getZ()  << "~" << wp->getYaw()  << "~" << wp->getAutoContinue() << "~" << wp->getCurrent() << "\n";
+        in << "\t" << wp->getId() << "\t" << wp->getX() << "\t" << wp->getY()  << "\t" << wp->getZ()  << "\t" << wp->getYaw()  << "\t" << wp->getAutoContinue() << "\t" << wp->getCurrent() << wp->getOrbit() << "\t" << wp->getHoldTime() << "\n";
         in.flush();
     }
     file.close();
@@ -323,9 +323,9 @@ void WaypointList::loadWaypoints()
     QTextStream in(&file);
     while (!in.atEnd())
     {
-        QStringList wpParams = in.readLine().split("~");
+        QStringList wpParams = in.readLine().split("\t");
         if (wpParams.size() == 8)
-            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)));
+            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();
 }
diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h
index fecd118e95a3e8809c872f0038bc033b0402484e..1164b9f00ee2d4bde320c7827c8fe78a1379ee9b 100644
--- a/src/ui/WaypointList.h
+++ b/src/ui/WaypointList.h
@@ -72,7 +72,7 @@ public slots:
     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);
+    void setWaypoint(int uasId, 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);
diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc
index 17e09197045ccc27f4d9d590d13134a298751f20..bb79f74d47562782b9544d40477747dceba148ec 100644
--- a/src/ui/WaypointView.cc
+++ b/src/ui/WaypointView.cc
@@ -54,7 +54,9 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
     m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.);
     m_ui->selectedBox->setChecked(wp->getCurrent());
     m_ui->autoContinue->setChecked(wp->getAutoContinue());
-    m_ui->idLabel->setText(QString("%1").arg(wp->getId()));
+    m_ui->idLabel->setText(QString("%1").arg(wp->getId()));\
+    m_ui->orbitSpinBox->setValue(wp->getOrbit());
+    m_ui->holdTimeSpinBox->setValue(wp->getHoldTime());
 
     connect(m_ui->xSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double)));
     connect(m_ui->ySpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double)));
@@ -70,6 +72,9 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
 
     connect(m_ui->autoContinue, SIGNAL(stateChanged(int)), this, SLOT(changedAutoContinue(int)));
     connect(m_ui->selectedBox, SIGNAL(stateChanged(int)), this, SLOT(changedCurrent(int)));
+
+    connect(m_ui->orbitSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setOrbit(double)));
+    connect(m_ui->holdTimeSpinBox, SIGNAL(valueChanged(int)), wp, SLOT(setHoldTime(int)));
 }
 
 void WaypointView::setYaw(int yawDegree)
diff --git a/src/ui/WaypointView.ui b/src/ui/WaypointView.ui
index e344c5682654fe0875aec64827002b469abf7d11..97f8013a66de0b7e79a093fb3d69baa380693e57 100644
--- a/src/ui/WaypointView.ui
+++ b/src/ui/WaypointView.ui
@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>595</width>
+    <width>763</width>
     <height>40</height>
    </rect>
   </property>
@@ -155,7 +155,7 @@ QProgressBar::chunk#thrustBar {
      <property name="title">
       <string/>
      </property>
-     <layout class="QHBoxLayout" name="horizontalLayout" stretch="0,0,5,0,0,0,0,0,0,0,0">
+     <layout class="QHBoxLayout" name="horizontalLayout" stretch="0,0,0,0,0,0,0,0,0,0,0,0,0">
       <property name="spacing">
        <number>2</number>
       </property>
@@ -216,13 +216,13 @@ QProgressBar::chunk#thrustBar {
          <string>Position X coordinate</string>
         </property>
         <property name="suffix">
-         <string>m</string>
+         <string> m</string>
         </property>
         <property name="minimum">
-         <double>-100000.000000000000000</double>
+         <double>-1000.000000000000000</double>
         </property>
         <property name="maximum">
-         <double>100000.000000000000000</double>
+         <double>1000.000000000000000</double>
         </property>
         <property name="singleStep">
          <double>0.050000000000000</double>
@@ -235,13 +235,13 @@ QProgressBar::chunk#thrustBar {
          <string>Position Y coordinate</string>
         </property>
         <property name="suffix">
-         <string>m</string>
+         <string> m</string>
         </property>
         <property name="minimum">
-         <double>-100000.000000000000000</double>
+         <double>-1000.000000000000000</double>
         </property>
         <property name="maximum">
-         <double>100000.000000000000000</double>
+         <double>1000.000000000000000</double>
         </property>
         <property name="singleStep">
          <double>0.050000000000000</double>
@@ -257,10 +257,10 @@ QProgressBar::chunk#thrustBar {
          <string>Position Z coordinate</string>
         </property>
         <property name="suffix">
-         <string>m</string>
+         <string> m</string>
         </property>
         <property name="minimum">
-         <double>-100000.000000000000000</double>
+         <double>-1000.000000000000000</double>
         </property>
         <property name="maximum">
          <double>0.000000000000000</double>
@@ -284,6 +284,47 @@ QProgressBar::chunk#thrustBar {
         <property name="maximum">
          <number>359</number>
         </property>
+        <property name="singleStep">
+         <number>10</number>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <widget class="QDoubleSpinBox" name="orbitSpinBox">
+        <property name="toolTip">
+         <string>Orbit radius</string>
+        </property>
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="minimum">
+         <double>0.050000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>1.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.050000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <widget class="QSpinBox" name="holdTimeSpinBox">
+        <property name="statusTip">
+         <string>Time in milliseconds that the MAV has to stay inside the orbit before advancing</string>
+        </property>
+        <property name="suffix">
+         <string> ms</string>
+        </property>
+        <property name="maximum">
+         <number>60000</number>
+        </property>
+        <property name="singleStep">
+         <number>500</number>
+        </property>
+        <property name="value">
+         <number>0</number>
+        </property>
        </widget>
       </item>
       <item>