diff --git a/files/mav_type_quadrotor/mav_autopilot_pixhawk/parameters/parameters-bravo.txt b/files/mav_type_quadrotor/mav_autopilot_pixhawk/parameters/parameters-bravo.txt
new file mode 100644
index 0000000000000000000000000000000000000000..16ed4cb98604ed0fee4d795b3fac9389acae2bad
--- /dev/null
+++ b/files/mav_type_quadrotor/mav_autopilot_pixhawk/parameters/parameters-bravo.txt
@@ -0,0 +1,119 @@
+# Onboard parameters for system MAV 042
+#
+# MAV ID  COMPONENT ID  PARAM NAME  VALUE (FLOAT)
+42	1	HANDLEWPDELAY	            0.20000000298
+42	1	POSFILTER	                        1
+42	1	PROTDELAY	                       40
+42	1	PROTTIMEOUT	                        2
+42	1	SETPOINTDELAY	                        1
+42	1	YAWTOLERANCE	           0.174500003457
+42	200	ACC_NAV_OFFS_X	                        0
+42	200	ACC_NAV_OFFS_Y	                        0
+42	200	ACC_NAV_OFFS_Z	                    -1000
+42	200	ATT_KAL_KACC	         0.00329999998212
+42	200	ATT_KAL_YAWMOD	                        3
+42	200	ATT_OFFSET_X	                        0
+42	200	ATT_OFFSET_Y	                        0
+42	200	ATT_OFFSET_Z	         -0.0799999982119
+42	200	CAL_ACC_X	                        0
+42	200	CAL_ACC_Y	                        0
+42	200	CAL_ACC_Z	                        0
+42	200	CAL_FIT_ACTIVE	                        0
+42	200	CAL_FIT_GYRO_X	            31496.8007812
+42	200	CAL_FIT_GYRO_Y	            29383.4003906
+42	200	CAL_FIT_GYRO_Z	            30151.0996094
+42	200	CAL_GYRO_X	                    29767
+42	200	CAL_GYRO_Y	                    29654
+42	200	CAL_GYRO_Z	                    29570
+42	200	CAL_MAG_X	                      375
+42	200	CAL_MAG_Y	                      -25
+42	200	CAL_MAG_Z	                    -1000
+42	200	CAL_PRES_DIFF	                    10000
+42	200	CAL_TEMP	                    29051
+42	200	CAM_ANG_X_FAC	                        0
+42	200	CAM_ANG_X_OFF	                        0
+42	200	CAM_ANG_Y_FAC	                        0
+42	200	CAM_ANG_Y_OFF	           0.428000003099
+42	200	CAM_EXP	                     1000
+42	200	CAM_INTERVAL	                   200000
+42	200	DEBUG_1	                        0
+42	200	DEBUG_2	                        0
+42	200	DEBUG_3	                        0
+42	200	DEBUG_4	                        0
+42	200	DEBUG_5	                        1
+42	200	DEBUG_6	                        0
+42	200	GPS_MODE	                        0
+42	200	KAL_VEL_AX	                        1
+42	200	KAL_VEL_AY	                        1
+42	200	KAL_VEL_BX	                        0
+42	200	KAL_VEL_BY	                        0
+42	200	MIX_OFFSET	                        0
+42	200	MIX_POSITION	                        0
+42	200	MIX_POS_YAW	                        0
+42	200	MIX_REMOTE	                        0
+42	200	MIX_Z_POSITION	                        0
+42	200	PID_ATT_AWU	           0.300000011921
+42	200	PID_ATT_D	                       30
+42	200	PID_ATT_I	                       60
+42	200	PID_ATT_LIM	                      100
+42	200	PID_ATT_P	                       90
+42	200	PID_POS_AWU	                        5
+42	200	PID_POS_D	                        2
+42	200	PID_POS_I	            0.20000000298
+42	200	PID_POS_LIM	            0.20000000298
+42	200	PID_POS_P	            1.79999995232
+42	200	PID_POS_Z_AWU	                        3
+42	200	PID_POS_Z_D	            0.20000000298
+42	200	PID_POS_Z_I	           0.300000011921
+42	200	PID_POS_Z_LIM	           0.300000011921
+42	200	PID_POS_Z_P	                     0.25
+42	200	PID_YAWPOS_AWU	                        1
+42	200	PID_YAWPOS_D	                        1
+42	200	PID_YAWPOS_I	            0.10000000149
+42	200	PID_YAWPOS_LIM	                        3
+42	200	PID_YAWPOS_P	                        5
+42	200	PID_YAWSPEED_D	                        0
+42	200	PID_YAWSPEED_I	                        5
+42	200	PID_YAWSPEED_P	                       15
+42	200	PID_YAWSPE_AWU	                        1
+42	200	PID_YAWSPE_LIM	                       50
+42	200	POS_HOV_TRUST	           0.300000011921
+42	200	POS_SON_MODE	                        0
+42	200	POS_SON_SCALE	                        1
+42	200	POS_SP_ACCEPT	                        1
+42	200	POS_SP_X	                        0
+42	200	POS_SP_Y	                        0
+42	200	POS_SP_YAW	                        0
+42	200	POS_SP_Z	          -0.213201999664
+42	200	POS_TIMEOUT	                  2000000
+42	200	POS_YAW_TRACK	                        0
+42	200	RC_NICK_CHAN	                        1
+42	200	RC_ROLL_CHAN	                        2
+42	200	RC_SAFETY_CHAN	                        5
+42	200	RC_THRUST_CHAN	                        3
+42	200	RC_TRIM_CHAN	                        0
+42	200	RC_TUNE_CHAN1	                        5
+42	200	RC_TUNE_CHAN2	                        6
+42	200	RC_TUNE_CHAN3	                        7
+42	200	RC_TUNE_CHAN4	                        8
+42	200	RC_YAW_CHAN	                        4
+42	200	SEND_DEBUGCHAN	                        0
+42	200	SLOT_ATTITUDE	                        1
+42	200	SLOT_CONTROL	                        0
+42	200	SLOT_RAW_IMU	                        0
+42	200	SLOT_RC	                        0
+42	200	SYS_COMP_ID	                      200
+42	200	SYS_ID	                       42
+42	200	SYS_IMU_RESET	                        0
+42	200	SYS_SW_VER	                     2000
+42	200	SYS_TYPE	                        2
+42	200	UART_0_BAUD	                   115200
+42	200	UART_1_BAUD	                   115200
+42	200	VEL_DAMP	           0.949999988079
+42	200	VEL_OFFSET_X	                        0
+42	200	VEL_OFFSET_Y	                        0
+42	200	VEL_OFFSET_Z	                        0
+42	200	VICON_MODE	                        0
+42	200	VICON_TKO_DIST	                      0.5
+42	200	VICON_TKO_TIME	                        2
+42	200	VIS_OUTL_TRESH	            0.20000000298
diff --git a/files/mav_type_quadrotor/mav_autopilot_pixhawk/widgets/pixhawk-widget.qgw b/files/mav_type_quadrotor/mav_autopilot_pixhawk/widgets/pixhawk-widget.qgw
new file mode 100644
index 0000000000000000000000000000000000000000..6add259e9681ad6393eab8198607e1de20173825
--- /dev/null
+++ b/files/mav_type_quadrotor/mav_autopilot_pixhawk/widgets/pixhawk-widget.qgw
@@ -0,0 +1,14 @@
+[PIXHAWK%20Quadrotor%20Commands]
+QGC_TOOL_WIDGET_ITEMS\1\TYPE=BUTTON
+QGC_TOOL_WIDGET_ITEMS\1\QGC_ACTION_BUTTON_DESCRIPTION=CALIBRATE GYRO
+QGC_TOOL_WIDGET_ITEMS\1\QGC_ACTION_BUTTON_BUTTONTEXT=Calibrate
+QGC_TOOL_WIDGET_ITEMS\1\QGC_ACTION_BUTTON_ACTIONID=17
+QGC_TOOL_WIDGET_ITEMS\2\TYPE=BUTTON
+QGC_TOOL_WIDGET_ITEMS\2\QGC_ACTION_BUTTON_DESCRIPTION=Record logfile and images
+QGC_TOOL_WIDGET_ITEMS\2\QGC_ACTION_BUTTON_BUTTONTEXT=Start
+QGC_TOOL_WIDGET_ITEMS\2\QGC_ACTION_BUTTON_ACTIONID=21
+QGC_TOOL_WIDGET_ITEMS\3\TYPE=BUTTON
+QGC_TOOL_WIDGET_ITEMS\3\QGC_ACTION_BUTTON_DESCRIPTION=Stop recording
+QGC_TOOL_WIDGET_ITEMS\3\QGC_ACTION_BUTTON_BUTTONTEXT=Stop
+QGC_TOOL_WIDGET_ITEMS\3\QGC_ACTION_BUTTON_ACTIONID=23
+QGC_TOOL_WIDGET_ITEMS\size=3
diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri
index 407bc985898ccd61bc23fcf61f815cbe01e06712..59cc0514c0119c9f1b995403f12a35926d479b71 100644
--- a/qgroundcontrol.pri
+++ b/qgroundcontrol.pri
@@ -84,6 +84,8 @@ macx {
 
     # Copy audio files if needed
     QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/qgroundcontrol.app/Contents/MacOS
+    # Copy contributed files
+    QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$TARGETDIR/qgroundcontrol.app/Contents/MacOS
     # Copy google earth starter file
     QMAKE_POST_LINK += && cp -f $$BASEDIR/images/earth.html $$TARGETDIR/qgroundcontrol.app/Contents/MacOS
     # Copy CSS stylesheets
@@ -221,7 +223,10 @@ message("Compiling for linux 32")
     DEFINES += QGC_LIBFREENECT_ENABLED
     }
 
+    # Validated copy commands
+    QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$DESTDIR
     QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$DESTDIR
+
     QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$DESTDIR
     QMAKE_POST_LINK += && mkdir -p $$DESTDIR/images
     QMAKE_POST_LINK += && cp -rf $$BASEDIR/images/Vera.ttf $$DESTDIR/images/Vera.ttf
@@ -295,6 +300,8 @@ linux-g++-64 {
     DEFINES += QGC_LIBFREENECT_ENABLED
     }
 
+    # Validated copy commands
+    QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$DESTDIR
     QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$DESTDIR
     QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$DESTDIR
     QMAKE_POST_LINK += && mkdir -p $$DESTDIR/images
@@ -371,6 +378,7 @@ DEFINES += QGC_OSG_ENABLED
     exists($$TARGETDIR/debug) {
 	QMAKE_POST_LINK += $$quote(copy /Y "$$BASEDIR_WIN\\lib\\sdl\\win32\\SDL.dll" "$$TARGETDIR_WIN\\debug"$$escape_expand(\\n))
 	QMAKE_POST_LINK += $$quote(xcopy /Y "$$BASEDIR_WIN\\audio" "$$TARGETDIR_WIN\\debug\\audio" /E /I $$escape_expand(\\n))
+        QMAKE_POST_LINK += $$quote(xcopy /Y "$$BASEDIR_WIN\\files" "$$TARGETDIR_WIN\\debug\\files" /E /I $$escape_expand(\\n))
 	QMAKE_POST_LINK += $$quote(xcopy /Y "$$BASEDIR_WIN\\models" "$$TARGETDIR_WIN\\debug\\models" /E /I $$escape_expand(\\n))
 	QMAKE_POST_LINK += $$quote(copy /Y "$$BASEDIR_WIN\\images\\earth.html" "$$TARGETDIR_WIN\\debug"$$escape_expand(\\n))
 	QMAKE_POST_LINK += $$quote(copy /Y "$$BASEDIR_WIN\\thirdParty\\libxbee\\lib\\libxbee.dll" "$$TARGETDIR_WIN\\debug"$$escape_expand(\\n))
@@ -391,6 +399,7 @@ DEFINES += QGC_OSG_ENABLED
     exists($$TARGETDIR/release) {
 	QMAKE_POST_LINK += $$quote(copy /Y "$$BASEDIR_WIN\\lib\\sdl\\win32\\SDL.dll" "$$TARGETDIR_WIN\\release"$$escape_expand(\\n))
 	QMAKE_POST_LINK += $$quote(xcopy /Y "$$BASEDIR_WIN\\audio" "$$TARGETDIR_WIN\\release\\audio" /E /I $$escape_expand(\\n))
+        QMAKE_POST_LINK += $$quote(xcopy /Y "$$BASEDIR_WIN\\files" "$$TARGETDIR_WIN\\release\\files" /E /I $$escape_expand(\\n))
 	QMAKE_POST_LINK += $$quote(xcopy /Y "$$BASEDIR_WIN\\models" "$$TARGETDIR_WIN\\release\\models" /E /I $$escape_expand(\\n))
 	QMAKE_POST_LINK += $$quote(copy /Y "$$BASEDIR_WIN\\images\\earth.html" "$$TARGETDIR_WIN\\release\\earth.html" $$escape_expand(\\n))
 	QMAKE_POST_LINK += $$quote(copy /Y "$$BASEDIR_WIN\\thirdParty\\libxbee\\lib\\libxbee.dll" "$$TARGETDIR_WIN\\release"$$escape_expand(\\n))
diff --git a/src/GAudioOutput.cc b/src/GAudioOutput.cc
index c8ba5a25b5f7c2cb47103f00e205e408831a5165..18b36bed1fbf171ded47ff6abd37a64404f63763 100644
--- a/src/GAudioOutput.cc
+++ b/src/GAudioOutput.cc
@@ -137,12 +137,12 @@ GAudioOutput::GAudioOutput(QObject* parent) : QObject(parent),
     }
 }
 
-GAudioOutput::~GAudioOutput()
-{
-#ifdef _MSC_VER2
-    ::CoUninitialize();
-#endif
-}
+//GAudioOutput::~GAudioOutput()
+//{
+//#ifdef _MSC_VER2
+//    ::CoUninitialize();
+//#endif
+//}
 
 void GAudioOutput::mute(bool mute)
 {
diff --git a/src/GAudioOutput.h b/src/GAudioOutput.h
index 6154a315ce810a19cf3b1b0e814b0d5a76cf5417..2856ea19cbfde2f19c8e8abbb6f117bfcbab1ee1 100644
--- a/src/GAudioOutput.h
+++ b/src/GAudioOutput.h
@@ -124,7 +124,7 @@ protected:
     bool muted;
 private:
     GAudioOutput(QObject* parent=NULL);
-    ~GAudioOutput();
+//    ~GAudioOutput();
 };
 
 #endif // AUDIOOUTPUT_H
diff --git a/src/QGC.cc b/src/QGC.cc
index 08467d7e51ec4794a3b057872dae5848a48ec269..17f4c226f16621363d51ffba79b960483e428700 100644
--- a/src/QGC.cc
+++ b/src/QGC.cc
@@ -49,28 +49,28 @@ quint64 groundTimeMilliseconds()
 
 float limitAngleToPMPIf(float angle)
 {
-    while (angle > ((float)M_PI+FLT_EPSILON)) {
-        angle -= 2.0f * (float)M_PI;
-    }
+//    while (angle > ((float)M_PI+FLT_EPSILON)) {
+//        angle -= 2.0f * (float)M_PI;
+//    }
 
-    while (angle <= -((float)M_PI+FLT_EPSILON)) {
-        angle += 2.0f * (float)M_PI;
-    }
+//    while (angle <= -((float)M_PI+FLT_EPSILON)) {
+//        angle += 2.0f * (float)M_PI;
+//    }
 
     return angle;
 }
 
 double limitAngleToPMPId(double angle)
 {
-    if (angle < -M_PI) {
-        while (angle < -M_PI) {
-            angle += M_PI;
-        }
-    } else if (angle > M_PI) {
-        while (angle > M_PI) {
-            angle -= M_PI;
-        }
-    }
+//    if (angle < -M_PI) {
+//        while (angle < -M_PI) {
+//            angle += M_PI;
+//        }
+//    } else if (angle > M_PI) {
+//        while (angle > M_PI) {
+//            angle -= M_PI;
+//        }
+//    }
     return angle;
 }
 
diff --git a/src/QGCCore.cc b/src/QGCCore.cc
index c1f460a0c75b0df5e0d1eff122da698a8f1a48f6..fa047198e25de952bbcd2c62894900dcaa3712ad 100644
--- a/src/QGCCore.cc
+++ b/src/QGCCore.cc
@@ -203,7 +203,7 @@ QGCCore::QGCCore(int &argc, char* argv[]) : QApplication(argc, argv)
 QGCCore::~QGCCore()
 {
     //mainWindow->storeSettings();
-    mainWindow->hide();
+    mainWindow->close();
     mainWindow->deleteLater();
     // Delete singletons
     delete LinkManager::instance();
diff --git a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.h b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.h
index 5faecda723b75dc518ed9d814dba6957fb4d0050..0ad3fcc93a7a7260fd467acb47cbda3707cc058a 100644
--- a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.h
+++ b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.h
@@ -66,6 +66,22 @@ protected:
     DomModel* model;
     void changeEvent(QEvent *e);
 
+signals:
+    void visibilityChanged(bool visible);
+
+protected:
+    void showEvent(QShowEvent* event)
+    {
+        QWidget::showEvent(event);
+        emit visibilityChanged(true);
+    }
+
+    void hideEvent(QHideEvent* event)
+    {
+        QWidget::hideEvent(event);
+        emit visibilityChanged(false);
+    }
+
 private:
     Ui::XMLCommProtocolWidget *m_ui;
 };
diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc
index 9168e0c4a1de42f6cb04fac6430636c72e2ab763..f85bdb25f5ae7b5e53bf6feae650bef72dc5f8bf 100644
--- a/src/uas/PxQuadMAV.cc
+++ b/src/uas/PxQuadMAV.cc
@@ -56,7 +56,6 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
             // FIXME Kind of a hack to load data from disk
             mavlink_image_triggered_t img;
             mavlink_msg_image_triggered_decode(&message, &img);
-            qDebug() << "IMAGE AVAILABLE:" << img.timestamp;
             emit imageStarted(img.timestamp);
         }
         break;
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 37aacd46dbe02e4593440d505ad55b31db6011f4..4805d91a365b3f36a6361191f7f12223b63e7c48 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -77,7 +77,9 @@ attitudeKnown(false),
 paramManager(NULL),
 attitudeStamped(false),
 lastAttitude(0),
-    simulation(new QGCFlightGearLink(this))
+simulation(new QGCFlightGearLink(this)),
+isLocalPositionKnown(false),
+isGlobalPositionKnown(false)
 {
     color = UASInterface::getNextColor();
     setBattery(LIPOLY, 3);
@@ -343,40 +345,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit dropRateChanged(this->getUASID(), state.drop_rate_comm/10000.0f);
             }
             break;
-        case MAVLINK_MSG_ID_RAW_IMU:
-            {
-                //mavlink_raw_imu_t raw;
-                //mavlink_msg_raw_imu_decode(&message, &raw);
-                //quint64 time = getUnixTime(raw.time_usec);
-
-                // FIXME REMOVE LATER emit valueChanged(uasId, "accel x", "raw", static_cast<double>(raw.xacc), time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "accel y", "raw", static_cast<double>(raw.yacc), time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "accel z", "raw", static_cast<double>(raw.zacc), time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gyro roll", "raw", static_cast<double>(raw.xgyro), time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gyro pitch", "raw", static_cast<double>(raw.ygyro), time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gyro yaw", "raw", static_cast<double>(raw.zgyro), time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "mag x", "raw", static_cast<double>(raw.xmag), time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "mag y", "raw", static_cast<double>(raw.ymag), time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "mag z", "raw", static_cast<double>(raw.zmag), time);
-            }
-            break;
-        case MAVLINK_MSG_ID_SCALED_IMU:
-            {
-                //mavlink_scaled_imu_t scaled;
-                //mavlink_msg_scaled_imu_decode(&message, &scaled);
-                //quint64 time = getUnixTime(scaled.time_boot_ms);
-
-                // FIXME REMOVE LATER emit valueChanged(uasId, "accel x", "g", scaled.xacc/1000.0f, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "accel y", "g", scaled.yacc/1000.0f, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "accel z", "g", scaled.zacc/1000.0f, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gyro roll", "rad/s", scaled.xgyro/1000.0f, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gyro pitch", "rad/s", scaled.ygyro/1000.0f, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gyro yaw", "rad/s", scaled.zgyro/1000.0f, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "mag x", "uTesla", scaled.xmag/100.0f, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "mag y", "uTesla", scaled.ymag/100.0f, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "mag z", "uTesla", scaled.zmag/100.0f, time);
-            }
-            break;
         case MAVLINK_MSG_ID_ATTITUDE:
             {
                 mavlink_attitude_t attitude;
@@ -386,31 +354,20 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 roll = QGC::limitAngleToPMPIf(attitude.roll);
                 pitch = QGC::limitAngleToPMPIf(attitude.pitch);
                 yaw = QGC::limitAngleToPMPIf(attitude.yaw);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "roll", "rad", roll, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "pitch", "rad", pitch, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "yaw", "rad", yaw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "rollspeed", "rad/s", attitude.rollspeed, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "pitchspeed", "rad/s", attitude.pitchspeed, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "yawspeed", "rad/s", attitude.yawspeed, time);
 
                 // Emit in angles
 
                 // Convert yaw angle to compass value
                 // in 0 - 360 deg range
                 float compass = (yaw/M_PI)*180.0+360.0f;
-                while (compass > 360.0f) {
-                    compass -= 360.0f;
+                if (compass > -10000 && compass < 10000)
+                {
+                    while (compass > 360.0f) {
+                        compass -= 360.0f;
+                    }
                 }
 
                 attitudeKnown = true;
-
-                // FIXME REMOVE LATER emit valueChanged(uasId, "roll deg", "deg", (roll/M_PI)*180.0, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "pitch deg", "deg", (pitch/M_PI)*180.0, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "heading deg", "deg", compass, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "rollspeed d/s", "deg/s", (attitude.rollspeed/M_PI)*180.0, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "pitchspeed d/s", "deg/s", (attitude.pitchspeed/M_PI)*180.0, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "yawspeed d/s", "deg/s", (attitude.yawspeed/M_PI)*180.0, time);
-
                 emit attitudeChanged(this, roll, pitch, yaw, time);
                 emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time);
             }
@@ -428,12 +385,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 mavlink_msg_vfr_hud_decode(&message, &hud);
                 quint64 time = getUnixTime();
                 // Display updated values
-                // FIXME REMOVE LATER emit valueChanged(uasId, "airspeed", "m/s", hud.airspeed, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "groundspeed", "m/s", hud.groundspeed, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", hud.alt, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "heading", "deg", hud.heading, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "climbrate", "m/s", hud.climb, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "throttle", "%", hud.throttle, time);
                 emit thrustChanged(this, hud.throttle/100.0);
 
                 if (!attitudeKnown)
@@ -443,26 +394,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 }
 
                 emit altitudeChanged(uasId, hud.alt);
-                //yaw = (hud.heading-180.0f/360.0f)*M_PI;
                 emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, time);
             }
             break;
-        case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
-            {
-                //mavlink_nav_controller_output_t nav;
-                //mavlink_msg_nav_controller_output_decode(&message, &nav);
-                //quint64 time = getUnixTime();
-                // Update UI
-                // FIXME REMOVE LATER emit valueChanged(uasId, "nav roll", "deg", nav.nav_roll, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "nav pitch", "deg", nav.nav_pitch, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "nav bearing", "deg", nav.nav_bearing, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "target bearing", "deg", nav.target_bearing, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "wp dist", "m", nav.wp_dist, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "alt err", "m", nav.alt_error, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "airspeed err", "m/s", nav.alt_error, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "xtrack err", "m", nav.xtrack_error, time);
-            }
-            break;
         case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
             //std::cerr << std::endl;
             //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
@@ -473,25 +407,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 localX = pos.x;
                 localY = pos.y;
                 localZ = pos.z;
-                // FIXME REMOVE LATER emit valueChanged(uasId, "x", "m", pos.x, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "y", "m", pos.y, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "z", "m", pos.z, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "x speed", "m/s", pos.vx, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "y speed", "m/s", pos.vy, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "z speed", "m/s", pos.vz, time);
                 emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
                 emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
 
-                //                qDebug()<<"Local Position = "<<pos.x<<" - "<<pos.y<<" - "<<pos.z;
-                //                qDebug()<<"Speed Local Position = "<<pos.vx<<" - "<<pos.vy<<" - "<<pos.vz;
-
-                //emit attitudeChanged(this, pos.roll, pos.pitch, pos.yaw, time);
                 // Set internal state
                 if (!positionLock) {
                     // If position was not locked before, notify positive
                     GAudioOutput::instance()->notifyPositive();
                 }
                 positionLock = true;
+                isLocalPositionKnown = true;
             }
             break;
         case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
@@ -507,11 +432,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 speedX = pos.vx/100.0;
                 speedY = pos.vy/100.0;
                 speedZ = pos.vz/100.0;
-                // FIXME REMOVE LATER emit valueChanged(uasId, "latitude", "deg", latitude, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "longitude", "deg", longitude, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", altitude, time);
-//                double totalSpeed = sqrt(speedX*speedX + speedY*speedY + speedZ*speedZ);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gps speed", "m/s", totalSpeed, time);
                 emit globalPositionChanged(this, latitude, longitude, altitude, time);
                 emit speedChanged(this, speedX, speedY, speedZ, time);
                 // Set internal state
@@ -520,6 +440,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     GAudioOutput::instance()->notifyPositive();
                 }
                 positionLock = true;
+                isGlobalPositionKnown = true;
                 //TODO fix this hack for forwarding of global position for patch antenna tracking
                 forwardMessage(message);
             }
@@ -534,14 +455,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 // quint64 time = getUnixTime(pos.time_usec);
                 quint64 time = getUnixTime(pos.time_usec);
 
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gps latitude", "deg", pos.lat/(double)1E7, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gps longitude", "deg", pos.lon/(double)1E7, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gps speed", "m/s", pos.vel, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gps eph", "m", pos.eph/(double)1E2, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gps epv", "m", pos.eph/(double)1E2, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gps fix", "raw", pos.fix_type, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "gps course", "raw", pos.cog, time);
-
                 if (pos.fix_type > 2) {
                     emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time);
                     latitude = pos.lat/(double)1E7;
@@ -587,29 +500,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit homePositionChanged(uasId, pos.latitude, pos.longitude, pos.altitude);
             }
             break;
-        case MAVLINK_MSG_ID_RAW_PRESSURE:
-            {
-                //mavlink_raw_pressure_t pressure;
-                //mavlink_msg_raw_pressure_decode(&message, &pressure);
-                //quint64 time = this->getUnixTime(pressure.time_usec);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "abs pressure", "raw", pressure.press_abs, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "diff pressure 1", "raw", pressure.press_diff1, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "diff pressure 2", "raw", pressure.press_diff2, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "temperature", "raw", pressure.temperature, time);
-            }
-            break;
-
-        case MAVLINK_MSG_ID_SCALED_PRESSURE:
-            {
-                //mavlink_scaled_pressure_t pressure;
-                //mavlink_msg_scaled_pressure_decode(&message, &pressure);
-                //quint64 time = this->getUnixTime(pressure.time_boot_ms);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "abs pressure", "hPa", pressure.press_abs, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "diff pressure", "hPa", pressure.press_diff, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "temperature", "C", pressure.temperature/100.0, time);
-            }
-            break;
-
         case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
             {
                 mavlink_rc_channels_raw_t channels;
@@ -623,15 +513,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit remoteControlChannelRawChanged(5, channels.chan6_raw);
                 emit remoteControlChannelRawChanged(6, channels.chan7_raw);
                 emit remoteControlChannelRawChanged(7, channels.chan8_raw);
-//                quint64 time = getUnixTime();
-                // FIXME REMOVE LATER emit valueChanged(uasId, "rc in #1", "us", channels.chan1_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "rc in #2", "us", channels.chan2_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "rc in #3", "us", channels.chan3_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "rc in #4", "us", channels.chan4_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "rc in #5", "us", channels.chan5_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "rc in #6", "us", channels.chan6_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "rc in #7", "us", channels.chan7_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "rc in #8", "us", channels.chan8_raw, time);
             }
             break;
         case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
@@ -820,34 +701,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs());
             }
             break;
-        case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
-            {
-                //mavlink_servo_output_raw_t servos;
-                //mavlink_msg_servo_output_raw_decode(&message, &servos);
-                //quint64 time = getUnixTime();
-                // FIXME REMOVE LATER emit valueChanged(uasId, "servo #1", "us", servos.servo1_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "servo #2", "us", servos.servo2_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "servo #3", "us", servos.servo3_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "servo #4", "us", servos.servo4_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "servo #5", "us", servos.servo5_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "servo #6", "us", servos.servo6_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "servo #7", "us", servos.servo7_raw, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "servo #8", "us", servos.servo8_raw, time);
-            }
-            break;
-
-        case MAVLINK_MSG_ID_OPTICAL_FLOW:
-            {
-                //mavlink_optical_flow_t flow;
-                //mavlink_msg_optical_flow_decode(&message, &flow);
-                //quint64 time = getUnixTime(flow.time_usec);
-
-                // FIXME REMOVE LATER emit valueChanged(uasId, QString("opt_flow_%1.x").arg(flow.sensor_id), "Pixel", flow.flow_x, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, QString("opt_flow_%1.y").arg(flow.sensor_id), "Pixel", flow.flow_y, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, QString("opt_flow_%1.qual").arg(flow.sensor_id), "0-255", flow.quality, time);
-                // FIXME REMOVE LATER emit valueChanged(uasId, QString("opt_flow_%1.dist").arg(flow.sensor_id), "m", flow.ground_distance, time);
-            }
-            break;
         case MAVLINK_MSG_ID_STATUSTEXT:
             {
                 QByteArray b;
@@ -912,17 +765,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             }
             break;
 #endif
-        case MAVLINK_MSG_ID_DEBUG_VECT:
-        {
-            //mavlink_debug_vect_t vect;
-            //mavlink_msg_debug_vect_decode(&message, &vect);
-            //QString str((const char*)vect.name);
-            //quint64 time = getUnixTime(vect.time_usec);
-            // FIXME REMOVE LATER emit valueChanged(uasId, str+".x", "raw", vect.x, time);
-            // FIXME REMOVE LATER emit valueChanged(uasId, str+".y", "raw", vect.y, time);
-            // FIXME REMOVE LATER emit valueChanged(uasId, str+".z", "raw", vect.z, time);
-        }
-        break;
 //        case MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT:
 //        {
 //            mavlink_object_detection_event_t event;
@@ -1027,6 +869,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 #endif
             // Messages to ignore
         case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT:
