Merge branch 'experimental' of github.com:pixhawk/qgroundcontrol into experimental

images/earth.html

@@ -71,6 +71,11 @@ function init()
 	google.earth.createInstance("map3d", initCallback, failureCallback);
 }
 
+function setFollowEnabled(enable)
+{
+	followEnabled = enable;
+}
+
 
 
 function setCurrAircraft(id)
@@ -214,8 +219,8 @@ function initCallback(object)
     ge.getWindow().setVisibility(true);
     ge.getOptions().setStatusBarVisibility(true); 
     ge.getOptions().setScaleLegendVisibility(true);
-    ge.getOptions().setFlyToSpeed(5.0); 
-    //ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT); 
+    //ge.getOptions().setFlyToSpeed(5.0); 
+    ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT); 
     ge.getNavigationControl().setVisibility(ge.VISIBILITY_AUTO);
         
     ge.getLayerRoot().enableLayerById(ge.LAYER_TERRAIN, true); 
@@ -233,23 +238,31 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
 		if (lastLat == 0)
 		{
 			lastLat = currLat;
-			lastLon = lastLon;
+			lastLon = currLon;
 		}
 		currLat = lat;
 		currLon = lon;
 		currAlt = alt;
+		// Interpolate between t-1 and t and set new states
+		lastLat = lastLat*0.8+currLat*0.2;
+		lastLon = lastLon*0.8+currLon*0.2;
+		lastAlt = lastAlt*0.8+currAlt*0.2;
 		//currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
 	
 	
 	// FIXME Currently invalid conversion from right-handed z-down to z-up frame
-	planeOrient.setRoll(((roll/M_PI)+1.0)*360.0);
-	planeOrient.setTilt(((pitch/M_PI)+1.0)*360.0);
-	planeOrient.setHeading(((yaw/M_PI)+1.0)*360.0);
+	planeOrient.setRoll(((roll/M_PI))*180.0+180.0);
+	planeOrient.setTilt(((pitch/M_PI))*180.0+180.0);
+	planeOrient.setHeading(((yaw/M_PI))*180.0-90.0);
 	
-    planeLoc.setLatitude(lat);
-    planeLoc.setLongitude(lon);
-    planeLoc.setAltitude(alt);
+	currFollowHeading = ((yaw/M_PI))*180.0;
+    
+    planeLoc.setLatitude(lastLat);
+    planeLoc.setLongitude(lastLon);
+    planeLoc.setAltitude(lastAlt);
     planeModel.setLocation(planeLoc);
+    
+    if (followEnabled) updateFollowAircraft();
     }
 }
 
@@ -284,17 +297,13 @@ function setCurrentAircraft(id)
 function updateFollowAircraft()
 {
 	currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
-	// Interpolate between t-1 and t and set new states
-	lastLat = lastLat*0.7+currLat*0.3;
-	lastLon = lastLon*0.7+currLon*0.3;
-	lastAlt = lastAlt*0.7+currAlt*0.3;
 	currView.setLatitude(lastLat);
 	currView.setLongitude(lastLon);
 	currView.setAltitude(lastAlt);
 
 	currView.setRange(currViewRange);
 	currView.setTilt(currFollowTilt);
-	currView.setHeading(currFollowHeading-90.0);
+	currView.setHeading(currFollowHeading);
 	
 	ge.getView().setAbstractView(currView);
 }

lib/QMapControl/src/geometry.h

@@ -119,7 +119,7 @@ namespace qmapcontrol
         virtual bool hasClickedPoints() const;
         virtual void setPen(QPen* pen);
         virtual QList<Geometry*> clickedPoints();
-        virtual QList<Point*> points()=0;
+        virtual QList<Point*>& points()=0;
 
     private:
         Geometry* myparentGeometry;

lib/QMapControl/src/linestring.cpp

@@ -56,7 +56,12 @@ namespace qmapcontrol
         vertices.append(point);
     }
 
-    QList<Point*> LineString::points()
+    void LineString::clearPoints()
+    {
+        vertices.clear();
+    }
+
+    QList<Point*>& LineString::points()
     {
         return vertices;
     }

lib/QMapControl/src/linestring.h

@@ -56,7 +56,7 @@ namespace qmapcontrol
         /*!
          * @return  a list with the points of the LineString
          */
-        QList<Point*>	points();
+        QList<Point*>&	points();
 
         //! adds a point at the end of the LineString
         /*!
@@ -64,6 +64,9 @@ namespace qmapcontrol
          */
         void addPoint ( Point* point );
 
+        //! Remove all points from the LineString
+        void clearPoints();
+
         //! sets the given list as points of the LineString
         /*!
          * @param points the points which should be set for the LineString

lib/QMapControl/src/point.cpp

@@ -293,12 +293,11 @@ namespace qmapcontrol
 
         emit(positionChanged(this));
     }
-    QList<Point*> Point::points()
+    QList<Point*>& Point::points()
     {
-        //TODO: assigning temp?!
-        QList<Point*> points;
-        points.append(this);
-        return points;
+        // FIXME TODO: THIS IS ABSOLUTELY WRONG IN THE ORIGINAL LIBRARY
+        // NEEDS AN INHERITANCE REWRITE!!!!
+        return m_points;
     }
 
     QWidget* Point::widget()

lib/QMapControl/src/point.h

@@ -139,7 +139,7 @@ namespace qmapcontrol
          */
         QPointF coordinate() const;
 
-        virtual QList<Point*> points();
+        virtual QList<Point*>& points();
 
         /*! \brief returns the widget of the point
         @return the widget of the point
@@ -191,6 +191,7 @@ namespace qmapcontrol
         QSize displaysize;
         QSize minsize;
         QSize maxsize;
+        QList<Point*> m_points;
 
 
         void drawWidget(const MapAdapter* mapadapter, const QPoint offset);

qgroundcontrol.pro

@@ -140,7 +140,6 @@ FORMS += src/ui/MainWindow.ui \
     src/ui/QGCPxImuFirmwareUpdate.ui \
     src/ui/QGCDataPlot2D.ui \
     src/ui/QGCRemoteControlView.ui \
-    src/ui/WaypointGlobalView.ui \
     src/ui/QMap3D.ui \
     src/ui/QGCWebView.ui \
     src/ui/map3D/QGCGoogleEarthView.ui \

src/Waypoint.cc

-/*=====================================================================
-
+/*===================================================================
 QGroundControl Open Source Ground Control Station
 
 (c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
@@ -33,8 +32,8 @@ This file is part of the QGROUNDCONTROL project
 #include "Waypoint.h"
 #include <QStringList>
 
-Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _autocontinue, bool _current, float _orbit, int _holdTime, MAV_FRAME _frame, MAV_ACTION _action)
-: id(_id),
+Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _yaw, bool _autocontinue, bool _current, double _orbit, int _holdTime, MAV_FRAME _frame, MAV_ACTION _action)
+    : id(_id),
     x(_x),
     y(_y),
     z(_z),
@@ -44,7 +43,8 @@ Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _
     autocontinue(_autocontinue),
     current(_current),
     orbit(_orbit),
-  holdTime(_holdTime)
+    holdTime(_holdTime),
+    name(QString("WP%1").arg(id, 2, 10, QChar('0')))
 {
 }
 
@@ -90,79 +90,141 @@ bool Waypoint::load(QTextStream &loadStream)
 void Waypoint::setId(quint16 id)
 {
     this->id = id;
+    this->name = QString("WP%1").arg(id, 2, 10, QChar('0'));
+    emit changed(this);
 }
 
-void Waypoint::setX(float x)
+void Waypoint::setX(double x)
 {
+    if (this->x != x)
+    {
         this->x = x;
+        emit changed(this);
+    }
 }
 
-void Waypoint::setY(float y)
+void Waypoint::setY(double y)
 {
+    if (this->y != y)
+    {
         this->y = y;
+        emit changed(this);
+    }
 }
 
-void Waypoint::setZ(float z)
+void Waypoint::setZ(double z)
 {
+    if (this->z != z)
+    {
         this->z = z;
+        emit changed(this);
+    }
 }
 
-void Waypoint::setYaw(float yaw)
+void Waypoint::setYaw(double yaw)
 {
+    if (this->yaw != yaw)
+    {
         this->yaw = yaw;
+        emit changed(this);
+    }
 }
 
 void Waypoint::setAction(MAV_ACTION action)
 {
+    if (this->action != action)
+    {
         this->action = action;
+        emit changed(this);
+    }
 }
 
 void Waypoint::setFrame(MAV_FRAME frame)
 {
+    if (this->frame != frame)
+    {
         this->frame = frame;
+        emit changed(this);
+    }
 }
 
 void Waypoint::setAutocontinue(bool autoContinue)
 {
+    if (this->autocontinue != autocontinue)
+    {
         this->autocontinue = autoContinue;
+        emit changed(this);
+    }
 }
 
 void Waypoint::setCurrent(bool current)
 {
+    if (this->current != current)
+    {
         this->current = current;
+        emit changed(this);
+    }
 }
 
-void Waypoint::setOrbit(float orbit)
+void Waypoint::setOrbit(double orbit)
 {
+    if (this->orbit != orbit)
+    {
         this->orbit = orbit;
+        emit changed(this);
+    }
 }
 
 void Waypoint::setHoldTime(int holdTime)
 {
+    if (this->holdTime != holdTime)
+    {
         this->holdTime = holdTime;
+        emit changed(this);
+    }
 }
 
-void Waypoint::setX(double x)
-{
-    this->x = x;
-}
-
-void Waypoint::setY(double y)
-{
-    this->y = y;
-}
-
-void Waypoint::setZ(double z)
-{
-    this->z = z;
-}
-
-void Waypoint::setYaw(double yaw)
-{
-    this->yaw = yaw;
-}
-
-void Waypoint::setOrbit(double orbit)
-{
-    this->orbit = orbit;
-}
+//void Waypoint::setX(double x)
+//{
+//    if (this->x != static_cast<double>(x))
+//    {
+//        this->x = x;
+//        emit changed(this);
+//    }
+//}
+
+//void Waypoint::setY(double y)
+//{
+//    if (this->y != static_cast<double>(y))
+//    {
+//        this->y = y;
+//        emit changed(this);
+//    }
+//}
+
+//void Waypoint::setZ(double z)
+//{
+//    if (this->z != static_cast<double>(z))
+//    {
+//        this->z = z;
+//        emit changed(this);
+//    }
+//}
+
+//void Waypoint::setYaw(double yaw)
+//{
+//    if (this->yaw != static_cast<double>(yaw))
+//    {
+//        this->yaw = yaw;
+//        emit changed(this);
+//    }
+//}
+
+//void Waypoint::setOrbit(double orbit)
+//{
+//    if (this->orbit != static_cast<double>(orbit))
+//    {
+//        this->orbit = orbit;
+//        emit changed(this);
+//    }
+//}

src/Waypoint.h

@@ -43,22 +43,23 @@ class Waypoint : public QObject
     Q_OBJECT
 
 public:
-    Waypoint(quint16 id = 0, float x = 0.0f, float y = 0.0f, float z = 0.0f, float yaw = 0.0f, bool autocontinue = false,
-             bool current = false, float orbit = 0.15f, int holdTime = 0,
+    Waypoint(quint16 id = 0, double x = 0.0f, double y = 0.0f, double z = 0.0f, double yaw = 0.0f, bool autocontinue = false,
+             bool current = false, double orbit = 0.15f, int holdTime = 0,
              MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_ACTION action=MAV_ACTION_NAVIGATE);
     ~Waypoint();
 
     quint16 getId() const { return id; }
-    float getX() const { return x; }
-    float getY() const { return y; }
-    float getZ() const { return z; }
-    float getYaw() const { return yaw; }
+    double getX() const { return x; }
+    double getY() const { return y; }
+    double getZ() const { return z; }
+    double getYaw() const { return yaw; }
     bool getAutoContinue() const { return autocontinue; }
     bool getCurrent() const { return current; }
-    float getOrbit() const { return orbit; }
-    float getHoldTime() const { return holdTime; }
+    double getOrbit() const { return orbit; }
+    double getHoldTime() const { return holdTime; }
     MAV_FRAME getFrame() const { return frame; }
     MAV_ACTION getAction() const { return action; }
+    const QString& getName() const { return name; }
 
     void save(QTextStream &saveStream);
     bool load(QTextStream &loadStream);
@@ -66,37 +67,42 @@ public:
 
 protected:
     quint16 id;
-    float x;
-    float y;
-    float z;
-    float yaw;
+    double x;
+    double y;
+    double z;
+    double yaw;
     MAV_FRAME frame;
     MAV_ACTION action;
     bool autocontinue;
     bool current;
-    float orbit;
+    double orbit;
     int holdTime;
+    QString name;
 
 public slots:
     void setId(quint16 id);
-    void setX(float x);
-    void setY(float y);
-    void setZ(float z);
-    void setYaw(float yaw);
+    void setX(double x);
+    void setY(double y);
+    void setZ(double z);
+    void setYaw(double yaw);
     void setAction(MAV_ACTION action);
     void setFrame(MAV_FRAME frame);
     void setAutocontinue(bool autoContinue);
     void setCurrent(bool current);
-    void setOrbit(float orbit);
+    void setOrbit(double orbit);
     void setHoldTime(int holdTime);
 
 
-    //for QDoubleSpin
-    void setX(double x);
-    void setY(double y);
-    void setZ(double z);
-    void setYaw(double yaw);
-    void setOrbit(double orbit);
+//    //for QDoubleSpin
+//    void setX(double x);
+//    void setY(double y);
+//    void setZ(double z);
+//    void setYaw(double yaw);
+//    void setOrbit(double orbit);
+
+signals:
+    /** @brief Announces a change to the waypoint data */
+    void changed(Waypoint* wp);
 };
 
 #endif // WAYPOINT_H

