diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 3d9468c1f3220637a82d3d274d1a34645e6f3acf..5579dc04244f15292b712ee5f5e525b05aad173e 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -139,7 +139,6 @@ FORMS += src/ui/MainWindow.ui \
     src/ui/WaypointGlobalView.ui \
     src/ui/SlugsDataSensorView.ui \
     src/ui/SlugsHilSim.ui \
-    src/ui/SlugsDataSensorView.ui \
     src/ui/SlugsPIDControl.ui \
     src/ui/SlugsVideoCamControl.ui
 INCLUDEPATH += src \
diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc
index 71efcfe33a742bbc593336dd8931432732226cfb..5e7a6df3b4b3ce68255753022be81be4c315d8e8 100644
--- a/src/comm/SerialLink.cc
+++ b/src/comm/SerialLink.cc
@@ -89,7 +89,8 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P
     }
 #else
     // *nix (Linux, MacOS tested) serial port support
-    port = new QextSerialPort(porthandle, QextSerialPort::Polling);
+    //port = new QextSerialPort(porthandle, QextSerialPort::Polling);
+    port = new QextSerialPort(porthandle, QextSerialPort::EventDriven);
     port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time
     port->setBaudRate(baudrate);
     port->setFlowControl(flow);
diff --git a/src/configuration.h b/src/configuration.h
index a8c70a607173ecf65c732aec825cf668a656cb09..3420e0f39a6bf6678f63ee613042781b1df6142f 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -1,8 +1,10 @@
 #ifndef CONFIGURATION_H
 #define CONFIGURATION_H
 
+#include "mavlink.h"
+
 /** @brief Polling interval in ms */
-#ifdef MAVLINK_ENABLED_SLUGS_MESSAGES_QGC
+#ifdef MAVLINK_ENABLED_SLUGS
   #define SERIAL_POLL_INTERVAL 7
 #else
   #define SERIAL_POLL_INTERVAL 2
diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc
index 877d1beb4bd731d94ce783805f476ad43702eca2..19beb486adb624593f8518e69d669d2b3624d4d5 100644
--- a/src/uas/SlugsMAV.cc
+++ b/src/uas/SlugsMAV.cc
@@ -6,7 +6,46 @@ SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) :
         UAS(mavlink, id)//,
         // Place other initializers here
 {
+  widgetTimer = new QTimer (this);
+  widgetTimer->setInterval(SLUGS_UPDATE_RATE);
 
+  connect (widgetTimer, SIGNAL(timeout()),
+           this, SLOT(emitSignals()));
+
+  widgetTimer->start();
+
+  // clear all the state structures
+   memset(&mlRawImuData ,0, sizeof(mavlink_raw_imu_t));
+
+#ifdef MAVLINK_ENABLED_SLUGS
+
+
+    memset(&mlGpsData, 0, sizeof(mavlink_gps_raw_t));
+    memset(&mlCpuLoadData, 0, sizeof(mavlink_cpu_load_t));
+    memset(&mlAirData, 0, sizeof(mavlink_air_data_t));
+    memset(&mlSensorBiasData, 0, sizeof(mavlink_sensor_bias_t));
+    memset(&mlDiagnosticData, 0, sizeof(mavlink_diagnostic_t));
+    memset(&mlPilotConsoleData, 0, sizeof(mavlink_pilot_console_t));
+    memset(&mlFilteredData ,0, sizeof(mavlink_filtered_data_t));
+    memset(&mlBoot ,0, sizeof(mavlink_boot_t));
+    memset(&mlGpsDateTime ,0, sizeof(mavlink_gps_date_time_t));
+    memset(&mlApMode ,0, sizeof(mavlink_set_mode_t));
+    memset(&mlPwmCommands ,0, sizeof(mavlink_pwm_commands_t));
+    memset(&mlPidValues ,0, sizeof(mavlink_pid_values_t));
+    memset(&mlSinglePid ,0, sizeof(mavlink_pid_t));
+    memset(&mlNavigation ,0, sizeof(mavlink_slugs_navigation_t));
+    memset(&mlDataLog ,0, sizeof(mavlink_data_log_t));
+    memset(&mlPassthrough ,0, sizeof(mavlink_ctrl_srfc_pt_t));
+    memset(&mlActionAck,0, sizeof(mavlink_action_ack_t));
+    memset(&mlAction ,0, sizeof(mavlink_slugs_action_t));
+
+    updateRoundRobin = 0;
+
+
+
+    uasId = id;
+
+  #endif
 }
 
 /**
@@ -20,336 +59,186 @@ SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) :
 void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
 {
     // Let UAS handle the default message set
-    UAS::receiveMessage(link, message);
+    //UAS::receiveMessage(link, message);
+
 
     // Handle your special messages mavlink_message_t* msg = &message;
     switch (message.msgid)
     {
-    case MAVLINK_MSG_ID_HEARTBEAT:
-        {
-            qDebug() << "SLUGS RECEIVED HEARTBEAT";
-            break;
-        }
-
-#ifdef MAVLINK_ENABLED_SLUGS
 
-    case MAVLINK_MSG_ID_CPU_LOAD://170
-        {
-            mavlink_cpu_load_t cpu_load;
-            quint64 time = getUnixTime(0);
-            mavlink_msg_cpu_load_decode(&message,&cpu_load);
-
-                emit valueChanged(uasId, tr("SensorDSC Load"), cpu_load.sensLoad, time);
-                emit valueChanged(uasId, tr("ControlDSC Load"), cpu_load.ctrlLoad, time);
-                emit valueChanged(uasId, tr("Battery Volt"), cpu_load.batVolt, time);
-
-                emit slugsCPULoad(uasId,
-                                  cpu_load.sensLoad,
-                                  cpu_load.ctrlLoad,
-                                  cpu_load.batVolt,
-                                  0);
-//qDebug()<<"--------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_CPU_LOAD 170";
-            break;
-        }
-    case MAVLINK_MSG_ID_AIR_DATA://171
-        {
-            mavlink_air_data_t air_data;
-            quint64 time = getUnixTime(0);
-            mavlink_msg_air_data_decode(&message,&air_data);
-                emit valueChanged(uasId, tr("Dynamic Pressure"), air_data.dynamicPressure,time);
-                emit valueChanged(uasId, tr("Static Pressure"),air_data.staticPressure, time);
-                emit valueChanged(uasId, tr("Temp"),air_data.temperature,time);
-
-                emit slugsAirData(uasId,
-                                  air_data.dynamicPressure,
-                                  air_data.staticPressure,
-                                  air_data.temperature,
-                                  time);
-//qDebug()<<"--------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_AIR_DATA 171";
-            break;
-        }
-    case MAVLINK_MSG_ID_SENSOR_BIAS://172
-        {
-            mavlink_sensor_bias_t sensor_bias;
-            quint64 time = getUnixTime(0);
-            mavlink_msg_sensor_bias_decode(&message,&sensor_bias);
-                emit valueChanged(uasId,tr("Ax Bias"),sensor_bias.axBias, time);
-                emit valueChanged(uasId,tr("Ay Bias"),sensor_bias.ayBias,time);
-                emit valueChanged(uasId,tr("Az Bias"),sensor_bias.azBias,time);
-                emit valueChanged(uasId,tr("Gx Bias"),sensor_bias.gxBias,time);
-                emit valueChanged(uasId,tr("Gy Bias"),sensor_bias.gyBias,time);
-                emit valueChanged(uasId,tr("Gz Bias"),sensor_bias.gzBias,time);
-
-                emit slugsSensorBias(uasId,
-                                     sensor_bias.axBias,
-                                     sensor_bias.ayBias,
-                                     sensor_bias.azBias,
-                                     sensor_bias.gxBias,
-                                     sensor_bias.gyBias,
-                                     sensor_bias.gzBias,
-                                     0);
-          //     qDebug()<<"------------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_SENSOR_BIAS 172";
 
 
-            break;
-        }
-    case MAVLINK_MSG_ID_DIAGNOSTIC://173
+    case MAVLINK_MSG_ID_HEARTBEAT:
+        emit heartbeat(this);
+        // Set new type if it has changed
+        if (this->type != mavlink_msg_heartbeat_get_type(&message))
         {
-            mavlink_diagnostic_t diagnostic;
-            quint64 time = getUnixTime(0);
-            mavlink_msg_diagnostic_decode(&message,&diagnostic);
-                emit valueChanged(uasId,tr("Diag F1"),diagnostic.diagFl1,time);
-                emit valueChanged(uasId,tr("Diag F2"),diagnostic.diagFl2,time);
-                emit valueChanged(uasId,tr("Diag F3"),diagnostic.diagFl3,time);
-                emit valueChanged(uasId,tr("Diag S1"),diagnostic.diagSh1,time);
-                emit valueChanged(uasId,tr("Diag S2"),diagnostic.diagSh2,time);
-                emit valueChanged(uasId,tr("Diag S3"),diagnostic.diagSh3,time);
-
-                emit slugsDiagnostic(uasId,
-                                     diagnostic.diagFl1,
-                                     diagnostic.diagFl2,
-                                     diagnostic.diagFl3,
-                                     diagnostic.diagSh1,
-                                     diagnostic.diagSh2,
-                                     diagnostic.diagSh3,
-                                     0);
-//qDebug()<<"------->>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_DIAGNOSTIC 173";
-            break;
+            this->type = mavlink_msg_heartbeat_get_type(&message);
+            emit systemTypeSet(this, type);
         }
-    case MAVLINK_MSG_ID_PILOT_CONSOLE://174
-        {
-            mavlink_pilot_console_t pilot;
-            quint64 time = getUnixTime(0);
-            mavlink_msg_pilot_console_decode(&message,&pilot);
-                emit valueChanged(uasId,"dt",pilot.dt,time);
-                emit valueChanged(uasId,"dla",pilot.dla,time);
-                emit valueChanged(uasId,"dra",pilot.dra,time);
-                emit valueChanged(uasId,"dr",pilot.dr,time);
-                emit valueChanged(uasId,"de",pilot.de,time);
-
-                emit slugsPilotConsolePWM(uasId,
-                                          pilot.dt,
-                                          pilot.dla,
-                                          pilot.dra,
-                                          pilot.dr,
-                                          pilot.de,
-                                          0);
-             //   qDebug()<<"---------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_PILOT_CONSOLE 174";
-
+      break;
 
+      case MAVLINK_MSG_ID_RAW_IMU:
+        mavlink_msg_raw_imu_decode(&message, &mlRawImuData);
             break;
-        }
-    case MAVLINK_MSG_ID_PWM_COMMANDS://175
-        {
-            mavlink_pwm_commands_t pwm;
-            quint64 time = getUnixTime(0);
-            mavlink_msg_pwm_commands_decode(&message,&pwm);
-                emit valueChanged(uasId,"dt_c",pwm.dt_c,time);
-                emit valueChanged(uasId,"dla_c",pwm.dla_c,time);
-                emit valueChanged(uasId,"dra_c",pwm.dra_c,time);
-                emit valueChanged(uasId,"dr_c",pwm.dr_c,time);
-                emit valueChanged(uasId,"dle_c",pwm.dle_c,time);
-                emit valueChanged(uasId,"dre_c",pwm.dre_c,time);
-                emit valueChanged(uasId,"dlf_c",pwm.dlf_c,time);
-                emit valueChanged(uasId,"drf_c",pwm.drf_c,time);
-                emit valueChanged(uasId,"da1_c",pwm.aux1,time);
-                emit valueChanged(uasId,"da2_c",pwm.aux2,time);
-
-                emit slugsPWM(uasId,
-                              pwm.dt_c,
-                              pwm.dla_c,
-                              pwm.dra_c,
-                              pwm.dr_c,
-                              pwm.dle_c,
-                              pwm.dre_c,
-                              pwm.dlf_c,
-                              pwm.drf_c,
-                              pwm.aux1,
-                              pwm.aux2,
-                              0);
-//qDebug()<<"----------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_PWM_COMMANDS 175";
 
-            break;
-        }
-    case MAVLINK_MSG_ID_SLUGS_NAVIGATION://176
-        {
-            mavlink_slugs_navigation_t nav;
-            quint64 time = getUnixTime(0);
-            mavlink_msg_slugs_navigation_decode(&message,&nav);
-                emit valueChanged(uasId,"u_m",nav.u_m,time);
-                emit valueChanged(uasId,"phi_c",nav.phi_c,time);
-                emit valueChanged(uasId,"theta_c",nav.theta_c,time);
-                emit valueChanged(uasId,"psiDot_c",nav.psiDot_c,time);
-                emit valueChanged(uasId,"ay_body",nav.ay_body,time);
-                emit valueChanged(uasId,"totalDist",nav.totalDist,time);
-                emit valueChanged(uasId,"dist2Go",nav.dist2Go,time);
-                emit valueChanged(uasId,"fromWP",nav.fromWP,time);
-                emit valueChanged(uasId,"toWP",nav.toWP,time);
-
-                emit slugsNavegation(uasId,
-                                     nav.u_m,
-                                     nav.phi_c,
-                                     nav.theta_c,
-                                     nav.psiDot_c,
-                                     nav.ay_body,
-                                     nav.totalDist,
-                                     nav.dist2Go,
-                                     nav.fromWP,
-                                     nav.toWP,
-                                     time);
-
-           // qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_SLUGS_NAVIGATION 176";
-
-            break;
-        }
-    case MAVLINK_MSG_ID_DATA_LOG://177
-        {
-            mavlink_data_log_t dataLog;
-            quint64 time = getUnixTime(0);
-            mavlink_msg_data_log_decode(&message,&dataLog);
-                emit valueChanged(uasId,"fl_1",dataLog.fl_1,time);
-                emit valueChanged(uasId,"fl_2",dataLog.fl_2,time);
-                emit valueChanged(uasId,"fl_3",dataLog.fl_3,time);
-                emit valueChanged(uasId,"fl_4",dataLog.fl_4,time);
-                emit valueChanged(uasId,"fl_5",dataLog.fl_5,time);
-                emit valueChanged(uasId,"fl_6",dataLog.fl_6,time);
+#ifdef MAVLINK_ENABLED_SLUGS
 
-                emit slugsDataLog(uasId,
-                                  dataLog.fl_1,
-                                  dataLog.fl_2,
-                                  dataLog.fl_3,
-                                  dataLog.fl_4,
-                                  dataLog.fl_5,
-                                  dataLog.fl_6,
-                                  time);
+      case MAVLINK_MSG_ID_BOOT:
+        mavlink_msg_boot_decode(&message,&mlBoot);
+        emit slugsBootMsg(uasId, mlBoot);
+      break;
 
-             //   qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO LOG DATA 177";
 
 
+      case MAVLINK_MSG_ID_ATTITUDE:
+        mavlink_msg_attitude_decode(&message, &mlAttitude);
+      break;
 
+      case MAVLINK_MSG_ID_GPS_RAW:
+        mavlink_msg_gps_raw_decode(&message, &mlGpsData);
             break;
-        }
-
-    case MAVLINK_MSG_ID_FILTERED_DATA://178
-        {
-
-            mavlink_filtered_data_t fil;
-            quint64 time = getUnixTime(0);
-            mavlink_msg_filtered_data_decode(&message,&fil);
-                emit valueChanged(uasId,"aX",fil.aX,time);
-                emit valueChanged(uasId,"aY",fil.aY,time);
-                emit valueChanged(uasId,"aZ",fil.aZ,time);
-                emit valueChanged(uasId,"gX",fil.gX,time);
-                emit valueChanged(uasId,"gY",fil.gY,time);
-                emit valueChanged(uasId,"gZ",fil.gZ,time);
-                emit valueChanged(uasId,"mX",fil.mX,time);
-                emit valueChanged(uasId,"mY",fil.mY,time);
-                emit valueChanged(uasId,"mZ",fil.mZ,time);
-
-                emit slugsFilteredData(uasId,
-                                       fil.aX,
-                                       fil.aY,
-                                       fil.aZ,
-                                       fil.gX,
-                                       fil.gY,
-                                       fil.gZ,
-                                       fil.mX,
-                                       fil.mY,
-                                       fil.mZ,
-                                       time);
-
 
+      case MAVLINK_MSG_ID_ACTION_ACK:      // 62
+        mavlink_msg_action_ack_decode(&message,&mlActionAck);
+            break;
 
-              //  qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO filtering data 178";
 
 
+      case MAVLINK_MSG_ID_CPU_LOAD:       //170
+        mavlink_msg_cpu_load_decode(&message,&mlCpuLoadData);
+            break;
 
+      case MAVLINK_MSG_ID_AIR_DATA:       //171
+        mavlink_msg_air_data_decode(&message,&mlAirData);
             break;
-        }
-    case MAVLINK_MSG_ID_GPS_DATE_TIME://179
-        {
-            mavlink_gps_date_time_t gps;
-            quint64 time = getUnixTime(0);
-            mavlink_msg_gps_date_time_decode(&message,&gps);
-                emit valueChanged(uasId,"year",gps.year,time);
-                emit valueChanged(uasId,"month",gps.month,time);
-                emit valueChanged(uasId,"day",gps.day,time);
-                emit valueChanged(uasId,"hour",gps.hour,time);
-                emit valueChanged(uasId,"min",gps.min,time);
-                emit valueChanged(uasId,"sec",gps.sec,time);
-                emit valueChanged(uasId,"visSat",gps.visSat,time);
 
+      case MAVLINK_MSG_ID_SENSOR_BIAS:    //172
+        mavlink_msg_sensor_bias_decode(&message,&mlSensorBiasData);
+      break;
 
-                emit slugsGPSDateTime(uasId,
-                                       gps.year,
-                                       gps.month,
-                                       gps.day,
-                                       gps.hour,
-                                       gps.min,
-                                       gps.sec,
-                                       gps.visSat,
-                                       time);
+      case MAVLINK_MSG_ID_DIAGNOSTIC:     //173
+        mavlink_msg_diagnostic_decode(&message,&mlDiagnosticData);
+      break;
 
+      case MAVLINK_MSG_ID_PILOT_CONSOLE:  //174
+        mavlink_msg_pilot_console_decode(&message,&mlPilotConsoleData);
+            break;
 
+      case MAVLINK_MSG_ID_PWM_COMMANDS:   //175
+        mavlink_msg_pwm_commands_decode(&message,&mlPwmCommands);
+      break;
 
-              //  qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_GPS_DATE_TIME 179";
+      case MAVLINK_MSG_ID_SLUGS_NAVIGATION://176
+        mavlink_msg_slugs_navigation_decode(&message,&mlNavigation);
+            break;
 
+      case MAVLINK_MSG_ID_DATA_LOG:       //177
+        mavlink_msg_data_log_decode(&message,&mlDataLog);
+      break;
 
+      case MAVLINK_MSG_ID_FILTERED_DATA:  //178
+        mavlink_msg_filtered_data_decode(&message,&mlFilteredData);
+      break;
 
+      case MAVLINK_MSG_ID_GPS_DATE_TIME:    //179
+        mavlink_msg_gps_date_time_decode(&message,&mlGpsDateTime);
             break;
-        }
-    case MAVLINK_MSG_ID_ACTION_ACK:
-        {
-            qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_ACTION_ACK";
-            mavlink_action_ack_t ack;
-
-            mavlink_msg_action_ack_decode(&message,&ack);
-            emit slugsActionAck(uasId,ack.action,ack.result);
-        }
-//    case MAVLINK_MSG_ID_MID_LVL_CMDS://180
-//        {
 
+      case MAVLINK_MSG_ID_MID_LVL_CMDS:     //180
 
-//             //   qDebug()<<"------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_MID_LVL_CMDS 180";
+      break;
 
+      case MAVLINK_MSG_ID_CTRL_SRFC_PT:     //181
 
+            break;
 
-//            break;
-//        }
-//    case MAVLINK_MSG_ID_CTRL_SRFC_PT://181
-//        {
+      case MAVLINK_MSG_ID_PID:              //182
 
+      break;
 
-//               // qDebug()<<"--------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_CTRL_SRFC_PT 181";
+      case MAVLINK_MSG_ID_SLUGS_ACTION:     //183
 
+      break;
 
+#endif
 
-//            break;
-//        }
-//    case MAVLINK_MSG_ID_PID://182
-//        {
+      default:
+//        qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid;
+            break;
+        }
+}
 
 
-//               // qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_PID 182";
 
+void SlugsMAV::emitSignals (void){
+#ifdef MAVLINK_ENABLED_SLUGS
+  switch(updateRoundRobin){
+    case 1:
+      emit slugsCPULoad(uasId, mlCpuLoadData);
+      emit slugsSensorBias(uasId,mlSensorBiasData);
+    break;
+
+    case 2:
+      emit slugsAirData(uasId, mlAirData);
+      emit slugsDiagnostic(uasId,mlDiagnosticData);
+    break;
+
+    case 3:
+      emit slugsPilotConsolePWM(uasId,mlPilotConsoleData);
+      emit slugsPWM(uasId, mlPwmCommands);
+    break;
+
+    case 4:
+      emit slugsNavegation(uasId, mlNavigation);
+      emit slugsDataLog(uasId, mlDataLog);
+    break;
+
+    case 5:
+      emit slugsFilteredData(uasId,mlFilteredData);
+      emit slugsGPSDateTime(uasId, mlGpsDateTime);
+            break;
 
+    case 6:
+      emit slugsActionAck(uasId,mlActionAck);
+      emit emitGpsSignals();
+    break;
+        }
 
-//            break;
-//        }
-//    case MAVLINK_MSG_ID_SLUGS_ACTION://183
-//        {
+  emit slugsAttitude(uasId, mlAttitude);
+  emit attitudeChanged(this,
+                       mlAttitude.roll,
+                       mlAttitude.pitch,
+                       mlAttitude.yaw,
+                       0.0);
+#endif
 
+  emit slugsRawImu(uasId, mlRawImuData);
 
-//               // qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_SLUGS_ACTION 183";
 
+  // wrap around
+  updateRoundRobin = updateRoundRobin > 10? 1: updateRoundRobin + 1;
 
 
-//            break;
-//        }
-#endif
+}
 
-    default:
-        //qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid;
-        break;
+#ifdef MAVLINK_ENABLED_SLUGS
+void SlugsMAV::emitGpsSignals (void){
+
+  if (mlGpsData.fix_type > 0){
+    emit globalPositionChanged(this,
+                               mlGpsData.lon,
+                               mlGpsData.lat,
+                               mlGpsData.alt,
+                               0.0);
+
+    // Smaller than threshold and not NaN
+    if (mlGpsData.v < 1000000 && mlGpsData.v == mlGpsData.v){
+      emit speedChanged(this, (double)mlGpsData.v, 0.0, 0.0, 0.0);
+    } else {
+      emit textMessageReceived(uasId, uasId, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(mlGpsData.v));
+    }
     }
 }
+
+#endif // MAVLINK_ENABLED_SLUGS
diff --git a/src/uas/SlugsMAV.h b/src/uas/SlugsMAV.h
index f25e6b28b339636c3e2895cbe91a66dc1f36be39..4fc690f92b614c720951dcda0c500d9d42fb9da0 100644
--- a/src/uas/SlugsMAV.h
+++ b/src/uas/SlugsMAV.h
@@ -26,7 +26,9 @@ This file is part of the QGROUNDCONTROL project
 
 #include "UAS.h"
 #include "mavlink.h"
+#include <QTimer>
 
+#define SLUGS_UPDATE_RATE   100   // in ms
 class SlugsMAV : public UAS
 {
     Q_OBJECT
@@ -38,111 +40,81 @@ public slots:
     /** @brief Receive a MAVLink message from this MAV */
     void receiveMessage(LinkInterface* link, mavlink_message_t message);
 
+    void emitSignals (void);
+
+
+
 signals:
-    // ESPECIAL SLUGS MESSAGE
-    void slugsCPULoad(int systemId,
-                      uint8_t sensLoad,
-                      uint8_t ctrlLoad,
-                      uint8_t batVolt,
-                      quint64 time);
-
-    void slugsAirData(int systemId,
-                      float dinamicPressure,
-                      float staticPresure,
-                      uint16_t temperature,
-                      quint64 time);
-
-    void slugsSensorBias(int systemId,
-                         double axBias,
-                         double ayBias,
-                         double azBias,
-                         double gxBias,
-                         double gyBias,
-                         double gzBias,
-                         quint64 time);
-
-    void slugsDiagnostic(int systemId,
-                         double diagFl1,
-                         double diagFl2,
-                         double diagFl3,
-                         int16_t diagSh1,
-                         int16_t diagSh2,
-                         int16_t diagSh3,
-                         quint64 time);
-
-    void slugsPilotConsolePWM(int systemId,
-                              uint16_t dt,
-                              uint16_t dla,
-                              uint16_t dra,
-                              uint16_t dr,
-                              uint16_t de,
-                              quint64 time);
-
-    void slugsPWM(int systemId,
-                  uint16_t dt_c,
-                  uint16_t dla_c,
-                  uint16_t dra_c,
-                  uint16_t dr_c,
-                  uint16_t dle_c,
-                  uint16_t dre_c,
-                  uint16_t dlf_c,
-                  uint16_t drf_c,
-                  uint16_t da1_c,
-                  uint16_t da2_c,
-                  quint64 time);
-
-    void slugsNavegation(int systemId,
-                          double u_m,
-                          double phi_c,
-                          double theta_c,
-                          double psiDot_c,
-                          double ay_body,
-                          double totalDist,
-                          double dist2Go,
-                          uint8_t fromWP,
-                          uint8_t toWP,
-                          quint64 time);
-
-   void slugsDataLog(int systemId,
-                 double logfl_1,
-                 double logfl_2,
-                 double logfl_3,
-                 double logfl_4,
-                 double logfl_5,
-                 double logfl_6,
-                 quint64 time);
-
-
-   void slugsFilteredData(int systemId,
-                          double filaX,
-                          double filaY,
-                          double filaZ,
-                          double filgX,
-                          double filgY,
-                          double filgZ,
-                          double filmX,
-                          double filmY,
-                          double filmZ,
-                          quint64 time);
-
-   void slugsGPSDateTime(int systemId,
-                         uint8_t gpsyear,
-                         uint8_t gpsmonth,
-                         uint8_t gpsday,
-                         uint8_t gpshour,
-                         uint8_t gpsmin,
-                         uint8_t gpssec,
-                         uint8_t gpsvisSat,
-                         quint64 time);
+
+    void slugsRawImu(int uasId, const mavlink_raw_imu_t& rawData);
+
+#ifdef MAVLINK_ENABLED_SLUGS
+
+    void slugsCPULoad(int systemId, const mavlink_cpu_load_t& cpuLoad);
+    void slugsAirData(int systemId, const mavlink_air_data_t& airData);
+    void slugsSensorBias(int systemId, const mavlink_sensor_bias_t& sensorBias);
+    void slugsDiagnostic(int systemId, const mavlink_diagnostic_t& diagnostic);
+    void slugsPilotConsolePWM(int systemId, const mavlink_pilot_console_t& pilotConsole);
+    void slugsPWM(int systemId, const mavlink_pwm_commands_t& pwmCommands);
+    void slugsNavegation(int systemId, const mavlink_slugs_navigation_t& slugsNavigation);
+    void slugsDataLog(int systemId, const mavlink_data_log_t& dataLog);
+    void slugsFilteredData(int systemId, const mavlink_filtered_data_t& filteredData);
+    void slugsGPSDateTime(int systemId, const mavlink_gps_date_time_t& gpsDateTime);
+    void slugsActionAck(int systemId, const mavlink_action_ack_t& actionAck);
+
+    void slugsBootMsg(int uasId, mavlink_boot_t& boot);
+    void slugsAttitude(int uasId, mavlink_attitude_t& attitude);
+
+#endif
+
+protected:
+
+   typedef struct _mavlink_pid_values_t {
+         float P[11];
+         float I[11];
+         float D[11];
+     }mavlink_pid_values_t;
+
+   unsigned char updateRoundRobin;
+   QTimer* widgetTimer;
+
+
+   mavlink_raw_imu_t 			mlRawImuData;
+
+#ifdef MAVLINK_ENABLED_SLUGS
+   mavlink_gps_raw_t			mlGpsData;
+   mavlink_attitude_t           mlAttitude;
+   mavlink_cpu_load_t 			mlCpuLoadData;
+   mavlink_air_data_t 			mlAirData;
+   mavlink_sensor_bias_t 		mlSensorBiasData;
+   mavlink_diagnostic_t 		mlDiagnosticData;
+   mavlink_pilot_console_t 		mlPilotConsoleData;
+   mavlink_filtered_data_t 		mlFilteredData;
+   mavlink_boot_t 				mlBoot;
+   mavlink_gps_date_time_t 		mlGpsDateTime;
+   mavlink_mid_lvl_cmds_t 		mlMidLevelCommands;
+   mavlink_set_mode_t 			mlApMode;
+   mavlink_pwm_commands_t		mlPwmCommands;
+   mavlink_pid_values_t			mlPidValues;
+   mavlink_pid_t				mlSinglePid;
+
+   mavlink_slugs_navigation_t	mlNavigation;
+   mavlink_data_log_t			mlDataLog;
+   mavlink_ctrl_srfc_pt_t		mlPassthrough;
+   mavlink_action_ack_t			mlActionAck;
+
+   mavlink_slugs_action_t		mlAction;
 
 
    // Standart messages MAVLINK used by SLUGS
-   void slugsActionAck(int systemId,
-                       uint8_t action,
-                       uint8_t result);
+private:
+
 
+   void emitGpsSignals (void);
 
+   int uasId;
 
+#endif // if SLUGS
 
 };
 
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 347244ea1b2252fa1676514d3a9d54f8420346a5..703ae4f219fa814585d9d8bea86dce9d841f3810 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -611,11 +611,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     throttle << radioMsg.throttle[i];
 
                 QPointer<RadioCalibrationData> radioData = new RadioCalibrationData(aileron,
-                                                                                    elevator,
-                                                                                    rudder,
-                                                                                    gyro,
-                                                                                    pitch,
-                                                                                    throttle);
                 emit radioCalibrationReceived(radioData);
                 delete radioData;
             }
diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc
index 3481f7675c07e8640e8a247351d26ad6aad8ec14..f20ed035fd6d997d8b72c943383266a07c267eee 100644
--- a/src/ui/MainWindow.cc
+++ b/src/ui/MainWindow.cc
@@ -227,8 +227,14 @@ void MainWindow::connectWidgets()
         connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float)));
     }
 
-    if (slugsHilSimWidget->widget()){
-      connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), dynamic_cast<SlugsHilSim*>(slugsHilSimWidget->widget()), SLOT(activeUasSet(UASInterface*)) );
+    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*)));
     }
 
 
@@ -558,16 +564,16 @@ void MainWindow::UASCreated(UASInterface* uas)
         PxQuadMAV* mav = dynamic_cast<PxQuadMAV*>(uas);
         if (mav) loadPixhawkView();
 
-        SlugsMAV* mav2 = dynamic_cast<SlugsMAV*>(uas);
-        if (mav2)
-        {
-          dynamic_cast<SlugsDataSensorView*>(slugsDataWidget->widget())->addUAS(uas);
-          loadSlugsView();
-
+        if (slugsDataWidget) {
+          SlugsDataSensorView* dataWidget = dynamic_cast<SlugsDataSensorView*>(slugsDataWidget->widget());
+          if (dataWidget) {
+            SlugsMAV* mav2 = dynamic_cast<SlugsMAV*>(uas);
+            if (mav2) {
+              dataWidget->addUAS(uas);
+              loadSlugsView();
+            }
+          }
         }
-
-
-
     }
 }
 
diff --git a/src/ui/SlugsDataSensorView.cc b/src/ui/SlugsDataSensorView.cc
index 53558f533f0a2cc49e5dbd939cda23b4552afb44..fae39cc0cb9a00da4f7bb1c9c3890f7f3d9786fc 100644
--- a/src/ui/SlugsDataSensorView.cc
+++ b/src/ui/SlugsDataSensorView.cc
@@ -12,130 +12,9 @@ SlugsDataSensorView::SlugsDataSensorView(QWidget *parent) :
 {
     ui->setupUi(this);
 
-
-    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
-
     activeUAS = NULL;
 
-    loadParameters();
-
-
     this->setVisible(false);
-
-    // timer for refresh UI
-    updateTimer = new QTimer(this);
-    connect(updateTimer, SIGNAL(timeout()), this, SLOT(refresh()));
-    updateTimer->start(200);
-}
-
-void SlugsDataSensorView::loadParameters()
-{
-
-    //Global Position
-    Latitude = 0;
-    Longitude = 0;
-    Height = 0;
-    timeGlobalPosition = 0;
-
-    Xpos = 0;
-    Ypos = 0;
-    Zpos = 0;
-    TimeActualPosition = 0;
-
-    VXpos = 0;
-    VYpos = 0;
-    VZpos = 0;
-    TimeActualSpeed =0;
-
-    roll = 0;
-    pitch = 0;
-    yaw = 0;
-    TimeActualAttitude = 0;
-
-    //Sensor Biases
-    //Acelerometer
-    Axb = 0;
-    Ayb = 0;
-    Azb = 0;
-    //Gyro
-    Gxb = 0;
-    Gyb = 0;
-    Gzb = 0;
-    TimeActualBias = 0;
-
-    //Diagnostic Message
-    diagFl1 = 0;
-    diagFl2 = 0;
-    diagFl3 = 0;
-    diagSh1 = 0;
-    diagSh2 = 0;
-    diagSh3 = 0;
-    timeDiagnostic = 0;
-
-    //CPU Load
-    sensLoad = 0;
-    ctrlLoad = 0;
-    batVolt = 0;
-    timeCpuLoad = 0;
-
-    //navigation
-    u_m = 0;
-    phi_c = 0;
-    theta_c = 0;
-    psiDot_c = 0;
-    ay_body = 0;
-    totalDist = 0;
-    dist2Go = 0;
-    fromWP = 0;
-    toWP = 0;
-    timeNavigation = 0;
-
-    // Data Log
-    Logfl_1 = 0;
-    Logfl_2 = 0;
-    Logfl_3 = 0;
-    Logfl_4 = 0;
-    Logfl_5 = 0;
-    Logfl_6 = 0;
-    timeDataLog = 0;
-
-    //pwm commands
-     dt_c = 0; ///< AutoPilot's throttle command
-     dla_c = 0; ///< AutoPilot's left aileron command
-     dra_c = 0; ///< AutoPilot's right aileron command
-     dr_c  = 0; ///< AutoPilot's rudder command
-     dle_c = 0; ///< AutoPilot's left elevator command
-     dre_c  = 0; ///< AutoPilot's right elevator command
-     dlf_c = 0; ///< AutoPilot's left  flap command
-     drf_c = 0; ///< AutoPilot's right flap command
-     aux1 = 0; ///< AutoPilot's aux1 command
-     aux2 = 0; ///< AutoPilot's aux2 command
-     timePWMCommand = 0;
-
-
-     //filtered data
-      aX = 0; ///< Accelerometer X value (m/s^2)
-      aY = 0; ///< Accelerometer Y value (m/s^2)
-      aZ = 0; ///< Accelerometer Z value (m/s^2)
-      gX = 0; ///< Gyro X value (rad/s)
-      gY = 0; ///< Gyro Y value (rad/s)
-      gZ = 0; ///< Gyro Z value (rad/s)
-      mX = 0; ///< Magnetometer X (normalized to 1)
-      mY = 0; ///< Magnetometer Y (normalized to 1)
-      mZ = 0; ///< Magnetometer Z (normalized to 1)
-      timeFiltered = 0;
-
-      //gps date and time
-       year = 0  ; ///< Year reported by Gps
-       month =0; ///< Month reported by Gps
-       day = 0; ///< Day reported by Gps
-       hour = 0; ///< Hour reported by Gps
-       min = 0; ///< Min reported by Gps
-       sec = 0; ///< Sec reported by Gps
-       visSat = 0; ///< Visible sattelites reported by Gps
-       timeGPSDateTime = 0;
-
-
 }
 
 SlugsDataSensorView::~SlugsDataSensorView()
@@ -147,141 +26,64 @@ void SlugsDataSensorView::addUAS(UASInterface* uas)
 {
     SlugsMAV* slugsMav = dynamic_cast<SlugsMAV*>(uas);
 
-    if (slugsMav != NULL)
-    {
-        //connect standar messages
-        connect(slugsMav, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugLocalPositionChange(UASInterface*,double,double,double,quint64)));
-        connect(slugsMav, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64)));
-        connect(slugsMav, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugAttitudeChanged(UASInterface*,double,double,double,quint64)));
-        connect(slugsMav, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugsGlobalPositionChanged(UASInterface*,double,double,double,quint64)));
+  if (slugsMav != NULL) {
 
+    connect(slugsMav, SIGNAL(slugsRawImu(int, const mavlink_raw_imu_t&)), this, SLOT(slugRawDataChanged(int, const mavlink_raw_imu_t&)));
+
+    #ifdef MAVLINK_ENABLED_SLUGS
+
+        //connect standar messages
+    connect(slugsMav, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugLocalPositionChanged(UASInterface*,double,double,double,quint64)));
+    connect(slugsMav, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64)));
+    connect(slugsMav, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugAttitudeChanged(UASInterface*,double,double,double,quint64)));
+    connect(slugsMav, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugsGlobalPositionChanged(UASInterface*,double,double,double,quint64)));
 