+        case MAVLINK_MSG_ID_RAW_IMU:
+        case MAVLINK_MSG_ID_SCALED_IMU:
+        case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
+        case MAVLINK_MSG_ID_RAW_PRESSURE:
+        case MAVLINK_MSG_ID_SCALED_PRESSURE:
+        case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
+        case MAVLINK_MSG_ID_OPTICAL_FLOW:
+        case MAVLINK_MSG_ID_DEBUG_VECT:
             break;
         default:
             {
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 65a9318d7ad138486ac71b22908c5c0fbdc3713f..a0051b76f7288250f4f22184be76df1d02aefe76 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -108,6 +108,14 @@ public:
     double getAltitude() const {
         return altitude;
     }
+    virtual bool localPositionKnown() const
+    {
+        return isLocalPositionKnown;
+    }
+    virtual bool globalPositionKnown() const
+    {
+        return isGlobalPositionKnown;
+    }
 
     double getRoll() const {
         return roll;
@@ -201,15 +209,17 @@ protected: //COMMENTS FOR TEST UNIT
     quint64 imageStart;
 
     QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters
-    bool paramsOnceRequested;   ///< If the parameter list has been read at least once
-    int airframe;               ///< The airframe type
-    bool attitudeKnown;         ///< True if attitude was received, false else
+    bool paramsOnceRequested;       ///< If the parameter list has been read at least once
+    int airframe;                   ///< The airframe type
+    bool attitudeKnown;             ///< True if attitude was received, false else
     QGCUASParamManager* paramManager; ///< Parameter manager class
-    QString shortStateText;     ///< Short textual state description
-    QString shortModeText;      ///< Short textual mode description
-    bool attitudeStamped;       ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV
-    quint64 lastAttitude;       ///< Timestamp of last attitude measurement
-    QGCFlightGearLink* simulation; ///< Hardware in the loop simulation link
+    QString shortStateText;         ///< Short textual state description
+    QString shortModeText;          ///< Short textual mode description
+    bool attitudeStamped;           ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV
+    quint64 lastAttitude;           ///< Timestamp of last attitude measurement
+    QGCFlightGearLink* simulation;  ///< Hardware in the loop simulation link
+    bool isLocalPositionKnown;      ///< If the local position has been received for this MAV
+    bool isGlobalPositionKnown;     ///< If the global position has been received for this MAV
 
 public:
     /** @brief Set the current battery type */
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index 4b1f2161fa429d2291cf07ce10776e5270c2f11c..8dce73f70c078052b3abd3ac256d67cb8b23d25f 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -76,10 +76,12 @@ public:
     virtual double getLocalX() const = 0;
     virtual double getLocalY() const = 0;
     virtual double getLocalZ() const = 0;
+    virtual bool localPositionKnown() const = 0;
 
     virtual double getLatitude() const = 0;
     virtual double getLongitude() const = 0;
     virtual double getAltitude() const = 0;
+    virtual bool globalPositionKnown() const = 0;
 
     virtual double getRoll() const = 0;
     virtual double getPitch() const = 0;
@@ -181,15 +183,15 @@ public:
     /** @brief Get the type of the system (airplane, quadrotor, helicopter,..)*/
     virtual int getSystemType() = 0;
     virtual QString getSystemTypeName() = 0;
+    /** @brief Get the type of the autopilot (PIXHAWK, APM, UDB, PPZ,..) */
+    virtual int getAutopilotType() = 0;
+    virtual QString getAutopilotTypeName() = 0;
+    virtual void setAutopilotType(int apType)= 0;
 
     QColor getColor() {
         return color;
     }
 
-    virtual int getAutopilotType() = 0;
-    virtual QString getAutopilotTypeName() = 0;
-    virtual void setAutopilotType(int apType)= 0;
-
 public slots:
 
     /** @brief Set a new name for the system */
@@ -490,6 +492,6 @@ protected:
 
 };
 
-Q_DECLARE_INTERFACE(UASInterface, "org.qgroundcontrol/1.0");
+Q_DECLARE_INTERFACE(UASInterface, "org.qgroundcontrol/1.0")
 
 #endif // _UASINTERFACE_H_
diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc
index 10d30ec0ec85a4be522293a48e3ee78657f71f00..3874059930a71b750f2191a87920f9a2dd65112c 100644
--- a/src/ui/CommConfigurationWindow.cc
+++ b/src/ui/CommConfigurationWindow.cc
@@ -267,7 +267,7 @@ void CommConfigurationWindow::setLinkType(int linktype)
 	foreach (QAction* act, actions) 
 	{
         if (act->data().toInt() == LinkManager::instance()->getLinks().indexOf(tmpLink)) 
-		{
+        {
             act->trigger();
             break;
         }
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index 0a717ecb339536c4b55148ed6c85e9cb7f1c1355..1d4725bb0ca36dd247dfbdce2b50932f196193f2 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -58,8 +58,8 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
     posXSet(0.0f),
     posYSet(0.0f),
     posZSet(0.0f),
-    attXSaturation(0.5f),
-    attYSaturation(0.5f),
+    attXSaturation(0.2f),
+    attYSaturation(0.2f),
     attYawSaturation(0.5f),
     posXSaturation(0.05f),
     posYSaturation(0.05f),
@@ -474,7 +474,7 @@ QPointF HSIDisplay::metricWorldToBody(QPointF world)
 {
     // First translate to body-centered coordinates
     // Rotate around -yaw
-    float angle = yaw + M_PI;
+    float angle = -yaw - M_PI;
     QPointF result(cos(angle) * (x - world.x()) - sin(angle) * (y - world.y()), sin(angle) * (x - world.x()) + cos(angle) * (y - world.y()));
     return result;
 }
@@ -945,7 +945,7 @@ void HSIDisplay::drawPositionDirection(float xRef, float yRef, float radius, con
 {
     if (xyControlKnown && xyControlEnabled) {
         // Draw the needle
-        const float maxWidth = radius / 10.0f;
+        const float maxWidth = radius / 5.0f;
         const float minWidth = maxWidth * 0.3f;
 
         float angle = atan2(posXSet, -posYSet);
@@ -985,10 +985,9 @@ void HSIDisplay::drawAttitudeDirection(float xRef, float yRef, float radius, con
         const float maxWidth = radius / 10.0f;
         const float minWidth = maxWidth * 0.3f;
 
-        float angle = atan2(attXSet, attYSet);
-        angle -= (float)M_PI/2.0f;
+        float angle = atan2(attYSet, -attXSet);
 
-        radius *= sqrt(pow(attXSet, 2) + pow(attYSet, 2)) / sqrt(attXSaturation + attYSaturation);
+        radius *= sqrt(attXSet*attXSet + attYSet*attYSet) / sqrt(attXSaturation*attXSaturation + attYSaturation*attYSaturation);
 
         QPolygonF p(6);
 
diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc
index d5bf4bf0a8a0392c473fbb8760390556f8ce8708..9bf493fdbe8d5919aa918893d1808be20029d18f 100644
--- a/src/ui/HUD.cc
+++ b/src/ui/HUD.cc
@@ -200,14 +200,16 @@ void HUD::showEvent(QShowEvent* event)
     // events
     QGLWidget::showEvent(event);
     refreshTimer->start(updateInterval);
+    emit visibilityChanged(true);
 }
 
 void HUD::hideEvent(QHideEvent* event)
 {
     // React only to internal (pre-display)
     // events
-    QGLWidget::hideEvent(event);
     refreshTimer->stop();
+    QGLWidget::hideEvent(event);
+    emit visibilityChanged(false);
 }
 
 void HUD::contextMenuEvent (QContextMenuEvent* event)
@@ -1047,10 +1049,10 @@ void HUD::paintPitchLines(float pitch, QPainter* painter)
     float offset = pitch;
     if (offset < 0) offset = -offset;
     int offsetCount = 0;
-    while (offset > lineDistance) {
-        offset -= lineDistance;
-        offsetCount++;
-    }
+//    while (offset > lineDistance) {
+//        offset -= lineDistance;
+//        offsetCount++;
+//    }
 
     int iPos = (int)(0.5f + lineDistance); ///< The first line
     int iNeg = (int)(-0.5f - lineDistance); ///< The first line
diff --git a/src/ui/HUD.h b/src/ui/HUD.h
index 884a6db768093af6f7af8436500332ee19d17b9f..a6412ba7db91cba122cc7f8b6879b9cf13a8bc25 100644
--- a/src/ui/HUD.h
+++ b/src/ui/HUD.h
@@ -113,6 +113,9 @@ protected slots:
 
     void drawPolygon(QPolygonF refPolygon, QPainter* painter);
 
+signals:
+    void visibilityChanged(bool visible);
+
 protected:
     void commitRawDataToGL();
     /** @brief Convert reference coordinates to screen coordinates */
diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc
index b085ef76d90c1f945ab368813c4ed0253aed96ae..2166f1e5fc46d575b9601c27211625ea5e6e6876 100644
--- a/src/ui/MainWindow.cc
+++ b/src/ui/MainWindow.cc
@@ -64,7 +64,8 @@ This file is part of the QGROUNDCONTROL project
 MainWindow* MainWindow::instance()
 {
     static MainWindow* _instance = 0;
-    if(_instance == 0) {
+    if(_instance == 0)
+    {
         _instance = new MainWindow();
 
         /* Set the application as parent to ensure that this object
@@ -83,44 +84,48 @@ MainWindow* MainWindow::instance()
 **/
 MainWindow::MainWindow(QWidget *parent):
     QMainWindow(parent),
-    toolsMenuActions(),
     currentView(VIEW_UNCONNECTED),
     aboutToCloseFlag(false),
     changingViewsFlag(false),
     styleFileName(QCoreApplication::applicationDirPath() + "/style-indoor.css"),
     autoReconnect(false),
     currentStyle(QGC_MAINWINDOW_STYLE_INDOOR),
-    lowPowerMode(false)
+    lowPowerMode(false),
+    centerStackActionGroup(this)
 {
     loadSettings();
-    if (!settings.contains("CURRENT_VIEW")) {
+    if (!settings.contains("CURRENT_VIEW"))
+    {
         // Set this view as default view
         settings.setValue("CURRENT_VIEW", currentView);
-    } else {
+    }
+    else
+    {
         // LOAD THE LAST VIEW
         VIEW_SECTIONS currentViewCandidate = (VIEW_SECTIONS) settings.value("CURRENT_VIEW", currentView).toInt();
         if (currentViewCandidate != VIEW_ENGINEER &&
                 currentViewCandidate != VIEW_OPERATOR &&
-                currentViewCandidate != VIEW_PILOT) {
+                currentViewCandidate != VIEW_PILOT &&
+                currentViewCandidate != VIEW_FULL)
+        {
             currentView = currentViewCandidate;
         }
     }
 
-    setDefaultSettingsForAp();
-
     settings.sync();
 
+    // Setup UI state machines
+    centerStackActionGroup.setExclusive(true);
+
     // Setup user interface
     ui.setupUi(this);
 
-    setVisible(false);
+    centerStack = new QStackedWidget(this);
+    setCentralWidget(centerStack);
 
     buildCommonWidgets();
-
     connectCommonWidgets();
 
-    arrangeCommonCenterStack();
-
     configureWindowName();
 
     loadStyle(currentStyle);
@@ -133,18 +138,28 @@ MainWindow::MainWindow(QWidget *parent):
 
     statusBar()->setSizeGripEnabled(true);
 
+    // Restore the window setup
+    if (settings.contains(getWindowStateKey()))
+    {
+        loadViewState();
+    }
+
     // Restore the window position and size
-    if (settings.contains(getWindowGeometryKey())) {
+    if (settings.contains(getWindowGeometryKey()))
+    {
         // Restore the window geometry
         restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray());
-    } else {
+    }
+    else
+    {
         // Adjust the size
         adjustSize();
     }
 
     // Populate link menu
     QList<LinkInterface*> links = LinkManager::instance()->getLinks();
-    foreach(LinkInterface* link, links) {
+    foreach(LinkInterface* link, links)
+    {
         this->addLink(link);
     }
 
@@ -165,11 +180,9 @@ MainWindow::MainWindow(QWidget *parent):
     toolBar->addPerspectiveChangeAction(ui.actionEngineersView);
     toolBar->addPerspectiveChangeAction(ui.actionPilotsView);
 
-    // Enable and update view
-    presentView();
-
     // Connect link
-    if (autoReconnect) {
+    if (autoReconnect)
+    {
         SerialLink* link = new SerialLink();
         // Add to registry
         LinkManager::instance()->add(link);
@@ -182,13 +195,12 @@ MainWindow::MainWindow(QWidget *parent):
 
     // Initialize window state
     windowStateVal = windowState();
+
+    show();
 }
 
 MainWindow::~MainWindow()
 {
-    // Store settings
-    storeSettings();
-
     delete mavlink;
     delete joystick;
 
@@ -198,77 +210,34 @@ MainWindow::~MainWindow()
 
     QObjectList::iterator i;
     QDockWidget* dockWidget;
-    for (i = childList.begin(); i != childList.end(); ++i) {
+    for (i = childList.begin(); i != childList.end(); ++i)
+    {
         dockWidget = dynamic_cast<QDockWidget*>(*i);
-        if (dockWidget) {
+        if (dockWidget)
+        {
             // Remove dock widget from main window
             removeDockWidget(dockWidget);
             delete dockWidget->widget();
             delete dockWidget;
         }
-    }
-}
-
-/**
- * Set default settings for this AP type.
- */
-void MainWindow::setDefaultSettingsForAp()
-{
-    // Check if the settings exist, instantiate defaults if necessary
-
-    // UNCONNECTED VIEW DEFAULT
-    QString centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_UNCONNECTED);
-    if (!settings.contains(centralKey)) {
-        settings.setValue(centralKey,true);
-
-        // ENABLE UAS LIST
-        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST, VIEW_UNCONNECTED), true);
-        // ENABLE COMMUNICATION CONSOLE
-        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_DEBUG_CONSOLE, VIEW_UNCONNECTED), true);
-    }
-
-    // OPERATOR VIEW DEFAULT
-    centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_OPERATOR);
-    if (!settings.contains(centralKey)) {
-        settings.setValue(centralKey,true);
-
-        // ENABLE UAS LIST
-        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST,VIEW_OPERATOR), true);
-        // ENABLE HUD TOOL WIDGET
-        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HUD,VIEW_OPERATOR), true);
-        // ENABLE WAYPOINTS
-        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_WAYPOINTS,VIEW_OPERATOR), true);
-    }
-
-    // ENGINEER VIEW DEFAULT
-    centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_LINECHART, VIEW_ENGINEER);
-    if (!settings.contains(centralKey)) {
-        settings.setValue(centralKey,true);
-        // Enable Parameter widget
-        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_PARAMETERS,VIEW_ENGINEER), true);
-    }
-
-    // MAVLINK VIEW DEFAULT
-    centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_PROTOCOL, VIEW_MAVLINK);
-    if (!settings.contains(centralKey)) {
-        settings.setValue(centralKey,true);
+        else
+        {
+            delete dynamic_cast<QObject*>(*i);
+        }
     }
 