src/comm/MAVLinkSimulationLink.cc

@@ -154,12 +154,12 @@ void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg)
     unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg);
 
     // Pack to link buffer
-//    readyBufferMutex.lock();
+    readyBufferMutex.lock();
     for (unsigned int i = 0; i < bufferlength; i++)
     {
         readyBuffer.enqueue(*(buf + i));
     }
-//    readyBufferMutex.unlock();
+    readyBufferMutex.unlock();
 }
 
 void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg)
@@ -451,6 +451,11 @@ void MAVLinkSimulationLink::mainloop()
         memcpy(stream+streampointer,buffer, bufferlength);
         streampointer += bufferlength;
 
+        // ATTITUDE VEHICLE 2
+        mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0);
+        sendMAVLinkMessage(&ret);
+
+
 //        // GLOBAL POSITION VEHICLE 3
 //        mavlink_msg_global_position_int_pack(60, componentId, &ret, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0);
 //        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
@@ -627,7 +632,7 @@ void MAVLinkSimulationLink::mainloop()
         // HEARTBEAT VEHICLE 2
 
         // Pack message and get size of encoded byte string
-        messageSize = mavlink_msg_heartbeat_pack(54, componentId, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_ARDUPILOTMEGA);
+        messageSize = mavlink_msg_heartbeat_pack(54, componentId, &msg, MAV_HELICOPTER, MAV_AUTOPILOT_ARDUPILOTMEGA);
         // Allocate buffer with packet data
         bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
         //add data into datastream
@@ -886,8 +891,7 @@ void MAVLinkSimulationLink::readBytes() {
     readyBufferMutex.lock();
     const qint64 maxLength = 2048;
     char data[maxLength];
-    qint64 len = maxLength;
-    if (maxLength > readyBuffer.size()) len = readyBuffer.size();
+    qint64 len = qMin((qint64)readyBuffer.size(), maxLength);
 
     for (unsigned int i = 0; i < len; i++)
     {

src/comm/MAVLinkSimulationMAV.cc

@@ -11,25 +11,29 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
         timer25Hz(0),
         timer10Hz(0),
         timer1Hz(0),
-    latitude(0.0),
-    longitude(0.0),
+        latitude(47.376389),
+        longitude(8.548056),
         altitude(0.0),
-    x(0.0),
-    y(0.0),
-    z(0.0),
+        x(8.548056),
+        y(47.376389),
+        z(550),
         roll(0.0),
         pitch(0.0),
         yaw(0.0),
         globalNavigation(true),
         firstWP(false),
-    previousSPX(0.0),
-    previousSPY(0.0),
-    previousSPZ(0.0),
+        previousSPX(8.548056),
+        previousSPY(47.376389),
+        previousSPZ(550),
         previousSPYaw(0.0),
-    nextSPX(0.0),
-    nextSPY(0.0),
-    nextSPZ(0.0),
-    nextSPYaw(0.0)
+        nextSPX(8.548056),
+        nextSPY(47.376389),
+        nextSPZ(550),
+        nextSPYaw(0.0),
+        sys_mode(MAV_MODE_READY),
+        sys_state(MAV_STATE_STANDBY),
+        nav_mode(MAV_NAV_GROUNDED),
+        flying(false)
 {
     // Please note: The waypoint planner is running
     connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop()));
@@ -42,28 +46,15 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
 void MAVLinkSimulationMAV::mainloop()
 {
     // Calculate new simulator values
-//    double maxSpeed = 0.0001; // rad/s in earth coordinate frame
+    //    double maxSpeed = 0.0001; // rad/s in earth coordinate frame
 
-//        double xNew = // (nextSPX - previousSPX)
+    //        double xNew = // (nextSPX - previousSPX)
 
-    if (!firstWP)
+    if (flying)
     {
-        double xm = (nextSPX - x) * 0.01;
-        double ym = (nextSPY - y) * 0.01;
-        double zm = (nextSPZ - z) * 0.1;
-
-        x += xm;
-        y += ym;
-        z += zm;
-
-        //if (xm < 0.001) xm
-    }
-    else
-    {
-        x = nextSPX;
-        y = nextSPY;
-        z = nextSPZ;
-        firstWP = false;
+        sys_state = MAV_STATE_ACTIVE;
+        sys_mode = MAV_MODE_AUTO;
+        nav_mode = MAV_NAV_WAYPOINT;
     }
 
     // 1 Hz execution
@@ -72,12 +63,59 @@ void MAVLinkSimulationMAV::mainloop()
         mavlink_message_t msg;
         mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK);
         link->sendMAVLinkMessage(&msg);
+        planner.handleMessage(msg);
         timer1Hz = 50;
     }
 
     // 10 Hz execution
     if (timer10Hz <= 0)
     {
+        if (!firstWP)
+        {
+            double radPer100ms = 0.00006;
+            double altPer100ms = 1.0;
+            double xm = (nextSPX - x);
+            double ym = (nextSPY - y);
+            double zm = (nextSPZ - z);
+
+            float zsign = (zm < 0) ? -1.0f : 1.0f;
+
+            //float trueyaw = atan2f(xm, ym);
+
+            float newYaw = atan2f(xm, ym);
+
+            if (fabs(yaw - newYaw) < 90)
+            {
+                yaw = yaw*0.7 + 0.3*newYaw;
+            }
+            else
+            {
+                yaw = newYaw;
+            }
+
+            //qDebug() << "SIMULATION MAV: x:" << xm << "y:" << ym << "z:" << zm << "yaw:" << yaw;
+
+            //if (sqrt(xm*xm+ym*ym) > 0.0000001)
+            if (flying)
+            {
+                x += sin(yaw)*radPer100ms;
+                y += cos(yaw)*radPer100ms;
+                z += altPer100ms*zsign;
+            }
+
+            //if (xm < 0.001) xm
+        }
+        else
+        {
+            x = nextSPX;
+            y = nextSPY;
+            z = nextSPZ;
+            firstWP = false;
+            qDebug() << "INIT STEP";
+        }
+
+
+        // GLOBAL POSITION
         mavlink_message_t msg;
         mavlink_global_position_int_t pos;
         pos.alt = z*1000.0;
@@ -88,12 +126,90 @@ void MAVLinkSimulationMAV::mainloop()
         pos.vz = 0;
         mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos);
         link->sendMAVLinkMessage(&msg);
+        planner.handleMessage(msg);
+
+        // ATTITUDE
+        mavlink_attitude_t attitude;
+        attitude.roll = 0.0f;
+        attitude.pitch = 0.0f;
+        attitude.yaw = yaw;
+
+        mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude);
+        link->sendMAVLinkMessage(&msg);
+
+        // SYSTEM STATUS
+        mavlink_sys_status_t status;
+        status.load = 300;
+        status.motor_block = 1;
+        status.mode = sys_mode;
+        status.nav_mode = nav_mode;
+        status.packet_drop = 0;
+        status.vbat = 10500;
+        status.status = sys_state;
+
+        mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status);
+        link->sendMAVLinkMessage(&msg);
         timer10Hz = 5;
     }
 
     // 25 Hz execution
     if (timer25Hz <= 0)
     {
+
+        // Send a named value with name "FLOAT" and 0.5f as value
+
+        // The message container to be used for sending
+        mavlink_message_t ret;
+        // The C-struct holding the data to be sent
+        mavlink_named_value_float_t val;
+
+        // Fill in the data
+        // Name of the value, maximum 10 characters
+        // see full message specs at:
+        // http://pixhawk.ethz.ch/wiki/mavlink/
+        strcpy(val.name, "FLOAT");
+        // Value, in this case 0.5
+        val.value = 0.5f;
+
+        // Encode the data (adding header and checksums, etc.)
+        mavlink_msg_named_value_float_encode(systemid, MAV_COMP_ID_IMU, &ret, &val);
+        // And send it
+        link->sendMAVLinkMessage(&ret);
+
+        // MICROCONTROLLER SEND CODE:
+
+        // uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+        // int16_t len = mavlink_msg_to_send_buffer(buf, &ret);
+        // uart0_transmit(buf, len);
+
+
+        // SEND INTEGER VALUE
+
+        // We are reusing the "mavlink_message_t ret"
+        // message buffer
+
+        // The C-struct holding the data to be sent
+        mavlink_named_value_int_t valint;
+
+        // Fill in the data
+        // Name of the value, maximum 10 characters
+        // see full message specs at:
+        // http://pixhawk.ethz.ch/wiki/mavlink/
+        strcpy(valint.name, "INTEGER");
+        // Value, in this case 18000
+        valint.value = 18000;
+
+        // Encode the data (adding header and checksums, etc.)
+        mavlink_msg_named_value_int_encode(systemid, MAV_COMP_ID_IMU, &ret, &valint);
+        // And send it
+        link->sendMAVLinkMessage(&ret);
+
+        // MICROCONTROLLER SEND CODE:
+
+        // uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+        // int16_t len = mavlink_msg_to_send_buffer(buf, &ret);
+        // uart0_transmit(buf, len);
+
         timer25Hz = 2;
     }
 
@@ -110,6 +226,37 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
     {
     case MAVLINK_MSG_ID_ATTITUDE:
         break;
+    case MAVLINK_MSG_ID_SET_MODE:
+        {
+            mavlink_set_mode_t mode;
+            mavlink_msg_set_mode_decode(&msg, &mode);
+            if (systemid == mode.target) sys_mode = mode.mode;
+        }
+        break;
+    case MAVLINK_MSG_ID_ACTION:
+        {
+            mavlink_action_t action;
+            mavlink_msg_action_decode(&msg, &action);
+            if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU))
+            {
+                switch (action.action)
+                {
+                case MAV_ACTION_LAUNCH:
+                    flying = true;
+                    break;
+                default:
+                    {
+                        mavlink_statustext_t text;
+                        mavlink_message_t r_msg;
+                        sprintf((char*)text.text, "MAV%d ignored unknown action %d", systemid, action.action);
+                        mavlink_msg_statustext_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &text);
+                        link->sendMAVLinkMessage(&r_msg);
+                    }
+                    break;
+                }
+            }
+        }
+        break;
     case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET:
         {
             mavlink_local_position_setpoint_set_t sp;
@@ -127,9 +274,9 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
                 //nextSPYaw = sp.yaw;
 
                 // Airplane
-                yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY);
+                //yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY);
 
-                if (!firstWP) firstWP = true;
+                //if (!firstWP) firstWP = true;
             }
             //qDebug() << "UPDATED SP:" << "X" << nextSPX << "Y" << nextSPY << "Z" << nextSPZ;
         }

src/comm/MAVLinkSimulationMAV.h

@@ -49,6 +49,10 @@ protected:
     double nextSPY;
     double nextSPZ;
     double nextSPYaw;
+    uint8_t sys_mode;
+    uint8_t sys_state;
+    uint8_t nav_mode;
+    bool flying;
 
 };
 

src/comm/MAVLinkSimulationWaypointPlanner.cc

@@ -437,6 +437,20 @@ float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, f
         return -1.f;
 }
 
+float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y)
+{
+        if (seq < waypoints->size())
+        {
+                mavlink_waypoint_t *cur = waypoints->at(seq);
+
+                const PxVector3 A(cur->x, cur->y, 0);
+                const PxVector3 C(x, y, 0);
+
+                return (C-A).length();
+        }
+        return -1.f;
+}
+
 void MAVLinkSimulationWaypointPlanner::handleMessage(const mavlink_message_t& msg)
 {
     mavlink_handler(&msg);
@@ -449,7 +463,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
 
         //check for timed-out operations
 
-        qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid;
+        //qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid;
 
         uint64_t now = QGC::groundTimeUsecs()/1000;
         if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE)
@@ -502,6 +516,10 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                                 if (att.yaw - yaw_tolerance < wp->yaw || wp->yaw < upperBound)
                                                         yawReached = true;
                                         }
+
+                                        // FIXME HACK: Ignore yaw:
+
+                                        yawReached = true;
                                 }
                         }
                         break;
@@ -543,6 +561,49 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                         break;
                 }
 
+        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
+        {
+                if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
+                {
+                        mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
+
+                        if(wp->frame == 0)
+                        {
+                                mavlink_global_position_int_t pos;
+                                mavlink_msg_global_position_int_decode(msg, &pos);
+
+                                float x = static_cast<double>(pos.lon)/1E7;
+                                float y = static_cast<double>(pos.lat)/1E7;
+                                float z = static_cast<double>(pos.alt)/1000;
+
+                                qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z;
+
+                                posReached = false;
+                                yawReached = true;
+
+                                // FIXME big hack for simulation!
+                                //float oneDegreeOfLatMeters = 111131.745f;
+                                float orbit = 0.00008;
+
+                                // compare current position (given in message) with current waypoint
+                                //float orbit = wp->param1;
+
+                                // Convert to degrees
+
+
+                                float dist;
+                                dist = distanceToPoint(current_active_wp_id, x, y);
+
+                                if (dist >= 0.f && dist <= orbit && yawReached)
+                                {
+                                        posReached = true;
+                                        qDebug() << "WP PLANNER: REACHED POSITION";
+                                }
+                        }
+                }
+                break;
+        }
+
                 case MAVLINK_MSG_ID_ACTION: // special action from ground station
                 {
                         mavlink_action_t action;
@@ -947,7 +1008,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                         {
                                                 //the last waypoint was reached, if auto continue is
                                                 //activated restart the waypoint list from the beginning
-                                                current_active_wp_id = 1;
+                                                current_active_wp_id = 0;
                                         }
                                         else
                                         {

src/comm/MAVLinkSimulationWaypointPlanner.h

@@ -68,6 +68,7 @@ protected:
     void send_waypoint_reached(uint16_t seq);
     float distanceToSegment(uint16_t seq, float x, float y, float z);
     float distanceToPoint(uint16_t seq, float x, float y, float z);
+    float distanceToPoint(uint16_t seq, float x, float y);
     void mavlink_handler(const mavlink_message_t* msg);
 
 };

src/configuration.h

@@ -17,7 +17,7 @@
 #define WITH_TEXT_TO_SPEECH 1
 
 #define QGC_APPLICATION_NAME "QGroundControl"
-#define QGC_APPLICATION_VERSION "v. 0.8.3 (Alpha)"
+#define QGC_APPLICATION_VERSION "v. 0.8.3 (Alpha RC6)"
 
 namespace QGC
 

src/uas/UAS.cc

@@ -146,18 +146,31 @@ void UAS::updateState()
 
 void UAS::setSelected()
 {
+    if (UASManager::instance()->getActiveUAS() != this)
+    {
         UASManager::instance()->setActiveUAS(this);
+        emit systemSelected(true);
+    }
+}
+
+bool UAS::getSelected() const
+{
+    return (UASManager::instance()->getActiveUAS() == this);
 }
 
 void UAS::receiveMessageNamedValue(const mavlink_message_t& message)
 {
     if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_FLOAT)
     {
-
+        mavlink_named_value_float_t val;
+        mavlink_msg_named_value_float_decode(&message, &val);
+        emit valueChanged(this->getUASID(), QString(val.name), tr("raw"), val.value, getUnixTime(0));
     }
     else if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT)
     {
-
+        mavlink_named_value_int_t val;
+        mavlink_msg_named_value_int_decode(&message, &val);
+        emit valueChanged(this->getUASID(), QString(val.name), tr("raw"), (float)val.value, getUnixTime(0));
     }
 }
 
@@ -181,6 +194,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         QString uasState;
         QString stateDescription;
         QString patternPath;
+
+        // Receive named value message
+        receiveMessageNamedValue(message);
+
         switch (message.msgid)
         {
         case MAVLINK_MSG_ID_HEARTBEAT:
@@ -326,15 +343,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 mavlink_msg_raw_imu_decode(&message, &raw);
                 quint64 time = getUnixTime(raw.usec);
 
-                emit valueChanged(uasId, "accel x", "raw", raw.xacc, time);
-                emit valueChanged(uasId, "accel y", "raw", raw.yacc, time);
-                emit valueChanged(uasId, "accel z", "raw", raw.zacc, time);
+                emit valueChanged(uasId, "accel x", "raw", static_cast<double>(raw.xacc), time);
+                emit valueChanged(uasId, "accel y", "raw", static_cast<double>(raw.yacc), time);
+                emit valueChanged(uasId, "accel z", "raw", static_cast<double>(raw.zacc), time);
                 emit valueChanged(uasId, "gyro roll", "raw", static_cast<double>(raw.xgyro), time);
                 emit valueChanged(uasId, "gyro pitch", "raw", static_cast<double>(raw.ygyro), time);
                 emit valueChanged(uasId, "gyro yaw", "raw", static_cast<double>(raw.zgyro), time);
-                emit valueChanged(uasId, "mag x", "raw", raw.xmag, time);
-                emit valueChanged(uasId, "mag y", "raw", raw.ymag, time);
-                emit valueChanged(uasId, "mag z", "raw", raw.zmag, time);
+                emit valueChanged(uasId, "mag x", "raw", static_cast<double>(raw.xmag), time);
+                emit valueChanged(uasId, "mag y", "raw", static_cast<double>(raw.ymag), time);
+                emit valueChanged(uasId, "mag z", "raw", static_cast<double>(raw.zmag), time);
             }
             break;
         case MAVLINK_MSG_ID_ATTITUDE:
@@ -1038,7 +1055,7 @@ void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDesc
         break;
     case MAV_STATE_EMERGENCY:
         uasState = tr("EMERGENCY");
-        stateDescription = tr("EMERGENCY: Please land");
+        stateDescription = tr("EMERGENCY: Land!");
         break;
     case MAV_STATE_POWEROFF:
         uasState = tr("SHUTDOWN");

src/uas/UAS.h

@@ -89,9 +89,10 @@ public:
     double getRoll() const { return roll; }
     double getPitch() const { return pitch; }
     double getYaw() const { return yaw; }
-
+    bool getSelected() const;
 
 friend class UASWaypointManager;
+
 protected: //COMMENTS FOR TEST UNIT
     int uasId;                    ///< Unique system ID
     unsigned char type;           ///< UAS type (from type enum)

src/uas/UASInterface.h

@@ -78,6 +78,8 @@ public:
     virtual double getPitch() const = 0;
     virtual double getYaw() const = 0;
 
+    virtual bool getSelected() const = 0;
+
     /** @brief Get reference to the waypoint manager **/
     virtual UASWaypointManager* getWaypointManager(void) = 0;
 
@@ -419,6 +421,8 @@ signals:
     void heartbeatTimeout(unsigned int ms);
     /** @brief Name of system changed */
     void nameChanged(QString newName);
+    /** @brief System has been selected as focused system */
+    void systemSelected(bool selected);
 
 protected:
 

src/uas/UASManager.cc

@@ -203,6 +203,7 @@ void UASManager::setActiveUAS(UASInterface* uas)
         activeUAS = uas;
         activeUASMutex.unlock();
 
+        activeUAS->setSelected();
         emit activeUASSet(uas);
         emit activeUASSet(uas->getUASID());
         emit activeUASSetListIndex(systems.indexOf(uas));

src/uas/UASWaypointManager.cc

@@ -138,9 +138,9 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
 
         if(wp->seq == current_wp_id)
         {
-            qDebug() << "Got WP: " << wp->seq << wp->x <<  wp->y << wp->z << wp->yaw << wp->autocontinue << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action;
+            qDebug() << "Got WP: " << wp->seq << wp->x <<  wp->y << wp->z << wp->yaw << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action;
             Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2, (MAV_FRAME) wp->frame, (MAV_ACTION) wp->action);
-            addWaypoint(lwp);
+            addWaypoint(lwp, false);
 
             //get next waypoint
             current_wp_id++;
@@ -267,9 +267,17 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
 
 void UASWaypointManager::notifyOfChange(Waypoint* wp)
 {
-    Q_UNUSED(wp);
+    qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId();
+    // If only one waypoint was changed, emit only WP signal
+    if (wp != NULL)
+    {
+        emit waypointChanged(uas.getUASID(), wp);
+    }
+    else
+    {
         emit waypointListChanged();
         emit waypointListChanged(uas.getUASID());
+    }
 }
 
 int UASWaypointManager::setCurrentWaypoint(quint16 seq)
@@ -312,17 +320,20 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq)
     return -1;
 }
 
-void UASWaypointManager::addWaypoint(Waypoint *wp)
+/**
+ * @param enforceFirstActive Enforces that the first waypoint is set as active
+ */
+void UASWaypointManager::addWaypoint(Waypoint *wp, bool enforceFirstActive)
 {
     if (wp)
     {
         wp->setId(waypoints.size());
+        if (enforceFirstActive && waypoints.size() == 0) wp->setCurrent(true);
         waypoints.insert(waypoints.size(), wp);
+        connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*)));
 
         emit waypointListChanged();
         emit waypointListChanged(uas.getUASID());
-
-        qDebug() << "ADDED WAYPOINT WITH ID:" << wp->getId();
     }
 }
 
