diff --git a/images/earth.html b/images/earth.html
index bed577520d26b7cd3de3b0c5b1d237e8ce95b066..e30a9ca3384badda3b9b25fd5a481bf986030646 100644
--- a/images/earth.html
+++ b/images/earth.html
@@ -36,6 +36,7 @@ var currViewRange = 50.0;	///<< The current viewing range from this position (in
 var currTilt = 40.0;		///<< The tilt angle (in degrees)
 var currFollowTilt = 40.0;
 var currView = null;
+var distanceMode = 0;
 
 var viewMode = 0;
 var lastTilt = 0;
@@ -94,6 +95,11 @@ function getDraggingAllowed()
 	return draggingAllowed;
 }
 
+function setDistanceMode(mode)
+{
+	distanceMode = mode;
+}
+
 function setDraggingAllowed(allowed)
 {
 	draggingAllowed = allowed;
diff --git a/src/Waypoint.cc b/src/Waypoint.cc
index 3f3f484fef8bd1437212d299d128f51b4cf0dccf..8127e620db1e17ed21cf521c4587271c96014531 100644
--- a/src/Waypoint.cc
+++ b/src/Waypoint.cc
@@ -210,9 +210,9 @@ void Waypoint::setCurrent(bool current)
 
 void Waypoint::setAcceptanceRadius(double radius)
 {
-    if (this->param1 != radius)
+    if (this->param2 != radius)
     {
-        this->param1 = radius;
+        this->param2 = radius;
         emit changed(this);
     }
 }
@@ -291,9 +291,9 @@ void Waypoint::setLoiterOrbit(double orbit)
 
 void Waypoint::setHoldTime(int holdTime)
 {
-    if (this->param2 != holdTime)
+    if (this->param1 != holdTime)
     {
-        this->param2 = holdTime;
+        this->param1 = holdTime;
         emit changed(this);
     }
 }
diff --git a/src/Waypoint.h b/src/Waypoint.h
index d454758bcb0654e30779cc8227a60a5880e2cbe8..5dbf6e46caaac9027ae8a2a0b1f67c243788db73 100644
--- a/src/Waypoint.h
+++ b/src/Waypoint.h
@@ -59,8 +59,8 @@ public:
     bool getAutoContinue() const { return autocontinue; }
     bool getCurrent() const { return current; }
     double getLoiterOrbit() const { return orbit; }
-    double getAcceptanceRadius() const { return param1; }
-    double getHoldTime() const { return param2; }
+    double getAcceptanceRadius() const { return param2; }
+    double getHoldTime() const { return param1; }
     double getParam1() const { return param1; }
     double getParam2() const { return param2; }
     double getParam3() const { return orbit; }
diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index 781024c0632a47089fc9a974e988094b6f0d393b..9cac0f6f0b11cd5405209a7e703df4ed01bf1e07 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/src/comm/MAVLinkSimulationLink.cc
@@ -1001,9 +1001,8 @@ bool MAVLinkSimulationLink::connect()
     start(LowPriority);
     MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1, 47.376, 8.548);
     Q_UNUSED(mav1);
-    //MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2);
-    //
-    //Q_UNUSED(mav2);
+//    MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2, 47.375, 8.548, 1);
+//    Q_UNUSED(mav2);
     //    timer->start(rate);
     return true;
 }
diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc
index 70e25f0f3ffc56ee7c1957924cb38e39963ad60f..92f8877f7cd3d2f34938c7f514434d33f0a34e56 100644
--- a/src/comm/MAVLinkSimulationMAV.cc
+++ b/src/comm/MAVLinkSimulationMAV.cc
@@ -4,7 +4,7 @@
 
 #include "MAVLinkSimulationMAV.h"
 
-MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat, double lon) :
+MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat, double lon, int version) :
         QObject(parent),
         link(parent),
         planner(parent, systemid),
@@ -34,7 +34,8 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
         sys_mode(MAV_MODE_READY),
         sys_state(MAV_STATE_STANDBY),
         nav_mode(MAV_NAV_GROUNDED),
-        flying(false)
+        flying(false),
+        mavlink_version(version)
 {
     // Please note: The waypoint planner is running
     connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop()));