-    // PILOT VIEW DEFAULT
-    centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_HUD, VIEW_PILOT);
-    if (!settings.contains(centralKey)) {
-        settings.setValue(centralKey,true);
-        // Enable Flight display
-        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HDD_1,VIEW_PILOT), true);
-    }
+    // Delete all UAS objects
 }
 
 void MainWindow::resizeEvent(QResizeEvent * event)
 {
     Q_UNUSED(event);
-    if (height() < 800) {
+    if (height() < 800)
+    {
         ui.statusBar->setVisible(false);
-    } else {
+    }
+    else
+    {
         ui.statusBar->setVisible(true);
         ui.statusBar->setSizeGripEnabled(true);
     }
@@ -288,22 +257,23 @@ QString MainWindow::getWindowGeometryKey()
 void MainWindow::buildCustomWidget()
 {
     // Show custom widgets only if UAS is connected
-    if (UASManager::instance()->getActiveUAS() != NULL) {
-        // Enable custom widgets
-        ui.actionNewCustomWidget->setEnabled(true);
-
+    if (UASManager::instance()->getActiveUAS() != NULL)
+    {
         // Create custom widgets
         QList<QGCToolWidget*> widgets = QGCToolWidget::createWidgetsFromSettings(this);
 
-        if (widgets.size() > 0) {
+        if (widgets.size() > 0)
+        {
             ui.menuTools->addSeparator();
         }
 
-        for(int i = 0; i < widgets.size(); ++i) {
+        for(int i = 0; i < widgets.size(); ++i)
+        {
             // Check if this widget already has a parent, do not create it in this case
             QGCToolWidget* tool = widgets.at(i);
             QDockWidget* dock = dynamic_cast<QDockWidget*>(tool->parentWidget());
-            if (!dock) {
+            if (!dock)
+            {
                 QDockWidget* dock = new QDockWidget(tool->windowTitle(), this);
                 dock->setObjectName(tool->objectName()+"_DOCK");
                 dock->setWidget(tool);
@@ -315,9 +285,6 @@ void MainWindow::buildCustomWidget()
                 widgets.at(i)->setMainMenuAction(showAction);
                 ui.menuTools->addAction(showAction);
 
-                // Load visibility for view (default is off)
-                //dock->setVisible(tool->isVisible(currentView));
-
                 // Load dock widget location (default is bottom)
                 Qt::DockWidgetArea location = static_cast <Qt::DockWidgetArea>(tool->getDockWidgetArea(currentView));
 
@@ -334,46 +301,52 @@ void MainWindow::buildCommonWidgets()
     connect(mavlink, SIGNAL(protocolStatusMessage(QString,QString)), this, SLOT(showCriticalMessage(QString,QString)), Qt::QueuedConnection);
 
     // Dock widgets
-    if (!controlDockWidget) {
+    if (!controlDockWidget)
+    {
         controlDockWidget = new QDockWidget(tr("Control"), this);
         controlDockWidget->setObjectName("UNMANNED_SYSTEM_CONTROL_DOCKWIDGET");
         controlDockWidget->setWidget( new UASControlWidget(this) );
-        addToToolsMenu (controlDockWidget, tr("Control"), SLOT(showToolWidget(bool)), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea);
+        addTool(controlDockWidget, tr("Control"), Qt::LeftDockWidgetArea);
     }
 
-    if (!listDockWidget) {
+    if (!listDockWidget)
+    {
         listDockWidget = new QDockWidget(tr("Unmanned Systems"), this);
         listDockWidget->setWidget( new UASListWidget(this) );
         listDockWidget->setObjectName("UNMANNED_SYSTEMS_LIST_DOCKWIDGET");
-        addToToolsMenu (listDockWidget, tr("Unmanned Systems"), SLOT(showToolWidget(bool)), MENU_UAS_LIST, Qt::RightDockWidgetArea);
+        addTool(listDockWidget, tr("Unmanned Systems"), Qt::RightDockWidgetArea);
     }
 
-    if (!waypointsDockWidget) {
+    if (!waypointsDockWidget)
+    {
         waypointsDockWidget = new QDockWidget(tr("Mission Plan"), this);
         waypointsDockWidget->setWidget( new QGCWaypointListMulti(this) );
         waypointsDockWidget->setObjectName("WAYPOINT_LIST_DOCKWIDGET");
-        addToToolsMenu (waypointsDockWidget, tr("Mission Plan"), SLOT(showToolWidget(bool)), MENU_WAYPOINTS, Qt::BottomDockWidgetArea);
+        addTool(waypointsDockWidget, tr("Mission Plan"), Qt::BottomDockWidgetArea);
     }
 
-    if (!infoDockWidget) {
+    if (!infoDockWidget)
+    {
         infoDockWidget = new QDockWidget(tr("Status Details"), this);
         infoDockWidget->setWidget( new UASInfoWidget(this) );
         infoDockWidget->setObjectName("UAS_STATUS_DETAILS_DOCKWIDGET");
-        addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget(bool)), MENU_STATUS, Qt::RightDockWidgetArea);
+        addTool(infoDockWidget, tr("Status Details"), Qt::RightDockWidgetArea);
     }
 
-    if (!debugConsoleDockWidget) {
+    if (!debugConsoleDockWidget)
+    {
         debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this);
         debugConsoleDockWidget->setWidget( new DebugConsole(this) );
         debugConsoleDockWidget->setObjectName("COMMUNICATION_DEBUG_CONSOLE_DOCKWIDGET");
-        addToToolsMenu (debugConsoleDockWidget, tr("Communication Console"), SLOT(showToolWidget(bool)), MENU_DEBUG_CONSOLE, Qt::BottomDockWidgetArea);
+        addTool(debugConsoleDockWidget, tr("Communication Console"), Qt::BottomDockWidgetArea);
     }
 
-    if (!logPlayerDockWidget) {
+    if (!logPlayerDockWidget)
+    {
         logPlayerDockWidget = new QDockWidget(tr("MAVLink Log Player"), this);
         logPlayerDockWidget->setWidget( new QGCMAVLinkLogPlayer(mavlink, this) );
         logPlayerDockWidget->setObjectName("MAVLINK_LOG_PLAYER_DOCKWIDGET");
-        addToToolsMenu(logPlayerDockWidget, tr("MAVLink Log Replay"), SLOT(showToolWidget(bool)), MENU_MAVLINK_LOG_PLAYER, Qt::RightDockWidgetArea);
+        addTool(logPlayerDockWidget, tr("MAVLink Log Replay"), Qt::RightDockWidgetArea);
     }
 
     if (!mavlinkInspectorWidget)
@@ -381,36 +354,26 @@ void MainWindow::buildCommonWidgets()
         mavlinkInspectorWidget = new QDockWidget(tr("MAVLink Message Inspector"), this);
         mavlinkInspectorWidget->setWidget( new QGCMAVLinkInspector(mavlink, this) );
         mavlinkInspectorWidget->setObjectName("MAVLINK_INSPECTOR_DOCKWIDGET");
-        addToToolsMenu(mavlinkInspectorWidget, tr("MAVLink Inspector"), SLOT(showToolWidget(bool)), MENU_MAVLINK_INSPECTOR, Qt::RightDockWidgetArea);
+        addTool(mavlinkInspectorWidget, tr("MAVLink Inspector"), Qt::RightDockWidgetArea);
     }
 
     // Center widgets
     if (!mapWidget)
     {
         mapWidget = new QGCMapTool(this);
-        addToCentralWidgetsMenu (mapWidget, "Maps", SLOT(showCentralWidget()),CENTRAL_MAP);
+        addCentralWidget(mapWidget, "Maps");
     }
 
-    if (!protocolWidget) {
+    if (!protocolWidget)
+    {
         protocolWidget    = new XMLCommProtocolWidget(this);
-        addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL);
+        addCentralWidget(protocolWidget, "Mavlink Generator");
     }
 
-    if (!dataplotWidget) {
+    if (!dataplotWidget)
+    {
         dataplotWidget    = new QGCDataPlot2D(this);
-        addToCentralWidgetsMenu (dataplotWidget, "Logfile Plot", SLOT(showCentralWidget()),CENTRAL_DATA_PLOT);
-    }
-
-    if (!hudWidget) {
-        hudWidget         = new HUD(320, 240, this);
-        addToCentralWidgetsMenu(hudWidget, tr("Head Up Display"), SLOT(showCentralWidget()), CENTRAL_HUD);
-    }
-
-    if (!hsiDockWidget) {
-        hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this);
-        hsiDockWidget->setWidget( new HSIDisplay(this) );
-        hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET");
-        addToToolsMenu (hsiDockWidget, tr("Horizontal Situation"), SLOT(showToolWidget(bool)), MENU_HSI, Qt::BottomDockWidgetArea);
+        addCentralWidget(dataplotWidget, "Logfile Plot");
     }
 
     //FIXME: memory of acceptList will never be freed again
@@ -434,428 +397,84 @@ void MainWindow::buildCommonWidgets()
     acceptList2->append("900,servo #8,us,2100,s");
     acceptList2->append("0,abs pressure,hPa,65500");
 
-    if (!headDown1DockWidget) {
-        headDown1DockWidget = new QDockWidget(tr("Flight Instruments"), this);
-        headDown1DockWidget->setWidget( new HDDisplay(acceptList, "Flight Display", this) );
-        headDown1DockWidget->setObjectName("HEAD_DOWN_DISPLAY_1_DOCK_WIDGET");
-        addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget(bool)), MENU_HDD_1, Qt::RightDockWidgetArea);
-    }
-
-    if (!headDown2DockWidget) {
-        headDown2DockWidget = new QDockWidget(tr("Payload Instruments"), this);
-        headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Actuator Status", this) );
-        headDown2DockWidget->setObjectName("HEAD_DOWN_DISPLAY_2_DOCK_WIDGET");
-        addToToolsMenu (headDown2DockWidget, tr("Actuator Status"), SLOT(showToolWidget(bool)), MENU_HDD_2, Qt::RightDockWidgetArea);
-    }
-
-    if (!rcViewDockWidget) {
-        rcViewDockWidget = new QDockWidget(tr("Radio Control"), this);
-        rcViewDockWidget->setWidget( new QGCRemoteControlView(this) );
-        rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET");
-        addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea);
-    }
-
-    if (!headUpDockWidget) {
-        headUpDockWidget = new QDockWidget(tr("HUD"), this);
-        headUpDockWidget->setWidget( new HUD(320, 240, this));
-        headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET");
-        addToToolsMenu (headUpDockWidget, tr("Head Up Display"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::RightDockWidgetArea);
+    if (!hudWidget) {
+        hudWidget         = new HUD(320, 240, this);
+        addCentralWidget(hudWidget, tr("Head Up Display"));
     }
 
-    if (!parametersDockWidget) {
-        parametersDockWidget = new QDockWidget(tr("Onboard Parameters"), this);
-        parametersDockWidget->setWidget( new ParameterInterface(this) );
-        parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET");
-        addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget(bool)), MENU_PARAMETERS, Qt::RightDockWidgetArea);
+    if (!dataplotWidget) {
+        dataplotWidget    = new QGCDataPlot2D(this);
+        addCentralWidget(dataplotWidget, tr("Logfile Plot"));
     }
 
 #ifdef QGC_OSG_ENABLED
     if (!_3DWidget) {
         _3DWidget         = Q3DWidgetFactory::get("PIXHAWK");
-        addToCentralWidgetsMenu(_3DWidget, tr("Local 3D"), SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
+        addCentralWidget(_3DWidget, tr("Local 3D"));
     }
 #endif
 
 #if (defined _MSC_VER) | (defined Q_OS_MAC)
     if (!gEarthWidget) {
         gEarthWidget = new QGCGoogleEarthView(this);
-        addToCentralWidgetsMenu(gEarthWidget, tr("Google Earth"), SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH);
+        addCentralWidget(gEarthWidget, tr("Google Earth"));
     }
 #endif
 }
 
-
-void MainWindow::buildPxWidgets()
+void MainWindow::addTool(QDockWidget* widget, const QString& title, Qt::DockWidgetArea area)
 {
-    // Dock widgets
-    if (!detectionDockWidget)
-    {
-        detectionDockWidget = new QDockWidget(tr("Object Recognition"), this);
-        detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) );
-        detectionDockWidget->setObjectName("OBJECT_DETECTION_DOCK_WIDGET");
-        addToToolsMenu (detectionDockWidget, tr("Object Recognition"), SLOT(showToolWidget(bool)), MENU_DETECTION, Qt::RightDockWidgetArea);
-    }
-
-    if (!watchdogControlDockWidget)
-    {
-        watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this);
-        watchdogControlDockWidget->setWidget( new WatchdogControl(this) );
-        watchdogControlDockWidget->setObjectName("WATCHDOG_CONTROL_DOCKWIDGET");
-        addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget(bool)), MENU_WATCHDOG, Qt::BottomDockWidgetArea);
-    }
-
-    if (!video1DockWidget)
-    {
-        video1DockWidget = new QDockWidget(tr("Video Stream 1"), this);
-        HUD* video1 =  new HUD(160, 120, this);
-        video1->enableHUDInstruments(false);
-        video1->enableVideo(true);
-        // FIXME select video stream as well
-        video1DockWidget->setWidget(video1);
-        video1DockWidget->setObjectName("VIDEO_STREAM_1_DOCK_WIDGET");
-        addToToolsMenu (video1DockWidget, tr("Video Stream 1"), SLOT(showToolWidget(bool)), MENU_VIDEO_STREAM_1, Qt::LeftDockWidgetArea);
-    }
-
-    if (!video2DockWidget)
-    {
-        video2DockWidget = new QDockWidget(tr("Video Stream 2"), this);
-        HUD* video2 =  new HUD(160, 120, this);
-        video2->enableHUDInstruments(false);
-        video2->enableVideo(true);
-        // FIXME select video stream as well
-        video2DockWidget->setWidget(video2);
-        video2DockWidget->setObjectName("VIDEO_STREAM_2_DOCK_WIDGET");
-        addToToolsMenu (video2DockWidget, tr("Video Stream 2"), SLOT(showToolWidget(bool)), MENU_VIDEO_STREAM_2, Qt::LeftDockWidgetArea);
-    }
-
-    // Dialogue widgets
-    //FIXME: free memory in destructor
-}
-
-void MainWindow::buildSlugsWidgets()
-{
-    if (!linechartWidget) {
-        // Center widgets
-        linechartWidget   = new Linecharts(this);
-        addToCentralWidgetsMenu(linechartWidget, tr("Realtime Plot"), SLOT(showCentralWidget()), CENTRAL_LINECHART);
-    }
-
-    if (!headUpDockWidget) {
-        // Dock widgets
-        headUpDockWidget = new QDockWidget(tr("Control Indicator"), this);
-        headUpDockWidget->setWidget( new HUD(320, 240, this));
-        headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET");
-        addToToolsMenu (headUpDockWidget, tr("Head Up Display"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::LeftDockWidgetArea);
-    }
-
-    if (!rcViewDockWidget) {
-        rcViewDockWidget = new QDockWidget(tr("Radio Control"), this);
-        rcViewDockWidget->setWidget( new QGCRemoteControlView(this) );
-        rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET");
-        addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea);
-    }
-
-#if (defined _MSC_VER) | (defined Q_OS_MAC)
-    if (!gEarthWidget) {
-        gEarthWidget = new QGCGoogleEarthView(this);
-        addToCentralWidgetsMenu(gEarthWidget, tr("Google Earth"), SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH);
-    }
-
-#endif
-
-    if (!slugsDataWidget) {
-        // Dialog widgets
-        slugsDataWidget = new QDockWidget(tr("Slugs Data"), this);
-        slugsDataWidget->setWidget( new SlugsDataSensorView(this));
-        slugsDataWidget->setObjectName("SLUGS_DATA_DOCK_WIDGET");
-        addToToolsMenu (slugsDataWidget, tr("Telemetry Data"), SLOT(showToolWidget(bool)), MENU_SLUGS_DATA, Qt::RightDockWidgetArea);
-    }
-
-
-    if (!slugsHilSimWidget) {
-        slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this);
-        slugsHilSimWidget->setWidget( new SlugsHilSim(this));
-        slugsHilSimWidget->setObjectName("SLUGS_HIL_SIM_DOCK_WIDGET");
-        addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget(bool)), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea);
-    }
-
-    if (!controlParameterWidget) {
-        controlParameterWidget = new QDockWidget(tr("Control Parameters"), this);
-        controlParameterWidget->setObjectName("UNMANNED_SYSTEM_CONTROL_PARAMETERWIDGET");
-        controlParameterWidget->setWidget( new UASControlParameters(this) );
-        addToToolsMenu (controlParameterWidget, tr("Control Parameters"), SLOT(showToolWidget(bool)), MENU_UAS_CONTROL_PARAM, Qt::LeftDockWidgetArea);
-    }
-
-    if (!parametersDockWidget) {
-        parametersDockWidget = new QDockWidget(tr("Calibration and Onboard Parameters"), this);
-        parametersDockWidget->setWidget( new ParameterInterface(this) );
-        parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET");
-        addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget(bool)), MENU_PARAMETERS, Qt::RightDockWidgetArea);
-    }
-
-    if (!slugsCamControlWidget) {
-        slugsCamControlWidget = new QDockWidget(tr("Camera Control"), this);
-        slugsCamControlWidget->setWidget(new SlugsPadCameraControl(this));
-        slugsCamControlWidget->setObjectName("SLUGS_CAM_CONTROL_DOCK_WIDGET");
-        addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget(bool)), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea);
-    }
-
-}
-
-
-void MainWindow::addToCentralWidgetsMenu ( QWidget* widget,
-        const QString title,
-        const char * slotName,
-        TOOLS_WIDGET_NAMES centralWidget)
-{
-    QAction* tempAction;
-
-    tempAction = ui.menuMain->addAction(title);
+    QAction* tempAction = ui.menuTools->addAction(title);
 
     tempAction->setCheckable(true);
-    tempAction->setData(centralWidget);
-
-    // populate the Hashes
-    toolsMenuActions[centralWidget] = tempAction;
-    dockWidgets[centralWidget] = widget;
-
-    QString chKey = buildMenuKey(SUB_SECTION_CHECKED, centralWidget, currentView);
-
-    if (!settings.contains(chKey))
-    {
-        settings.setValue(chKey,false);
-        tempAction->setChecked(false);
-    }
-    else
-    {
-        tempAction->setChecked(settings.value(chKey).toBool());
-    }
-
-    // connect the action
-    connect(tempAction,SIGNAL(triggered(bool)),this, slotName);
+    QVariant var;
+    var.setValue((QWidget*)widget);
+    tempAction->setData(var);
+    connect(tempAction,SIGNAL(triggered(bool)),this, SLOT(showTool(bool)));
+    connect(widget, SIGNAL(visibilityChanged(bool)), tempAction, SLOT(setChecked(bool)));
+    tempAction->setChecked(widget->isVisible());
+    addDockWidget(area, widget);
 }
 
 
-void MainWindow::showCentralWidget()
+void MainWindow::showTool(bool show)
 {
-    QAction* senderAction = qobject_cast<QAction *>(sender());
-
-    // Block sender action while manipulating state
-    senderAction->blockSignals(true);
-
-    int tool = senderAction->data().toInt();
-    QString chKey;
-
-    // check the current action
-
-    if (senderAction && dockWidgets[tool])
-    {
-        // uncheck all central widget actions
-        QHashIterator<int, QAction*> i(toolsMenuActions);
-        while (i.hasNext()) {
-            i.next();
-            //qDebug() << "shCW" << i.key() << "read";
-            if (i.value() && i.value()->data().toInt() > 255)
-            {
-                // Block signals and uncheck action
-                // firing would be unneccesary
-                i.value()->blockSignals(true);
-                i.value()->setChecked(false);
-                i.value()->blockSignals(false);
-
-                // update the settings
-                chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(i.value()->data().toInt()), currentView);
-                settings.setValue(chKey,false);
-            }
-        }
-
-        // check the current action
-        //qDebug() << senderAction->text();
-        senderAction->setChecked(true);
-
-        // update the central widget
-        centerStack->setCurrentWidget(dockWidgets[tool]);
-
-        // store the selected central widget
-        chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(tool), currentView);
-        settings.setValue(chKey,true);
-
-        // Unblock sender action
-        senderAction->blockSignals(false);
-
-        presentView();
-    }
+    QAction* act = qobject_cast<QAction *>(sender());
+    QWidget* widget = qVariantValue<QWidget *>(act->data());
+    widget->setVisible(show);
 }
 
-/**
- * Adds a widget to the tools menu and sets it visible if it was
- * enabled last time.
- */
-void MainWindow::addToToolsMenu ( QWidget* widget,
-                                  const QString title,
-                                  const char * slotName,
-                                  TOOLS_WIDGET_NAMES tool,
-                                  Qt::DockWidgetArea location)
+void MainWindow::addCentralWidget(QWidget* widget, const QString& title)
 {
-    QAction* tempAction;
-    QString posKey, chKey;
-
-
-    if (toolsMenuActions[CENTRAL_SEPARATOR]) {
-        tempAction = new QAction(title, this);
-        ui.menuTools->insertAction(toolsMenuActions[CENTRAL_SEPARATOR],
-                                   tempAction);
-    } else {
-        tempAction = ui.menuTools->addAction(title);
-    }
-
-    tempAction->setCheckable(true);
-    tempAction->setData(tool);
-
-    // populate the Hashes
-    toolsMenuActions[tool] = tempAction;
-    dockWidgets[tool] = widget;
-    //qDebug() << widget;
-
-    posKey = buildMenuKey (SUB_SECTION_LOCATION,tool, currentView);
-
-    if (!settings.contains(posKey)) {
-        settings.setValue(posKey,location);
-        dockWidgetLocations[tool] = location;
-    } else {
-        dockWidgetLocations[tool] = static_cast <Qt::DockWidgetArea> (settings.value(posKey, Qt::RightDockWidgetArea).toInt());
-    }
-
-    chKey = buildMenuKey(SUB_SECTION_CHECKED,tool, currentView);
-
-    if (!settings.contains(chKey)) {
-        settings.setValue(chKey,false);
-        tempAction->setChecked(false);
-        widget->setVisible(false);
-    } else {
-        tempAction->setChecked(settings.value(chKey, false).toBool());
-        widget->setVisible(settings.value(chKey, false).toBool());
-    }
-
-    // connect the action
-    connect(tempAction,SIGNAL(toggled(bool)),this, slotName);
-
-    connect(qobject_cast <QDockWidget *>(dockWidgets[tool]),
-            SIGNAL(visibilityChanged(bool)), this, SLOT(showToolWidget(bool)));
-
-    //  connect(qobject_cast <QDockWidget *>(dockWidgets[tool]),
-    //          SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool)));
-
-    connect(qobject_cast <QDockWidget *>(dockWidgets[tool]),
-            SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(updateLocationSettings(Qt::DockWidgetArea)));
-}
-
-void MainWindow::showToolWidget(bool visible)
-{
-    if (!aboutToCloseFlag && !changingViewsFlag) {
-        QAction* action = qobject_cast<QAction *>(sender());
-
-        // Prevent this to fire if undocked
-        if (action) {
-            int tool = action->data().toInt();
-
-            QDockWidget* dockWidget = qobject_cast<QDockWidget *> (dockWidgets[tool]);
-
-            if (dockWidget && dockWidget->isVisible() != visible) {
-                if (visible) {
-                    addDockWidget(dockWidgetLocations[tool], dockWidget);
-                    dockWidget->show();
-                } else {
-                    removeDockWidget(dockWidget);
-                }
-
-                QHashIterator<int, QWidget*> i(dockWidgets);
-                while (i.hasNext()) {
-                    i.next();
-                    if ((static_cast <QDockWidget *>(dockWidgets[i.key()])) == dockWidget) {
-                        QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(i.key()), currentView);
-                        settings.setValue(chKey,visible);
-                        //qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible;
-                        break;
-                    }
-                }
-            }
-        }
-
-        QDockWidget* dockWidget = qobject_cast<QDockWidget*>(QObject::sender());
+    // Check if this widget already has been added
+    if (centerStack->indexOf(widget) == -1)
+    {
+        centerStack->addWidget(widget);
 
-        //qDebug() << "Trying to cast dockwidget" << dockWidget << "isvisible" << visible;
+        QAction* tempAction = ui.menuMain->addAction(title);
 
-        if (dockWidget)
-        {
-            // Get action
-            int tool = dockWidgets.key(dockWidget);
-
-            //qDebug() << "Updating widget setting" << tool << "to" << visible;
-
-            QAction* action = toolsMenuActions[tool];
-            action->blockSignals(true);
-            action->setChecked(visible);
-            action->blockSignals(false);
-
-            QHashIterator<int, QWidget*> i(dockWidgets);
-            while (i.hasNext()) {
-                i.next();
-                if ((static_cast <QDockWidget *>(dockWidgets[i.key()])) == dockWidget) {
-                    QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(i.key()), currentView);
-                    settings.setValue(chKey,visible);
-                    // qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible;
-                    break;
-                }
-            }
-        }
+        tempAction->setCheckable(true);
+        QVariant var;
+        var.setValue((QWidget*)widget);
+        tempAction->setData(var);
+        centerStackActionGroup.addAction(tempAction);
+        connect(tempAction,SIGNAL(triggered()),this, SLOT(showCentralWidget()));
+        connect(widget, SIGNAL(visibilityChanged(bool)), tempAction, SLOT(setChecked(bool)));
+        tempAction->setChecked(widget->isVisible());
     }
 }
 
 