@@ -510,6 +521,7 @@ void UASWaypointManager::writeWaypoints()
 {
     if (current_state == WP_IDLE)
     {
+        // Send clear all if count == 0
         if (waypoints.count() > 0)
         {
             protocol_timer.start(PROTOCOL_TIMEOUT_MS);
@@ -522,7 +534,9 @@ void UASWaypointManager::writeWaypoints()
             current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
 
             //clear local buffer
-            //TODO: Why not replace with waypoint_buffer.clear() ? - because this will lead to memory leaks, the waypoint-structs have to be deleted, clear() would only delete the pointers.
+            // Why not replace with waypoint_buffer.clear() ?
+            // because this will lead to memory leaks, the waypoint-structs
+            // have to be deleted, clear() would only delete the pointers.
             while(!waypoint_buffer.empty())
             {
                 delete waypoint_buffer.back();

src/uas/UASWaypointManager.h

@@ -86,16 +86,6 @@ public:
     /** @name Waypoint list operations */
     /*@{*/
     const QVector<Waypoint *> &getWaypointList(void) { return waypoints; }  ///< Returns a const reference to the waypoint list.
-    int removeWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
-    void moveWaypoint(quint16 cur_seq, quint16 new_seq);   ///< locally move a waypoint from its current position cur_seq to a new position new_seq
-    void saveWaypoints(const QString &saveFile);           ///< saves the local waypoint list to saveFile
-    void loadWaypoints(const QString &loadFile);           ///< loads a waypoint list from loadFile
-    void notifyOfChange(Waypoint* wp);                     ///< Notifies manager to changes to a waypoint
-    void localAddWaypoint(Waypoint *wp);                        ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
-    int localRemoveWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
-    void localMoveWaypoint(quint16 cur_seq, quint16 new_seq);   ///< locally move a waypoint from its current position cur_seq to a new position new_seq
-    void localSaveWaypoints(const QString &saveFile);           ///< saves the local waypoint list to saveFile
-    void localLoadWaypoints(const QString &loadFile);           ///< loads a waypoint list from loadFile
     int getIndexOf(Waypoint* wp);                               ///< Get the index of a waypoint in the list
     /*@}*/
 
@@ -122,7 +112,15 @@ private:
 
 public slots:
     void timeout();                                 ///< Called by the timer if a response times out. Handles send retries.
-    void addWaypoint(Waypoint *wp);                 ///< adds a new waypoint to the end of the list and changes its sequence number accordingly
+    /** @name Waypoint list operations */
+    /*@{*/
+    void addWaypoint(Waypoint *wp, bool enforceFirstActive=true);                 ///< adds a new waypoint to the end of the list and changes its sequence number accordingly
+    int removeWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
+    void moveWaypoint(quint16 cur_seq, quint16 new_seq);   ///< locally move a waypoint from its current position cur_seq to a new position new_seq
+    void saveWaypoints(const QString &saveFile);           ///< saves the local waypoint list to saveFile
+    void loadWaypoints(const QString &loadFile);           ///< loads a waypoint list from loadFile
+    void notifyOfChange(Waypoint* wp);                     ///< Notifies manager to changes to a waypoint
+    /*@}*/
 
 signals:
     void waypointListChanged(void);                 ///< emits signal that the waypoint list has been changed

src/ui/MainWindow.cc

@@ -38,7 +38,7 @@
 #include "SlugsMAV.h"
 
 
-#include "LogCompressor.h"
+#include "LogCompressor.h"s
 
 MainWindow* MainWindow::instance()
 {
@@ -72,13 +72,6 @@ MainWindow::MainWindow(QWidget *parent):
     {
         // Set this view as default view
         settings.setValue("CURRENT_VIEW", currentView);
-
-        // Enable default tools
-
-        // ENABLE UAS LIST
-        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST,currentView), true);
-        // ENABLE COMMUNICATION CONSOLE
-        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_DEBUG_CONSOLE,currentView), true);
     }
     else
     {
@@ -98,32 +91,9 @@ MainWindow::MainWindow(QWidget *parent):
 
     // Setup user interface
     ui.setupUi(this);
-    ui.actionNewCustomWidget->setEnabled(false);
 
     setVisible(false);
 
-    // Bind together the perspective actions
-    QActionGroup* perspectives = new QActionGroup(ui.menuPerspectives);
-    perspectives->addAction(ui.actionEngineersView);
-    perspectives->addAction(ui.actionMavlinkView);
-    perspectives->addAction(ui.actionPilotsView);
-    perspectives->addAction(ui.actionOperatorsView);
-    perspectives->addAction(ui.actionUnconnectedView);
-    perspectives->setExclusive(true);
-
-    // Mark the right one as selected
-    if (currentView == VIEW_ENGINEER) ui.actionEngineersView->setChecked(true);
-    if (currentView == VIEW_MAVLINK) ui.actionMavlinkView->setChecked(true);
-    if (currentView == VIEW_PILOT) ui.actionPilotsView->setChecked(true);
-    if (currentView == VIEW_OPERATOR) ui.actionOperatorsView->setChecked(true);
-    if (currentView == VIEW_UNCONNECTED) ui.actionUnconnectedView->setChecked(true);
-
-    // The pilot, engineer and operator view are not available on startup
-    // since they only make sense with a system connected.
-    ui.actionPilotsView->setEnabled(false);
-    ui.actionOperatorsView->setEnabled(false);
-    ui.actionEngineersView->setEnabled(false);
-
     buildCommonWidgets();
 
     connectCommonWidgets();
@@ -204,6 +174,11 @@ void MainWindow::setDefaultSettingsForAp()
     if (!settings.contains(centralKey))
     {
         settings.setValue(centralKey,true);
+
+        // ENABLE UAS LIST
+        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST, VIEW_UNCONNECTED), true);
+        // ENABLE COMMUNICATION CONSOLE
+        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_DEBUG_CONSOLE, VIEW_UNCONNECTED), true);
     }
 
     // OPERATOR VIEW DEFAULT
@@ -223,6 +198,8 @@ void MainWindow::setDefaultSettingsForAp()
     if (!settings.contains(centralKey))
     {
         settings.setValue(centralKey,true);
+        // Enable Parameter widget
+        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_PARAMETERS,VIEW_ENGINEER), true);
     }
 
     // MAVLINK VIEW DEFAULT
@@ -237,6 +214,8 @@ void MainWindow::setDefaultSettingsForAp()
     if (!settings.contains(centralKey))
     {
         settings.setValue(centralKey,true);
+        // Enable Flight display
+        settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HDD_1,VIEW_PILOT), true);
     }
 }
 
@@ -880,9 +859,11 @@ QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES t
     // Key is built as follows: autopilot_type/section_menu/view/tool/section
     int apType;
 
-    apType = (UASManager::instance() && UASManager::instance()->silentGetActiveUAS())?
-             UASManager::instance()->getActiveUAS()->getAutopilotType():
-             -1;
+//    apType = (UASManager::instance() && UASManager::instance()->silentGetActiveUAS())?
+//             UASManager::instance()->getActiveUAS()->getAutopilotType():
+//             -1;
+
+    apType = 1;
 
     return (QString::number(apType) + "_" +
             QString::number(SECTION_MENU) + "_" +
@@ -1251,6 +1232,36 @@ void MainWindow::showInfoMessage(const QString& title, const QString& message)
 **/
 void MainWindow::connectCommonActions()
 {
+    ui.actionNewCustomWidget->setEnabled(false);
+
+    // Bind together the perspective actions
+    QActionGroup* perspectives = new QActionGroup(ui.menuPerspectives);
+    perspectives->addAction(ui.actionEngineersView);
+    perspectives->addAction(ui.actionMavlinkView);
+    perspectives->addAction(ui.actionPilotsView);
+    perspectives->addAction(ui.actionOperatorsView);
+    perspectives->addAction(ui.actionUnconnectedView);
+    perspectives->setExclusive(true);
+
+    // Mark the right one as selected
+    if (currentView == VIEW_ENGINEER) ui.actionEngineersView->setChecked(true);
+    if (currentView == VIEW_MAVLINK) ui.actionMavlinkView->setChecked(true);
+    if (currentView == VIEW_PILOT) ui.actionPilotsView->setChecked(true);
+    if (currentView == VIEW_OPERATOR) ui.actionOperatorsView->setChecked(true);
+    if (currentView == VIEW_UNCONNECTED) ui.actionUnconnectedView->setChecked(true);
+
+    // The pilot, engineer and operator view are not available on startup
+    // since they only make sense with a system connected.
+    ui.actionPilotsView->setEnabled(false);
+    ui.actionOperatorsView->setEnabled(false);
+    ui.actionEngineersView->setEnabled(false);
+    // The UAS actions are not enabled without connection to system
+    ui.actionLiftoff->setEnabled(false);
+    ui.actionLand->setEnabled(false);
+    ui.actionEmergency_Kill->setEnabled(false);
+    ui.actionEmergency_Land->setEnabled(false);
+    ui.actionShutdownMAV->setEnabled(false);
+
     // Connect actions from ui
     connect(ui.actionAdd_Link, SIGNAL(triggered()), this, SLOT(addLink()));
 
@@ -1263,7 +1274,7 @@ void MainWindow::connectCommonActions()
     connect(ui.actionLand, SIGNAL(triggered()), UASManager::instance(), SLOT(returnActiveUAS()));
     connect(ui.actionEmergency_Land, SIGNAL(triggered()), UASManager::instance(), SLOT(stopActiveUAS()));
     connect(ui.actionEmergency_Kill, SIGNAL(triggered()), UASManager::instance(), SLOT(killActiveUAS()));
-
+    connect(ui.actionShutdownMAV, SIGNAL(triggered()), UASManager::instance(), SLOT(shutdownActiveUAS()));
     connect(ui.actionConfiguration, SIGNAL(triggered()), UASManager::instance(), SLOT(configureActiveUAS()));
 
     // Views actions
@@ -1424,6 +1435,12 @@ void MainWindow::UASCreated(UASInterface* uas)
         ui.actionPilotsView->setEnabled(true);
         ui.actionOperatorsView->setEnabled(true);
         ui.actionEngineersView->setEnabled(true);
+        // The UAS actions are not enabled without connection to system
+        ui.actionLiftoff->setEnabled(true);
+        ui.actionLand->setEnabled(true);
+        ui.actionEmergency_Kill->setEnabled(true);
+        ui.actionEmergency_Land->setEnabled(true);
+        ui.actionShutdownMAV->setEnabled(true);
 
         QIcon icon;
         // Set matching icon
@@ -1629,6 +1646,7 @@ void MainWindow::clearView()
         {
             // Remove dock widget from main window
             removeDockWidget(dockWidget);
+            dockWidget->hide();
             //dockWidget->setVisible(false);
         }
     }

src/ui/MainWindow.ui

@@ -77,7 +77,7 @@
    </widget>
    <widget class="QMenu" name="menuConnected_Systems">
     <property name="enabled">
-     <bool>false</bool>
+     <bool>true</bool>
     </property>
     <property name="title">
      <string>Select System</string>
@@ -85,7 +85,7 @@
    </widget>
    <widget class="QMenu" name="menuUnmanned_System">
     <property name="enabled">
-     <bool>false</bool>
+     <bool>true</bool>
     </property>
     <property name="title">
      <string>Unmanned System</string>
@@ -93,6 +93,7 @@
     <property name="separatorsCollapsible">
      <bool>false</bool>
     </property>
+    <addaction name="actionShutdownMAV"/>
     <addaction name="actionLiftoff"/>
     <addaction name="actionLand"/>
     <addaction name="actionEmergency_Land"/>
@@ -413,6 +414,21 @@
     <string>Meta+U</string>
    </property>
   </action>
+  <action name="actionShutdownMAV">
+   <property name="icon">
+    <iconset resource="../../mavground.qrc">
+     <normaloff>:/images/actions/system-log-out.svg</normaloff>:/images/actions/system-log-out.svg</iconset>
+   </property>
+   <property name="text">
+    <string>Shutdown MAV</string>
+   </property>
+   <property name="toolTip">
+    <string>Shutdown the onboard computer - works not during flight</string>
+   </property>
+   <property name="statusTip">
+    <string>Shutdown the onboard computer - works not during flight</string>
+   </property>
+  </action>
  </widget>
  <layoutdefault spacing="6" margin="11"/>
  <resources>

src/ui/MapWidget.h

@@ -68,6 +68,10 @@ public:
 public slots:
     void addUAS(UASInterface* uas);
     void activeUASSet(UASInterface* uas);
+    /** @brief Update the selected system */
+    void updateSelectedSystem(int uas);
+    /** @brief Update the attitude */
+    void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec);
     void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec);
     void updatePosition(float time, double lat, double lon);
     void updateCameraPosition(double distance, double bearing, QString dir);
@@ -85,6 +89,7 @@ public slots:
 
     /** @brief Update waypoint */
     void updateWaypoint(int uas, Waypoint* wp);
+    void updateWaypoint(int uas, Waypoint* wp, bool updateView);
 
     void drawBorderCamAtMap(bool status);
     /** @brief Bring up dialog to go to a specific location */

src/ui/QGCMAVLinkLogPlayer.cc

@@ -18,6 +18,8 @@ QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *pare
         mavlink(mavlink),
         logLink(NULL),
         loopCounter(0),
