Commit 24fb0976 authored by pixhawk's avatar pixhawk

Added coordinate transform

parent bc8363d8
...@@ -8,7 +8,7 @@ namespace QGC ...@@ -8,7 +8,7 @@ namespace QGC
{ {
const QColor ColorCyan(55, 154, 195); const QColor ColorCyan(55, 154, 195);
quint64 groundTimeUsecs() static quint64 groundTimeUsecs()
{ {
QDateTime time = QDateTime::currentDateTime(); QDateTime time = QDateTime::currentDateTime();
time = time.toUTC(); time = time.toUTC();
......
...@@ -42,6 +42,7 @@ This file is part of the PIXHAWK project ...@@ -42,6 +42,7 @@ This file is part of the PIXHAWK project
#include "MAVLinkSimulationLink.h" #include "MAVLinkSimulationLink.h"
// MAVLINK includes // MAVLINK includes
#include <mavlink.h> #include <mavlink.h>
#include "QGC.h"
/** /**
* Create a simulated link. This link is connected to an input and output file. * Create a simulated link. This link is connected to an input and output file.
...@@ -97,6 +98,11 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile ...@@ -97,6 +98,11 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
// Open packet log // Open packet log
mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName()); mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName());
mavlinkLogFile->open(QIODevice::ReadOnly); mavlinkLogFile->open(QIODevice::ReadOnly);
x = 0;
y = 0;
z = 0;
yaw = 0;
} }
MAVLinkSimulationLink::~MAVLinkSimulationLink() MAVLinkSimulationLink::~MAVLinkSimulationLink()
...@@ -369,6 +375,11 @@ void MAVLinkSimulationLink::mainloop() ...@@ -369,6 +375,11 @@ void MAVLinkSimulationLink::mainloop()
if (rate10hzCounter == 1000 / rate / 9) if (rate10hzCounter == 1000 / rate / 9)
{ {
rate10hzCounter = 1; rate10hzCounter = 1;
// Move X Position
x += sin(QGC::groundTimeUsecs());
y += sin(QGC::groundTimeUsecs());
z += sin(QGC::groundTimeUsecs());
} }
// 1 HZ TASKS // 1 HZ TASKS
...@@ -538,6 +549,25 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) ...@@ -538,6 +549,25 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
// Set mode indepent of mode.target // Set mode indepent of mode.target
status.mode = mode.mode; 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 // EXECUTE OPERATOR ACTIONS
case MAVLINK_MSG_ID_ACTION: case MAVLINK_MSG_ID_ACTION:
{ {
......
...@@ -93,6 +93,8 @@ protected: ...@@ -93,6 +93,8 @@ protected:
// UAS properties // UAS properties
float roll, pitch, yaw; float roll, pitch, yaw;
float x, y, z;
float spX, spY, spZ, spYaw;
int battery; int battery;
QTimer* timer; QTimer* timer;
......
...@@ -199,11 +199,27 @@ void HSIDisplay::paintDisplay() ...@@ -199,11 +199,27 @@ void HSIDisplay::paintDisplay()
paintText(tr("E"), ringColor, 3.5f, + baseRadius + 2.0f, - 1.75f, &painter); paintText(tr("E"), ringColor, 3.5f, + baseRadius + 2.0f, - 1.75f, &painter);
paintText(tr("W"), ringColor, 3.5f, - baseRadius - 5.5f, - 1.75f, &painter); paintText(tr("W"), ringColor, 3.5f, - baseRadius - 5.5f, - 1.75f, &painter);
// FIXME Just for testing // // FIXME Just for testing
bodyXSetCoordinate = 0.95 * bodyXSetCoordinate + 0.05 * uiXSetCoordinate; // bodyXSetCoordinate = 0.95 * bodyXSetCoordinate + 0.05 * uiXSetCoordinate;
bodyYSetCoordinate = 0.95 * bodyYSetCoordinate + 0.05 * uiYSetCoordinate; // bodyYSetCoordinate = 0.95 * bodyYSetCoordinate + 0.05 * uiYSetCoordinate;
bodyZSetCoordinate = 0.95 * bodyZSetCoordinate + 0.05 * uiZSetCoordinate; // bodyZSetCoordinate = 0.95 * bodyZSetCoordinate + 0.05 * uiZSetCoordinate;
bodyYawSet = 0.95 * bodyYawSet + 0.05 * uiYawSet; // 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) QPointF HSIDisplay::screenToMetricBody(QPointF ref)
......
...@@ -81,7 +81,12 @@ protected slots: ...@@ -81,7 +81,12 @@ protected slots:
void mouseDoubleClickEvent(QMouseEvent* event); void mouseDoubleClickEvent(QMouseEvent* event);
protected: protected:
/** @brief Get color from GPS signal-to-noise colormap */
static QColor getColorForSNR(float snr); 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 */ /** @brief Screen coordinates of widget to metric coordinates in body frame */
QPointF screenToMetricBody(QPointF ref); QPointF screenToMetricBody(QPointF ref);
/** @brief Reference coordinates to metric coordinates */ /** @brief Reference coordinates to metric coordinates */
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment