From 7388388b845bd8af352dc7fed9fb65954eaaa598 Mon Sep 17 00:00:00 2001
From: LM <lm@student.ethz.ch>
Date: Fri, 2 Sep 2011 12:18:59 +0200
Subject: [PATCH] Fixed a bug in the default options

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

diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc
index 80f402110..997541d92 100644
--- a/src/uas/PxQuadMAV.cc
+++ b/src/uas/PxQuadMAV.cc
@@ -132,6 +132,36 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
             emit valueChanged(uasId, "vis. speed z", "m/s", speed.z, time);
         }
         break;
+        case MAVLINK_MSG_ID_CONTROL_STATUS:
+            {
+                mavlink_control_status_t status;
+                mavlink_msg_control_status_decode(&message, &status);
+                // Emit control status vector
+                emit attitudeControlEnabled(static_cast<bool>(status.control_att));
+                emit positionXYControlEnabled(static_cast<bool>(status.control_pos_xy));
+                emit positionZControlEnabled(static_cast<bool>(status.control_pos_z));
+                emit positionYawControlEnabled(static_cast<bool>(status.control_pos_yaw));
+
+                // Emit localization status vector
+                emit localizationChanged(this, status.position_fix);
+                emit visionLocalizationChanged(this, status.vision_fix);
+                emit gpsLocalizationChanged(this, status.gps_fix);
+            }
+            break;
+        case MAVLINK_MSG_ID_VISUAL_ODOMETRY:
+            {
+                mavlink_visual_odometry_t pos;
+                mavlink_msg_visual_odometry_decode(&message, &pos);
+                quint64 time = getUnixTime(pos.frame1_time_us);
+                //emit valueChanged(uasId, "vis. time", pos.usec, time);
+                emit valueChanged(uasId, "vis-o. roll", "rad", pos.roll, time);
+                emit valueChanged(uasId, "vis-o. pitch", "rad", pos.pitch, time);
+                emit valueChanged(uasId, "vis-o. yaw", "rad", pos.yaw, time);
+                emit valueChanged(uasId, "vis-o. x", "m", pos.x, time);
+                emit valueChanged(uasId, "vis-o. y", "m", pos.y, time);
+                emit valueChanged(uasId, "vis-o. z", "m", pos.z, time);
+            }
+            break;
         case MAVLINK_MSG_ID_AUX_STATUS: {
             mavlink_aux_status_t status;
             mavlink_msg_aux_status_decode(&message, &status);
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 0f0df0805..edb73983e 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -75,7 +75,7 @@ paramsOnceRequested(false),
 airframe(0),
 attitudeKnown(false),
 paramManager(NULL),
-attitudeStamped(true),
+attitudeStamped(false),
 lastAttitude(0)
 {
     color = UASInterface::getNextColor();
@@ -403,35 +403,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 }
             }
             break;
-
-#ifdef MAVLINK_ENABLED_PIXHAWK
-        case MAVLINK_MSG_ID_CONTROL_STATUS:
-            {
-                mavlink_control_status_t status;
-                mavlink_msg_control_status_decode(&message, &status);
-                // Emit control status vector
-                emit attitudeControlEnabled(static_cast<bool>(status.control_att));
-                emit positionXYControlEnabled(static_cast<bool>(status.control_pos_xy));
-                emit positionZControlEnabled(static_cast<bool>(status.control_pos_z));
-                emit positionYawControlEnabled(static_cast<bool>(status.control_pos_yaw));
-
-                // Emit localization status vector
-                emit localizationChanged(this, status.position_fix);
-                emit visionLocalizationChanged(this, status.vision_fix);
-                emit gpsLocalizationChanged(this, status.gps_fix);
-            }
-            break;
-        case MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE:
-            {
-                mavlink_vision_speed_estimate_t speed;
-                mavlink_msg_vision_speed_estimate_decode(&message, &speed);
-                quint64 time = getUnixTime(speed.usec);
-                emit valueChanged(uasId, "vis.speed x", "m/s", static_cast<double>(speed.x), time);
-                emit valueChanged(uasId, "vis.speed y", "m/s", static_cast<double>(speed.y), time);
-                emit valueChanged(uasId, "vis.speed z", "m/s", static_cast<double>(speed.z), time);
-            }
-            break;
-#endif // PIXHAWK
         case MAVLINK_MSG_ID_RAW_IMU:
             {
                 mavlink_raw_imu_t raw;
-- 
2.22.0