Merged HSI display

src/comm/MAVLinkSimulationLink.cc

@@ -42,6 +42,7 @@ This file is part of the PIXHAWK project
 #include "MAVLinkSimulationLink.h"
 // MAVLINK includes
 #include <mavlink.h>
+#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:
                 {

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;

src/ui/HSIDisplay.cc

@@ -206,6 +206,22 @@ void HSIDisplay::paintDisplay()
 //    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)
 {
     return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth - x, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth - y);

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 */

src/ui/WaypointList.cc

@@ -66,7 +66,7 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
     connect(m_ui->transmitButton, SIGNAL(clicked()), this, SLOT(transmit()));
 
     // REQUEST WAYPOINTS
-    connect(m_ui->readButton, SIGNAL(clicked()), this, SIGNAL(requestWaypoints()));
+    connect(m_ui->readButton, SIGNAL(clicked()), this, SLOT(read()));
 
     // SAVE/LOAD WAYPOINTS
     connect(m_ui->saveButton, SIGNAL(clicked()), this, SLOT(saveWaypoints()));
@@ -105,11 +105,6 @@ void WaypointList::setUAS(UASInterface* uas)
         connect(this, SIGNAL(clearWaypointList()),                          &uas->getWaypointManager(), SLOT(clearWaypointList()));
 
         connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &)));
-
-        // This slot is not implemented in UAS: connect(this, SIGNAL(removeWaypointId(int)), uas, SLOT(removeWaypoint(Waypoint*)));
-
-        //qDebug() << "Requesting waypoints";
-        //emit requestWaypoints();
     }
 }
 
@@ -158,29 +153,24 @@ void WaypointList::waypointReached(UASInterface* uas, quint16 waypointId)
     }*/
 }
 
-void WaypointList::transmit()
+void WaypointList::read()
 {
-    transmitDelay->start(1000);
-    m_ui->transmitButton->setEnabled(false);
-    emit clearWaypointList();
-
-    for(int i = 0; i < waypoints.size(); i++)
-    {
-        //Waypoint* wp = waypoints[i];
-        //emit waypointChanged(wp);
-        //if (wp->current)
-        //    emit currentWaypointChanged(wp->id);
-    }
-
-    emit sendWaypoints(waypoints);
-
     while(waypoints.size()>0)
     {
         removeWaypoint(waypoints[0]);
     }
+    
     emit requestWaypoints();
 }
 
+void WaypointList::transmit()
+{
+    transmitDelay->start(1000);
+    m_ui->transmitButton->setEnabled(false);
+
+    emit sendWaypoints(waypoints);
+}
+
 void WaypointList::add()
 {
     // Only add waypoints if UAS is present
@@ -287,11 +277,6 @@ void WaypointList::moveDown(Waypoint* wp)
     }
 }
 
-/*void WaypointList::removeWaypointAndName(Waypoint* wp)
-{
-    removeWaypoint(wp);
-}*/
-
 void WaypointList::removeWaypoint(Waypoint* wp)
 {
     // Delete from list

src/ui/WaypointList.h

@@ -62,6 +62,7 @@ public slots:
     void saveWaypoints();
     void loadWaypoints();    
     void transmit();
+    void read();
     void add();
     void moveUp(Waypoint* wp);
     void moveDown(Waypoint* wp);