-        //connect slugs especial messages
-        connect(slugsMav, SIGNAL(slugsSensorBias(int,double,double,double,double,double,double,quint64)), this, SLOT(slugsSensorBiasChanged(int,double,double,double,double,double,double,quint64)));
-        connect(slugsMav, SIGNAL(slugsDiagnostic(int,double,double,double,int16_t,int16_t,int16_t,quint64)), this, SLOT(slugsDiagnosticMessageChanged(int,double,double,double,int16_t,int16_t,int16_t,quint64)));
-        connect(slugsMav, SIGNAL(slugsCPULoad(int,uint8_t,uint8_t,uint8_t,quint64)), this, SLOT(slugsCpuLoadChanged(int,uint8_t,uint8_t,uint8_t,quint64)));
-        connect(slugsMav, SIGNAL(slugsNavegation(int,double,double,double,double,double,double,double,uint8_t,uint8_t,quint64)),this,SLOT(slugsNavegationChanged(int,double,double,double,double,double,double,double,uint8_t,uint8_t,quint64)));
-        connect(slugsMav, SIGNAL(slugsDataLog(int,double,double,double,double,double,double,quint64)), this, SLOT(slugsDataLogChanged(int,double,double,double,double,double,double,quint64)));
-        connect(slugsMav, SIGNAL(slugsPWM(int,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,quint64)),this,SLOT(slugsPWMChanged(int,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,quint64)));
-        connect(slugsMav, SIGNAL(slugsFilteredData(int,double,double,double,double,double,double,double,double,double,quint64)),this,SLOT(slugsFilteredDataChanged(int,double,double,double,double,double,double,double,double,double,quint64)));
-        connect(slugsMav, SIGNAL(slugsGPSDateTime(int,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,quint64)),this,SLOT(slugsGPSDateTimeChanged(int,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,quint64)));
 
 
+        //connect slugs especial messages
+    connect(slugsMav, SIGNAL(slugsSensorBias(int,const mavlink_sensor_bias_t&)), this, SLOT(slugsSensorBiasChanged(int,const mavlink_sensor_bias_t&)));
+    connect(slugsMav, SIGNAL(slugsDiagnostic(int,const mavlink_diagnostic_t&)), this, SLOT(slugsDiagnosticMessageChanged(int,const mavlink_diagnostic_t&)));
+    connect(slugsMav, SIGNAL(slugsCPULoad(int,const mavlink_cpu_load_t&)), this, SLOT(slugsCpuLoadChanged(int,const mavlink_cpu_load_t&)));
+    connect(slugsMav, SIGNAL(slugsNavegation(int,const mavlink_slugs_navigation_t&)),this,SLOT(slugsNavegationChanged(int,const mavlink_slugs_navigation_t&)));
+    connect(slugsMav, SIGNAL(slugsDataLog(int,const mavlink_data_log_t&)), this, SLOT(slugsDataLogChanged(int,const mavlink_data_log_t&)));
+    connect(slugsMav, SIGNAL(slugsPWM(int,const mavlink_pwm_commands_t&)),this,SLOT(slugsPWMChanged(int,const mavlink_pwm_commands_t&)));
+    connect(slugsMav, SIGNAL(slugsFilteredData(int,const mavlink_filtered_data_t&)),this,SLOT(slugsFilteredDataChanged(int,const mavlink_filtered_data_t&)));
+    connect(slugsMav, SIGNAL(slugsGPSDateTime(int,const mavlink_gps_date_time_t&)),this,SLOT(slugsGPSDateTimeChanged(int,const mavlink_gps_date_time_t&)));
+
+    #endif // MAVLINK_ENABLED_SLUGS
         // Set this UAS as active if it is the first one
-        if(activeUAS == 0)
-        {
+    if(activeUAS == 0) {
             activeUAS = uas;
         }
+
     }
 }
 