+        mavlinkLogFormat(true),
+        binaryBaudRate(57600),
         ui(new Ui::QGCMAVLinkLogPlayer)
 {
     ui->setupUi(this);
@@ -62,9 +64,22 @@ void QGCMAVLinkLogPlayer::play()
         logLink = new MAVLinkSimulationLink("");
 
         // Start timer
+        if (mavlinkLogFormat)
+        {
             loopTimer.start(1);
         }
         else
+        {
+            // Read len bytes at a time
+            int len = 100;
+            // Calculate the number of times to read 100 bytes per second
+            // to guarantee the baud rate, then divide 1000 by the number of read
+            // operations to obtain the interval in milliseconds
+            int interval = 1000 / ((binaryBaudRate / 10) / len);
+            loopTimer.start(interval*accelerationFactor);
+        }
+    }
+    else
     {
         ui->playButton->setChecked(false);
         QMessageBox msgBox;
@@ -96,6 +111,7 @@ bool QGCMAVLinkLogPlayer::reset(int packetIndex)
     // Reset only for valid values
     if (packetIndex >= 0 && packetIndex <= logFile.size() - (packetLen+timeLen))
     {
+
         bool result = true;
         pause();
         loopCounter = 0;
@@ -124,7 +140,7 @@ bool QGCMAVLinkLogPlayer::reset(int packetIndex)
 
 void QGCMAVLinkLogPlayer::selectLogFile()
 {
-    QString fileName = QFileDialog::getOpenFileName(this, tr("Specify MAVLink log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("MAVLink Logfile (*.mavlink);;"));
+    QString fileName = QFileDialog::getOpenFileName(this, tr("Specify MAVLink log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("MAVLink or Binary Logfile (*.mavlink; *.bin);;"));
 
     loadLogFile(fileName);
 }
@@ -146,6 +162,19 @@ void QGCMAVLinkLogPlayer::setAccelerationFactorInt(int factor)
         accelerationFactor = 1+(f/2.0f);
     }
 
+    // Update timer interval
+    if (!mavlinkLogFormat)
+    {
+        // Read len bytes at a time
+        int len = 100;
+        // Calculate the number of times to read 100 bytes per second
+        // to guarantee the baud rate, then divide 1000 by the number of read
+        // operations to obtain the interval in milliseconds
+        int interval = 1000 / ((binaryBaudRate / 10) / len);
+        loopTimer.stop();
+        loopTimer.start(interval/accelerationFactor);
+    }
+
     //qDebug() << "FACTOR:" << accelerationFactor;
 
     ui->speedLabel->setText(tr("Speed: %1X").arg(accelerationFactor, 5, 'f', 2, '0'));
@@ -180,6 +209,11 @@ void QGCMAVLinkLogPlayer::loadLogFile(const QString& file)
         startTime = 0;
         ui->logFileNameLabel->setText(tr("%1").arg(logFileInfo.baseName()));
 
+        // Select if binary or MAVLink log format is used
+        mavlinkLogFormat = file.endsWith(".mavlink");
+
+        if (mavlinkLogFormat)
+        {
             // Get the time interval from the logfile
             QByteArray timestamp = logFile.read(timeLen);
 
@@ -205,6 +239,35 @@ void QGCMAVLinkLogPlayer::loadLogFile(const QString& file)
             QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2);
             ui->logStatsLabel->setText(tr("%2 MB, %3 packets, %4").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))).arg(timelabel));
         }
+        else
+        {
+            // Load in binary mode
+
+            // Set baud rate if any present
+            QStringList parts = logFileInfo.baseName().split("_");
+
+            if (parts.count() > 1)
+            {
+                bool ok;
+                int rate = parts.last().toInt(&ok);
+                // 9600 baud to 100 MBit
+                if (ok && (rate > 9600 && rate < 100000000))
+                {
+                    // Accept this as valid baudrate
+                    binaryBaudRate = rate;
+                }
+            }
+
+            int seconds = logFileInfo.size() / (binaryBaudRate / 10);
+            int minutes = seconds / 60;
+            int hours = minutes / 60;
+            seconds -= 60*minutes;
+            minutes -= 60*hours;
+
+            QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2);
+            ui->logStatsLabel->setText(tr("%2 MB, %4 at %5 KB/s").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(timelabel).arg(binaryBaudRate/10.0f/1024.0f, 0, 'f', 2));
+        }
+    }
 }
 
 /**
@@ -220,9 +283,11 @@ void QGCMAVLinkLogPlayer::jumpToSliderVal(int slidervalue)
 
         // Do only accept valid jumps
         if (reset(packetIndex))
+        {
+            if (mavlinkLogFormat)
             {
                 ui->logStatsLabel->setText(tr("Jumped to packet %1").arg(packetIndex));
-        //qDebug() << "SET POSITION TO PACKET:" << packetIndex;
+            }
         }
 }
 
@@ -236,6 +301,8 @@ void QGCMAVLinkLogPlayer::jumpToSliderVal(int slidervalue)
  */
 void QGCMAVLinkLogPlayer::logLoop()
 {
+    if (mavlinkLogFormat)
+    {
         bool ok;
 
         // First check initialization
@@ -327,8 +394,34 @@ void QGCMAVLinkLogPlayer::logLoop()
             {
                 loopTimer.start(nextExecutionTime);
             }
-//        loopTimer.start(20);
+        }
+    }
+    else
+    {
+        // Binary format - read at fixed rate
+        const int len = 100;
+        QByteArray chunk = logFile.read(len);
+
+        // Emit this packet
+        emit bytesReady(logLink, chunk);
+
+        // Check if reached end of file before reading next timestamp
+        if (chunk.length() < len || logFile.atEnd())
+        {
+            // Reached end of file
+            reset();
+
+            QString status = tr("Reached end of binary log file.");
+            ui->logStatsLabel->setText(status);
+            MainWindow::instance()->showStatusMessage(status);
+            return;
+        }
+    }
+    // Ui update: Only every 20 messages
+    // to prevent flickering and high CPU load
 
+    // Update status label
+    // Update progress bar
     if (loopCounter % 40 == 0)
     {
         QFileInfo logFileInfo(logFile);
@@ -339,12 +432,6 @@ void QGCMAVLinkLogPlayer::logLoop()
         ui->positionSlider->blockSignals(false);
     }
     loopCounter++;
-        // Ui update: Only every 20 messages
-        // to prevent flickering and high CPU load
-
-        // Update status label
-        // Update progress bar
-    }
 }
 
 void QGCMAVLinkLogPlayer::changeEvent(QEvent *e)

src/ui/QGCMAVLinkLogPlayer.h

@@ -61,6 +61,8 @@ protected:
     QFile logFile;
     QTimer loopTimer;
     int loopCounter;
+    bool mavlinkLogFormat;
+    int binaryBaudRate;
     static const int packetLen = MAVLINK_MAX_PACKET_LEN;
     static const int timeLen = sizeof(quint64);
     void changeEvent(QEvent *e);

src/ui/UASView.ui

@@ -146,10 +146,20 @@ QPushButton:pressed#killButton {
 }
 
 
-QProgressBar {
+QProgressBar:horizontal {
+	border: 1px solid #4A4A4F;
+	border-radius: 4px;
+	text-align: center;
+	padding: 2px;
+	color: #DDDDDF;
+	background-color: #111118;
+}
+
+QProgressBar:vertical {
 	border: 1px solid #4A4A4F;
 	border-radius: 4px;
 	text-align: center;
+	font-size: 7px;
 	padding: 2px;
 	color: #DDDDDF;
 	background-color: #111118;
@@ -159,12 +169,16 @@ QProgressBar:horizontal {
 	height: 10px;
 }
 
-QProgressBar QLabel {
-	font-size: 8px;
+QProgressBar:horizontal QLabel {
+	font-size: 9px;
+}
+
+QProgressBar:vertical QLabel {
+	font-size: 7px;
 }
 
 QProgressBar:vertical {
-	width: 12px;
+	width: 14px;
 }
 
 QProgressBar::chunk {
@@ -231,7 +245,7 @@ QMenu::separator {
      <property name="title">
       <string/>
      </property>
-     <layout class="QGridLayout" name="gridLayout">
+     <layout class="QGridLayout" name="gridLayout" columnstretch="10,10,1,10,10,10,100,100">
       <property name="horizontalSpacing">
        <number>4</number>
       </property>
@@ -281,9 +295,12 @@ QMenu::separator {
         <property name="orientation">
          <enum>Qt::Horizontal</enum>
         </property>
+        <property name="sizeType">
+         <enum>QSizePolicy::MinimumExpanding</enum>
+        </property>
         <property name="sizeHint" stdset="0">
          <size>
-          <width>6</width>
+          <width>4</width>
           <height>88</height>
          </size>
         </property>
@@ -327,7 +344,7 @@ QMenu::separator {
          </font>
         </property>
         <property name="text">
-         <string>Waiting for first update..</string>
+         <string/>
         </property>
        </widget>
       </item>
@@ -466,7 +483,7 @@ QMenu::separator {
          </font>
         </property>
         <property name="text">
-         <string>UNINIT</string>
+         <string/>
         </property>
        </widget>
       </item>
@@ -481,14 +498,14 @@ QMenu::separator {
          </font>
         </property>
         <property name="text">
-         <string>WPX</string>
+         <string>---</string>
         </property>
         <property name="alignment">
          <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
         </property>
        </widget>
       </item>
-      <item row="2" column="6">
+      <item row="2" column="6" colspan="2">
        <widget class="QLabel" name="positionLabel">
         <property name="minimumSize">
          <size>
@@ -515,33 +532,6 @@ QMenu::separator {
         </property>
        </widget>
       </item>
-      <item row="2" column="7">
-       <widget class="QLabel" name="gpsLabel">
-        <property name="minimumSize">
-         <size>
-          <width>0</width>
-          <height>12</height>
-         </size>
-        </property>
-        <property name="maximumSize">
-         <size>
-          <width>16777215</width>
-          <height>12</height>
-         </size>
-        </property>
-        <property name="font">
-         <font>
-          <pointsize>-1</pointsize>
-          <weight>50</weight>
-          <italic>false</italic>
-          <bold>false</bold>
-         </font>
-        </property>
-        <property name="text">
-         <string>00 00 00 N  00 00 00 E</string>
-        </property>
-       </widget>
-      </item>
       <item row="5" column="6" colspan="2">
        <widget class="QLabel" name="statusTextLabel">
         <property name="maximumSize">
@@ -556,7 +546,7 @@ QMenu::separator {
          </font>
         </property>
         <property name="text">
-         <string>Unknown status</string>
+         <string/>
         </property>
        </widget>
       </item>

src/ui/WaypointGlobalView.h

-#ifndef WAYPOINTGLOBALVIEW_H
-#define WAYPOINTGLOBALVIEW_H
-
-#include <QWidget>
-#include "Waypoint.h"
-
-namespace Ui {
-    class WaypointGlobalView;
-}
-
-class WaypointGlobalView : public QWidget
-{
-    Q_OBJECT
-
-public:
-    explicit WaypointGlobalView(Waypoint* wp, QWidget *parent = 0);
-    ~WaypointGlobalView();
-
-public slots:
-
-    void updateValues(void);
-    void remove();
-    QString getLatitudString(float lat);
-    QString getLongitudString(float lon);
-    void getLatitudeGradoMin(float lat, int *gradoLat, float *minLat, QString *dirLat);
-    void getLongitudGradoMin(float lon, int *gradoLon, float *minLon, QString *dirLon);
-     void changeOrbitalState(int state);
-     void updateCoordValues(float lat, float lon);
-
-
-
-    //update latitude
-     void updateLatitudeWP(int value);
-     void updateLatitudeMinuteWP(double value);
-     void changeDirectionLatitudeWP();
-
-     //update longitude
-     void updateLongitudeWP(int value);
-     void updateLongitudeMinuteWP(double value);
-     void changeDirectionLongitudeWP();
-
-
-
-signals:
-
-    void removeWaypoint(Waypoint*);
-    void changePositionWP(Waypoint*);
-
-
-protected:
-    virtual void changeEvent(QEvent *e);
-
-    Waypoint* wp;
-
-private:
-    Ui::WaypointGlobalView *ui;
-
-private slots:
-
-};
-
-#endif // WAYPOINTGLOBALVIEW_H

src/ui/WaypointList.cc

@@ -129,6 +129,7 @@ void WaypointList::setUAS(UASInterface* uas)
 
         connect(uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)),        this, SLOT(updateStatusLabel(const QString &)));
         connect(uas->getWaypointManager(), SIGNAL(waypointListChanged(void)),                  this, SLOT(waypointListChanged(void)));
+        connect(uas->getWaypointManager(), SIGNAL(waypointChanged(int,Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
         connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)),            this, SLOT(currentWaypointChanged(quint16)));
         connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP()));
         connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool)));
@@ -214,10 +215,10 @@ void WaypointList::add()
                     wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE);
                     uas->getWaypointManager()->addWaypoint(wp);
             }
-            if (wp->getFrame() == MAV_FRAME_GLOBAL)
-            {
-                emit createWaypointAtMap(QPointF(wp->getX(), wp->getY()));
-            }
+//            if (wp->getFrame() == MAV_FRAME_GLOBAL)
+//            {
+//                emit createWaypointAtMap(QPointF(wp->getX(), wp->getY()));
+//            }
         }
     }
 }