-void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view)
-{
-    bool tempVisible;
-    Qt::DockWidgetArea tempLocation;
-    QDockWidget* tempWidget = static_cast <QDockWidget *>(dockWidgets[widget]);
-
-    tempVisible =  settings.value(buildMenuKey(SUB_SECTION_CHECKED,widget,view), false).toBool();
-
-    //qDebug() << "showTheWidget(): Set key" << buildMenuKey(SUB_SECTION_CHECKED,widget,view) << "to" << tempVisible;
-
-    if (tempWidget) {
-        toolsMenuActions[widget]->setChecked(tempVisible);
-    }
-
-
-    //qDebug() <<  buildMenuKey (SUB_SECTION_CHECKED,widget,view) << tempVisible;
-
-    tempLocation = static_cast <Qt::DockWidgetArea>(settings.value(buildMenuKey (SUB_SECTION_LOCATION,widget, view), QVariant(Qt::RightDockWidgetArea)).toInt());
-
-    if (tempWidget != NULL)
-    {
-        if (tempVisible)
-        {
-            addDockWidget(tempLocation, tempWidget);
-            tempWidget->show();
-        }
-    }
-}
-
-QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view)
+void MainWindow::showCentralWidget()
 {
-    // Key is built as follows: autopilot_type/section_menu/view/tool/section
-    int apType = 1;
-
-    return (QString::number(apType) + "_" +
-            QString::number(SECTION_MENU) + "_" +
-            QString::number(view) + "_" +
-            QString::number(tool) + "_" +
-            QString::number(section) + "_" );
+    QAction* act = qobject_cast<QAction *>(sender());
+    QWidget* widget = qVariantValue<QWidget *>(act->data());
+    centerStack->setCurrentWidget(widget);
 }
 
 void MainWindow::closeEvent(QCloseEvent *event)
 {
+    if (isVisible()) storeViewState();
     storeSettings();
     aboutToCloseFlag = true;
     mavlink->storeSettings();
@@ -863,62 +482,6 @@ void MainWindow::closeEvent(QCloseEvent *event)
     QMainWindow::closeEvent(event);
 }
 
-void MainWindow::showDockWidget (bool vis)
-{
-    if (!aboutToCloseFlag && !changingViewsFlag) {
-        QDockWidget* temp = qobject_cast<QDockWidget *>(sender());
-
-        if (temp) {
-            QHashIterator<int, QWidget*> i(dockWidgets);
-            while (i.hasNext()) {
-                i.next();
-                if ((static_cast <QDockWidget *>(dockWidgets[i.key()])) == temp) {
-                    QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(i.key()), currentView);
-                    settings.setValue(chKey,vis);
-                    toolsMenuActions[i.key()]->setChecked(vis);
-                    break;
-                }
-            }
-        }
-    }
-}
-
-void MainWindow::updateVisibilitySettings (bool vis)
-{
-    if (!aboutToCloseFlag && !changingViewsFlag) {
-        QDockWidget* temp = qobject_cast<QDockWidget *>(sender());
-
-        if (temp) {
-            QHashIterator<int, QWidget*> i(dockWidgets);
-            while (i.hasNext()) {
-                i.next();
-                if ((static_cast <QDockWidget *>(dockWidgets[i.key()])) == temp) {
-                    QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(i.key()), currentView);
-                    settings.setValue(chKey,vis);
-                    toolsMenuActions[i.key()]->setChecked(vis);
-                    break;
-                }
-            }
-        }
-    }
-}
-
-void MainWindow::updateLocationSettings (Qt::DockWidgetArea location)
-{
-    QDockWidget* temp = qobject_cast<QDockWidget *>(sender());
-
-    QHashIterator<int, QWidget*> i(dockWidgets);
-    while (i.hasNext()) {
-        i.next();
-        if ((static_cast <QDockWidget *>(dockWidgets[i.key()])) == temp) {
-            QString posKey = buildMenuKey (SUB_SECTION_LOCATION,static_cast<TOOLS_WIDGET_NAMES>(i.key()), currentView);
-            settings.setValue(posKey,location);
-            break;
-        }
-    }
-}
-
-
 /**
  * Connect the signals and slots of the common window widgets
  */
@@ -958,19 +521,14 @@ void MainWindow::loadCustomWidget()
 {
     QString widgetFileExtension(".qgw");
     QString fileName = QFileDialog::getOpenFileName(this, tr("Specify Widget File Name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("QGroundControl Widget (*%1);;").arg(widgetFileExtension));
+    if (fileName != "") loadCustomWidget(fileName);
+}
 
-    if (fileName.length() > 0)
+void MainWindow::loadCustomWidget(const QString& fileName, bool singleinstance)
+{
+    QGCToolWidget* tool = new QGCToolWidget("", this);
+    if (tool->loadSettings(fileName, true) || !singleinstance)
     {
-
-        QGCToolWidget* tool = new QGCToolWidget("", this);
-        tool->loadSettings(fileName);
-
-        if (QGCToolWidget::instances()->size() < 2)
-        {
-            // This is the first widget
-            ui.menuTools->addSeparator();
-        }
-
         // Add widget to UI
         QDockWidget* dock = new QDockWidget(tool->getTitle(), this);
         connect(tool, SIGNAL(destroyed()), dock, SLOT(deleteLater()));
@@ -986,73 +544,36 @@ void MainWindow::loadCustomWidget()
         this->addDockWidget(Qt::BottomDockWidgetArea, dock);
         dock->setVisible(true);
     }
-}
-
-void MainWindow::connectPxWidgets()
-{
-    // No special connections necessary at this point
-}
-
-void MainWindow::connectSlugsWidgets()
-{
-    if (slugsHilSimWidget && slugsHilSimWidget->widget()) {
-        connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
-                slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*)));
-    }
-
-    if (slugsDataWidget && slugsDataWidget->widget()) {
-        connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
-                slugsDataWidget->widget(), SLOT(setActiveUAS(UASInterface*)));
-    }
-
-    if (controlParameterWidget && controlParameterWidget->widget()) {
-        connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
-                controlParameterWidget->widget(), SLOT(activeUasSet(UASInterface*)));
-    }
-
-    if(controlDockWidget && controlParameterWidget) {
-        connect(controlDockWidget->widget(), SIGNAL(changedMode(int)), controlParameterWidget->widget(), SLOT(changedMode(int)));
+    else
+    {
+        return;
     }
 }
 
-void MainWindow::arrangeCommonCenterStack()
+void MainWindow::loadCustomWidgetsFromDefaults(const QString& systemType, const QString& autopilotType)
 {
-    centerStack = new QStackedWidget(this);
-    centerStack->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
-
-    if (!centerStack) return;
-
-    if (mapWidget && (centerStack->indexOf(mapWidget) == -1)) centerStack->addWidget(mapWidget);
-    if (dataplotWidget && (centerStack->indexOf(dataplotWidget) == -1)) centerStack->addWidget(dataplotWidget);
-    if (protocolWidget && (centerStack->indexOf(protocolWidget) == -1)) centerStack->addWidget(protocolWidget);
-    if (linechartWidget && (centerStack->indexOf(linechartWidget) == -1)) centerStack->addWidget(linechartWidget);
+    QString defaultsDir = qApp->applicationDirPath() + "/files/" + systemType.toLower() + "/" + autopilotType.toLower() + "/widgets/";
 
-#ifdef QGC_OSG_ENABLED
-    if (_3DWidget && (centerStack->indexOf(_3DWidget) == -1)) centerStack->addWidget(_3DWidget);
-#endif
-#if (defined _MSC_VER) | (defined Q_OS_MAC)
-    if (gEarthWidget && (centerStack->indexOf(gEarthWidget) == -1)) centerStack->addWidget(gEarthWidget);
-#endif
-    if (hudWidget && (centerStack->indexOf(hudWidget) == -1)) centerStack->addWidget(hudWidget);
-    if (dataplotWidget && (centerStack->indexOf(dataplotWidget) == -1)) centerStack->addWidget(dataplotWidget);
-
-    setCentralWidget(centerStack);
-}
-
-void MainWindow::arrangePxCenterStack()
-{
-    if (!centerStack) {
-        qDebug() << "Center Stack not Created!";
-        return;
+    QDir widgets(defaultsDir);
+    QStringList files = widgets.entryList();
+    if (files.count() == 0)
+    {
+        qDebug() << "No default custom widgets for system " << systemType << "autopilot" << autopilotType << " found";
+        qDebug() << "Tried with path: " << defaultsDir;
+        showStatusMessage(tr("Did not find any custom widgets in %1").arg(defaultsDir));
     }
-}
 
-void MainWindow::arrangeSlugsCenterStack()
-{
-    if (!centerStack)
+    // Load all custom widgets found in the AP folder
+    for(int i = 0; i < files.count(); ++i)
     {
-        qDebug() << "Center Stack not Created!";
-        return;
+        QString file = files[i];
+        if (file.endsWith(".qgw"))
+        {
+            // Will only be loaded if not already a custom widget with
+            // the same name is present
+            loadCustomWidget(defaultsDir+"/"+file, true);
+            showStatusMessage(tr("Loaded custom widget %1").arg(defaultsDir+"/"+file));
+        }
     }
 }
 
@@ -1073,14 +594,17 @@ void MainWindow::storeSettings()
     settings.setValue("AUTO_RECONNECT", autoReconnect);
     settings.setValue("CURRENT_STYLE", currentStyle);
     settings.endGroup();
-    settings.setValue(getWindowGeometryKey(), saveGeometry());
-    // Save the last current view in any case
-    settings.setValue("CURRENT_VIEW", currentView);
-    // Save the current window state, but only if a system is connected (else no real number of widgets would be present)
-    if (UASManager::instance()->getUASList().length() > 0) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion()));
-    // Save the current view only if a UAS is connected
-    if (UASManager::instance()->getUASList().length() > 0) settings.setValue("CURRENT_VIEW_WITH_UAS_CONNECTED", currentView);
-    // Save the current power mode
+    if (!aboutToCloseFlag && isVisible())
+    {
+        settings.setValue(getWindowGeometryKey(), saveGeometry());
+        // Save the last current view in any case
+        settings.setValue("CURRENT_VIEW", currentView);
+        // Save the current window state, but only if a system is connected (else no real number of widgets would be present)
+        if (UASManager::instance()->getUASList().length() > 0) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion()));
+        // Save the current view only if a UAS is connected
+        if (UASManager::instance()->getUASList().length() > 0) settings.setValue("CURRENT_VIEW_WITH_UAS_CONNECTED", currentView);
+        // Save the current power mode
+    }
     settings.setValue("LOW_POWER_MODE", lowPowerMode);
     settings.sync();
 }
@@ -1093,9 +617,11 @@ void MainWindow::configureWindowName()
 
     windowname.append(" (" + QHostInfo::localHostName() + ": ");
 