@@ -62,7 +63,7 @@ void MAVLinkSimulationMAV::mainloop()
     if (timer1Hz <= 0)
     {
         mavlink_message_t msg;
-        mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK);
+        mavlink_msg_heartbeat_pack_version_free(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK, mavlink_version);
         link->sendMAVLinkMessage(&msg);
         planner.handleMessage(msg);
 
diff --git a/src/comm/MAVLinkSimulationMAV.h b/src/comm/MAVLinkSimulationMAV.h
index 3a9d37adaec3db61513cf332de520b04f76fb2cd..32f04d8445550133f59adbcd9cfc5fdae319848d 100644
--- a/src/comm/MAVLinkSimulationMAV.h
+++ b/src/comm/MAVLinkSimulationMAV.h
@@ -11,7 +11,7 @@ class MAVLinkSimulationMAV : public QObject
 {
     Q_OBJECT
 public:
-    explicit MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat=47.376389, double lon=8.548056);
+    explicit MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat=47.376389, double lon=8.548056, int version=MAVLINK_VERSION);
 
 signals:
 
@@ -53,6 +53,19 @@ protected:
     uint8_t sys_state;
     uint8_t nav_mode;
     bool flying;
+    int mavlink_version;
+
+    static inline uint16_t mavlink_msg_heartbeat_pack_version_free(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot, uint8_t version)
+    {
+            uint16_t i = 0;
+            msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
+
+            i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
+            i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
+            i += put_uint8_t_by_index(version, i, msg->payload); // MAVLink version
+
+            return mavlink_finalize_message(msg, system_id, component_id, i);
+    }
 
 };
 
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 9d0eb3ee57c1dafcdb7b6f698575c965d0d39b3c..76c7c7a801f4af7c4dec110d033cb1ee53a3999a 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -894,7 +894,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit textMessageReceived(uasId, message.compid, severity, text);
             }
             break;
-
+#ifdef MAVLINK_ENABLED_PIXHAWK
         case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE:
             {
                 qDebug() << "RECIEVED ACK TO GET IMAGE";
@@ -939,7 +939,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 }
             }
             break;
-
+#endif
         case MAVLINK_MSG_ID_DEBUG_VECT:
             {
                 mavlink_debug_vect_t vect;
@@ -1363,6 +1363,7 @@ QImage UAS::getImage()
 
 void UAS::requestImage()
 {
+    #ifdef MAVLINK_ENABLED_PIXHAWK
     qDebug() << "trying to get an image from the uas...";
 
     if (imagePacketsArrived == 0)
@@ -1371,7 +1372,7 @@ void UAS::requestImage()
         mavlink_msg_data_transmission_handshake_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, DATA_TYPE_JPEG_IMAGE, 0, 0, 0, 50);
         sendMessage(msg);
     }
-    else if (MG::TIME::getGroundTimeNow() - imageStart >= 1000)
+    else if (QGC::groundTimeMilliseconds() - imageStart >= 1000)
     {
         // handshake happened more than 1 second ago, packets should have arrived by now
         // maybe we missed some packets (dropped along the way)
@@ -1380,6 +1381,7 @@ void UAS::requestImage()
         // Restart statemachine
         imagePacketsArrived = 0;
     }
+#endif
     // default else, wait?
 }
 
diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc
index 3d143b8f69676ef13490258efb12fe2f0b5f3fbc..ed12e1a2d6a8cb3cc2053a23693e8cf5344b9992 100644
--- a/src/ui/MapWidget.cc
+++ b/src/ui/MapWidget.cc
@@ -33,9 +33,14 @@ MapWidget::MapWidget(QWidget *parent) :
         uasTrails(),
         mav(NULL),
         lastUpdate(0),
+        initialized(false),
         m_ui(new Ui::MapWidget)
 {
     m_ui->setupUi(this);
+}
+
+void MapWidget::init()
+{
     mc = new qmapcontrol::MapControl(this->size());
 
     //   VISUAL MAP STYLE
@@ -268,6 +273,9 @@ MapWidget::MapWidget(QWidget *parent) :
 
     drawCamBorder = false;
     radioCamera = 10;
+
+    // Done set state
+    initialized = true;
 }
 
 void MapWidget::goTo()
@@ -1013,6 +1021,8 @@ void MapWidget::hideEvent(QHideEvent* event)
     settings.setValue("LAST_ZOOM", mc->currentZoom());
     settings.endGroup();
     settings.sync();