-void SlugsDataSensorView::setActiveUAS(UASInterface* uas)
-{
-    activeUAS = uas;
-
+void SlugsDataSensorView::slugRawDataChanged(int uasId, const mavlink_raw_imu_t &rawData){
+ Q_UNUSED(uasId);
 
+ ui->m_Axr->setText(QString::number(rawData.xacc));
+ ui->m_Ayr->setText(QString::number(rawData.yacc));
+ ui->m_Azr->setText(QString::number(rawData.zacc));
 }
 
-
-void SlugsDataSensorView::refresh()
-{
-    if(activeUAS)
-    {
-        //refresh Global Position
-        ui->m_GpsLatitude->setText(QString::number(Latitude, 'f', 4));
-        ui->m_GpsLongitude->setText(QString::number(Longitude, 'f', 4));
-        ui->m_Gpsheigth->setText(QString::number(Height, 'f', 4));
-
-        //refresh GPS Date and Time
-        ui->m_GpsDate->setText(QString::number(day)+ "-" + QString::number(month)+ "-" + QString::number(year));
-        ui->m_GpsTime->setText(QString::number(hour)+ "-" + QString::number(min)+ "-" + QString::number(sec));
-        ui->m_GpsSat->setText(QString::number(visSat));
-        ui->m_GpsCog->setText("No Data");
-        ui->m_GpsSog->setText("No Data");
-
-
-            //refresh UI position data
-            ui->ed_x->setPlainText(QString::number(Xpos, 'f', 4));
-            ui->ed_y->setPlainText(QString::number(Ypos, 'f', 4));
-            ui->ed_z->setPlainText(QString::number(Zpos, 'f', 4));
-
-            //refresh UI speed position data
-            ui->ed_vx->setPlainText(QString::number(VXpos,'f',4));
-            ui->ed_vy->setPlainText(QString::number(VYpos,'f',4));
-            ui->ed_vz->setPlainText(QString::number(VZpos,'f',4));
-
-            //refresh UI attitude data
-            ui->m_SlugAttitudeRoll_plainTextEdit->setPlainText(QString::number(roll,'f',4));
-            ui->m_SlugAttitudePitch_plainTextEdit->setPlainText(QString::number(pitch,'f',4));
-            ui->m_SlugAttitudeYaw_plainTextEdit->setPlainText(QString::number(yaw,'f',4));
-
-            //refresh UI sensor bias acelerometer data
-            ui->m_AxBiases->setText(QString::number(Axb, 'f', 4));
-            ui->m_AyBiases->setText(QString::number(Ayb, 'f', 4));
-            ui->m_AzBiases->setText(QString::number(Azb, 'f', 4));
-            ui->m_GxBiases->setText(QString::number(Gxb, 'f', 4));
-            ui->m_GyBiases->setText(QString::number(Gyb, 'f', 4));
-            ui->m_GzBiases->setText(QString::number(Gzb, 'f', 4));
-
-            //refresh UI diagnostic data
-            ui->m_SlugsFl1_textEdit->setText(QString::number(diagFl1, 'f', 4));
-            ui->m_SlugsFl2_textEdit->setText(QString::number(diagFl2, 'f', 4));
-            ui->m_SlugsFl3_textEdit->setText(QString::number(diagFl3, 'f', 4));
-            ui->m_SlugsSh1_textEdit->setText(QString::number(diagSh1, 'f', 4));
-            ui->m_SlugsSh2_textEdit->setText(QString::number(diagSh2, 'f', 4));
-            ui->m_SlugsSh3_textEdit->setText(QString::number(diagSh3, 'f', 4));
-
-            //refresh UI navegation data
-            ui->m_SlugsUm_textEdit->setText(QString::number(u_m, 'f', 4));
-            ui->m_SlugsPitchC_textEdit->setText(QString::number(pitch, 'f', 4));
-            ui->m_SlugsPsidC_textEdit->setText(QString::number(psiDot_c, 'f', 4));
-            ui->m_SlugsPhiC_textEdit->setText(QString::number(phi_c, 'f', 4));
-            ui->m_SlugsAyBody_textEdit->setText(QString::number(ay_body, 'f', 4));
-            ui->m_SlugsFromWP_textEdit->setText(QString::number(fromWP, 'f', 4));
-            ui->m_SlugsToWP_textEdit->setText(QString::number(toWP, 'f', 4));
-            ui->m_SlugsTotRun_textEdit->setText(QString::number(totalDist, 'f', 4));
-            ui->m_SlugsDistToGround_textEdit->setText(QString::number(dist2Go, 'f', 4));
-
-            //refresh UI Data Log
-            ui->m_logFl1_textEdit->setText(QString::number(Logfl_1, 'f', 4));
-            ui->m_logFl2_textEdit->setText(QString::number(Logfl_2, 'f', 4));
-            ui->m_logFl3_textEdit->setText(QString::number(Logfl_3, 'f', 4));
-            ui->m_logFl4_textEdit->setText(QString::number(Logfl_4, 'f', 4));
-            ui->m_logFl5_textEdit->setText(QString::number(Logfl_5, 'f', 4));
-            ui->m_logFl6_textEdit->setText(QString::number(Logfl_6, 'f', 4));
-
-            //refresh UI PWM Commands
-            ui->m_pwmThro->setText(QString::number(dt_c, 'f', 4));
-            ui->m_pwmThroTrim->setText(QString::number(dre_c, 'f', 4));
-            ui->m_pwmAile->setText(QString::number(dla_c, 'f', 4));
-            ui->m_pwmAileTrim->setText(QString::number(dlf_c, 'f', 4));
-            ui->m_pwmElev->setText(QString::number(dle_c, 'f', 4));
-            ui->m_pwmElevTrim->setText(QString::number(drf_c, 'f', 4));
-            ui->m_pwmRudd->setText(QString::number(dr_c, 'f', 4));
-            ui->m_pwmRuddTrim->setText(QString::number(aux1, 'f', 4));
-            ui->m_pwmFailSafe->setText(QString::number(dre_c, 'f', 4));
-            ui->m_pwmAvailable->setText("No Data");
-
-
-    }
-
+void SlugsDataSensorView::setActiveUAS(UASInterface* uas){
+    activeUAS = uas;
 }
 
+#ifdef MAVLINK_ENABLED_SLUGS
+
 void SlugsDataSensorView::slugsGlobalPositionChanged(UASInterface *uas,
                                                      double lat,
                                                      double lon,
                                                      double alt,
-                                                     quint64 time)
-
-{
-    Q_UNUSED( uas);
-    Latitude = lat;
-    Longitude = lon;
-    Height = alt;
-    timeGlobalPosition = time;
-
-
+                                                     quint64 time) {
+ Q_UNUSED(uas);
+ Q_UNUSED(time);
 
+ ui->m_GpsLatitude->setText(QString::number(lat));
+ ui->m_GpsLongitude->setText(QString::number(lon));
+ ui->m_GpsHeight->setText(QString::number(alt));
 }
 
 
@@ -289,14 +91,13 @@ void SlugsDataSensorView::slugLocalPositionChanged(UASInterface* uas,
                                                    double x,
                                                    double y,
                                                    double z,
-                                                   quint64 time)
-{
-    Q_UNUSED( uas);
+                                                   quint64 time) {
+  Q_UNUSED(uas);
+  Q_UNUSED(time);
 
-    Xpos = x;
-    Ypos = y;
-    Zpos = z;
-    TimeActualPosition = time;
+  ui->ed_x->setPlainText(QString::number(x));
+  ui->ed_y->setPlainText(QString::number(y));
+  ui->ed_z->setPlainText(QString::number(z));
 
 }
 
@@ -304,14 +105,14 @@ void SlugsDataSensorView::slugSpeedLocalPositionChanged(UASInterface* uas,
                                                         double vx,
                                                         double vy,
                                                         double vz,
-                                                        quint64 time)
-{
+                                                        quint64 time) {
     Q_UNUSED( uas);
+  Q_UNUSED(time);
+
+  ui->ed_vx->setPlainText(QString::number(vx));
+  ui->ed_vy->setPlainText(QString::number(vy));
+  ui->ed_vz->setPlainText(QString::number(vz));
 
-    VXpos = vx;
-    VYpos = vy;
-    VZpos = vz;
-    TimeActualSpeed = time;
 }
 
 void SlugsDataSensorView::slugAttitudeChanged(UASInterface* uas,
@@ -321,190 +122,118 @@ void SlugsDataSensorView::slugAttitudeChanged(UASInterface* uas,
                                               quint64 time)
 {
     Q_UNUSED( uas);
+    Q_UNUSED(time);
+
+    ui->m_Roll->setPlainText(QString::number(slugroll));
+    ui->m_Pitch->setPlainText(QString::number(slugpitch));
+    ui->m_Yaw->setPlainText(QString::number(slugyaw));
 
-    roll = slugroll;
-    pitch = slugpitch;
-    yaw = slugyaw;
-    TimeActualAttitude = time;
 }
 
-void SlugsDataSensorView::slugsSensorBiasChanged(int systemId,
-                                                 double axb,
-                                                 double ayb,
-                                                 double azb,
-                                                 double gxb,
-                                                 double gyb,
-                                                 double gzb,
-                                                 quint64 time)
-{
 
+void SlugsDataSensorView::slugsSensorBiasChanged(int systemId,
+                                                 const mavlink_sensor_bias_t& sensorBias){
      Q_UNUSED( systemId);
 
-     Axb = axb;
-     Ayb = ayb;
-     Azb = azb;
-     Gxb = gxb;
-     Gyb = gyb;
-     Gzb = gzb;
-     TimeActualBias = time;
-
+  ui->m_AxBiases->setText(QString::number(sensorBias.axBias));
+  ui->m_AyBiases->setText(QString::number(sensorBias.ayBias));
+  ui->m_AzBiases->setText(QString::number(sensorBias.azBias));
+  ui->m_GxBiases->setText(QString::number(sensorBias.gxBias));
+  ui->m_GyBiases->setText(QString::number(sensorBias.gyBias));
+  ui->m_GzBiases->setText(QString::number(sensorBias.gzBias));
 
 }
 
 void SlugsDataSensorView::slugsDiagnosticMessageChanged(int systemId,
-                                                        double diagfl1,
-                                                        double diagfl2,
-                                                        double diagfl3,
-                                                        int16_t diagsh1,
-                                                        int16_t diagsh2,
-                                                        int16_t diagsh3,
-                                                        quint64 time)
-{
+                                                        const mavlink_diagnostic_t& diagnostic){
     Q_UNUSED(systemId);
 
-    diagFl1 = diagfl1;
-    diagFl2 = diagfl2;
-    diagFl3 = diagfl3;
-    diagSh1 = diagsh1;
-    diagSh2 = diagsh2;
-    diagSh3 = diagsh3;
-    timeDiagnostic = time;
+  ui->m_Fl1->setText(QString::number(diagnostic.diagFl1));
+  ui->m_Fl2->setText(QString::number(diagnostic.diagFl2));
+  ui->m_Fl3->setText(QString::number(diagnostic.diagFl2));
 
+  ui->m_Sh1->setText(QString::number(diagnostic.diagSh1));
+  ui->m_Sh2->setText(QString::number(diagnostic.diagSh2));
+  ui->m_Sh3->setText(QString::number(diagnostic.diagSh3));
 }
 
 
 void SlugsDataSensorView::slugsCpuLoadChanged(int systemId,
-                                              uint8_t sensload,
-                                              uint8_t ctrlload,
-                                              uint8_t batvolt,
-                                              quint64 time)
-{
+                                              const mavlink_cpu_load_t& cpuLoad){
      Q_UNUSED(systemId);
-    sensLoad = sensload;
-    ctrlLoad = ctrlload;
-    batVolt = batvolt;
-    timeCpuLoad = time;
+  ui->ed_sens->setText(QString::number(cpuLoad.sensLoad));
+  ui->ed_control->setText(QString::number(cpuLoad.ctrlLoad));
+  ui->ed_batvolt->setText(QString::number(cpuLoad.batVolt));
 }
 
 void SlugsDataSensorView::slugsNavegationChanged(int systemId,
-                                                 double navu_m,
-                                                 double navphi_c,
-                                                 double navtheta_c,
-                                                 double navpsiDot_c,
-                                                 double navay_body,
-                                                 double navtotalDist,
-                                                 double navdist2Go,
-                                                 uint8_t navfromWP,
-                                                 uint8_t navtoWP,
-                                                 quint64 time)
-{
+                                                 const mavlink_slugs_navigation_t& slugsNavigation){
      Q_UNUSED(systemId);
-     u_m = navu_m;
-     phi_c = navphi_c;
-     theta_c = navtheta_c;
-     psiDot_c = navpsiDot_c;
-     ay_body = navay_body;
-     totalDist = navtotalDist;
-     dist2Go = navdist2Go;
-     fromWP = navfromWP;
-     toWP = navtoWP;
-     timeNavigation = time;
+  ui->m_Um->setText(QString::number(slugsNavigation.u_m));
+  ui->m_PhiC->setText(QString::number(slugsNavigation.phi_c));
+  ui->m_PitchC->setText(QString::number(slugsNavigation.theta_c));
+  ui->m_PsidC->setText(QString::number(slugsNavigation.psiDot_c));
+  ui->m_AyBody->setText(QString::number(slugsNavigation.ay_body));
+  ui->m_TotRun->setText(QString::number(slugsNavigation.totalDist));
+  ui->m_DistToGo->setText(QString::number(slugsNavigation.dist2Go));
+  ui->m_FromWP->setText(QString::number(slugsNavigation.fromWP));
+  ui->m_ToWP->setText(QString::number(slugsNavigation.toWP));
 }
 
 
 
 void SlugsDataSensorView::slugsDataLogChanged(int systemId,
-                                              double logfl_1,
-                                              double logfl_2,
-                                              double logfl_3,
-                                              double logfl_4,
-                                              double logfl_5,
-                                              double logfl_6,
-                                              quint64 time)
-{
+                                              const mavlink_data_log_t& dataLog){
      Q_UNUSED(systemId);
-    Logfl_1 = logfl_1;
-    Logfl_2 = logfl_2;
-    Logfl_3 = logfl_3;
-    Logfl_4 = logfl_4;
-    Logfl_5 = logfl_5;
-    Logfl_6 = logfl_6;
-    timeDataLog = time;
+  ui->m_logFl1->setText(QString::number(dataLog.fl_1));
+  ui->m_logFl2->setText(QString::number(dataLog.fl_2));
+  ui->m_logFl3->setText(QString::number(dataLog.fl_3));
+  ui->m_logFl4->setText(QString::number(dataLog.fl_4));
+  ui->m_logFl5->setText(QString::number(dataLog.fl_5));
+  ui->m_logFl6->setText(QString::number(dataLog.fl_6));
 }
 
 void SlugsDataSensorView::slugsPWMChanged(int systemId,
-                                          uint16_t vdt_c,
-                                          uint16_t vdla_c,
-                                          uint16_t vdra_c,
-                                          uint16_t vdr_c,
-                                          uint16_t vdle_c,
-                                          uint16_t vdre_c,
-                                          uint16_t vdlf_c,
-                                          uint16_t vdrf_c,
-                                          uint16_t vda1_c,
-                                          uint16_t vda2_c,
-                                          quint64 time)
-{
+                                          const mavlink_pwm_commands_t& pwmCommands){
        Q_UNUSED(systemId);
-       dt_c = vdt_c; ///< AutoPilot's throttle command
-       dla_c = vdla_c; ///< AutoPilot's left aileron command
-       dra_c = vdra_c; ///< AutoPilot's right aileron command
-       dr_c  = vdr_c; ///< AutoPilot's rudder command
-       dle_c = vdle_c; ///< AutoPilot's left elevator command
-       dre_c  = vdre_c; ///< AutoPilot's right elevator command
-       dlf_c = vdlf_c; ///< AutoPilot's left  flap command
-       drf_c = vdrf_c; ///< AutoPilot's right flap command
-       aux1 = vda1_c; ///< AutoPilot's aux1 command
-       aux2 = vda2_c; ///< AutoPilot's aux2 command
-       timePWMCommand = time;
+  ui->m_pwmThro->setText(QString::number(pwmCommands.dt_c));
+  ui->m_pwmAile->setText(QString::number(pwmCommands.dla_c));
+  ui->m_pwmElev->setText(QString::number(pwmCommands.dle_c));
+  ui->m_pwmRudd->setText(QString::number(pwmCommands.dr_c));
+
+  ui->m_pwmThroTrim->setText(QString::number(pwmCommands.dre_c));
+  ui->m_pwmAileTrim->setText(QString::number(pwmCommands.dlf_c));
+  ui->m_pwmElevTrim->setText(QString::number(pwmCommands.drf_c));
+  ui->m_pwmRuddTrim->setText(QString::number(pwmCommands.aux1));
 
 }
 
 void SlugsDataSensorView::slugsFilteredDataChanged(int systemId,
-                                                   double filaX,
-                                                   double filaY,
-                                                   double filaZ,
-                                                   double filgX,
-                                                   double filgY,
-                                                   double filgZ,
-                                                   double filmX,
-                                                   double filmY,
-                                                   double filmZ,
-                                                   quint64 time)
-{
+                                                   const mavlink_filtered_data_t& filteredData){
     Q_UNUSED(systemId);
-
-    aX = filaX; ///< Accelerometer X value (m/s^2)
-    aY = filaY; ///< Accelerometer Y value (m/s^2)
-    aZ = filaZ; ///< Accelerometer Z value (m/s^2)
-    gX = filgX; ///< Gyro X value (rad/s)
-    gY = filgY; ///< Gyro Y value (rad/s)
-    gZ = filgZ; ///< Gyro Z value (rad/s)
-    mX = filmX; ///< Magnetometer X (normalized to 1)
-    mY = filmY; ///< Magnetometer Y (normalized to 1)
-    mZ = filmZ; ///< Magnetometer Z (normalized to 1)
-    timeFiltered = time;
+  ui->m_Axf->setText(QString::number(filteredData.aX));
+  ui->m_Ayf->setText(QString::number(filteredData.aY));
+  ui->m_Azf->setText(QString::number(filteredData.aZ));
+  ui->m_Gxf->setText(QString::number(filteredData.gX));
+  ui->m_Gyf->setText(QString::number(filteredData.gY));
+  ui->m_Gzf->setText(QString::number(filteredData.gZ));
+  ui->m_Mxf->setText(QString::number(filteredData.mX));
+  ui->m_Myf->setText(QString::number(filteredData.mY));
+  ui->m_Mzf->setText(QString::number(filteredData.mZ));
 }
 
 void SlugsDataSensorView::slugsGPSDateTimeChanged(int systemId,
-                                                  uint8_t gpsyear,
-                                                  uint8_t gpsmonth,
-                                                  uint8_t gpsday,
-                                                  uint8_t gpshour,
-                                                  uint8_t gpsmin,
-                                                  uint8_t gpssec,
-                                                  uint8_t gpsvisSat,
-                                                  quint64 time)
-{
+                                                  const mavlink_gps_date_time_t& gpsDateTime){
     Q_UNUSED(systemId);
+  ui->m_GpsDate->setText(QString::number(gpsDateTime.month) + "/" +
+                         QString::number(gpsDateTime.day) + "/" +
+                         QString::number(gpsDateTime.year));
 
-    year = gpsyear  ; ///< Year reported by Gps
-    month = gpsmonth; ///< Month reported by Gps
-    day = gpsday; ///< Day reported by Gps
-    hour = gpshour; ///< Hour reported by Gps
-    min = gpsmin; ///< Min reported by Gps
-    sec = gpssec; ///< Sec reported by Gps
-    visSat = gpsvisSat; ///< Visible sattelites reported by Gps
-    timeGPSDateTime = time;
+  ui->m_GpsTime->setText(QString::number(gpsDateTime.hour) + ":" +
+                         QString::number(gpsDateTime.min) + ":" +
+                         QString::number(gpsDateTime.sec));
 
+  ui->m_GpsSat->setText(QString::number(gpsDateTime.visSat));
 }
+
+#endif // MAVLINK_ENABLED_SLUGS
diff --git a/src/ui/SlugsDataSensorView.h b/src/ui/SlugsDataSensorView.h
index 6f23ff528c130d1c254275a63783b32d624e8600..6cd40b01bbd70d9dca789d52c40a78c5b8cd5aad 100644
--- a/src/ui/SlugsDataSensorView.h
+++ b/src/ui/SlugsDataSensorView.h
@@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project
 
 #include "UASInterface.h"
 #include "SlugsMAV.h"
+#include "mavlink.h"
 
 namespace Ui {
     class SlugsDataSensorView;
@@ -67,17 +68,10 @@ public slots:
     */
     void setActiveUAS(UASInterface* uas);
 
-    /**
-         * @brief Adds the UAS for data display
-         *
-         * Adds the UAS and makes all the correct connections for data display on the Widgets
-    */
-
-    void refresh();
-
-
 
+    void slugRawDataChanged (int uasId, const mavlink_raw_imu_t& rawData);
 
+#ifdef MAVLINK_ENABLED_SLUGS
     /**
          * @brief Adds the UAS for data display
          *
@@ -120,240 +114,62 @@ public slots:
                                     double alt,
                                     quint64 time);
     /**
-         * @brief Adds the UAS for data display
-         *
-         * Adds the UAS and makes all the correct connections for data display on the Widgets
+         * @brief Updates the sensor bias widget
     */
     void slugsSensorBiasChanged(int systemId,
-                                double axb,
-                                double ayb,
-                                double azb,
-                                double gxb,
-                                double gyb,
-                                double gzb,
-                                quint64 time);
+                                const mavlink_sensor_bias_t& sensorBias);
+
     /**
-         * @brief Adds the UAS for data display
-         *
-         * Adds the UAS and makes all the correct connections for data display on the Widgets
+         * @brief Updates the diagnostic widget
     */
     void slugsDiagnosticMessageChanged(int systemId,
-                                       double diagfl1,
-                                       double diagfl2,
-                                       double diagfl3,
-                                       int16_t diagsh1,
-                                       int16_t diagsh2,
-                                       int16_t diagsh3,
-                                       quint64 time);
+                                       const mavlink_diagnostic_t& diagnostic);
 
     /**
-         * @brief Adds the UAS for data display
-         *
-         * Adds the UAS and makes all the correct connections for data display on the Widgets
+         * @brief Updates the CPU load widget
     */
     void slugsCpuLoadChanged(int systemId,
-                             uint8_t sensload,
-                             uint8_t ctrlload,
-                             uint8_t batvolt,
-                             quint64 time);
+                             const mavlink_cpu_load_t& cpuLoad);
 
 
     /**
-         * @brief Adds the UAS for data display
-         *
-         * Adds the UAS and makes all the correct connections for data display on the Widgets
+         * @brief Updates the Navigation widget
     */
     void slugsNavegationChanged(int systemId,
-                                double navu_m,
-                                double navphi_c,
-                                double navtheta_c,
-                                double navpsiDot_c,
-                                double navay_body,
-                                double navtotalDist,
-                                double navdist2Go,
-                                uint8_t navfromWP,
-                                uint8_t navtoWP,
-                                quint64 time);
+                                const mavlink_slugs_navigation_t& slugsNavigation);
 
     /**
-         * @brief Adds the UAS for data display
-         *
-         * Adds the UAS and makes all the correct connections for data display on the Widgets
+         * @brief Updates the Data Log widget
     */
    void  slugsDataLogChanged(int systemId,
-                            double logfl_1,
-                            double logfl_2,
-                            double logfl_3,
-                            double logfl_4,
-                            double logfl_5,
-                            double logfl_6,
-                            quint64 time);
+                             const mavlink_data_log_t& dataLog);
 
    /**
-        * @brief Adds the UAS for data display
-        *
-        * Adds the UAS and makes all the correct connections for data display on the Widgets
+        * @brief Updates the PWM Commands widget
    */
