From 09ebae051e22aabcecee623dfe3dc03aa94029b9 Mon Sep 17 00:00:00 2001
From: lm <pixhawk@pixhawk02.(none)>
Date: Fri, 4 Jun 2010 16:13:35 +0200
Subject: [PATCH] Adapted to new MAVLink messages

---
 src/uas/PxQuadMAV.cc | 26 ++++++++++++++++++++++++++
 src/uas/UAS.cc       | 31 -------------------------------
 2 files changed, 26 insertions(+), 31 deletions(-)

diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc
index 72beac8bec..71348cee4c 100644
--- a/src/uas/PxQuadMAV.cc
+++ b/src/uas/PxQuadMAV.cc
@@ -1,4 +1,5 @@
 #include "PxQuadMAV.h"
+#include "GAudioOutput.h"
 
 PxQuadMAV::PxQuadMAV(MAVLinkProtocol* mavlink, int id) :
         UAS(mavlink, id)
@@ -60,6 +61,31 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
             emit valueChanged(uasId, str+".z", vect.z, MG::TIME::getGroundTimeNow());
         }
         break;
+    case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
+        {
+            mavlink_vision_position_estimate_t pos;
+            mavlink_msg_vision_position_estimate_decode(&message, &pos);
+            quint64 time = getUnixTime(pos.usec);
+            emit valueChanged(uasId, "vis. time", pos.usec, time);
+            emit valueChanged(uasId, "vis. roll", pos.roll, time);
+            emit valueChanged(uasId, "vis. pitch", pos.pitch, time);
+            emit valueChanged(uasId, "vis. yaw", pos.yaw, time);
+            emit valueChanged(uasId, "vis. x", pos.x, time);
+            emit valueChanged(uasId, "vis. y", pos.y, time);
+            emit valueChanged(uasId, "vis. z", pos.z, time);
+            emit valueChanged(uasId, "vis. vx", pos.vx, time);
+            emit valueChanged(uasId, "vis. vy", pos.vy, time);
+            emit valueChanged(uasId, "vis. vz", pos.vz, time);
+            emit valueChanged(uasId, "vis. vyaw", pos.vyaw, time);
+            // Set internal state
+            if (!positionLock)
+            {
+                // If position was not locked before, notify positive
+                GAudioOutput::instance()->notifyPositive();
+            }
+            positionLock = true;
+        }
+        break;
     default:
         // Do nothing
         break;
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index c1b6ca9635..f48ce8faf9 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -306,37 +306,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit attitudeChanged(this, mavlink_msg_attitude_get_roll(&message), mavlink_msg_attitude_get_pitch(&message), mavlink_msg_attitude_get_yaw(&message), time);
             }
             break;
-        case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
-            {
-                mavlink_vision_position_estimate_t pos;
-                mavlink_msg_vision_position_estimate_decode(&message, &pos);
-                quint64 time = getUnixTime(pos.usec);
-                emit valueChanged(uasId, "vis. time", pos.usec, time);
-                emit valueChanged(uasId, "vis. roll", pos.roll, time);
-                emit valueChanged(uasId, "vis. pitch", pos.pitch, time);
-                emit valueChanged(uasId, "vis. yaw", pos.yaw, time);
-                emit valueChanged(uasId, "vis. x", pos.x, time);
-                emit valueChanged(uasId, "vis. y", pos.y, time);
-                emit valueChanged(uasId, "vis. z", pos.z, time);
-                // FIXME Only for testing for now
-                emit valueChanged(uasId, "vis. rot r1", pos.r1, time);
-                emit valueChanged(uasId, "vis. rot r2", pos.r2, time);
-                emit valueChanged(uasId, "vis. rot r3", pos.r3, time);
-                emit valueChanged(uasId, "vis. rot r4", pos.r4, time);
-                emit valueChanged(uasId, "vis. rot r5", pos.r5, time);
-                emit valueChanged(uasId, "vis. rot r6", pos.r6, time);
-                emit valueChanged(uasId, "vis. rot r7", pos.r7, time);
-                emit valueChanged(uasId, "vis. rot r8", pos.r8, time);
-                emit valueChanged(uasId, "vis. rot r9", pos.r9, time);
-                // Set internal state
-                if (!positionLock)
-                {
-                    // If position was not locked before, notify positive
-                    GAudioOutput::instance()->notifyPositive();
-                }
-                positionLock = true;
-            }
-            break;
         case MAVLINK_MSG_ID_LOCAL_POSITION:
             //std::cerr << std::endl;
             //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
-- 
GitLab