From a076f256428c1a8d049c23bb2ed87b985a910e6f Mon Sep 17 00:00:00 2001
From: pixhawk <pixhawk@student.ethz.ch>
Date: Tue, 22 Jun 2010 09:13:16 +0200
Subject: [PATCH] Fixed local global transform

---
 src/ui/HSIDisplay.cc                   | 14 ++++++++++----
 src/ui/HSIDisplay.h                    |  1 +
 src/ui/watchdog/WatchdogProcessView.cc |  2 +-
 3 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index 7ede06b9d..34c7963dc 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -70,7 +70,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
         localAvailable(0),
         roll(0),
         pitch(0),
-        yaw(0.0f),
+        yaw(1.0f), // FIXME Should be 0
         bodyXSetCoordinate(0.0f),
         bodyYSetCoordinate(0.0f),
         bodyZSetCoordinate(0.0f),
@@ -83,7 +83,8 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
         positionLock(false),
         attControlEnabled(false),
         xyControlEnabled(false),
-        zControlEnabled(false)
+        zControlEnabled(false),
+        mavInitialized(false)
 {
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
     refreshTimer->setInterval(60);
@@ -272,7 +273,7 @@ QPointF HSIDisplay::metricWorldToBody(QPointF world)
 {
     // First translate to body-centered coordinates
     // Rotate around -yaw
-    QPointF result(cos(yaw) * (world.x() - x) + -sin(yaw) * (world.x() - x), sin(yaw) * (world.y() - y) + cos(yaw) * (world.y() - y));
+    QPointF result(cos(yaw) * (x - world.x()) + -sin(yaw) * (x - world.x()), sin(yaw) * (y - world.y()) + cos(yaw) * (y - world.y()));
     return result;
 }
 
@@ -400,7 +401,9 @@ void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
     uiXSetCoordinate = sp.x();
     uiYSetCoordinate = sp.y();
 
-    if (uas)
+    qDebug() << "Attempting to set new setpoint at x: " << x << "metric y:" << y;
+
+    if (uas && mavInitialized)
     {
         uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
         qDebug() << "Setting new setpoint at x: " << x << "metric y:" << y;
@@ -445,6 +448,9 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir
     bodyYSetCoordinate = yDesired;
     bodyZSetCoordinate = zDesired;
     bodyYawSet = yawDesired;
+    mavInitialized = true;
+
+    qDebug() << "Received setpoint at x: " << x << "metric y:" << y;
 //    posXSet = xDesired;
 //    posYSet = yDesired;
 //    posZSet = zDesired;
diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h
index 20b7ba3ab..18e7a8952 100644
--- a/src/ui/HSIDisplay.h
+++ b/src/ui/HSIDisplay.h
@@ -195,6 +195,7 @@ protected:
     bool attControlEnabled;
     bool xyControlEnabled;
     bool zControlEnabled;
+    bool mavInitialized;      ///< The MAV is initialized once the setpoint has been received
 
 private:
 };
diff --git a/src/ui/watchdog/WatchdogProcessView.cc b/src/ui/watchdog/WatchdogProcessView.cc
index 37d22f407..1a65ac642 100644
--- a/src/ui/watchdog/WatchdogProcessView.cc
+++ b/src/ui/watchdog/WatchdogProcessView.cc
@@ -1,7 +1,7 @@
 #include "WatchdogProcessView.h"
 #include "ui_WatchdogProcessView.h"
 
-WatchdogProcessView::WatchdogProcessView(QWidget *parent) :
+WatchdogProcessView::WatchdogProcessView(int processid, QWidget *parent) :
     QWidget(parent),
     m_ui(new Ui::WatchdogProcessView)
 {
-- 
2.22.0