-
    void slugsPWMChanged(int systemId,
-                        uint16_t vdt_c,
-                        uint16_t vdla_c,
-                        uint16_t vdra_c,
-                        uint16_t vdr_c,
-                        uint16_t vdle_c,
-                        uint16_t vdre_c,
-                        uint16_t vdlf_c,
-                        uint16_t vdrf_c,
-                        uint16_t vda1_c,
-                        uint16_t vda2_c,
-                        quint64 time);
+                        const mavlink_pwm_commands_t& pwmCommands);
 
    /**
-        * @brief Adds the UAS for data display
-        *
-        * Adds the UAS and makes all the correct connections for data display on the Widgets
+        * @brief Updates the filtered sensor measurements widget
    */
    void slugsFilteredDataChanged(int systemId,
-                                 double filaX,
-                                 double filaY,
-                                 double filaZ,
-                                 double filgX,
-                                 double filgY,
-                                 double filgZ,
-                                 double filmX,
-                                 double filmY,
-                                 double filmZ,
-                                 quint64 time);
-
-
-
+                                 const mavlink_filtered_data_t& filteredData);
 
 
+   /**
+        * @brief Updates the gps Date Time widget
+   */
     void slugsGPSDateTimeChanged(int systemId,
-                                    uint8_t gpsyear,
-                                     uint8_t gpsmonth,
-                                     uint8_t gpsday,
-                                     uint8_t gpshour,
-                                     uint8_t gpsmin,
-                                     uint8_t gpssec,
-                                    uint8_t gpsvisSat,
-                                    quint64 time);
+                                 const mavlink_gps_date_time_t& gpsDateTime);
 
+#endif // MAVLINK_ENABLED_SLUGS
 
 protected:
-    QTimer* updateTimer;
      UASInterface* activeUAS;
 
-     //Global Position
-     double Latitude;
-     double Longitude;
-     double Height;
-     quint64 timeGlobalPosition;
-
-     // Position and Attitude
-     //Position
-     double Xpos;
-     double Ypos;
-     double Zpos;
-     quint64 TimeActualPosition;
-     //Speed
-     double VXpos;
-     double VYpos;
-     double VZpos;
-     quint64 TimeActualSpeed;
-     //Attitude
-     double roll;
-     double pitch;
-     double yaw;
-     quint64 TimeActualAttitude;
-
-     //Sensor Biases
-     //Acelerometer
-     double Axb;
-     double Ayb;
-     double Azb;
-
-     //Gyro
-     double Gxb;
-     double Gyb;
-     double Gzb;
-     quint64 TimeActualBias;
-
-     //Diagnostic
-     double diagFl1;
-     double diagFl2;
-     double diagFl3;
-     int16_t diagSh1;
-     int16_t diagSh2;
-     int16_t diagSh3;
-     quint64 timeDiagnostic;
-
-     //CPU Load
-     uint8_t sensLoad;
-     uint8_t ctrlLoad;
-     uint8_t batVolt;
-     quint64 timeCpuLoad;
-
-     //navigation data
-     double u_m;
-     double phi_c;
-     double theta_c;
-     double psiDot_c;
-     double ay_body;
-     double totalDist;
-     double dist2Go;
-     uint8_t fromWP;
-     uint8_t toWP;
-     quint64 timeNavigation;
-
-     // Data Log
-     double Logfl_1;
-     double Logfl_2;
-     double Logfl_3;
-     double Logfl_4;
-     double Logfl_5;
-     double Logfl_6;
-     quint64 timeDataLog;
-
-     //pwm commands
-     uint16_t dt_c; ///< AutoPilot's throttle command
-     uint16_t dla_c; ///< AutoPilot's left aileron command
-     uint16_t dra_c; ///< AutoPilot's right aileron command
-     uint16_t dr_c; ///< AutoPilot's rudder command
-     uint16_t dle_c; ///< AutoPilot's left elevator command
-     uint16_t dre_c; ///< AutoPilot's right elevator command
-     uint16_t dlf_c; ///< AutoPilot's left  flap command
-     uint16_t drf_c; ///< AutoPilot's right flap command
-     uint16_t aux1; ///< AutoPilot's aux1 command
-     uint16_t aux2; ///< AutoPilot's aux2 command
-     quint64 timePWMCommand;
-
-     //filtered data
-     double aX; ///< Accelerometer X value (m/s^2)
-     double aY; ///< Accelerometer Y value (m/s^2)
-     double aZ; ///< Accelerometer Z value (m/s^2)
-     double gX; ///< Gyro X value (rad/s)
-     double gY; ///< Gyro Y value (rad/s)
-     double gZ; ///< Gyro Z value (rad/s)
-     double mX; ///< Magnetometer X (normalized to 1)
-     double mY; ///< Magnetometer Y (normalized to 1)
-     double mZ; ///< Magnetometer Z (normalized to 1)
-     quint64 timeFiltered;
-
-     //gps date and time
-     uint8_t year; ///< Year reported by Gps
-     uint8_t month; ///< Month reported by Gps
-     uint8_t day; ///< Day reported by Gps
-     uint8_t hour; ///< Hour reported by Gps
-     uint8_t min; ///< Min reported by Gps
-     uint8_t sec; ///< Sec reported by Gps
-     uint8_t visSat; ///< Visible sattelites reported by Gps
-     quint64 timeGPSDateTime;
-
-
 private:
     Ui::SlugsDataSensorView *ui;
-    void loadParameters();
 
 
 };
diff --git a/src/ui/SlugsDataSensorView.ui b/src/ui/SlugsDataSensorView.ui
index 8c61108fb347415f7583116a3cbba48a98ce9fbc..917e4571738f72967e0f48d57d1ea724e850e682 100644
--- a/src/ui/SlugsDataSensorView.ui
+++ b/src/ui/SlugsDataSensorView.ui
@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>394</width>
+    <width>399</width>
     <height>667</height>
    </rect>
   </property>
@@ -31,11 +31,11 @@
   <property name="windowTitle">
    <string>Form</string>
   </property>
-  <layout class="QHBoxLayout" name="horizontalLayout_3">
+  <layout class="QVBoxLayout" name="verticalLayout_17">
    <item>
     <widget class="QTabWidget" name="SlugsSensorView_tabWidget">
      <property name="currentIndex">
-      <number>3</number>
+      <number>2</number>
      </property>
      <widget class="QWidget" name="tab">
       <attribute name="title">
@@ -496,7 +496,7 @@
                </widget>
               </item>
               <item row="0" column="1">
-               <widget class="QPlainTextEdit" name="m_SlugAttitudeRoll_plainTextEdit">
+               <widget class="QPlainTextEdit" name="m_Roll">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -556,7 +556,7 @@
                </widget>
               </item>
               <item row="0" column="3">
-               <widget class="QPlainTextEdit" name="m_SlugAttitudePitch_plainTextEdit">
+               <widget class="QPlainTextEdit" name="m_Pitch">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -616,7 +616,7 @@
                </widget>
               </item>
               <item row="0" column="5">
-               <widget class="QPlainTextEdit" name="m_SlugAttitudeYaw_plainTextEdit">
+               <widget class="QPlainTextEdit" name="m_Yaw">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -708,7 +708,7 @@
                </widget>
               </item>
               <item row="0" column="1">
-               <widget class="QTextEdit" name="m_SlugsUm_textEdit">
+               <widget class="QTextEdit" name="m_Um">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -768,7 +768,7 @@
                </widget>
               </item>
               <item row="0" column="3">
-               <widget class="QTextEdit" name="m_SlugsFromWP_textEdit">
+               <widget class="QTextEdit" name="m_FromWP">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -828,7 +828,7 @@
                </widget>
               </item>
               <item row="1" column="1">
-               <widget class="QTextEdit" name="m_SlugsPitchC_textEdit">
+               <widget class="QTextEdit" name="m_PitchC">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -888,7 +888,7 @@
                </widget>
               </item>
               <item row="1" column="3">
-               <widget class="QTextEdit" name="m_SlugsToWP_textEdit">
+               <widget class="QTextEdit" name="m_ToWP">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -948,7 +948,7 @@
                </widget>
               </item>
               <item row="2" column="1">
-               <widget class="QTextEdit" name="m_SlugsPsidC_textEdit">
+               <widget class="QTextEdit" name="m_PsidC">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1021,7 +1021,7 @@
                </widget>
               </item>
               <item row="3" column="1">
-               <widget class="QTextEdit" name="m_SlugsPhiC_textEdit">
+               <widget class="QTextEdit" name="m_PhiC">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1081,7 +1081,7 @@
                </widget>
               </item>
               <item row="3" column="3">
-               <widget class="QTextEdit" name="m_SlugsTotRun_textEdit">
+               <widget class="QTextEdit" name="m_TotRun">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1141,7 +1141,7 @@
                </widget>
               </item>
               <item row="4" column="1">
-               <widget class="QTextEdit" name="m_SlugsAyBody_textEdit">
+               <widget class="QTextEdit" name="m_AyBody">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1201,7 +1201,7 @@
                </widget>
               </item>
               <item row="4" column="3">
-               <widget class="QTextEdit" name="m_SlugsDistToGround_textEdit">
+               <widget class="QTextEdit" name="m_DistToGo">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1269,7 +1269,7 @@
                </widget>
               </item>
               <item row="0" column="1">
-               <widget class="QTextEdit" name="m_SlugsFl1_textEdit">
+               <widget class="QTextEdit" name="m_Fl1">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1311,7 +1311,7 @@
                </widget>
               </item>
               <item row="0" column="3">
-               <widget class="QTextEdit" name="m_SlugsSh1_textEdit">
+               <widget class="QTextEdit" name="m_Sh1">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1353,7 +1353,7 @@
                </widget>
               </item>
               <item row="1" column="1">
-               <widget class="QTextEdit" name="m_SlugsFl2_textEdit">
+               <widget class="QTextEdit" name="m_Fl2">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1395,7 +1395,7 @@
                </widget>
               </item>
               <item row="1" column="3">
-               <widget class="QTextEdit" name="m_SlugsSh2_textEdit">
+               <widget class="QTextEdit" name="m_Sh2">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1437,7 +1437,7 @@
                </widget>
               </item>
               <item row="2" column="1">
-               <widget class="QTextEdit" name="m_SlugsFl3_textEdit">
+               <widget class="QTextEdit" name="m_Fl3">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1479,7 +1479,7 @@
                </widget>
               </item>
               <item row="2" column="3">
-               <widget class="QTextEdit" name="m_SlugsSh3_textEdit">
+               <widget class="QTextEdit" name="m_Sh3">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1541,7 +1541,7 @@
                </widget>
               </item>
               <item row="0" column="1">
-               <widget class="QTextEdit" name="m_logFl1_textEdit">
+               <widget class="QTextEdit" name="m_logFl1">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1583,7 +1583,7 @@
                </widget>
               </item>
               <item row="0" column="3">
-               <widget class="QTextEdit" name="m_logFl4_textEdit">
+               <widget class="QTextEdit" name="m_logFl4">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1625,7 +1625,7 @@
                </widget>
               </item>
               <item row="1" column="1">
-               <widget class="QTextEdit" name="m_logFl2_textEdit">
+               <widget class="QTextEdit" name="m_logFl2">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1667,7 +1667,7 @@
                </widget>
               </item>
               <item row="1" column="3">
-               <widget class="QTextEdit" name="m_logFl5_textEdit">
+               <widget class="QTextEdit" name="m_logFl5">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1709,7 +1709,7 @@
                </widget>
               </item>
               <item row="2" column="1">
-               <widget class="QTextEdit" name="m_logFl3_textEdit">
+               <widget class="QTextEdit" name="m_logFl3">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -1751,7 +1751,7 @@
                </widget>
               </item>
               <item row="2" column="3">
-               <widget class="QTextEdit" name="m_logFl6_textEdit">
+               <widget class="QTextEdit" name="m_logFl6">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
                   <horstretch>0</horstretch>
@@ -2068,7 +2068,7 @@
                  </widget>
                 </item>
                 <item row="2" column="1">
-                 <widget class="QLineEdit" name="m_Gpsheigth">
+                 <widget class="QLineEdit" name="m_GpsHeight">
                   <property name="minimumSize">
                    <size>
                     <width>60</width>
@@ -2715,58 +2715,6 @@
                 </property>
                </widget>
               </item>
-              <item row="4" column="0">
-               <widget class="QLabel" name="label_20">
-                <property name="minimumSize">
-                 <size>
-                  <width>0</width>
-                  <height>0</height>
-                 </size>
-                </property>
-                <property name="maximumSize">
-                 <size>
-                  <width>16777215</width>
-                  <height>16777215</height>
-                 </size>
-                </property>
-                <property name="font">
-                 <font>
-                  <pointsize>10</pointsize>
-                  <weight>75</weight>
-                  <bold>true</bold>
-                 </font>
-                </property>
-                <property name="text">
-                 <string>FailSafe</string>
-                </property>
-               </widget>
-              </item>
-              <item row="4" column="2">
-               <widget class="QLabel" name="label_25">
-                <property name="minimumSize">
-                 <size>
-                  <width>0</width>
-                  <height>0</height>
-                 </size>
-                </property>
-                <property name="maximumSize">
-                 <size>
-                  <width>16777215</width>
-                  <height>16777215</height>
-                 </size>
-                </property>
-                <property name="font">
-                 <font>
-                  <pointsize>10</pointsize>
-                  <weight>75</weight>
-                  <bold>true</bold>
-                 </font>
-                </property>
-                <property name="text">
-                 <string>Available</string>
-                </property>
-               </widget>
-              </item>
               <item row="0" column="1">
                <widget class="QLineEdit" name="m_pwmThro">
                 <property name="sizePolicy">
@@ -2798,6 +2746,12 @@
                  </sizepolicy>
                 </property>
                 <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
                  <size>
                   <width>80</width>
                   <height>18</height>
@@ -2814,6 +2768,12 @@
                  </sizepolicy>
                 </property>
                 <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
                  <size>
                   <width>80</width>
                   <height>18</height>
@@ -2831,21 +2791,11 @@
                 </property>
                 <property name="minimumSize">
                  <size>
-                  <width>80</width>
+                  <width>60</width>
                   <height>18</height>
                  </size>
                 </property>
-               </widget>
-              </item>
-              <item row="4" column="1">
-               <widget class="QLineEdit" name="m_pwmFailSafe">
-                <property name="sizePolicy">
-                 <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
-                  <horstretch>60</horstretch>
-                  <verstretch>18</verstretch>
-                 </sizepolicy>
-                </property>
-                <property name="minimumSize">
+                <property name="maximumSize">
                  <size>
                   <width>80</width>
                   <height>18</height>
@@ -2862,6 +2812,12 @@
                  </sizepolicy>
                 </property>
                 <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
                  <size>
                   <width>80</width>
                   <height>18</height>
@@ -2878,6 +2834,12 @@
                  </sizepolicy>
                 </property>
                 <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
                  <size>
                   <width>80</width>
                   <height>18</height>
@@ -2894,6 +2856,12 @@
                  </sizepolicy>
                 </property>
                 <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
                  <size>
                   <width>80</width>
                   <height>18</height>
@@ -2911,29 +2879,16 @@
                 </property>
                 <property name="minimumSize">
                  <size>
-                  <width>80</width>
+                  <width>60</width>
                   <height>18</height>
                  </size>
                 </property>
-               </widget>
-              </item>
-              <item row="4" column="3">
-               <widget class="QLineEdit" name="m_pwmAvailable">
-                <property name="sizePolicy">
-                 <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
-                  <horstretch>60</horstretch>
-                  <verstretch>18</verstretch>
-                 </sizepolicy>
-                </property>
-                <property name="minimumSize">
+                <property name="maximumSize">
                  <size>
                   <width>80</width>
                   <height>18</height>
                  </size>
                 </property>
-                <property name="text">
-                 <string/>
-                </property>
                </widget>
               </item>
              </layout>
@@ -2949,6 +2904,1584 @@
       <attribute name="title">
        <string>Tab 3</string>
       </attribute>
+      <layout class="QVBoxLayout" name="verticalLayout_16">
+       <item>
+        <layout class="QHBoxLayout" name="horizontalLayout_4">
+         <item>
+          <widget class="QGroupBox" name="sensorBiases_groupBox_2">
+           <property name="minimumSize">
+            <size>
+             <width>0</width>
+             <height>0</height>
+            </size>
+           </property>
+           <property name="maximumSize">
+            <size>
+             <width>132123</width>
+             <height>123123</height>
+            </size>
+           </property>
+           <property name="title">
+            <string>CPU Load</string>
+           </property>
+           <layout class="QVBoxLayout" name="verticalLayout_12">
+            <item>
+             <layout class="QGridLayout" name="gridLayout_10">
+              <item row="0" column="0">
+               <widget class="QLabel" name="label_55">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Sensor</string>
+                </property>
+               </widget>
+              </item>
+              <item row="0" column="1">
+               <widget class="QTextEdit" name="ed_sens">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <widget class="QLabel" name="label_57">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Control</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="1">
+               <widget class="QTextEdit" name="ed_control">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="0">
+               <widget class="QLabel" name="label_59">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Batt Volt</string>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="1">
+               <widget class="QTextEdit" name="ed_batvolt">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+           </layout>
+          </widget>
+         </item>
+         <item>
+          <widget class="QGroupBox" name="sensorBiases_groupBox_3">
+           <property name="minimumSize">
+            <size>
+             <width>0</width>
+             <height>0</height>
+            </size>
+           </property>
+           <property name="maximumSize">
+            <size>
+             <width>132123</width>
+             <height>123123</height>
+            </size>
+           </property>
+           <property name="title">
+            <string>Air Data</string>
+           </property>
+           <layout class="QVBoxLayout" name="verticalLayout_13">
+            <item>
+             <layout class="QGridLayout" name="gridLayout_11">
+              <item row="0" column="0">
+               <widget class="QLabel" name="label_56">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Dynamic</string>
+                </property>
+               </widget>
+              </item>
+              <item row="0" column="1">
+               <widget class="QTextEdit" name="ed_dynamic">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <widget class="QLabel" name="label_58">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Static</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="1">
+               <widget class="QTextEdit" name="ed_static">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="0">
+               <widget class="QLabel" name="label_60">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Temperature</string>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="1">
+               <widget class="QTextEdit" name="ed_temp">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+           </layout>
+          </widget>
+         </item>
+        </layout>
+       </item>
+       <item>
+        <widget class="QGroupBox" name="sensorBiases_groupBox_4">
+         <property name="minimumSize">
+          <size>
+           <width>311</width>
+           <height>213</height>
+          </size>
+         </property>
+         <property name="maximumSize">
+          <size>
+           <width>371</width>
+           <height>213</height>
+          </size>
+         </property>
+         <property name="title">
+          <string>Filtered Data</string>
+         </property>
+         <layout class="QVBoxLayout" name="verticalLayout_14">
+          <item>
+           <layout class="QGridLayout" name="gridLayout_15">
+            <item row="0" column="0">
+             <layout class="QGridLayout" name="gridLayout_12">
+              <item row="0" column="0">
+               <widget class="QLabel" name="label_20">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Ax</string>
+                </property>
+               </widget>
+              </item>
+              <item row="0" column="1">
+               <widget class="QTextEdit" name="m_Axf">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <widget class="QLabel" name="label_61">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Ay</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="1">
+               <widget class="QTextEdit" name="m_Ayf">
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="0">
+               <widget class="QLabel" name="label_63">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Az</string>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="1">
+               <widget class="QTextEdit" name="m_Azf">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+            <item row="0" column="1" rowspan="2">
+             <layout class="QGridLayout" name="gridLayout_14">
+              <item row="0" column="0">
+               <widget class="QLabel" name="label_25">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Gx</string>
+                </property>
+               </widget>
+              </item>
+              <item row="0" column="1">
+               <widget class="QTextEdit" name="m_Gxf">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <widget class="QLabel" name="label_62">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Gy</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="1">
+               <widget class="QTextEdit" name="m_Gyf">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="0">
+               <widget class="QLabel" name="label_64">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Gz</string>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="1">
+               <widget class="QTextEdit" name="m_Gzf">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+            <item row="1" column="0" rowspan="2">
+             <layout class="QGridLayout" name="gridLayout_13">
+              <item row="0" column="0">
+               <widget class="QLabel" name="label_66">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Mx</string>
+                </property>
+               </widget>
+              </item>
+              <item row="0" column="1">
+               <widget class="QTextEdit" name="m_Mxf">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <widget class="QLabel" name="label_67">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>My</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="1">
+               <widget class="QTextEdit" name="m_Myf">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="0">
+               <widget class="QLabel" name="label_65">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Mz</string>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="1">
+               <widget class="QTextEdit" name="m_Mzf">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+            <item row="2" column="1">
+             <spacer name="verticalSpacer_3">
+              <property name="orientation">
+               <enum>Qt::Vertical</enum>
+              </property>
+              <property name="sizeHint" stdset="0">
+               <size>
+                <width>20</width>
+                <height>40</height>
+               </size>
+              </property>
+             </spacer>
+            </item>
+           </layout>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item>
+        <widget class="QGroupBox" name="sensorBiases_groupBox_5">
+         <property name="minimumSize">
+          <size>
+           <width>311</width>
+           <height>213</height>
+          </size>
+         </property>
+         <property name="maximumSize">
+          <size>
+           <width>361</width>
+           <height>213</height>
+          </size>
+         </property>
+         <property name="title">
+          <string>Raw Data</string>
+         </property>
+         <layout class="QVBoxLayout" name="verticalLayout_15">
+          <item>
+           <layout class="QGridLayout" name="gridLayout_16">
+            <item row="0" column="0">
+             <layout class="QGridLayout" name="gridLayout_17">
+              <item row="0" column="0">
+               <widget class="QLabel" name="label_68">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Ax</string>
+                </property>
+               </widget>
+              </item>
+              <item row="0" column="1">
+               <widget class="QTextEdit" name="m_Axr">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <widget class="QLabel" name="label_69">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Ay</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="1">
+               <widget class="QTextEdit" name="m_Ayr">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="0">
+               <widget class="QLabel" name="label_70">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Az</string>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="1">
+               <widget class="QTextEdit" name="m_Azr">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+            <item row="0" column="1" rowspan="2">
+             <layout class="QGridLayout" name="gridLayout_18">
+              <item row="0" column="0">
+               <widget class="QLabel" name="label_71">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Gx</string>
+                </property>
+               </widget>
+              </item>
+              <item row="0" column="1">
+               <widget class="QTextEdit" name="m_Gxr">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <widget class="QLabel" name="label_72">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Gy</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="1">
+               <widget class="QTextEdit" name="m_Gyr">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="0">
+               <widget class="QLabel" name="label_73">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Gz</string>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="1">
+               <widget class="QTextEdit" name="m_Gzr">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+            <item row="1" column="0" rowspan="2">
+             <layout class="QGridLayout" name="gridLayout_19">
+              <item row="0" column="0">
+               <widget class="QLabel" name="label_74">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Mx</string>
+                </property>
+               </widget>
+              </item>
+              <item row="0" column="1">
+               <widget class="QTextEdit" name="m_Mxr">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <widget class="QLabel" name="label_75">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>My</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="1">
+               <widget class="QTextEdit" name="m_Myr">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="0">
+               <widget class="QLabel" name="label_76">
+                <property name="minimumSize">
+                 <size>
+                  <width>0</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>16777215</width>
+                  <height>16777215</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Mz</string>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="1">
+               <widget class="QTextEdit" name="m_Mzr">
+                <property name="minimumSize">
+                 <size>
+                  <width>60</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="maximumSize">
+                 <size>
+                  <width>80</width>
+                  <height>18</height>
+                 </size>
+                </property>
+                <property name="font">
+                 <font>
+                  <pointsize>10</pointsize>
+                 </font>
+                </property>
+                <property name="frameShape">
+                 <enum>QFrame::WinPanel</enum>
+                </property>
+                <property name="verticalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="horizontalScrollBarPolicy">
+                 <enum>Qt::ScrollBarAlwaysOff</enum>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+            <item row="2" column="1">
+             <spacer name="verticalSpacer_4">
+              <property name="orientation">
+               <enum>Qt::Vertical</enum>
+              </property>
+              <property name="sizeHint" stdset="0">
+               <size>
+                <width>20</width>
+                <height>40</height>
+               </size>
+              </property>
+             </spacer>
+            </item>
+           </layout>
+          </item>
+         </layout>
+        </widget>
+       </item>
+      </layout>
      </widget>
      <widget class="QWidget" name="tab_2">
       <attribute name="title">
