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 7ede06b9d8..34c7963dcf 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 20b7ba3ab8..18e7a89523 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 37d22f407c..1a65ac642b 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)
{
--
GitLab