From 24fb0976808b996806ccf4475412982bbbc44d7c Mon Sep 17 00:00:00 2001 From: pixhawk Date: Wed, 16 Jun 2010 08:05:01 +0200 Subject: [PATCH] Added coordinate transform --- src/QGC.h | 2 +- src/comm/MAVLinkSimulationLink.cc | 30 ++++++++++++++++++++++++++++++ src/comm/MAVLinkSimulationLink.h | 2 ++ src/ui/HSIDisplay.cc | 26 +++++++++++++++++++++----- src/ui/HSIDisplay.h | 5 +++++ 5 files changed, 59 insertions(+), 6 deletions(-) diff --git a/src/QGC.h b/src/QGC.h index f8ed9f123..5dfdd881a 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -8,7 +8,7 @@ namespace QGC { const QColor ColorCyan(55, 154, 195); - quint64 groundTimeUsecs() + static quint64 groundTimeUsecs() { QDateTime time = QDateTime::currentDateTime(); time = time.toUTC(); diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 2049e55f7..7892a5f4b 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -42,6 +42,7 @@ This file is part of the PIXHAWK project #include "MAVLinkSimulationLink.h" // MAVLINK includes #include +#include "QGC.h" /** * Create a simulated link. This link is connected to an input and output file. @@ -97,6 +98,11 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile // Open packet log mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName()); mavlinkLogFile->open(QIODevice::ReadOnly); + + x = 0; + y = 0; + z = 0; + yaw = 0; } MAVLinkSimulationLink::~MAVLinkSimulationLink() @@ -369,6 +375,11 @@ void MAVLinkSimulationLink::mainloop() if (rate10hzCounter == 1000 / rate / 9) { rate10hzCounter = 1; + + // Move X Position + x += sin(QGC::groundTimeUsecs()); + y += sin(QGC::groundTimeUsecs()); + z += sin(QGC::groundTimeUsecs()); } // 1 HZ TASKS @@ -538,6 +549,25 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) // Set mode indepent of mode.target status.mode = mode.mode; } + break; + case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: + { + mavlink_local_position_setpoint_set_t set; + mavlink_msg_local_position_setpoint_set_decode(&msg, &set); + spX = set.x; + spY = set.y; + spZ = set.z; + spYaw = set.yaw; + + // Send back new setpoint + mavlink_message_t ret; + mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, spX, spY, spZ, spYaw); + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + //add data into datastream + memcpy(stream+streampointer,buffer, bufferlength); + streampointer += bufferlength; + } + break; // EXECUTE OPERATOR ACTIONS case MAVLINK_MSG_ID_ACTION: { diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h index 0b10e3753..5ff98d011 100644 --- a/src/comm/MAVLinkSimulationLink.h +++ b/src/comm/MAVLinkSimulationLink.h @@ -93,6 +93,8 @@ protected: // UAS properties float roll, pitch, yaw; + float x, y, z; + float spX, spY, spZ, spYaw; int battery; QTimer* timer; diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index e8466ea63..458472460 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -199,11 +199,27 @@ void HSIDisplay::paintDisplay() paintText(tr("E"), ringColor, 3.5f, + baseRadius + 2.0f, - 1.75f, &painter); paintText(tr("W"), ringColor, 3.5f, - baseRadius - 5.5f, - 1.75f, &painter); - // FIXME Just for testing - bodyXSetCoordinate = 0.95 * bodyXSetCoordinate + 0.05 * uiXSetCoordinate; - bodyYSetCoordinate = 0.95 * bodyYSetCoordinate + 0.05 * uiYSetCoordinate; - bodyZSetCoordinate = 0.95 * bodyZSetCoordinate + 0.05 * uiZSetCoordinate; - bodyYawSet = 0.95 * bodyYawSet + 0.05 * uiYawSet; +// // FIXME Just for testing +// bodyXSetCoordinate = 0.95 * bodyXSetCoordinate + 0.05 * uiXSetCoordinate; +// bodyYSetCoordinate = 0.95 * bodyYSetCoordinate + 0.05 * uiYSetCoordinate; +// bodyZSetCoordinate = 0.95 * bodyZSetCoordinate + 0.05 * uiZSetCoordinate; +// bodyYawSet = 0.95 * bodyYawSet + 0.05 * uiYawSet; +} + +QPointF HSIDisplay::metricWorldToBody(QPointF world) +{ + // First translate to body-centered coordinates + // Rotate around -yaw + QPointF result(sin(-yaw) * (world.x() - x), cos(-yaw) * (world.y() - y)); + return result; +} + +QPointF HSIDisplay::metricBodyToWorld(QPointF body) +{ + // First rotate into world coordinates + // then translate to world position + QPointF result((sin(yaw) * body.x()) + x, (cos(yaw) * body.y()) + y); + return result; } QPointF HSIDisplay::screenToMetricBody(QPointF ref) diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 4c7899ec6..a62f65e37 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -81,7 +81,12 @@ protected slots: void mouseDoubleClickEvent(QMouseEvent* event); protected: + /** @brief Get color from GPS signal-to-noise colormap */ static QColor getColorForSNR(float snr); + /** @brief Metric world coordinates to metric body coordinates */ + QPointF metricWorldToBody(QPointF world); + /** @brief Metric body coordinates to metric world coordinates */ + QPointF metricBodyToWorld(QPointF body); /** @brief Screen coordinates of widget to metric coordinates in body frame */ QPointF screenToMetricBody(QPointF ref); /** @brief Reference coordinates to metric coordinates */ -- 2.22.0