From 0bdc6c3429706e7875237f6a9ea496cb6a2e4f5f Mon Sep 17 00:00:00 2001
From: pixhawk <pixhawk@student.ethz.ch>
Date: Sat, 24 Apr 2010 19:53:20 +0200
Subject: [PATCH] Minor MAVLink update

---
 src/comm/MAVLinkSimulationLink.cc |  1 -
 src/uas/UAS.cc                    | 31 ++++++++-----------------------
 src/uas/UAS.h                     |  4 +---
 3 files changed, 9 insertions(+), 27 deletions(-)

diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index c782f77b1..ec2d7ace0 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 a2cf1b0e3..8e01db733 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 588b1ef37..a34ed90b3 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();
 
-- 
2.22.0