diff --git a/src/ui/SlugsPIDControl.cpp b/src/ui/SlugsPIDControl.cpp
index 6fe74d575e2cdb916e039aa47990dc18b2e83960..001a9e734d4b9d61c6ddade2c2729e15491b16b6 100644
--- a/src/ui/SlugsPIDControl.cpp
+++ b/src/ui/SlugsPIDControl.cpp
@@ -44,11 +44,13 @@ void SlugsPIDControl::activeUasSet(UASInterface* uas)
 {
     SlugsMAV* slugsMav = dynamic_cast<SlugsMAV*>(uas);
 
+#ifdef MAVLINK_ENABLED_SLUGS
     if (slugsMav != NULL)
     {
-        connect(slugsMav,SIGNAL(slugsActionAck(int,uint8_t,uint8_t)),this,SLOT(recibeMensaje(int,uint8_t,uint8_t)));
+        connect(slugsMav,SIGNAL(slugsActionAck(int,const mavlink_action_ack_t&)),this,SLOT(recibeMensaje(int,mavlink_action_ack_t)));
     }
 
+#endif // MAVLINK_ENABLED_SLUG
     // Set this UAS as active if it is the first one
         if(activeUAS == 0)
         {
@@ -154,6 +156,12 @@ void SlugsPIDControl::changeColor_RED_AirSpeed_groupBox(QString text)
 
 void SlugsPIDControl::changeColor_GREEN_AirSpeed_groupBox()
 {
+
+  SlugsMAV* slugsMav = dynamic_cast<SlugsMAV*>(activeUAS);
+
+  if (slugsMav != NULL)
+  {
+
     //create the packet
     pidMessage.target = activeUAS->getUASID();
     pidMessage.idx = 0;
@@ -161,16 +169,13 @@ void SlugsPIDControl::changeColor_GREEN_AirSpeed_groupBox()
     pidMessage.iVal = ui->dT_I_set->text().toFloat();
     pidMessage.dVal = ui->dT_D_set->text().toFloat();
 
-    UAS *uas = dynamic_cast<UAS*>(UASManager::instance()->getActiveUAS());
-
     mavlink_message_t msg;
 
     mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage);
-    uas->sendMessage(msg);
-
-
+    slugsMav->sendMessage(msg);
 
     ui->AirSpeedHold_groupBox->setStyleSheet(GREENcolorStyle);
+  }
 }
 
 
@@ -335,7 +340,11 @@ void SlugsPIDControl::connect_Pitch2dT_LineEdit()
     connect(ui->P2dT_FF_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_Pitch2dT_groupBox(QString)));
 }
 
-void SlugsPIDControl::recibeMensaje(int systemId, uint8_t action, uint8_t result)
+#ifdef MAVLINK_ENABLED_SLUGS
+
+void SlugsPIDControl::recibeMensaje(int systemId, const mavlink_action_ack_t& action)
 {
-    ui->recepcion_label->setText("Mensaje Recibido: " + QString::number(action));
+    ui->recepcion_label->setText(QString::number(action.action));
 }
+
+#endif // MAVLINK_ENABLED_SLUGS
diff --git a/src/ui/SlugsPIDControl.h b/src/ui/SlugsPIDControl.h
index 700a677604a9ead3fcab964ce53164236270766d..600ffc6f058783b3312bf6139b3f3600fbcfb0b9 100644
--- a/src/ui/SlugsPIDControl.h
+++ b/src/ui/SlugsPIDControl.h
@@ -6,7 +6,7 @@
 #include "UASInterface.h"
 #include "QGCMAVLink.h"
 #include "SlugsMAV.h"
-
+#include "mavlink.h"
 
 namespace Ui {
     class SlugsPIDControl;
@@ -184,9 +184,11 @@ public slots:
 
      //Create, send and get Messages PID
     // void createMessagePID();
+#ifdef MAVLINK_ENABLED_SLUGS
 
-     void recibeMensaje(int systemId, uint8_t action, uint8_t result);
+     void recibeMensaje(int systemId, const mavlink_action_ack_t& action);
 
+#endif // MAVLINK_ENABLED_SLUG
 
 private:
     Ui::SlugsPIDControl *ui;
diff --git a/src/ui/slugshilsim.cc b/src/ui/slugshilsim.cc
index 97f232479ed7b1916d9a7461173687f8ed4ebfe1..5cf3175e62dd21dd2cb3814a83790c50abb37640 100644
--- a/src/ui/slugshilsim.cc
+++ b/src/ui/slugshilsim.cc
@@ -41,7 +41,10 @@ SlugsHilSim::SlugsHilSim(QWidget *parent) :
     rxSocket = new QUdpSocket(this);
     txSocket = new QUdpSocket(this);
 
+    hilLink = NULL;
+
     connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addToCombo(LinkInterface*)));
+    connect(ui->cb_mavlinkLinks, SIGNAL(currentIndexChanged(int)), this, SLOT(linkSelected(int)));
     connect(ui->bt_startHil, SIGNAL(clicked()), this, SLOT(putInHilMode()));
     connect(rxSocket, SIGNAL(readyRead()), this, SLOT(readDatagram()));
 
@@ -54,23 +57,15 @@ SlugsHilSim::~SlugsHilSim()
     delete ui;
 }
 
-void SlugsHilSim::linkAdded(void){
-
-//  ui->cb_mavlinkLinks->clear();
-
-//  QList<LinkInterface *> linkList;
-//  linkList.append(LinkManager::instance()->getLinks()) ;
-
-//  for (int i = 0; i< linkList.size(); i++){
-//    ui->cb_mavlinkLinks->addItem((linkList.takeFirst())->getName());
-//  }
-
-}
-
 void SlugsHilSim::addToCombo(LinkInterface* theLink){
 
   ui->cb_mavlinkLinks->addItem(theLink->getName());
   linksAvailable.insert(ui->cb_mavlinkLinks->count(),theLink);
+
+  if (hilLink == NULL){
+    hilLink = theLink;
+  }
+
 }
 
 void SlugsHilSim::putInHilMode(void){
@@ -86,8 +81,7 @@ void SlugsHilSim::putInHilMode(void){
     msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No);
     msgBox.setDefaultButton(QMessageBox::No);
 
-    if(msgBox.exec() == QMessageBox::Yes)
-    {
+    if(msgBox.exec() == QMessageBox::Yes) {
       rxSocket->disconnectFromHost();
       rxSocket->bind(QHostAddress::Any, ui->ed_rxPort->text().toInt());
       //txSocket->bind(QHostAddress::Broadcast, ui->ed_txPort->text().toInt());
@@ -99,7 +93,6 @@ void SlugsHilSim::putInHilMode(void){
 
       ui->bt_startHil->setText(buttonCaption);
 
-
     } else {
       ui->bt_startHil->setChecked(false);
     }
@@ -170,9 +163,13 @@ void SlugsHilSim::processHilDatagram(const QByteArray* datagram){
   tmpGpsRaw.fix_type = datagram->at(i++);
   tmpGpsTime.visSat  = datagram->at(i++);
 
-  //mavlink_msg_gps_date_time_pack();
+  mavlink_msg_gps_date_time_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsTime);
+  activeUas->sendMessage(hilLink, msg);
+
+  memset(&msg, 0, sizeof(mavlink_message_t));
 
-  //activeUas->sendMessage();
+  mavlink_msg_gps_raw_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsRaw);
+  activeUas->sendMessage(hilLink,msg);
 
   // TODO: this is legacy of old HIL datagram. Need to remove from Simulink model
   i++;
@@ -190,11 +187,6 @@ float SlugsHilSim::getFloatFromDatagram (const QByteArray* datagram, unsigned ch
   tmpF2C.chData[2] = datagram->at((*i)++);
   tmpF2C.chData[3] = datagram->at((*i)++);
 
-
-//  if (uas != NULL) {
-//    //activeUas = uas;
-//  }
-
   return tmpF2C.flData;
 }
 
@@ -207,3 +199,7 @@ uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigne
   return tmpU2C.uiData;
 
 }
+
+void SlugsHilSim::linkSelected(int cbIndex){
+  //hilLink = linksAvailable
+}
diff --git a/src/ui/slugshilsim.h b/src/ui/slugshilsim.h
index 7434aec4b14e82ca4a07845161a53afa939c7086..dc0d8d1d7a90909656c29145de053c7231470cb3 100644
--- a/src/ui/slugshilsim.h
+++ b/src/ui/slugshilsim.h
@@ -60,7 +60,6 @@ protected:
     UAS* activeUas;
 
 public slots:
-    void linkAdded (void);
 
     /**
      * @brief Adds a link to the combo box listing so the user can select a link
@@ -98,6 +97,12 @@ public slots:
      */
     void activeUasSet(UASInterface* uas);
 
+    /**
+     * @brief Called when the Link combobox selects a new link.
+     *
+     * @param uas The new index of the selected link
+     */
+    void linkSelected (int cbIndex);
 
 public slots: