diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index c782f77b1d7db654cae24f5cbd6227f13f3d9adb..ec2d7ace0ac34ea81c663225ad7235cf340b726e 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/src/comm/MAVLinkSimulationLink.cc
@@ -155,7 +155,6 @@ void MAVLinkSimulationLink::mainloop()
     attitude_t attitude;
     raw_aux_t rawAuxValues;
     raw_imu_t rawImuValues;
-    raw_sensor_t rawSensorValues;
 
     uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
     int bufferlength;
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index a2cf1b0e3755d737ba57b65a5f4af438b484a7e1..8e01db733a2bde0b242a038449e675a3ad2f2195 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -172,11 +172,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     modeAudio = " is now in " + mode;
                 }
                 currentVoltage = state.vbat;
-                filterVoltage(currentVoltage);
+                lpVoltage = filterVoltage(currentVoltage);
                 if (startVoltage == 0) startVoltage = currentVoltage;
                 timeRemaining = calculateTimeRemaining();
                 //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining;
-                emit batteryChanged(this, filterVoltage(), getChargeLevel(), timeRemaining);
+                emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining);
                 emit voltageChanged(message.sysid, state.vbat/1000.0f);
 
                 // COMMUNICATIONS DROP RATE
@@ -241,11 +241,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "Mag. Z", raw.zmag, time);
             }
             break;
-        case MAVLINK_MSG_ID_RAW_SENSOR:
+        case MAVLINK_MSG_ID_RAW_AUX:
             {
-                raw_sensor_t raw;
-                message_raw_sensor_decode(&message, &raw);
-                quint64 time = raw.msec;
+                raw_aux_t raw;
+                message_raw_aux_decode(&message, &raw);
+                quint64 time = MG::TIME::getGroundTimeNow();//raw.msec;
                 if (time == 0)
                 {
                     time = MG::TIME::getGroundTimeNow();
@@ -258,16 +258,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     }
                     time += onboardTimeOffset;
                 }
-
-                emit valueChanged(uasId, "Accel. X", raw.xacc, time);
-                emit valueChanged(uasId, "Accel. Y", raw.yacc, time);
-                emit valueChanged(uasId, "Accel. Z", raw.zacc, time);
-                emit valueChanged(uasId, "Gyro Phi", raw.xgyro, time);
-                emit valueChanged(uasId, "Gyro Theta", raw.ygyro, time);
-                emit valueChanged(uasId, "Gyro Psi", raw.zgyro, time);
-                emit valueChanged(uasId, "Mag. X", raw.xmag, time);
-                emit valueChanged(uasId, "Mag. Y", raw.ymag, time);
-                emit valueChanged(uasId, "Mag. Z", raw.zmag, time);
                 emit valueChanged(uasId, "Pressure", raw.baro, time);
                 emit valueChanged(uasId, "Temperature", raw.temp, time);
             }
@@ -435,16 +425,12 @@ void UAS::sendMessage(LinkInterface* link, mavlink_message_t message)
     }
 }
 
-float UAS::filterVoltage()
-{
-    return lpVoltage;
-}
-
 /**
  * @param value battery voltage
  */
-float UAS::filterVoltage(float value)
+const float UAS::filterVoltage(float value)
 {
+    return lpVoltage * 0.8f + value * 0.2f;
     /*
     currentVoltage = value;
     static QList<float> voltages<float>(20);
@@ -460,7 +446,6 @@ float UAS::filterVoltage(float value)
     {
         lpVoltage += v;
     }*/
-    return currentVoltage;
 }
 
 void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription)
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 588b1ef37c85e2c5c2834283c12daf988aa6c7dc..a34ed90b3398404babb45c475891b71404f0eb4b 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -72,10 +72,8 @@ public:
     quint64 getUptime();
     /** @brief Get the status flag for the communication */
     int getCommunicationStatus();
-    /** @brief Get low-passed voltage */
-    float filterVoltage();
     /** @brief Add one measurement and get low-passed voltage */
-    float filterVoltage(float value);
+    const float filterVoltage(float value);
     /** @brief Get the links associated with this robot */
     QList<LinkInterface*>* getLinks();