@@ -293,6 +294,13 @@ void WaypointList::currentWaypointChanged(quint16 seq)
     }
 }
 
+void WaypointList::updateWaypoint(int uas, Waypoint* wp)
+{
+    Q_UNUSED(uas);
+    WaypointView *wpv = wpViews.value(wp);
+    wpv->updateValues();
+}
+
 void WaypointList::waypointListChanged()
 {
     if (uas)
@@ -301,63 +309,6 @@ void WaypointList::waypointListChanged()
         this->setUpdatesEnabled(false);
         const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
 
-        // For Global Waypoints
-        //if(isGlobalWP)
-        //{
-        //isLocalWP = false;
-        //// first remove all views of non existing waypoints
-        //if (!wpGlobalViews.empty())
-        //{
-        //QMapIterator<Waypoint*,WaypointGlobalView*> viewIt(wpGlobalViews);
-        //viewIt.toFront();
-        //while(viewIt.hasNext())
-        //{
-        //viewIt.next();
-        //Waypoint *cur = viewIt.key();
-        //int i;
-        //for (i = 0; i < waypoints.size(); i++)
-        //{
-        //if (waypoints[i] == cur)
-        //{
-        //break;
-        //}
-        //}
-        //if (i == waypoints.size())
-        //{
-        //WaypointGlobalView* widget = wpGlobalViews.find(cur).value();
-        //widget->hide();
-        //listLayout->removeWidget(widget);
-        //wpGlobalViews.remove(cur);
-        //}
-        //}
-        //}
-
-        //// then add/update the views for each waypoint in the list
-        //for(int i = 0; i < waypoints.size(); i++)
-        //{
-        //Waypoint *wp = waypoints[i];
-        //if (!wpGlobalViews.contains(wp))
-        //{
-        //WaypointGlobalView* wpview = new WaypointGlobalView(wp, this);
-        //wpGlobalViews.insert(wp, wpview);
-        //connect(wpview, SIGNAL(removeWaypoint(Waypoint*)),      this, SLOT(removeWaypoint(Waypoint*)));
-        //connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(changeWPPositionBySpinBox(Waypoint*)));
-        ////                        connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)),    this, SLOT(moveDown(Waypoint*)));
-        ////                        connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)),      this, SLOT(moveUp(Waypoint*)));
-        //// connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(waypointGlobalPositionChanged(Waypoint*)));
-        ////                        connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
-        ////                        connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
-        //}
-        //WaypointGlobalView *wpgv = wpGlobalViews.value(wp);
-        //wpgv->updateValues();
-        //listLayout->addWidget(wpgv);
-        //}
-
-        //}
-        //else
-        //{
-            // for local Waypoints
-            // first remove all views of non existing waypoints
             if (!wpViews.empty())
             {
                 QMapIterator<Waypoint*,WaypointView*> viewIt(wpViews);
@@ -408,7 +359,6 @@ void WaypointList::waypointListChanged()
 
 
         loadFileGlobalWP = false;
-    //}
 }
 
 void WaypointList::moveUp(Waypoint* wp)
@@ -499,29 +449,6 @@ void WaypointList::on_clearWPListButton_clicked()
     }
 }
 
-/** @brief Add a waypoint by mouse click over the map */
-void WaypointList::addWaypointMouse(QPointF coordinate)
-{
-    if (uas)
-    {
-        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
-        if (waypoints.size() > 0)
-        {
-            Waypoint *last = waypoints.at(waypoints.size()-1);
-            Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
-            uas->getWaypointManager()->addWaypoint(wp);
-        }
-        else
-        {
-            Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), -0.8, 0.0, true, true, 0.15, 2000);
-            uas->getWaypointManager()->addWaypoint(wp);
-        }
-
-
-    }
-
-}
-
 /** @brief The MapWidget informs that a waypoint global was changed on the map */
 
 void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP)

src/ui/WaypointList.h

@@ -41,7 +41,6 @@ This file is part of the QGROUNDCONTROL project
 #include "Waypoint.h"
 #include "UASInterface.h"
 #include "WaypointView.h"