-    for (int i = 0; i < hostAddresses.size(); i++) {
+    for (int i = 0; i < hostAddresses.size(); i++)
+    {
         // Exclude loopback IPv4 and all IPv6 addresses
-        if (hostAddresses.at(i) != QHostAddress("127.0.0.1") && !hostAddresses.at(i).toString().contains(":")) {
+        if (hostAddresses.at(i) != QHostAddress("127.0.0.1") && !hostAddresses.at(i).toString().contains(":"))
+        {
             if(prevAddr) windowname.append("/");
             windowname.append(hostAddresses.at(i).toString());
             prevAddr = true;
@@ -1140,7 +666,8 @@ void MainWindow::saveScreen()
     QPixmap window = QPixmap::grabWindow(this->winId());
     QString format = "bmp";
 
-    if (!screenFileName.isEmpty()) {
+    if (!screenFileName.isEmpty())
+    {
         window.save(screenFileName, format.toAscii());
     }
 }
@@ -1286,8 +813,6 @@ void MainWindow::showInfoMessage(const QString& title, const QString& message)
 **/
 void MainWindow::connectCommonActions()
 {
-    ui.actionNewCustomWidget->setEnabled(false);
-
     // Bind together the perspective actions
     QActionGroup* perspectives = new QActionGroup(ui.menuPerspectives);
     perspectives->addAction(ui.actionEngineersView);
@@ -1304,11 +829,6 @@ void MainWindow::connectCommonActions()
     if (currentView == VIEW_OPERATOR) ui.actionOperatorsView->setChecked(true);
     if (currentView == VIEW_UNCONNECTED) ui.actionUnconnectedView->setChecked(true);
 
-    // The pilot, engineer and operator view are not available on startup
-    // since they only make sense with a system connected.
-    ui.actionPilotsView->setEnabled(false);
-    ui.actionOperatorsView->setEnabled(false);
-    ui.actionEngineersView->setEnabled(false);
     // The UAS actions are not enabled without connection to system
     ui.actionLiftoff->setEnabled(false);
     ui.actionLand->setEnabled(false);
@@ -1369,17 +889,6 @@ void MainWindow::connectCommonActions()
     connect(ui.actionSettings, SIGNAL(triggered()), this, SLOT(showSettings()));
 }
 
-void MainWindow::connectPxActions()
-{
-
-
-}
-
-void MainWindow::connectSlugsActions()
-{
-
-}
-
 void MainWindow::showHelp()
 {
     if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/users/start"))) {
@@ -1395,7 +904,7 @@ void MainWindow::showHelp()
 
 void MainWindow::showCredits()
 {
-    if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/credits/"))) {
+    if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/credits"))) {
         QMessageBox msgBox;
         msgBox.setIcon(QMessageBox::Critical);
         msgBox.setText("Could not open credits in browser");
@@ -1447,8 +956,10 @@ void MainWindow::addLink()
     // Go fishing for this link's configuration window
     QList<QAction*> actions = ui.menuNetwork->actions();
 
-    foreach (QAction* act, actions) {
-        if (act->data().toInt() == LinkManager::instance()->getLinks().indexOf(link)) {
+    foreach (QAction* act, actions)
+    {
+        if (act->data().toInt() == LinkManager::instance()->getLinks().indexOf(link))
+        {
             act->trigger();
             break;
         }
@@ -1517,16 +1028,6 @@ void MainWindow::UASCreated(UASInterface* uas)
 
     if (uas != NULL)
     {
-        if (!linechartWidget) {
-            // Center widgets
-            linechartWidget   = new Linecharts(this);
-            linechartWidget->addSource(mavlinkDecoder);
-            addToCentralWidgetsMenu(linechartWidget, tr("Realtime Plot"), SLOT(showCentralWidget()), CENTRAL_LINECHART);
-        }
-
-        // Set default settings
-        setDefaultSettingsForAp();
-
         // The pilot, operator and engineer views were not available on startup, enable them now
         ui.actionPilotsView->setEnabled(true);
         ui.actionOperatorsView->setEnabled(true);
@@ -1598,74 +1099,46 @@ void MainWindow::UASCreated(UASInterface* uas)
             }
         }
 
-        switch (uas->getAutopilotType())
+        // Line chart
+        if (!linechartWidget)
         {
-        case (MAV_AUTOPILOT_SLUGS): {
-            // Build Slugs Widgets
-            buildSlugsWidgets();
-
-            // Connect Slugs Widgets
-            connectSlugsWidgets();
-
-            // Arrange Slugs Centerstack
-            arrangeSlugsCenterStack();
-
-            // Connect Slugs Actions
-            connectSlugsActions();
-
-//                if(slugsDataWidget)
-//                {
-//                    SlugsDataSensorView *mm = dynamic_cast<SlugsDataSensorView*>(slugsDataWidget->widget());
-//                    if(mm)
-//                    {
-//                        mm->addUAS(uas);
-//                    }
-//                }
-
-            // FIXME: This type checking might be redundant
-//                //            if (slugsDataWidget) {
-//                //              SlugsDataSensorView* dataWidget = dynamic_cast<SlugsDataSensorView*>(slugsDataWidget->widget());
-//                //              if (dataWidget) {
-//                //                SlugsMAV* mav2 = dynamic_cast<SlugsMAV*>(uas);
-//                //                if (mav2) {
-//                (dynamic_cast<SlugsDataSensorView*>(slugsDataWidget->widget()))->addUAS(uas);
-//                //                  //loadSlugsView();
-//                //                  loadGlobalOperatorView();
-//                //                }
-//                //              }
-//                //            }
-
+            // Center widgets
+            linechartWidget   = new Linecharts(this);
+            addCentralWidget(linechartWidget, tr("Realtime Plot"));
         }
-        break;
-        default:
-        case (MAV_AUTOPILOT_GENERIC):
-            break;
-        case (MAV_AUTOPILOT_ARDUPILOTMEGA):
-            break;
-        case (MAV_AUTOPILOT_PIXHAWK): {
-            // Build Pixhawk Widgets
-            buildPxWidgets();
 
-            // Connect Pixhawk Widgets
-            connectPxWidgets();
+        // Load default custom widgets for this autopilot type
+        loadCustomWidgetsFromDefaults(uas->getSystemTypeName(), uas->getAutopilotTypeName());
 
-            // Arrange Pixhawk Centerstack
-            arrangePxCenterStack();
 
-            // Connect Pixhawk Actions
-            connectPxActions();
-        }
-        break;
+        if (uas->getAutopilotType() == MAV_AUTOPILOT_PIXHAWK)
+        {
+            // Dock widgets
+            if (!detectionDockWidget)
+            {
+                detectionDockWidget = new QDockWidget(tr("Object Recognition"), this);
+                detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) );
+                detectionDockWidget->setObjectName("OBJECT_DETECTION_DOCK_WIDGET");
+                addTool(detectionDockWidget, tr("Object Recognition"), Qt::RightDockWidgetArea);
+            }
+
+            if (!watchdogControlDockWidget) {
+                watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this);
+                watchdogControlDockWidget->setWidget( new WatchdogControl(this) );
+                watchdogControlDockWidget->setObjectName("WATCHDOG_CONTROL_DOCKWIDGET");
+                addTool(watchdogControlDockWidget, tr("Process Control"), Qt::BottomDockWidgetArea);
+            }
         }
 
         // Change the view only if this is the first UAS
 
         // If this is the first connected UAS, it is both created as well as
         // the currently active UAS
-        if (UASManager::instance()->getUASList().size() == 1) {
+        if (UASManager::instance()->getUASList().size() == 1)
+        {
             // Load last view if setting is present
-            if (settings.contains("CURRENT_VIEW_WITH_UAS_CONNECTED")) {
-                clearView();
+            if (settings.contains("CURRENT_VIEW_WITH_UAS_CONNECTED"))
+            {
                 int view = settings.value("CURRENT_VIEW_WITH_UAS_CONNECTED").toInt();
                 switch (view) {
                 case VIEW_ENGINEER:
@@ -1685,7 +1158,9 @@ void MainWindow::UASCreated(UASInterface* uas)
                     loadOperatorView();
                     break;
                 }
-            } else {
+            }
+            else
+            {
                 loadOperatorView();
             }
         }
@@ -1694,285 +1169,142 @@ void MainWindow::UASCreated(UASInterface* uas)
 
     if (!ui.menuConnected_Systems->isEnabled()) ui.menuConnected_Systems->setEnabled(true);
 
-    // Custom widgets, added last to all menus and layouts
-    buildCustomWidget();
     // Restore the mainwindow size
-    if (settings.contains(getWindowGeometryKey())) {
+    if (settings.contains(getWindowGeometryKey()))
+    {
         restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray());
     }
 }
 
 /**
- * Clears the current view completely
+ * Stores the current view state
  */
-void MainWindow::clearView()
+void MainWindow::storeViewState()
 {
-    // Save current state
-    if (UASManager::instance()->getUASList().count() > 0) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion()));
-    // Although we want save the state of the window, we do not want to change the top-leve state (minimized, maximized, etc)
-    // therefore this state is stored here and restored after applying the rest of the settings in the new
-    // perspective.
-    windowStateVal = this->windowState();
-    settings.setValue(getWindowGeometryKey(), saveGeometry());
-
-    QAction* temp;
-
-    // Set tool widget visibility settings for this view
-    foreach (int key, toolsMenuActions.keys())
+    if (!aboutToCloseFlag)
     {
-        temp = toolsMenuActions[key];
-        QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(key), currentView);
-
-        if (temp) {
-            //qDebug() << "TOOL:" << chKey << "IS:" << temp->isChecked();
-            settings.setValue(chKey,temp->isChecked());
-        } else {
-            //qDebug() << "TOOL:" << chKey << "IS DEFAULT AND UNCHECKED";
-            settings.setValue(chKey,false);
-        }
+        // Save current state
+        settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion()));
+        settings.setValue(getWindowStateKey()+"CENTER_WIDGET", centerStack->currentIndex());
+        // Although we want save the state of the window, we do not want to change the top-leve state (minimized, maximized, etc)
+        // therefore this state is stored here and restored after applying the rest of the settings in the new
+        // perspective.
+        windowStateVal = this->windowState();
+        settings.setValue(getWindowGeometryKey(), saveGeometry());
     }
+}
 
-    changingViewsFlag = true;
-    // Remove all dock widgets from main window
-    QObjectList childList( this->children() );
+void MainWindow::loadViewState()
+{
+    // Restore center stack state
+    int index = settings.value(getWindowStateKey()+"CENTER_WIDGET", -1).toInt();
+    // The offline plot view is usually the consequence of a logging run, always show the realtime view first
+    if (centerStack->indexOf(dataplotWidget) == index)
+    {
+        // Rewrite to realtime plot
+        index = centerStack->indexOf(linechartWidget);
+    }
 
-    QObjectList::iterator i;
-    QDockWidget* dockWidget;
-    for (i = childList.begin(); i != childList.end(); ++i) {
-        dockWidget = dynamic_cast<QDockWidget*>(*i);
-        if (dockWidget) {
-            // Remove dock widget from main window
-            removeDockWidget(dockWidget);
-            dockWidget->hide();
-            //dockWidget->setVisible(false);
+    if (index != -1)
+    {
+        centerStack->setCurrentIndex(index);
+    }
+    else
+    {
+        // Load defaults
+        switch (currentView)
+        {
+        case VIEW_ENGINEER:
+            centerStack->setCurrentWidget(linechartWidget);
+            break;
+        case VIEW_PILOT:
+            centerStack->setCurrentWidget(hudWidget);
+            break;
+        case VIEW_MAVLINK:
+            centerStack->setCurrentWidget(protocolWidget);
+            break;
+        case VIEW_OPERATOR:
+        case VIEW_UNCONNECTED:
+        case VIEW_FULL:
+        default:
+            centerStack->setCurrentWidget(mapWidget);
+            break;
         }
     }
-    changingViewsFlag = false;
+
+    // Restore the widget positions and size
+    if (settings.contains(getWindowStateKey()))
+    {
+        restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion());
+    }
 }
 
 void MainWindow::loadEngineerView()
 {
     if (currentView != VIEW_ENGINEER) {
-        clearView();
+        storeViewState();
         currentView = VIEW_ENGINEER;
         ui.actionEngineersView->setChecked(true);
-        presentView();
+        loadViewState();
     }
 }
 
 void MainWindow::loadOperatorView()
 {
     if (currentView != VIEW_OPERATOR) {
-        clearView();
+        storeViewState();
         currentView = VIEW_OPERATOR;
         ui.actionOperatorsView->setChecked(true);
-        presentView();
+        loadViewState();
     }
 }
 
 void MainWindow::loadUnconnectedView()
 {
-    if (currentView != VIEW_UNCONNECTED) {
-        clearView();
+    if (currentView != VIEW_UNCONNECTED)
+    {
+        storeViewState();
         currentView = VIEW_UNCONNECTED;
         ui.actionUnconnectedView->setChecked(true);
-        presentView();
+        loadViewState();
     }
 }
 
 void MainWindow::loadPilotView()
 {
-    if (currentView != VIEW_PILOT) {
-        clearView();
+    if (currentView != VIEW_PILOT)
+    {
+        storeViewState();
         currentView = VIEW_PILOT;
         ui.actionPilotsView->setChecked(true);
-        presentView();
+        loadViewState();
     }
 }
 
 void MainWindow::loadMAVLinkView()
 {
-    if (currentView != VIEW_MAVLINK) {
-        clearView();
+    if (currentView != VIEW_MAVLINK)
+    {
+        storeViewState();
         currentView = VIEW_MAVLINK;
         ui.actionMavlinkView->setChecked(true);
-        presentView();
-    }
-}
-
-void MainWindow::presentView()
-{
-    // LINE CHART
-    showTheCentralWidget(CENTRAL_LINECHART, currentView);
-
-    // MAP
-    showTheCentralWidget(CENTRAL_MAP, currentView);
-
-    // PROTOCOL
-    showTheCentralWidget(CENTRAL_PROTOCOL, currentView);
-
-    // HEAD UP DISPLAY
-    showTheCentralWidget(CENTRAL_HUD, currentView);
-
-    // GOOGLE EARTH
-    showTheCentralWidget(CENTRAL_GOOGLE_EARTH, currentView);
-
-    // LOCAL 3D VIEW
-    showTheCentralWidget(CENTRAL_3D_LOCAL, currentView);
-
-    // GLOBAL 3D VIEW
-    showTheCentralWidget(CENTRAL_3D_MAP, currentView);
-
-    // DATA PLOT
-    showTheCentralWidget(CENTRAL_DATA_PLOT, currentView);
-
-
-    // Show docked widgets based on current view and autopilot type
-
-    // UAS CONTROL
-    showTheWidget(MENU_UAS_CONTROL, currentView);
-
-    showTheWidget(MENU_UAS_CONTROL_PARAM, currentView);
-
-    // UAS LIST
-    showTheWidget(MENU_UAS_LIST, currentView);
-
-    // WAYPOINT LIST
-    showTheWidget(MENU_WAYPOINTS, currentView);
-
-    // UAS STATUS
-    showTheWidget(MENU_STATUS, currentView);
-
-    // DETECTION
-    showTheWidget(MENU_DETECTION, currentView);
-
-    // DEBUG CONSOLE
-    showTheWidget(MENU_DEBUG_CONSOLE, currentView);
-
-    // ONBOARD PARAMETERS
-    showTheWidget(MENU_PARAMETERS, currentView);
-
-    // WATCHDOG
-    showTheWidget(MENU_WATCHDOG, currentView);
-
-    // HUD
-    showTheWidget(MENU_HUD, currentView);
-    if (headUpDockWidget) {
-        HUD* tmpHud = dynamic_cast<HUD*>( headUpDockWidget->widget() );
-        if (tmpHud) {
-            if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HUD,currentView)).toBool()) {
-                addDockWidget(static_cast <Qt::DockWidgetArea>(settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HUD, currentView)).toInt()),
-                              headUpDockWidget);
-                headUpDockWidget->show();
-            } else {
-                headUpDockWidget->hide();
-            }
-        }
-    }
-
-
-    // RC View
-    showTheWidget(MENU_RC_VIEW, currentView);
-
-    // SLUGS DATA
-    showTheWidget(MENU_SLUGS_DATA, currentView);
-
-    // SLUGS PID
-    showTheWidget(MENU_SLUGS_PID, currentView);
-
-    // SLUGS HIL
-    showTheWidget(MENU_SLUGS_HIL, currentView);
-
-    // SLUGS CAMERA
-    showTheWidget(MENU_SLUGS_CAMERA, currentView);
-
-    // HORIZONTAL SITUATION INDICATOR
-    showTheWidget(MENU_HSI, currentView);
-
-    // HEAD DOWN 1
-    showTheWidget(MENU_HDD_1, currentView);
-
-    // HEAD DOWN 2
-    showTheWidget(MENU_HDD_2, currentView);
-
-    // MAVLINK LOG PLAYER
-    showTheWidget(MENU_MAVLINK_LOG_PLAYER, currentView);
-
-    // VIDEO 1
-    showTheWidget(MENU_VIDEO_STREAM_1, currentView);
-
-    // VIDEO 2
-    showTheWidget(MENU_VIDEO_STREAM_2, currentView);
-
-    // Restore window state
-    if (UASManager::instance()->getUASList().count() > 0) {
-//        // Restore the mainwindow size
-//        if (settings.contains(getWindowGeometryKey()))
-//        {
-//            restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray());
-//        }
-
-        // Restore the widget positions and size
-        if (settings.contains(getWindowStateKey())) {
-            restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion());
-        }
-    }
-
-    // ACTIVATE HUD WIDGET
-    if (headUpDockWidget) {
-        HUD* tmpHud = dynamic_cast<HUD*>( headUpDockWidget->widget() );
-        if (tmpHud) {
-
-        }
-    }
-
-    //this->setWindowState(windowStateVal);
-    this->show();
-}
-
-void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view)
-{
-    bool tempVisible;
-    QWidget* tempWidget = dockWidgets[centralWidget];
-
-    tempVisible =  settings.value(buildMenuKey(SUB_SECTION_CHECKED,centralWidget,view), false).toBool();
-    //qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible;
-    if (toolsMenuActions[centralWidget]) {
-        //qDebug() << "SETTING TO:" << tempVisible;
-        toolsMenuActions[centralWidget]->setChecked(tempVisible);
-    }
-
-    if (centerStack && tempWidget && tempVisible) {
-        //qDebug() << "ACTIVATING MAIN WIDGET";
-        centerStack->setCurrentWidget(tempWidget);
+        loadViewState();
     }
 }
 
 void MainWindow::loadDataView(QString fileName)
 {
-    clearView();
-
-    // Unload line chart
-    QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(CENTRAL_LINECHART), currentView);
-    settings.setValue(chKey,false);
-
-    // Set data plot in settings as current widget and then run usual update procedure
-    chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast<TOOLS_WIDGET_NAMES>(CENTRAL_DATA_PLOT), currentView);
-    settings.setValue(chKey,true);
-
-    presentView();
-
     // Plot is now selected, now load data from file
-    if (dataplotWidget) {
+    if (dataplotWidget)
+    {
+        dataplotWidget->loadFile(fileName);
+    }
+    QStackedWidget *centerStack = dynamic_cast<QStackedWidget*>(centralWidget());
+    if (centerStack)
+    {
+        centerStack->setCurrentWidget(dataplotWidget);
         dataplotWidget->loadFile(fileName);
     }
-//        QStackedWidget *centerStack = dynamic_cast<QStackedWidget*>(centralWidget());
-//        if (centerStack)
-//        {
-//            centerStack->setCurrentWidget(dataplotWidget);
-//            dataplotWidget->loadFile(fileName);
-//        }
-//    }
 }
 
 
diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h
index 3caafc09a201f62b5ec7b1e64a278372c4bc1a4b..996e4bf733c7b67be4ce4d75a04c99eca4ed4dcb 100644
--- a/src/ui/MainWindow.h
+++ b/src/ui/MainWindow.h
@@ -113,9 +113,9 @@ public:
         return lowPowerMode;
     }
 
+    QList<QAction*> listLinkMenuActions(void);
+
 public slots:
-//    /** @brief Store the mainwindow settings */
-//    void storeSettings();
 
     /** @brief Shows a status message on the bottom status bar */
     void showStatusMessage(const QString& status, int timeout);
@@ -161,9 +161,6 @@ public slots:
     /** @brief Show the project roadmap */
     void showRoadMap();
 
-    /** @brief Shows the widgets based on configuration and current view and autopilot */
-    void presentView();
-
     /** @brief Reload the CSS style sheet */
     void reloadStylesheet();
     /** @brief Let the user select the CSS style sheet */
@@ -184,9 +181,15 @@ public slots:
     /** @brief Add a custom tool widget */
     void createCustomWidget();
 
-    /** @brief Load a custom tool widget from a file */
+    /** @brief Load a custom tool widget from a file chosen by user (QFileDialog) */
     void loadCustomWidget();
 
+    /** @brief Load a custom tool widget from a file */
+    void loadCustomWidget(const QString& fileName, bool singleinstance=false);
+
+    /** @brief Load custom widgets from default file */
+    void loadCustomWidgetsFromDefaults(const QString& systemType, const QString& autopilotType);
+
     void closeEvent(QCloseEvent* event);
 
     /** @brief Load data view, allowing to plot flight data */
@@ -195,100 +198,37 @@ public slots:
     /**
      * @brief Shows a Docked Widget based on the action sender
      *
-     * This slot is written to be used in conjunction with the addToToolsMenu function
+     * This slot is written to be used in conjunction with the addTool() function
      * It shows the QDockedWidget based on the action sender
      *
      */
-    void showToolWidget(bool visible);
+    void showTool(bool visible);
 
     /**
      * @brief Shows a Widget from the center stack based on the action sender
      *
-     * This slot is written to be used in conjunction with the addToCentralWidgetsMenu function
+     * This slot is written to be used in conjunction with the addCentralWidget() function
      * It shows the Widget based on the action sender
      *
      */
     void showCentralWidget();
 
-    /** @brief Change actively a QDockWidgets visibility by an action */
-    void showDockWidget(bool vis);
-    /** @brief Updates a QDockWidget's checked status based on its visibility */
-    void updateVisibilitySettings(bool vis);
-
-    /** @brief Updates a QDockWidget's location */
-    void updateLocationSettings (Qt::DockWidgetArea location);
-
-public:
-	QList<QAction*> listLinkMenuActions(void);
-
 protected:
 
     MainWindow(QWidget *parent = 0);
 
-    /** @brief Set default window settings for the current autopilot type */
-    void setDefaultSettingsForAp();
-
-    // These defines are used to save the settings when selecting with
-    // which widgets populate the views
-    // FIXME: DO NOT PUT CUSTOM VALUES IN THIS ENUM since it is iterated over
-    // this will be fixed in a future release.
-    typedef enum _TOOLS_WIDGET_NAMES {
-        MENU_UAS_CONTROL_PARAM,
-        MENU_UAS_CONTROL,
-        MENU_UAS_INFO,
-        MENU_CAMERA,
-        MENU_UAS_LIST,
-        MENU_WAYPOINTS,
-        MENU_STATUS,
-        MENU_DETECTION,
-        MENU_DEBUG_CONSOLE,
-        MENU_PARAMETERS,
-        MENU_HDD_1,
-        MENU_HDD_2,
-        MENU_WATCHDOG,
-        MENU_HUD,
-        MENU_HSI,
-        MENU_RC_VIEW,
-        MENU_SLUGS_DATA,
-        MENU_SLUGS_PID,
-        MENU_SLUGS_HIL,
-        MENU_SLUGS_CAMERA,
-        MENU_MAVLINK_LOG_PLAYER,
-        MENU_VIDEO_STREAM_1,
-        MENU_VIDEO_STREAM_2,
-        MENU_MAVLINK_INSPECTOR,
-        CENTRAL_SEPARATOR= 255, // do not change
-        CENTRAL_LINECHART,
-        CENTRAL_PROTOCOL,
-        CENTRAL_MAP,
-        CENTRAL_3D_LOCAL,
-        CENTRAL_3D_MAP,
-        CENTRAL_OSGEARTH,
-        CENTRAL_GOOGLE_EARTH,
-        CENTRAL_HUD,
-        CENTRAL_DATA_PLOT,
-
-    } TOOLS_WIDGET_NAMES;
-
-    typedef enum _SETTINGS_SECTIONS {
-        SECTION_MENU,
-        SUB_SECTION_CHECKED,
-        SUB_SECTION_LOCATION
-    } SETTINGS_SECTIONS;
-
-    typedef enum _VIEW_SECTIONS {
+    typedef enum _VIEW_SECTIONS
+    {
         VIEW_ENGINEER,
         VIEW_OPERATOR,
         VIEW_PILOT,
         VIEW_MAVLINK,
         VIEW_UNCONNECTED,    ///< View in unconnected mode, when no UAS is available
-        VIEW_FULL            ///< All widgets, mostly intended for multi-screen setups
+        VIEW_FULL            ///< All widgets shown at once
     } VIEW_SECTIONS;
 
-
-    QHash<int, QAction*> toolsMenuActions; // Holds ptr to the Menu Actions
-    QHash<int, QWidget*> dockWidgets;  // Holds ptr to the Actual Dock widget
-    QHash<int, Qt::DockWidgetArea> dockWidgetLocations; // Holds the location
+//    QHash<int, QAction*> toolsMenuActions; // Holds ptr to the Menu Actions
+//    QHash<int, QWidget*> dockWidgets;  // Holds ptr to the Actual Dock widget
 
     /**
      * @brief Adds an already instantiated QDockedWidget to the Tools Menu
@@ -297,24 +237,11 @@ protected:
      * tools menu and connects the QMenuAction to a slot that shows the widget and
      * checks/unchecks the tools menu item
      *
-     * @param widget    The QDockedWidget being added
+     * @param widget    The QDockWidget being added
      * @param title     The entry that will appear in the Menu and in the QDockedWidget title bar
-     * @param slotName  The slot to which the triggered() signal of the menu action will be connected.
-     * @param tool      The ENUM defined in MainWindow.h that is associated to the widget
      * @param location  The default location for the QDockedWidget in case there is no previous key in the settings
      */
-    void addToToolsMenu (QWidget* widget, const QString title, const char * slotName, TOOLS_WIDGET_NAMES tool, Qt::DockWidgetArea location=Qt::RightDockWidgetArea);
-
-    /**
-     * @brief Determines if a QDockWidget needs to be show and if so, shows it
-     *
-     *  Based on the the autopilot and the current view it queries the settings and shows the
-     *  widget if necessary
-     *
-     * @param widget    The QDockWidget requested to be shown
-     * @param view      The view for which the QDockWidget is requested
-     */
-    void showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view = VIEW_MAVLINK);
+    void addTool(QDockWidget* widget, const QString& title, Qt::DockWidgetArea location=Qt::RightDockWidgetArea);
 
     /**
      * @brief Adds an already instantiated QWidget to the center stack
@@ -326,21 +253,8 @@ protected:
      *
      * @param widget        The QWidget being added
      * @param title         The entry that will appear in the Menu
-     * @param slotName      The slot to which the triggered() signal of the menu action will be connected.
-     * @param centralWidget The ENUM defined in MainWindow.h that is associated to the widget
-     */
-    void addToCentralWidgetsMenu ( QWidget* widget, const QString title,const char * slotName, TOOLS_WIDGET_NAMES centralWidget);
-
-    /**
-     * @brief Determines if a QWidget needs to be show and if so, shows it
-     *
-     *  Based on the the autopilot and the current view it queries the settings and shows the
-     *  widget if necessary
-     *
-     * @param centralWidget    The QWidget requested to be shown
-     * @param view             The view for which the QWidget is requested
      */
-    void showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view);
+    void addCentralWidget(QWidget* widget, const QString& title);
 
     /** @brief Catch window resize events */
     void resizeEvent(QResizeEvent * event);
@@ -350,25 +264,13 @@ protected:
     bool aboutToCloseFlag;
     bool changingViewsFlag;
 
-    void clearView();
+    void storeViewState();
+    void loadViewState();
 
     void buildCustomWidget();
     void buildCommonWidgets();
-    void buildPxWidgets();
-    void buildSlugsWidgets();
-
     void connectCommonWidgets();
-    void connectPxWidgets();
-    void connectSlugsWidgets();
-
-    void arrangeCommonCenterStack();
-    void arrangePxCenterStack();
-    void arrangeSlugsCenterStack();
-
     void connectCommonActions();
-    void connectPxActions();
-    void connectSlugsActions();
-
 
     void configureWindowName();
     void loadSettings();
@@ -382,12 +284,11 @@ protected:
 
     QSettings settings;
     QStackedWidget *centerStack;
+    QActionGroup centerStackActionGroup;
 
     // Center widgets
     QPointer<Linecharts> linechartWidget;
-
     QPointer<HUD> hudWidget;
-
     QPointer<QGCMapTool> mapWidget;
     QPointer<XMLCommProtocolWidget> protocolWidget;
     QPointer<QGCDataPlot2D> dataplotWidget;
@@ -400,6 +301,7 @@ protected:
 #if (defined _MSC_VER) || (defined Q_OS_MAC)
     QPointer<QGCGoogleEarthView> gEarthWidget;
 #endif
+
     // Dock widgets
     QPointer<QDockWidget> controlDockWidget;
     QPointer<QDockWidget> controlParameterWidget;
@@ -460,7 +362,6 @@ protected:
 private:
     Ui::MainWindow ui;
 
-    QString buildMenuKey (SETTINGS_SECTIONS section , TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view);
     QString getWindowStateKey();
     QString getWindowGeometryKey();
 
diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui
index 8d134a94c8c4e5bd8235d78204d8efea9b5ff895..bbc2231f2019a57de3342d25ccda9c03fe018a08 100644
--- a/src/ui/MainWindow.ui
+++ b/src/ui/MainWindow.ui
@@ -51,7 +51,7 @@
      <x>0</x>
      <y>0</y>
      <width>800</width>
-     <height>22</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuMGround">
diff --git a/src/ui/QGCDataPlot2D.h b/src/ui/QGCDataPlot2D.h
index edfd248f4ef60d62471dcbf0d4f636f3475a6536..f87498bbaaef0ddeda9911f0465c7adac372944f 100644
--- a/src/ui/QGCDataPlot2D.h
+++ b/src/ui/QGCDataPlot2D.h
@@ -46,7 +46,22 @@ public slots:
     /** @brief Calculate and display regression function*/
     bool calculateRegression();
 
+signals:
+    void visibilityChanged(bool visible);
+
 protected:
+    void showEvent(QShowEvent* event)
+    {
+        QWidget::showEvent(event);
+        emit visibilityChanged(true);
+    }
+
+    void hideEvent(QHideEvent* event)
+    {
+        QWidget::hideEvent(event);
+        emit visibilityChanged(false);
+    }
+
     void changeEvent(QEvent *e);
     IncrementalPlot* plot;
     LogCompressor* compressor;
diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc
index e2013f5c21909b10a134b771750259cf27e03a65..bd1d9e97cc7933d7974df18211090aba3aa17b81 100644
--- a/src/ui/WaypointList.cc
+++ b/src/ui/WaypointList.cc
@@ -166,54 +166,74 @@ void WaypointList::read()
 
 void WaypointList::add()
 {
-    if (uas) {
+    if (uas)
+    {
         const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
         Waypoint *wp;
-
-
-        if (waypoints.size() > 0) {
+        if (waypoints.size() > 0)
+        {
             // Create waypoint with last frame
             Waypoint *last = waypoints.at(waypoints.size()-1);
             wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getParam1(), last->getParam2(), last->getParam3(), last->getParam4(),
                               last->getAutoContinue(), false, last->getFrame(), last->getAction());
             uas->getWaypointManager()->addWaypoint(wp);
-        } else {
-            // Create global frame waypoint per default
-            wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0.0, 0.0, 0.0, 0.0, true, true, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT);
-            uas->getWaypointManager()->addWaypoint(wp);
+        }
+        else
+        {
+            // Create first waypoint at current MAV position
+            addCurrentPositonWaypoint();
         }
     }
 }
 
 void WaypointList::addCurrentPositonWaypoint()
 {
-    /* TODO: implement with new waypoint structure
     if (uas)
     {
-        // For Global Waypoints
-        //if(isGlobalWP)
-        //{
-            //isLocalWP = false;
-        //}
-        //else
+        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
+        Waypoint *wp;
+        Waypoint *last = 0;
+        if (waypoints.size() > 0)
         {
-            const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
-            if (waypoints.size() > 0)
+            last = waypoints.at(waypoints.size()-1);
+        }
+
+        if (uas->globalPositionKnown())
+        {
+            float acceptanceRadiusGlobal = 10.0f;
+            float holdTime = 0.0f;
+            float yawGlobal = 0.0f;
+            if (last)
             {
-                Waypoint *last = waypoints.at(waypoints.size()-1);
-                Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
-                uas->getWaypointManager()->addWaypoint(wp);
+                acceptanceRadiusGlobal = last->getAcceptanceRadius();
+                holdTime = last->getHoldTime();
+                yawGlobal = last->getYaw();
             }
-            else
+            // Create global frame waypoint per default
+            wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), yawGlobal, acceptanceRadiusGlobal, holdTime, 0.0, true, true, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT);
+            uas->getWaypointManager()->addWaypoint(wp);
+            updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint"));
+        }
+        else if (uas->localPositionKnown())
+        {
+            float acceptanceRadiusLocal = 0.2f;
+            float holdTime = 0.5f;
+            if (last)
             {
-                Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000);
-                uas->getWaypointManager()->addWaypoint(wp);
+                acceptanceRadiusLocal = last->getAcceptanceRadius();
+                holdTime = last->getHoldTime();
             }
-
-             //isLocalWP = true;
+            // Create local frame waypoint as second option
+            wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT);
+            uas->getWaypointManager()->addWaypoint(wp);
+            updateStatusLabel(tr("Added LOCAL (NED) waypoint"));
+        }
+        else
+        {
+            // Do nothing
+            updateStatusLabel(tr("Not adding waypoint, no position known of MAV known yet."));
         }
     }
-    */
 }
 
 void WaypointList::updateStatusLabel(const QString &string)
@@ -223,23 +243,30 @@ void WaypointList::updateStatusLabel(const QString &string)
 
 void WaypointList::changeCurrentWaypoint(quint16 seq)
 {
-    if (uas) {
+    if (uas)
+    {
         uas->getWaypointManager()->setCurrentWaypoint(seq);
     }
 }
 
 void WaypointList::currentWaypointChanged(quint16 seq)
 {
-    if (uas) {
+    if (uas)
+    {
         const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
 
-        if (seq < waypoints.size()) {
-            for(int i = 0; i < waypoints.size(); i++) {
+        if (seq < waypoints.size())
+        {
+            for(int i = 0; i < waypoints.size(); i++)
+            {
                 WaypointView* widget = wpViews.find(waypoints[i]).value();
 
-                if (waypoints[i]->getId() == seq) {
+                if (waypoints[i]->getId() == seq)
+                {
                     widget->setCurrent(true);
-                } else {
+                }
+                else
+                {
                     widget->setCurrent(false);
                 }
             }
diff --git a/src/ui/designer/QGCToolWidget.cc b/src/ui/designer/QGCToolWidget.cc
index 9a3fb8ec26545aac87a753bbb5f2a673209d74e0..e94073785d0439bdc5c44d42584fcf77c7398f11 100644
--- a/src/ui/designer/QGCToolWidget.cc
+++ b/src/ui/designer/QGCToolWidget.cc
@@ -18,6 +18,7 @@ QGCToolWidget::QGCToolWidget(const QString& title, QWidget *parent) :
         QWidget(parent),
         mav(NULL),
         mainMenuAction(NULL),
+        widgetTitle(title),
         ui(new Ui::QGCToolWidget)
 {
     ui->setupUi(this);
@@ -41,6 +42,7 @@ QGCToolWidget::QGCToolWidget(const QString& title, QWidget *parent) :
     }
 
     this->setWindowTitle(title);
+    setObjectName(title+"WIDGET");
 
     QList<UASInterface*> systems = UASManager::instance()->getUASList();
     foreach (UASInterface* uas, systems) {
@@ -58,6 +60,18 @@ QGCToolWidget::~QGCToolWidget()
     delete ui;
 }
 
+void QGCToolWidget::setParent(QWidget *parent)
+{
+    QWidget::setParent(parent);
+    // Try with parent
+    QDockWidget* dock = dynamic_cast<QDockWidget*>(parent);
+    if (dock)
+    {
+        dock->setWindowTitle(getTitle());
+        dock->setObjectName(getTitle()+"DOCK");
+    }
+}
+
 /**
  * @param parent Object later holding these widgets, usually the main window
  * @return List of all widgets
@@ -107,14 +121,26 @@ QList<QGCToolWidget*> QGCToolWidget::createWidgetsFromSettings(QWidget* parent,
     return instances()->values();
 }
 
-void QGCToolWidget::loadSettings(const QString& settings)
+/**
+ * @param singleinstance If this is set to true, the widget settings will only be loaded if not another widget with the same title exists
+ */
+bool QGCToolWidget::loadSettings(const QString& settings, bool singleinstance)
 {
     QSettings set(settings, QSettings::IniFormat);
     QStringList groups = set.childGroups();
-    QString widgetName = groups.first();
-    setTitle(widgetName);
-    qDebug() << "WIDGET TITLE LOADED: " << widgetName;
-    loadSettings(set);
+    if (groups.length() > 0)
+    {
+        QString widgetName = groups.first();
+        if (singleinstance && QGCToolWidget::instances()->keys().contains(widgetName)) return false;
+        setTitle(widgetName);
+        qDebug() << "WIDGET TITLE LOADED: " << widgetName;
+        loadSettings(set);
+        return true;
+    }
+    else
+    {
+        return false;
+    }
 }
 
 void QGCToolWidget::loadSettings(QSettings& settings)
@@ -339,15 +365,9 @@ void QGCToolWidget::importWidget()
 
 const QString QGCToolWidget::getTitle()
 {
-    QDockWidget* parent = dynamic_cast<QDockWidget*>(this->parentWidget());
-    if (parent) {
-        return parent->windowTitle();
-    } else {
-        return this->windowTitle();
-    }
+    return widgetTitle;
 }
 
-
 void QGCToolWidget::setTitle()
 {
     QDockWidget* parent = dynamic_cast<QDockWidget*>(this->parentWidget());
@@ -373,6 +393,7 @@ void QGCToolWidget::setTitle()
 
 void QGCToolWidget::setTitle(QString title)
 {
+    widgetTitle = title;
     QDockWidget* parent = dynamic_cast<QDockWidget*>(this->parentWidget());
     if (parent) {
         QSettings settings;
diff --git a/src/ui/designer/QGCToolWidget.h b/src/ui/designer/QGCToolWidget.h
index 81c9cf4eb10edaff9bdb5026359aa48892500714..ac3de309a6d8cd3da1a253e7200e45a3921d0928 100644
--- a/src/ui/designer/QGCToolWidget.h
+++ b/src/ui/designer/QGCToolWidget.h
@@ -33,6 +33,7 @@ public:
 
     int isVisible(int view) { return viewVisible.value(view, false); }
     Qt::DockWidgetArea getDockWidgetArea(int view) { return dockWidgetArea.value(view, Qt::BottomDockWidgetArea); }
+    void setParent(QWidget *parent);
 
 public slots:
     void addUAS(UASInterface* uas);
@@ -47,7 +48,7 @@ public slots:
     /** @brief Load this widget from a QSettings object */
     void loadSettings(QSettings& settings);
     /** @brief Load this widget from a settings file */
-    void loadSettings(const QString& settings);
+    bool loadSettings(const QString& settings, bool singleinstance=false);
     /** @brief Store this widget to a QSettings object */
     void storeSettings(QSettings& settings);
     /** @brief Store this widget to a settings file */
@@ -71,6 +72,7 @@ protected:
     QAction* mainMenuAction;             ///< Main menu action
     QMap<int, Qt::DockWidgetArea> dockWidgetArea;   ///< Dock widget area desired by this widget
     QMap<int, bool> viewVisible;  ///< Visibility in one view
+    QString widgetTitle;
 
     void contextMenuEvent(QContextMenuEvent* event);
     void createActions();
diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc
index 8c04d930bb8bf1b136c1cea7437aa9597867ea09..a46e56895d4cb2fd65b4ae4114d3b120917ed99d 100644
--- a/src/ui/linechart/LinechartPlot.cc
+++ b/src/ui/linechart/LinechartPlot.cc
@@ -417,9 +417,12 @@ void LinechartPlot::setVisible(QString id, bool visible)
 {
     if(curves.contains(id)) {
         curves.value(id)->setVisible(visible);
-        if(visible) {
+        if(visible)
+        {
             curves.value(id)->attach(this);
-        } else {
+        }
+        else
+        {
             curves.value(id)->detach();
         }
     }
@@ -639,7 +642,7 @@ void LinechartPlot::paintRealtime()
         windowLock.unlock();
 
         // Defined both on windows 32- and 64 bit
-#ifndef _WIN32
+#if !(defined Q_OS_WIN)
 
         //    const bool cacheMode =
         //            canvas()->testPaintAttribute(QwtPlotCanvas::PaintCached);
@@ -648,11 +651,11 @@ void LinechartPlot::paintRealtime()
 
         const QPaintEngine *pe = canvas()->paintEngine();
         bool directPaint = pe->hasFeature(QPaintEngine::PaintOutsidePaintEvent);
-        if ( pe->type() == QPaintEngine::X11 ) {
+        //if ( pe->type() == QPaintEngine::X11 ) {
             // Even if not recommended by TrollTech, Qt::WA_PaintOutsidePaintEvent
             // works on X11. This has an tremendous effect on the performance..
             directPaint = true;
-        }
+        //}
         canvas()->setAttribute(Qt::WA_PaintOutsidePaintEvent, directPaint);
 #endif
 
@@ -665,7 +668,7 @@ void LinechartPlot::paintRealtime()
             replot();
         }
 
-#ifndef _WIN32
+#if !(defined Q_OS_WIN)
         canvas()->setAttribute(Qt::WA_PaintOutsidePaintEvent, oldDirectPaint);
 #endif
 
diff --git a/src/ui/linechart/LinechartPlot.h b/src/ui/linechart/LinechartPlot.h
index dac11fc5e0d9492c70430042231e7e13847b251e..ec4128c6efc94121a9b59eb27265b1aea4b28f41 100644
--- a/src/ui/linechart/LinechartPlot.h
+++ b/src/ui/linechart/LinechartPlot.h
@@ -219,7 +219,7 @@ public:
     static const int SCALE_BEST_FIT = 1;
     static const int SCALE_LOGARITHMIC = 2;
 
-    static const int DEFAULT_REFRESH_RATE = 50; ///< The default refresh rate is 25 Hz / every 100 ms
+    static const int DEFAULT_REFRESH_RATE = 100; ///< The default refresh rate is 10 Hz / every 100 ms
     static const int DEFAULT_PLOT_INTERVAL = 1000 * 8; ///< The default plot interval is 15 seconds
     static const int DEFAULT_SCALE_INTERVAL = 1000 * 8;
 
diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc
index 9ea49a887b027cdae144c4db09cef1d149056195..023d00039df430e11018b06b3c292a9ac057f1b7 100644
--- a/src/ui/linechart/LinechartWidget.cc
+++ b/src/ui/linechart/LinechartWidget.cc
@@ -143,7 +143,7 @@ LinechartWidget::LinechartWidget(int systemid, QWidget *parent) : QWidget(parent
     //connect(this, SIGNAL(plotWindowPositionUpdated(int)), scrollbar, SLOT(setValue(int)));
     //connect(scrollbar, SIGNAL(sliderMoved(int)), this, SLOT(setPlotWindowPosition(int)));
 
-    updateTimer->setInterval(300);
+    updateTimer->setInterval(updateInterval);
     connect(updateTimer, SIGNAL(timeout()), this, SLOT(refresh()));
     readSettings();
 }
@@ -433,6 +433,7 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString&
 
 void LinechartWidget::refresh()
 {
+    setUpdatesEnabled(false);
     QString str;
     // Value
     QMap<QString, QLabel*>::iterator i;
@@ -484,6 +485,7 @@ void LinechartWidget::refresh()
         str.sprintf("% 8.3e", activePlot->getVariance(l.key()));
         l.value()->setText(str);
     }
+    setUpdatesEnabled(true);
 }
 
 
@@ -510,11 +512,6 @@ void LinechartWidget::startLogging()
     // QString("./pixhawk-log-" + date.toString("yyyy-MM-dd") + "-" + QString::number(logindex) + ".log")
     QString fileName = QFileDialog::getSaveFileName(this, tr("Specify log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("Logfile (*.csv *.txt);;"));
 
-    if (!fileName.contains(".")) {
-        // .csv is default extension
-        fileName.append(".csv");
-    }
-
     while (!(fileName.endsWith(".txt") || fileName.endsWith(".csv")) && !abort && fileName != "") {
         QMessageBox msgBox;
         msgBox.setIcon(QMessageBox::Critical);
@@ -522,14 +519,16 @@ void LinechartWidget::startLogging()
         msgBox.setInformativeText("Please choose .txt or .csv as file extension. Click OK to change the file extension, cancel to not start logging.");
         msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel);
         msgBox.setDefaultButton(QMessageBox::Ok);
-        if(msgBox.exec() == QMessageBox::Cancel) {
+        if(msgBox.exec() != QMessageBox::Ok)
+        {
             abort = true;
             break;
         }
-        fileName = QFileDialog::getSaveFileName(this, tr("Specify log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("Logfile (*.txt, *.csv);;"));
-
+        fileName = QFileDialog::getSaveFileName(this, tr("Specify log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("Logfile (*.txt *.csv);;"));
     }
 
+    qDebug() << "SAVE FILE" << fileName;
+
     // Check if the user did not abort the file save dialog
     if (!abort && fileName != "") {
         logFile = new QFile(fileName);
@@ -626,6 +625,7 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit)
     curvesWidgetLayout->addWidget(checkBox, labelRow, 0);
 
     QWidget* colorIcon = new QWidget(this);
+    colorIcons.insert(curve+unit, colorIcon);
     colorIcon->setMinimumSize(QSize(5, 14));
     colorIcon->setMaximumSize(4, 14);
 
@@ -636,13 +636,11 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit)
 
     //checkBox->setText(QString());
     label->setText(curve);
-    QColor color = plot->getColorForCurve(curve+unit);
-    if(color.isValid()) {
-        QString colorstyle;
-        colorstyle = colorstyle.sprintf("QWidget { background-color: #%X%X%X; }", color.red(), color.green(), color.blue());
-        colorIcon->setStyleSheet(colorstyle);
-        colorIcon->setAutoFillBackground(true);
-    }
+    QColor color(Qt::gray);// = plot->getColorForCurve(curve+unit);
+    QString colorstyle;
+    colorstyle = colorstyle.sprintf("QWidget { background-color: #%X%X%X; }", color.red(), color.green(), color.blue());
+    colorIcon->setStyleSheet(colorstyle);
+    colorIcon->setAutoFillBackground(true);
 
     // Value
     value = new QLabel(this);
@@ -854,8 +852,22 @@ void LinechartWidget::takeButtonClick(bool checked)
 
     QCheckBox* button = qobject_cast<QCheckBox*>(QObject::sender());
 
-    if(button != NULL) {
+    if(button != NULL)
+    {
         activePlot->setVisible(button->objectName(), checked);
+
+        QColor color = activePlot->getColorForCurve(button->objectName());
+        if(color.isValid())
+        {
+            QString colorstyle;
+            colorstyle = colorstyle.sprintf("QWidget { background-color: #%X%X%X; }", color.red(), color.green(), color.blue());
+            QWidget* colorIcon = colorIcons.value(button->objectName(), 0);
+            if (colorIcon)
+            {
+                colorIcon->setStyleSheet(colorstyle);
+                colorIcon->setAutoFillBackground(true);
+            }
+        }
     }
 }
 
diff --git a/src/ui/linechart/LinechartWidget.h b/src/ui/linechart/LinechartWidget.h
index 4e33d52ed3ec9f67d6f12308ea27c8a06bde27d1..77b28afb28b944afbab3638dad049c21f3026909 100644
--- a/src/ui/linechart/LinechartWidget.h
+++ b/src/ui/linechart/LinechartWidget.h
@@ -134,6 +134,7 @@ protected:
     QMap<QString, QLabel*>* curveMedians; ///< References to the curve medians
     QMap<QString, QLabel*>* curveVariances; ///< References to the curve variances
     QMap<QString, int> intData;           ///< Current values for integer-valued curves
+    QMap<QString, QWidget*> colorIcons;    ///< Reference to color icons
 
     QWidget* curvesWidget;                ///< The QWidget containing the curve selection button
     QGridLayout* curvesWidgetLayout;      ///< The layout for the curvesWidget QWidget
@@ -161,7 +162,7 @@ protected:
     LogCompressor* compressor;
     QCheckBox* selectAllCheckBox;
     int selectedMAV; ///< The MAV for which plot items are accepted, -1 for all systems
-    static const int updateInterval = 400; ///< Time between number updates, in milliseconds
+    static const int updateInterval = 1000; ///< Time between number updates, in milliseconds
 
     static const int MAX_CURVE_MENUITEM_NUMBER = 8;
     static const int PAGESTEP_TIME_SCROLLBAR_VALUE = (MAX_TIME_SCROLLBAR_VALUE - MIN_TIME_SCROLLBAR_VALUE) / 10;
diff --git a/src/ui/linechart/Linecharts.cc b/src/ui/linechart/Linecharts.cc
index be59afe9910bc347f6f67eef254b1abd1c11438e..b7f954d578ca8c3b2e04a123bcb6f84726c6b7e1 100644
--- a/src/ui/linechart/Linecharts.cc
+++ b/src/ui/linechart/Linecharts.cc
@@ -30,32 +30,35 @@ void Linecharts::showEvent(QShowEvent* event)
 {
     // React only to internal (pre-display)
     // events
-    Q_UNUSED(event) {
-        QWidget* prevWidget = currentWidget();
-        if (prevWidget) {
-            LinechartWidget* chart = dynamic_cast<LinechartWidget*>(prevWidget);
-            if (chart) {
-                this->active = true;
-                chart->setActive(true);
-            }
+    Q_UNUSED(event)
+    QWidget* prevWidget = currentWidget();
+    if (prevWidget)
+    {
+        LinechartWidget* chart = dynamic_cast<LinechartWidget*>(prevWidget);
+        if (chart) {
+            this->active = true;
+            chart->setActive(true);
         }
     }
+    QWidget::showEvent(event);
+    emit visibilityChanged(true);
 }
 
 void Linecharts::hideEvent(QHideEvent* event)
 {
     // React only to internal (pre-display)
     // events
-    Q_UNUSED(event) {
-        QWidget* prevWidget = currentWidget();
-        if (prevWidget) {
-            LinechartWidget* chart = dynamic_cast<LinechartWidget*>(prevWidget);
-            if (chart) {
-                this->active = false;
-                chart->setActive(false);
-            }
+    Q_UNUSED(event)
+    QWidget* prevWidget = currentWidget();
+    if (prevWidget) {
+        LinechartWidget* chart = dynamic_cast<LinechartWidget*>(prevWidget);
+        if (chart) {
+            this->active = false;
+            chart->setActive(false);
         }
     }
+    QWidget::hideEvent(event);
+    emit visibilityChanged(false);
 }
 
 void Linecharts::selectSystem(int systemid)
diff --git a/src/ui/linechart/Linecharts.h b/src/ui/linechart/Linecharts.h
index 4d5d26b20b334565620672de2af0597acad2dc40..20474b760999e3a98f42832aceab582462ba60fb 100644
--- a/src/ui/linechart/Linecharts.h
+++ b/src/ui/linechart/Linecharts.h
@@ -17,6 +17,7 @@ public:
 signals:
     /** @brief This signal is emitted once a logfile has been finished writing */
     void logfileWritten(QString fileName);
+    void visibilityChanged(bool visible);
 
 public slots:
     /** @brief Select plot for one system */
@@ -27,6 +28,7 @@ public slots:
     void addSource(QObject* obj);
 
 protected:
+
     QMap<int, LinechartWidget*> plots;
     QVector<QObject*> genericSources;
     bool active;
diff --git a/src/ui/map/QGCMapTool.h b/src/ui/map/QGCMapTool.h
index 784bf786252a8e9b61a6a730eb169e7d1df55b5a..ca0e33ad917a89df4acd1e5c6cf182761439296b 100644
--- a/src/ui/map/QGCMapTool.h
+++ b/src/ui/map/QGCMapTool.h
@@ -20,6 +20,22 @@ public slots:
     /** @brief Update slider zoom from map change */
     void setZoom(int zoom);
 
+signals:
+    void visibilityChanged(bool visible);
+
+protected:
+    void showEvent(QShowEvent* event)
+    {
+        QWidget::showEvent(event);
+        emit visibilityChanged(true);
+    }
+
+    void hideEvent(QHideEvent* event)
+    {
+        QWidget::hideEvent(event);
+        emit visibilityChanged(false);
+    }
+
 private:
     Ui::QGCMapTool *ui;
 };
diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc
index 2c16bb010a0b2125b23a81226c030a3fe27ff98b..67550c75581a6c31a2e4b78d65f7bc3c5fbe604c 100644
--- a/src/ui/map3D/QGCGoogleEarthView.cc
+++ b/src/ui/map3D/QGCGoogleEarthView.cc
@@ -376,18 +376,20 @@ void QGCGoogleEarthView::moveToPosition()
 
 void QGCGoogleEarthView::hideEvent(QHideEvent* event)
 {
-    Q_UNUSED(event);
     updateTimer->stop();
+    QWidget::hideEvent(event);
+    emit visibilityChanged(false);
 }
 
 void QGCGoogleEarthView::showEvent(QShowEvent* event)
 {
     // React only to internal (pre-display)
     // events
-    Q_UNUSED(event)
+    QWidget::showEvent(event);
     // Enable widget, initialize on first run
 
-    if (!webViewInitialized) {
+    if (!webViewInitialized)
+    {
 #if (defined Q_OS_MAC)
         webViewMac->setPage(new QGCWebPage(webViewMac));
         webViewMac->settings()->setAttribute(QWebSettings::PluginsEnabled, true);
@@ -404,9 +406,12 @@ void QGCGoogleEarthView::showEvent(QShowEvent* event)
         gEarthInitialized = false;
 
         QTimer::singleShot(3000, this, SLOT(initializeGoogleEarth()));
-    } else {
+    }
+    else
+    {
         updateTimer->start(refreshRateMs);
     }
+    emit visibilityChanged(true);
 }
 
 void QGCGoogleEarthView::printWinException(int no, QString str1, QString str2, QString str3)
diff --git a/src/ui/map3D/QGCGoogleEarthView.h b/src/ui/map3D/QGCGoogleEarthView.h
index b572678bce8f65b1a078fda6a0640e02fa550e6c..117239aa4f9bf92880635e6fac7717409f8fa324 100644
--- a/src/ui/map3D/QGCGoogleEarthView.h
+++ b/src/ui/map3D/QGCGoogleEarthView.h
@@ -137,6 +137,9 @@ public:
     /** @brief Get a document element */
     QVariant documentElement(QString name);
 
+signals:
+    void visibilityChanged(bool visible);
+
 protected:
     void changeEvent(QEvent *e);
     QTimer* updateTimer;