From 7f9de5e1eaec8a221538856c58c66239dfedee04 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Fri, 9 Sep 2011 14:18:21 +0200 Subject: [PATCH] Cleaned up coordinate frame of HSI --- src/ui/HSIDisplay.cc | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 924c66a05..2aa8caf15 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -58,8 +58,8 @@ HSIDisplay::HSIDisplay(QWidget *parent) : posXSet(0.0f), posYSet(0.0f), posZSet(0.0f), - attXSaturation(0.5f), - attYSaturation(0.5f), + attXSaturation(0.2f), + attYSaturation(0.2f), attYawSaturation(0.5f), posXSaturation(0.05f), posYSaturation(0.05f), @@ -474,7 +474,7 @@ QPointF HSIDisplay::metricWorldToBody(QPointF world) { // First translate to body-centered coordinates // Rotate around -yaw - float angle = yaw + M_PI; + float angle = -yaw - M_PI; QPointF result(cos(angle) * (x - world.x()) - sin(angle) * (y - world.y()), sin(angle) * (x - world.x()) + cos(angle) * (y - world.y())); return result; } @@ -944,7 +944,7 @@ void HSIDisplay::drawPositionDirection(float xRef, float yRef, float radius, con { if (xyControlKnown && xyControlEnabled) { // Draw the needle - const float maxWidth = radius / 10.0f; + const float maxWidth = radius / 5.0f; const float minWidth = maxWidth * 0.3f; float angle = atan2(posXSet, -posYSet); @@ -984,10 +984,9 @@ void HSIDisplay::drawAttitudeDirection(float xRef, float yRef, float radius, con const float maxWidth = radius / 10.0f; const float minWidth = maxWidth * 0.3f; - float angle = atan2(attXSet, attYSet); - angle -= (float)M_PI/2.0f; + float angle = atan2(attYSet, -attXSet); - radius *= sqrt(pow(attXSet, 2) + pow(attYSet, 2)) / sqrt(attXSaturation + attYSaturation); + radius *= sqrt(attXSet*attXSet + attYSet*attYSet) / sqrt(attXSaturation*attXSaturation + attYSaturation*attYSaturation); QPolygonF p(6); -- 2.22.0