-#include "WaypointGlobalView.h"
 
 
 namespace Ui {
@@ -75,10 +74,6 @@ public slots:
     /** @brief Add a waypoint at the current MAV position */
     void addCurrentPositonWaypoint();
     /** @brief Add a waypoint by mouse click over the map */
-    void addWaypointMouse(QPointF coordinate);
-    /** @brief it notifies that a global waypoint goes to do created */
-    //void setIsWPGlobal(bool value, QPointF centerCoordinate);
-
 
     //Update events
     /** @brief sets statusLabel string */
@@ -87,6 +82,8 @@ public slots:
     void changeCurrentWaypoint(quint16 seq);
     /** @brief The waypoint planner changed the current waypoint */
     void currentWaypointChanged(quint16 seq);
+    /** @brief The waypoint manager informs that one waypoint was changed */
+    void updateWaypoint(int uas, Waypoint* wp);
     /** @brief The waypoint manager informs that the waypoint list was changed */
     void waypointListChanged(void);
 
@@ -111,17 +108,12 @@ public slots:
 signals:
   void clearPathclicked();
   void createWaypointAtMap(const QPointF coordinate);
- // void ChangeWaypointGlobalPosition(int index, QPointF coord);
-  void changePositionWPBySpinBox(int indexWP, float lat, float lon);
-
-
 
 protected:
     virtual void changeEvent(QEvent *e);
 
 protected:
     QMap<Waypoint*, WaypointView*> wpViews;
-    //QMap<Waypoint*, WaypointGlobalView*> wpGlobalViews;
     QVBoxLayout* listLayout;
     UASInterface* uas;
     double mavX;

src/ui/WaypointView.cc

-/*=====================================================================
-
-PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
-
-(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>
-
-This file is part of the PIXHAWK project
-
-    PIXHAWK is free software: you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation, either version 3 of the License, or
-    (at your option) any later version.
-
-    PIXHAWK is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
-
+/*===================================================================
 ======================================================================*/
 
 /**
@@ -159,14 +139,6 @@ void WaypointView::changedAction(int index)
 
 void WaypointView::changedFrame(int index)
 {
-    // hide everything to start
-    m_ui->lonSpinBox->hide();
-    m_ui->latSpinBox->hide();
-    m_ui->altSpinBox->hide();
-    m_ui->posNSpinBox->hide();
-    m_ui->posESpinBox->hide();
-    m_ui->posDSpinBox->hide();
-
     // set waypoint action
     MAV_FRAME frame = (MAV_FRAME)m_ui->comboBox_frame->itemData(index).toUInt();
     wp->setFrame(frame);
@@ -174,11 +146,17 @@ void WaypointView::changedFrame(int index)
     switch(frame)
     {
     case MAV_FRAME_GLOBAL:
+        m_ui->posNSpinBox->hide();
+        m_ui->posESpinBox->hide();
+        m_ui->posDSpinBox->hide();
         m_ui->lonSpinBox->show();
         m_ui->latSpinBox->show();
         m_ui->altSpinBox->show();
         break;
     case MAV_FRAME_LOCAL:
+        m_ui->lonSpinBox->hide();
+        m_ui->latSpinBox->hide();
+        m_ui->altSpinBox->hide();
         m_ui->posNSpinBox->show();
         m_ui->posESpinBox->show();
         m_ui->posDSpinBox->show();
@@ -192,34 +170,61 @@ void WaypointView::changedCurrent(int state)
 {
     if (state == 0)
     {
-        //m_ui->selectedBox->setChecked(true);
-        //m_ui->selectedBox->setCheckState(Qt::Checked);
-        //wp->setCurrent(false);
+        m_ui->selectedBox->setChecked(true);
+        m_ui->selectedBox->setCheckState(Qt::Checked);
+        wp->setCurrent(false);
     }
     else
     {
-        //wp->setCurrent(true);
+        wp->setCurrent(true);
         emit changeCurrentWaypoint(wp->getId());   //the slot changeCurrentWaypoint() in WaypointList sets all other current flags to false
     }
 }
 
 void WaypointView::updateValues()
 {
+    // Deactivate signals from the WP
+    wp->blockSignals(true);
     // update frame
     MAV_FRAME frame = wp->getFrame();
     int frame_index = m_ui->comboBox_frame->findData(frame);
+    if (m_ui->comboBox_frame->currentIndex() != frame_index)
+    {
         m_ui->comboBox_frame->setCurrentIndex(frame_index);
+    }
     switch(frame)
     {
     case(MAV_FRAME_LOCAL):
+        {
+            if (m_ui->posNSpinBox->value() != wp->getX())
+            {
                 m_ui->posNSpinBox->setValue(wp->getX());
+            }
+            if (m_ui->posESpinBox->value() != wp->getY())
+            {
                 m_ui->posESpinBox->setValue(wp->getY());
+            }
+            if (m_ui->posDSpinBox->value() != wp->getZ())
+            {
                 m_ui->posDSpinBox->setValue(wp->getZ());
+            }
+        }
         break;
     case(MAV_FRAME_GLOBAL):
+        {
+            if (m_ui->lonSpinBox->value() != wp->getX())
+            {
                 m_ui->lonSpinBox->setValue(wp->getX());
+            }
+            if (m_ui->latSpinBox->value() != wp->getY())
+            {
                 m_ui->latSpinBox->setValue(wp->getY());
+            }
+            if (m_ui->altSpinBox->value() != wp->getZ())
+            {
                 m_ui->altSpinBox->setValue(wp->getZ());
+            }
+        }
         break;
     }
     changedFrame(frame_index);
@@ -227,7 +232,10 @@ void WaypointView::updateValues()
     // update action
     MAV_ACTION action = wp->getAction();
     int action_index = m_ui->comboBox_action->findData(action);
+    if (m_ui->comboBox_action->currentIndex() != action_index)
+    {
         m_ui->comboBox_action->setCurrentIndex(action_index);
+    }
     switch(action)
     {
     case MAV_ACTION_TAKEOFF:
@@ -243,12 +251,28 @@ void WaypointView::updateValues()
     }
     changedAction(action_index);
 
+    if (m_ui->yawSpinBox->value() != wp->getYaw()/M_PI*180.)
+    {
         m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.);
+    }
+    if (m_ui->selectedBox->isChecked() != wp->getCurrent())
+    {
         m_ui->selectedBox->setChecked(wp->getCurrent());
+    }
+    if (m_ui->autoContinue->isChecked() != wp->getAutoContinue())
+    {
         m_ui->autoContinue->setChecked(wp->getAutoContinue());
-    m_ui->idLabel->setText(QString("%1").arg(wp->getId()));\
+    }
+    m_ui->idLabel->setText(QString("%1").arg(wp->getId()));
+    if (m_ui->orbitSpinBox->value() != wp->getOrbit())
+    {
         m_ui->orbitSpinBox->setValue(wp->getOrbit());
+    }
+    if (m_ui->holdTimeSpinBox->value() != wp->getHoldTime())
+    {
         m_ui->holdTimeSpinBox->setValue(wp->getHoldTime());
+    }
+    wp->blockSignals(false);
 }
 
 void WaypointView::setCurrent(bool state)

src/ui/WaypointView.ui

@@ -170,6 +170,9 @@ QProgressBar::chunk#thrustBar {
       </property>
       <item>
        <widget class="QCheckBox" name="selectedBox">
+        <property name="focusPolicy">
+         <enum>Qt::TabFocus</enum>
+        </property>
         <property name="toolTip">
          <string>Currently selected waypoint</string>
         </property>
@@ -231,9 +234,21 @@ QProgressBar::chunk#thrustBar {
           <verstretch>0</verstretch>
          </sizepolicy>
         </property>
+        <property name="focusPolicy">
+         <enum>Qt::WheelFocus</enum>
+        </property>
         <property name="toolTip">
          <string>Position X coordinate</string>
         </property>
+        <property name="wrapping">
+         <bool>false</bool>
+        </property>
+        <property name="frame">
+         <bool>true</bool>
+        </property>
+        <property name="accelerated">
+         <bool>true</bool>
+        </property>
         <property name="prefix">
          <string>N </string>
         </property>
@@ -259,6 +274,9 @@ QProgressBar::chunk#thrustBar {
           <verstretch>0</verstretch>
          </sizepolicy>
         </property>
+        <property name="focusPolicy">
+         <enum>Qt::WheelFocus</enum>
+        </property>
         <property name="toolTip">
          <string>Position Y coordinate</string>
         </property>
@@ -290,6 +308,9 @@ QProgressBar::chunk#thrustBar {
           <verstretch>0</verstretch>
          </sizepolicy>
         </property>
+        <property name="focusPolicy">
+         <enum>Qt::WheelFocus</enum>
+        </property>
         <property name="toolTip">
          <string>Position Z coordinate (negative)</string>
         </property>
@@ -318,6 +339,9 @@ QProgressBar::chunk#thrustBar {
           <verstretch>0</verstretch>
          </sizepolicy>
         </property>
+        <property name="focusPolicy">
+         <enum>Qt::WheelFocus</enum>
+        </property>
         <property name="prefix">
          <string>lat </string>
         </property>
@@ -333,6 +357,9 @@ QProgressBar::chunk#thrustBar {
         <property name="maximum">
          <double>90.000000000000000</double>
         </property>
+        <property name="singleStep">
+         <double>0.000010000000000</double>
+        </property>
        </widget>
       </item>
       <item>
@@ -343,6 +370,9 @@ QProgressBar::chunk#thrustBar {
           <verstretch>0</verstretch>
          </sizepolicy>
         </property>
+        <property name="focusPolicy">
+         <enum>Qt::WheelFocus</enum>
+        </property>
         <property name="prefix">
          <string>lon </string>
         </property>
@@ -358,6 +388,9 @@ QProgressBar::chunk#thrustBar {
         <property name="maximum">
          <double>180.000000000000000</double>
         </property>
+        <property name="singleStep">
+         <double>0.000010000000000</double>
+        </property>
        </widget>
       </item>
       <item>
@@ -393,6 +426,9 @@ QProgressBar::chunk#thrustBar {
           <verstretch>0</verstretch>
          </sizepolicy>
         </property>
+        <property name="focusPolicy">
+         <enum>Qt::StrongFocus</enum>
+        </property>
         <property name="toolTip">
          <string>Yaw angle</string>
         </property>
@@ -421,6 +457,9 @@ QProgressBar::chunk#thrustBar {
           <verstretch>0</verstretch>
          </sizepolicy>
         </property>
+        <property name="focusPolicy">
+         <enum>Qt::StrongFocus</enum>
+        </property>
         <property name="toolTip">
          <string>Orbit radius</string>
         </property>
@@ -449,6 +488,9 @@ QProgressBar::chunk#thrustBar {
           <verstretch>0</verstretch>
          </sizepolicy>
         </property>
+        <property name="focusPolicy">
+         <enum>Qt::StrongFocus</enum>
+        </property>
         <property name="toolTip">
          <string>Time in milliseconds that the MAV has to stay inside the orbit before advancing</string>
         </property>
@@ -477,6 +519,9 @@ QProgressBar::chunk#thrustBar {
           <verstretch>0</verstretch>
          </sizepolicy>
         </property>
+        <property name="focusPolicy">
+         <enum>Qt::StrongFocus</enum>
+        </property>
         <property name="toolTip">
          <string>Take off angle</string>
         </property>
@@ -506,6 +551,9 @@ QProgressBar::chunk#thrustBar {
           <height>22</height>
          </size>
         </property>
+        <property name="focusPolicy">
+         <enum>Qt::NoFocus</enum>
+        </property>
         <property name="toolTip">
          <string>Move Up</string>
         </property>
@@ -526,6 +574,9 @@ QProgressBar::chunk#thrustBar {
           <height>22</height>
          </size>
         </property>
+        <property name="focusPolicy">
+         <enum>Qt::NoFocus</enum>
+        </property>
         <property name="toolTip">
          <string>Move Down</string>
         </property>
@@ -546,6 +597,9 @@ QProgressBar::chunk#thrustBar {
           <height>22</height>
          </size>
         </property>
+        <property name="focusPolicy">
+         <enum>Qt::NoFocus</enum>
+        </property>
         <property name="toolTip">
          <string>Delete</string>
         </property>

src/ui/map/MAV2DIcon.cc

 #include "MAV2DIcon.h"
 #include <QPainter>
 
-MAV2DIcon::MAV2DIcon(qreal x, qreal y, int radius, QString name, Alignment alignment, QPen* pen)
-    : Point(x, y, name, alignment),
-    yaw(0.0f)
+#include <qmath.h>
+
+MAV2DIcon::MAV2DIcon(UASInterface* uas, int radius, int type, const QColor& color, QString name, Alignment alignment, QPen* pen)
+    : Point(uas->getLatitude(), uas->getLongitude(), name, alignment),
+    yaw(0.0f),
+    radius(radius),
+    type(type),
+    iconColor(color),
+    selected(uas->getSelected()),
+    uasid(uas->getUASID())
 {
+    //connect
     size = QSize(radius, radius);
     drawIcon(pen);
 }
 
 MAV2DIcon::MAV2DIcon(qreal x, qreal y, QString name, Alignment alignment, QPen* pen)
-    : Point(x, y, name, alignment)
+    : Point(x, y, name, alignment),
+    radius(20),
+    type(0),
+    iconColor(Qt::yellow),
+    selected(false),
+    uasid(0)
 {
-    int radius = 10;
     size = QSize(radius, radius);
-    if (pen)
-    {
     drawIcon(pen);
-    }
 }
 
 MAV2DIcon::~MAV2DIcon()
@@ -27,69 +36,179 @@ MAV2DIcon::~MAV2DIcon()
 
 void MAV2DIcon::setPen(QPen* pen)
 {
-    if (pen)
-    {
     mypen = pen;
     drawIcon(pen);
-    }
+}
+
+void MAV2DIcon::setSelectedUAS(bool selected)
+{
+    this->selected = selected;
+    drawIcon(mypen);
 }
 
 /**
+ * Yaw changes of less than ±15 degrees are ignored.
+ *
  * @param yaw in radians, 0 = north, positive = clockwise
  */
 void MAV2DIcon::setYaw(float yaw)
 {
+    //qDebug() << "MAV2Icon" << yaw;
+    float diff = fabs(yaw - this->yaw);
+    while (diff > M_PI)
+    {
+        diff -= M_PI;
+    }
+
+    if (diff > 0.1)
+    {
         this->yaw = yaw;
         drawIcon(mypen);
+    }
 }
 
 void MAV2DIcon::drawIcon(QPen* pen)
 {
-
-    mypixmap = new QPixmap(radius+1, radius+1);
+    Q_UNUSED(pen);
+    if (!mypixmap) mypixmap = new QPixmap(radius+1, radius+1);
     mypixmap->fill(Qt::transparent);
     QPainter painter(mypixmap);
+    painter.setRenderHint(QPainter::TextAntialiasing);
+    painter.setRenderHint(QPainter::Antialiasing);
+    painter.setRenderHint(QPainter::HighQualityAntialiasing);
+
+    // Rotate by yaw
+    painter.translate(radius/2, radius/2);
+
+    // Draw selected indicator
+    if (selected)
+    {
+//        qDebug() << "SYSTEM IS NOW SELECTED";
+//        QColor color(Qt::yellow);
+//        color.setAlpha(0.3f);
+        painter.setBrush(Qt::NoBrush);
+//        QPen selPen(color);
+//        int width = 5;
+//        selPen.setWidth(width);
+        QPen pen(Qt::yellow);
+        pen.setWidth(2);
+        painter.setPen(pen);
+        painter.drawEllipse(QPoint(0, 0), radius/2-1, radius/2-1);
+        //qDebug() << "Painting ellipse" << radius/2-width << width;
+        //selPen->deleteLater();
+    }
 
-    // DRAW WAYPOINT
-    QPointF p(radius/2, radius/2);
-
-    float waypointSize = radius;
-    QPolygonF poly(4);
-    // Top point
-    poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
-    // Right point
-    poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
-    // Bottom point
-    poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
-    poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
-
-//    // Select color based on if this is the current waypoint
-//    if (list.at(i)->getCurrent())
-//    {
-//        color = QGC::colorCyan;//uas->getColor();
-//        pen.setWidthF(refLineWidthToPen(0.8f));
-//    }
-//    else
-//    {
-//        color = uas->getColor();
-//        pen.setWidthF(refLineWidthToPen(0.4f));
-//    }
-
-    //pen.setColor(color);
-    if (pen)
+    switch (type)
     {
-        pen->setWidthF(2);
-        painter.setPen(*pen);
+    case MAV_ICON_AIRPLANE:
+        {
+            // DRAW AIRPLANE
+
+            // Rotate 180 degs more since the icon is oriented downwards
+            float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f+180.0f;
+
+            painter.rotate(yawRotate);
+
+            //qDebug() << "ICON SIZE:" << radius;
+
+            float iconSize = radius*0.9f;
+            QPolygonF poly(24);
+            poly.replace(0, QPointF(0.000000f*iconSize, -0.312500f*iconSize));
+            poly.replace(1, QPointF(0.025000f*iconSize, -0.287500f*iconSize));
+            poly.replace(2, QPointF(0.037500f*iconSize, -0.237500f*iconSize));
+            poly.replace(3, QPointF(0.031250f*iconSize, -0.187500f*iconSize));
+            poly.replace(4, QPointF(0.025000f*iconSize, -0.043750f*iconSize));
+            poly.replace(5, QPointF(0.500000f*iconSize, -0.025000f*iconSize));
+            poly.replace(6, QPointF(0.500000f*iconSize, 0.025000f*iconSize));
+            poly.replace(7, QPointF(0.025000f*iconSize, 0.043750f*iconSize));
+            poly.replace(8, QPointF(0.025000f*iconSize, 0.162500f*iconSize));
+            poly.replace(9, QPointF(0.137500f*iconSize, 0.181250f*iconSize));
+            poly.replace(10, QPointF(0.137500f*iconSize, 0.218750f*iconSize));
+            poly.replace(11, QPointF(0.025000f*iconSize, 0.206250f*iconSize));
+            poly.replace(12, QPointF(-0.025000f*iconSize, 0.206250f*iconSize));
+            poly.replace(13, QPointF(-0.137500f*iconSize, 0.218750f*iconSize));
+            poly.replace(14, QPointF(-0.137500f*iconSize, 0.181250f*iconSize));
+            poly.replace(15, QPointF(-0.025000f*iconSize, 0.162500f*iconSize));
+            poly.replace(16, QPointF(-0.025000f*iconSize, 0.043750f*iconSize));
+            poly.replace(17, QPointF(-0.500000f*iconSize, 0.025000f*iconSize));
+            poly.replace(18, QPointF(-0.500000f*iconSize, -0.025000f*iconSize));
+            poly.replace(19, QPointF(-0.025000f*iconSize, -0.043750f*iconSize));
+            poly.replace(20, QPointF(-0.031250f*iconSize, -0.187500f*iconSize));
+            poly.replace(21, QPointF(-0.037500f*iconSize, -0.237500f*iconSize));
+            poly.replace(22, QPointF(-0.031250f*iconSize, -0.262500f*iconSize));
+            poly.replace(23, QPointF(0.000000f*iconSize, -0.312500f*iconSize));
+
+            painter.setBrush(QBrush(iconColor));
+            QPen iconPen(Qt::black);
+            iconPen.setWidthF(1.0f);
+            painter.setPen(iconPen);
+
+            painter.drawPolygon(poly);
         }
-    else
+        break;
+    case MAV_ICON_QUADROTOR:
         {
-        QPen pen2(Qt::red);
-        pen2.setWidth(2);
-        painter.setPen(pen2);
+            // QUADROTOR
+            float iconSize = radius*0.9f;
+            float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f;
+
+            painter.rotate(yawRotate);
+
+            //qDebug() << "ICON SIZE:" << radius;
+
+            QPointF front(0, 0.2);
+            front = front *iconSize;
+            QPointF left(-0.2, 0);
+            left = left * iconSize;
+            QPointF right(0.2, 0.0);
+            right *= iconSize;
+            QPointF back(0, -0.2);
+            back *= iconSize;
+
+            QPolygonF poly(0);
+
+
+
+            painter.setBrush(QBrush(iconColor));
+            QPen iconPen(Qt::black);
+            iconPen.setWidthF(1.0f);
+            painter.setPen(iconPen);
+
+            painter.drawPolygon(poly);
+
+            painter.drawEllipse(left, radius/4/2, radius/4/2);
+            painter.drawEllipse(right, radius/4/2, radius/4/2);
+            painter.drawEllipse(back, radius/4/2, radius/4/2);
+
+            painter.setBrush(Qt::red);
+            painter.drawEllipse(front, radius/4/2, radius/4/2);
         }
-    painter.setBrush(Qt::NoBrush);
+        break;
+    case MAV_ICON_ROTARY_WING:
+        case MAV_ICON_GENERIC:
+    default:
+        {
+            // GENERIC
+
+            float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f;
+
+            painter.rotate(yawRotate);
+
+            //qDebug() << "ICON SIZE:" << radius;
+
+            float iconSize = radius*0.9f;
+            QPolygonF poly(3);
+            poly.replace(0, QPointF(0.0f*iconSize, 0.3f*iconSize));
+            poly.replace(1, QPointF(-0.2f*iconSize, -0.2f*iconSize));
+            poly.replace(2, QPointF(0.2f*iconSize, -0.2f*iconSize));
+
+            painter.setBrush(QBrush(iconColor));
+            QPen iconPen(Qt::black);
+            iconPen.setWidthF(1.0f);
+            painter.setPen(iconPen);
 
-    float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
-    painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
             painter.drawPolygon(poly);
+        }
+        break;
+    }
 }

src/ui/map/MAV2DIcon.h

 #ifndef MAV2DICON_H
 #define MAV2DICON_H
 
+#include <QGraphicsItem>
 #include "qmapcontrol.h"
 
+#include "UASInterface.h"
+
 class MAV2DIcon : public qmapcontrol::Point
 {
 public:
+    enum
+    {
+        MAV_ICON_GENERIC = 0,
+        MAV_ICON_AIRPLANE,
+        MAV_ICON_QUADROTOR,
+        MAV_ICON_ROTARY_WING
+    } MAV_ICON_TYPE;
+
+    //!
     /*!
      *
      * @param x longitude
      * @param y latitude
+     * @param radius the radius of the circle
      * @param name name of the circle point
      * @param alignment alignment (Middle or TopLeft)
      * @param pen QPen for drawing
      */
-    MAV2DIcon(qreal x, qreal y, QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
+    MAV2DIcon(UASInterface* uas, int radius = 10, int type=0, const QColor& color=QColor(Qt::red), QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
 
-    //!
     /*!
      *
      * @param x longitude
      * @param y latitude
-     * @param radius the radius of the circle
      * @param name name of the circle point
      * @param alignment alignment (Middle or TopLeft)
      * @param pen QPen for drawing
      */
-    MAV2DIcon(qreal x, qreal y, int radius = 10, QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
+    MAV2DIcon(qreal x, qreal y, QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
+
     virtual ~MAV2DIcon();
 
     //! sets the QPen which is used for drawing the circle
@@ -37,13 +49,22 @@ public:
      */
     virtual void setPen(QPen* pen);
 
+    /** @brief Mark this system as selected */
+    void setSelectedUAS(bool selected);
     void setYaw(float yaw);
+
+    /** @brief Get system id */
+    int getUASId() const { return uasid; }
+
     void drawIcon(QPen* pen);
 
 protected:
     float yaw;      ///< Yaw angle of the MAV
-    int radius; ///< Maximum dimension of the MAV icon
-
+    int radius;     ///< Radius / width of the icon
+    int type;       ///< Type of aircraft: 0: generic, 1: airplane, 2: quadrotor, 3-n: rotary wing
+    QColor iconColor; ///< Color to be used for the icon
+    bool selected;  ///< Wether this is the system currently in focus
+    int uasid;      ///< ID of tracked system
 
 };
 

src/ui/map/Waypoint2DIcon.cc

@@ -6,6 +6,16 @@ Waypoint2DIcon::Waypoint2DIcon(qreal x, qreal y, int radius, QString name, Align
     yaw(0.0f),
     radius(radius)
 {
+    waypoint = NULL;
+    size = QSize(radius, radius);
+    drawIcon(pen);
+}
+
+Waypoint2DIcon::Waypoint2DIcon(Waypoint* wp, int listIndex, int radius, Alignment alignment, QPen* pen)
+    : Point(wp->getX(), wp->getY(), QString("%1").arg(listIndex), alignment)
+{
+    waypoint = wp;
+    this->yaw = wp->getYaw();
     size = QSize(radius, radius);
     drawIcon(pen);
 }
@@ -13,7 +23,8 @@ Waypoint2DIcon::Waypoint2DIcon(qreal x, qreal y, int radius, QString name, Align
 Waypoint2DIcon::Waypoint2DIcon(qreal x, qreal y, QString name, Alignment alignment, QPen* pen)
     : Point(x, y, name, alignment)
 {
-    int radius = 10;
+    waypoint = NULL;
+    int radius = 20;
     size = QSize(radius, radius);
     drawIcon(pen);
 }
@@ -38,6 +49,18 @@ void Waypoint2DIcon::setYaw(float yaw)
     drawIcon(mypen);
 }
 
+void Waypoint2DIcon::updateWaypoint()
+{
+    if (waypoint)
+    {
+        if (waypoint->getYaw() != yaw)
+        {
+            yaw = waypoint->getYaw();
+            drawIcon(mypen);
+        }
+    }
+}
+
 void Waypoint2DIcon::drawIcon(QPen* pen)
 {
     mypixmap = new QPixmap(radius+1, radius+1);

src/ui/map/Waypoint2DIcon.h

@@ -4,6 +4,8 @@
 #include <QGraphicsItem>
 #include "qmapcontrol.h"
 
+#include "Waypoint.h"
+
 class Waypoint2DIcon : public qmapcontrol::Point
 {
 public:
@@ -27,7 +29,19 @@ public:
      * @param alignment alignment (Middle or TopLeft)
      * @param pen QPen for drawing
      */
-    Waypoint2DIcon(qreal x, qreal y, int radius = 10, QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
+    Waypoint2DIcon(qreal x, qreal y, int radius = 20, QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
+
+    /**
+     *
+     * @param wp Waypoint
+     * @param listIndex Index in the waypoint list
+     * @param radius the radius of the circle
+     * @param name name of the circle point
+     * @param alignment alignment (Middle or TopLeft)
+     * @param pen QPen for drawing
+     */
+    Waypoint2DIcon(Waypoint* wp, int listIndex, int radius = 20, Alignment alignment = Middle, QPen* pen=0);
+
     virtual ~Waypoint2DIcon();
 
     //! sets the QPen which is used for drawing the circle
@@ -42,9 +56,13 @@ public:
 
     void drawIcon(QPen* pen);
 
+public slots:
+    void updateWaypoint();
+
 protected:
     float yaw;      ///< Yaw angle of the MAV
-    int radius;     ///<
+    int radius;     ///< Radius / diameter of the icon in pixels
+    Waypoint* waypoint;   ///< Waypoint data container this icon represents
 
 };
 

src/ui/map3D/QGCGoogleEarthView.cc

@@ -185,6 +185,7 @@ void QGCGoogleEarthView::follow(bool follow)
 {
     ui->followAirplaneCheckbox->setChecked(follow);
     followCamera = follow;
+    if (gEarthInitialized) javaScript(QString("setFollowEnabled(%1)").arg(follow));
 }
 
 void QGCGoogleEarthView::goHome()
@@ -353,6 +354,8 @@ void QGCGoogleEarthView::initializeGoogleEarth()
             // Start update timer
             updateTimer->start(refreshRateMs);
 
+            follow(this->followCamera);
+
             gEarthInitialized = true;
         }
     }
@@ -395,11 +398,6 @@ void QGCGoogleEarthView::updateState()
                        .arg(pitch, 0, 'f', 9)
                        .arg(yaw, 0, 'f', 9));
         }
-
-        if (followCamera)
-        {
-            javaScript(QString("updateFollowAircraft()"));
-        }
     }
 }
 

src/ui/map3D/QGCGoogleEarthView.ui

@@ -83,10 +83,10 @@
       <string>Distance of the chase camera to the MAV</string>
      </property>
      <property name="minimum">
-      <number>30</number>
+      <number>100</number>
      </property>
      <property name="maximum">
-      <number>10000</number>
+      <number>20000</number>
      </property>
      <property name="value">
       <number>3000</number>

src/ui/uas/UASView.cc

@@ -51,7 +51,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
         uas(uas),
         load(0),
         state("UNKNOWN"),
-        stateDesc(tr("Unknown system state")),
+        stateDesc(tr("Unknown state")),
         mode("MAV_MODE_UNKNOWN"),
         thrust(0),
         isActive(false),
@@ -66,6 +66,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
         localFrame(false),
         removeAction(new QAction("Delete this system", this)),
         renameAction(new QAction("Rename..", this)),
+        selectAction(new QAction("Select this system", this )),
         m_ui(new Ui::UASView)
 {
     m_ui->setupUi(this);
@@ -102,6 +103,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
     // Allow to delete this widget
     connect(removeAction, SIGNAL(triggered()), this, SLOT(deleteLater()));
     connect(renameAction, SIGNAL(triggered()), this, SLOT(rename()));
+    connect(selectAction, SIGNAL(triggered()), uas, SLOT(setSelected()));
     connect(uas, SIGNAL(systemRemoved()), this, SLOT(deleteLater()));
 
     // Name changes
@@ -129,21 +131,15 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
     m_ui->killButton->hide();
     m_ui->shutdownButton->hide();
 
-    if (localFrame)
-    {
-        m_ui->gpsLabel->hide();
-    }
-    else
-    {
-        m_ui->positionLabel->hide();
-    }
-
     setSystemType(uas, uas->getSystemType());
 }
 
 UASView::~UASView()
 {
     delete m_ui;
+    delete removeAction;
+    delete renameAction;
+    delete selectAction;
 }
 
 void UASView::heartbeatTimeout()
@@ -332,8 +328,6 @@ void UASView::updateLocalPosition(UASInterface* uas, double x, double y, double
     if (!localFrame)
     {
         localFrame = true;
-        m_ui->gpsLabel->hide();
-        m_ui->positionLabel->show();
     }
 }
 
@@ -424,6 +418,7 @@ void UASView::contextMenuEvent (QContextMenuEvent* event)
     {
         menu.addAction(removeAction);
     }
+    menu.addAction(selectAction);
     menu.exec(event->globalPos());
 }
 
@@ -471,7 +466,7 @@ void UASView::refresh()
 
         // Position
         QString position;
-        position = position.sprintf("%05.1f %05.1f %05.1f m", x, y, z);
+        position = position.sprintf("%05.1f %05.1f %06.1f m", x, y, z);
         m_ui->positionLabel->setText(position);
         QString globalPosition;
         QString latIndicator;
@@ -492,17 +487,17 @@ void UASView::refresh()
         {
             lonIndicator = "W";
         }
-        globalPosition = globalPosition.sprintf("%05.1f%s %05.1f%s %05.1f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt);
-        m_ui->gpsLabel->setText(globalPosition);
+        globalPosition = globalPosition.sprintf("%05.1f%s %05.1f%s %06.1f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt);
+        m_ui->positionLabel->setText(globalPosition);
 
         // Altitude
         if (groundDistance == 0 && alt != 0)
         {
-            m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt, 5, 'f', 1, '0'));
+            m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt, 6, 'f', 1, '0'));
         }
         else
         {
-            m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance, 5, 'f', 1, '0'));
+            m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance, 6, 'f', 1, '0'));
         }
 
         // Speed

src/ui/uas/UASView.h

@@ -110,6 +110,7 @@ protected:
     bool localFrame;
     QAction* removeAction;
     QAction* renameAction;
+    QAction* selectAction;
     static const int updateInterval = 300;