+
+    if (!initialized) init();
 }
 
 
diff --git a/src/ui/MapWidget.h b/src/ui/MapWidget.h
index 1490e593fbbf6c5eabeafc203d3a90a472594805..fd8608b54670813296d359d99390d5be30dc487b 100644
--- a/src/ui/MapWidget.h
+++ b/src/ui/MapWidget.h
@@ -146,6 +146,10 @@ protected:
     //QMap<int, qmapcontrol::LineString*> waypointPaths;
     UASInterface* mav;
     quint64 lastUpdate;
+    bool initialized;
+
+    /** @brief Initialize view */
+    void init();
 
   protected slots:
     void captureMapClick (const QMouseEvent* event, const QPointF coordinate);
diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc
index 0ca69377f2143213ab23bce0e1485fdabece8349..459ff34b85c72a0d2f21f270485e26d5bde396d6 100644
--- a/src/ui/WaypointView.cc
+++ b/src/ui/WaypointView.cc
@@ -97,6 +97,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
     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(m_ui->takeOffAngleSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam1(double)));
 
     // Connect actions
     connect(customCommand->commandSpinBox, SIGNAL(valueChanged(int)),   wp, SLOT(setAction(int)));
@@ -246,8 +247,6 @@ void WaypointView::changedAction(int index)
     case MAV_CMD_NAV_LOITER_UNLIM:
     case MAV_CMD_NAV_LOITER_TURNS:
     case MAV_CMD_NAV_LOITER_TIME:
-    case MAV_CMD_CONDITION_DELAY:
-    case MAV_CMD_DO_SET_MODE:
         changeViewMode(QGC_WAYPOINTVIEW_MODE_NAV);
         // Update frame view
         updateFrameView(m_ui->comboBox_frame->currentIndex());
@@ -434,9 +433,14 @@ void WaypointView::updateValues()
     // Only update if changed
     if (m_ui->comboBox_action->currentIndex() != action_index)
     {
-        // TODO Action and frame should not be coupled
-        if (wp->getFrame() != MAV_FRAME_MISSION)
+        // If action is unknown, set direct editing mode
+        if (wp->getAction() < 0 || wp->getAction() > MAV_CMD_NAV_TAKEOFF)
         {
+            changeViewMode(QGC_WAYPOINTVIEW_MODE_DIRECT_EDITING);
+        }
+        else
+        {
+            // Action ID known, update
             m_ui->comboBox_action->setCurrentIndex(action_index);
             updateActionView(action);
         }
@@ -480,6 +484,14 @@ void WaypointView::updateValues()
     {
         m_ui->holdTimeSpinBox->setValue(wp->getHoldTime());
     }
+    if (m_ui->turnsSpinBox->value() != wp->getTurns())
+    {
+        m_ui->turnsSpinBox->setValue(wp->getTurns());
+    }
+    if (m_ui->takeOffAngleSpinBox->value() != wp->getParam1())
+    {
+        m_ui->takeOffAngleSpinBox->setValue(wp->getParam1());
+    }
 
     // UPDATE CUSTOM ACTION WIDGET
 
diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc
index 5e4c9d3797081ab1b916e51fe2ef31e239b26e6f..a42107489506bc3d4b613965e20e0a2d593a288c 100644
--- a/src/ui/map3D/QGCGoogleEarthView.cc
+++ b/src/ui/map3D/QGCGoogleEarthView.cc
@@ -121,6 +121,11 @@ void QGCGoogleEarthView::reloadHTML()
     show();
 }
 
+void QGCGoogleEarthView::enableEditMode(bool mode)
+{
+    javaScript(QString("setDraggingAllowed(%1);").arg(mode));
+}
+
 /**
  * @param range in meters (SI-units)
  */
@@ -129,6 +134,11 @@ void QGCGoogleEarthView::setViewRange(float range)
     javaScript(QString("setViewRange(%1);").arg(range, 0, 'f', 5));
 }
 
+void QGCGoogleEarthView::setDistanceMode(int mode)
+{
+    javaScript(QString("setDistanceMode(%1);").arg(mode));
+}
+
 void QGCGoogleEarthView::toggleViewMode()
 {
     switch (currentViewMode)
@@ -469,26 +479,31 @@ void QGCGoogleEarthView::initializeGoogleEarth()
             setActiveUAS(UASManager::instance()->getActiveUAS());
 
             // Add any further MAV automatically
-            connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)));
-            connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
+            connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)), Qt::UniqueConnection);
+            connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)), Qt::UniqueConnection);
 
             // Connect UI signals/slots
 
             // Follow checkbox
             ui->followAirplaneCheckbox->setChecked(followCamera);
-            connect(ui->followAirplaneCheckbox, SIGNAL(toggled(bool)), this, SLOT(follow(bool)));
+            connect(ui->followAirplaneCheckbox, SIGNAL(toggled(bool)), this, SLOT(follow(bool)), Qt::UniqueConnection);
 
             // Trail checkbox
             ui->trailCheckbox->setChecked(trailEnabled);
-            connect(ui->trailCheckbox, SIGNAL(toggled(bool)), this, SLOT(showTrail(bool)));
+            connect(ui->trailCheckbox, SIGNAL(toggled(bool)), this, SLOT(showTrail(bool)), Qt::UniqueConnection);
 
             // Go home
             connect(ui->goHomeButton, SIGNAL(clicked()), this, SLOT(goHome()));
 
             // Cam distance slider
-            connect(ui->camDistanceSlider, SIGNAL(valueChanged(int)), this, SLOT(setViewRangeScaledInt(int)));
+            connect(ui->camDistanceSlider, SIGNAL(valueChanged(int)), this, SLOT(setViewRangeScaledInt(int)), Qt::UniqueConnection);
             setViewRangeScaledInt(ui->camDistanceSlider->value());
 
+            // Distance combo box
+            connect(ui->camDistanceComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(setDistanceMode(int)), Qt::UniqueConnection);
+            // Edit mode button
+            connect(ui->editButton, SIGNAL(clicked(bool)), this, SLOT(enableEditMode(bool)), Qt::UniqueConnection);
+
             // Update waypoint list
             if (mav) updateWaypointList(mav->getUASID());
 
@@ -497,7 +512,8 @@ void QGCGoogleEarthView::initializeGoogleEarth()
 
             // Set current view mode
             setViewMode(currentViewMode);
-
+            setDistanceMode(ui->camDistanceComboBox->currentIndex());
+            enableEditMode(ui->editButton->isChecked());
             follow(this->followCamera);
 
             gEarthInitialized = true;
diff --git a/src/ui/map3D/QGCGoogleEarthView.h b/src/ui/map3D/QGCGoogleEarthView.h
index 994cca31e1c72d9a200d329788a3b60803285b7a..5d256ce4dea837b8c3885738fa72c96be8361794 100644
--- a/src/ui/map3D/QGCGoogleEarthView.h
+++ b/src/ui/map3D/QGCGoogleEarthView.h
@@ -102,8 +102,12 @@ public slots:
     void goHome();
     /** @brief Set the home location */
     void setHome(double lat, double lon, double alt);
+    /** @brief Allow waypoint editing */
+    void enableEditMode(bool mode);
     /** @brief Set camera view range to aircraft in meters */
     void setViewRange(float range);
+    /** @brief Set the distance mode - either to ground or to MAV */
+    void setDistanceMode(int mode);
     /** @brief Set the view mode */
     void setViewMode(VIEW_MODE mode);
     /** @brief Toggle through the different view modes */
diff --git a/src/ui/map3D/QGCGoogleEarthView.ui b/src/ui/map3D/QGCGoogleEarthView.ui
index ec6e4bae06664ba8c12204dba8f1fafe6f340584..81140f1d2c1934ebf97a3b7d07fadc8bfe0fbdce 100644
--- a/src/ui/map3D/QGCGoogleEarthView.ui
+++ b/src/ui/map3D/QGCGoogleEarthView.ui
@@ -13,7 +13,7 @@
   <property name="windowTitle">
    <string>Form</string>
   </property>
-  <layout class="QGridLayout" name="gridLayout">
+  <layout class="QGridLayout" name="gridLayout" columnstretch="1,1,1,1,1,1,30,1,100,1,1,1,1,1,1">
    <property name="horizontalSpacing">
     <number>8</number>
    </property>
@@ -23,7 +23,7 @@
    <property name="margin">
     <number>2</number>
    </property>
-   <item row="0" column="0" colspan="14">
+   <item row="0" column="0" colspan="15">
     <layout class="QVBoxLayout" name="webViewLayout"/>
    </item>
    <item row="1" column="1">
@@ -58,7 +58,7 @@
      </property>
     </widget>
    </item>
-   <item row="1" column="13">
+   <item row="1" column="14">
     <spacer name="horizontalSpacer">
      <property name="orientation">
       <enum>Qt::Horizontal</enum>
@@ -71,7 +71,7 @@
      </property>
     </spacer>
    </item>
-   <item row="1" column="9">
+   <item row="1" column="8">
     <widget class="QSlider" name="camDistanceSlider">
      <property name="toolTip">
       <string>Distance of the chase camera to the MAV</string>
@@ -96,22 +96,6 @@
      </property>
     </widget>
    </item>
-   <item row="1" column="8">
-    <widget class="QLabel" name="camDistanceLabel">
-     <property name="toolTip">
-      <string>Distance of the chase camera to the MAV</string>
-     </property>
-     <property name="statusTip">
-      <string>Distance of the chase camera to the MAV</string>
-     </property>
-     <property name="whatsThis">
-      <string>Distance of the chase camera to the MAV</string>
-     </property>
-     <property name="text">
-      <string>Dist</string>
-     </property>
-    </widget>
-   </item>
    <item row="1" column="4">
     <widget class="QCheckBox" name="waypointsCheckbox">
      <property name="toolTip">
@@ -128,7 +112,7 @@
      </property>
     </widget>
    </item>
-   <item row="1" column="10">
+   <item row="1" column="9">
     <widget class="QCheckBox" name="followAirplaneCheckbox">
      <property name="toolTip">
       <string>Chase the MAV</string>
@@ -145,7 +129,7 @@
     </widget>
    </item>
    <item row="1" column="6">
-    <widget class="QComboBox" name="mavComboBox">
+    <widget class="QComboBox" name="camDistanceComboBox">
      <property name="toolTip">
       <string>Select the MAV to chase</string>
      </property>
@@ -155,9 +139,40 @@
      <property name="whatsThis">
       <string>Select the MAV to chase</string>
      </property>
+     <item>
+      <property name="text">
+       <string>Ground Distance</string>
+      </property>
+     </item>
+     <item>
+      <property name="text">
+       <string>MAV Distance</string>
+      </property>
+     </item>
+    </widget>
+   </item>
+   <item row="1" column="5">
+    <widget class="Line" name="line">
+     <property name="orientation">
+      <enum>Qt::Vertical</enum>
+     </property>
     </widget>
    </item>
-   <item row="1" column="7">
+   <item row="1" column="13">
+    <widget class="QPushButton" name="resetButton">
+     <property name="text">
+      <string>Reset</string>
+     </property>
+    </widget>
+   </item>
+   <item row="1" column="10">
+    <widget class="QPushButton" name="changeViewButton">
+     <property name="text">
+      <string>Overhead</string>
+     </property>
+    </widget>
+   </item>
+   <item row="1" column="12">
     <widget class="QPushButton" name="clearTrailsButton">
      <property name="toolTip">
       <string>Delete all waypoints</string>
@@ -173,24 +188,26 @@
      </property>
     </widget>
    </item>
-   <item row="1" column="5">
-    <widget class="Line" name="line">
+   <item row="1" column="11">
+    <widget class="Line" name="line_2">
      <property name="orientation">
       <enum>Qt::Vertical</enum>
      </property>
     </widget>
    </item>
-   <item row="1" column="12">
-    <widget class="QPushButton" name="resetButton">
-     <property name="text">
-      <string>Reset</string>
+   <item row="1" column="2">
+    <widget class="QPushButton" name="editButton">
+     <property name="toolTip">
+      <string>Enable waypoint and home location edit mode</string>
+     </property>
+     <property name="statusTip">
+      <string>Enable waypoint and home location edit mode</string>
      </property>
-    </widget>
-   </item>
-   <item row="1" column="11">
-    <widget class="QPushButton" name="changeViewButton">
      <property name="text">
-      <string>Overhead</string>
+      <string>Edit</string>
+     </property>
+     <property name="checkable">
+      <bool>true</bool>
      </property>
     </widget>
    </item>