Fully working waypoint interface, thoroughly tested. Active waypoint setting has to be debugged in the simulator, but works fine on real hardware

images/earth.html

@@ -251,11 +251,11 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
 	
 	
 	// 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);
 	
-	currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
+	currFollowHeading = ((yaw/M_PI))*180.0;
     
     planeLoc.setLatitude(lastLat);
     planeLoc.setLongitude(lastLon);
@@ -303,7 +303,7 @@ function updateFollowAircraft()
 
 	currView.setRange(currViewRange);
 	currView.setTilt(currFollowTilt);
-	currView.setHeading(currFollowHeading-90.0);
+	currView.setHeading(currFollowHeading);
 	
 	ge.getView().setAbstractView(currView);
 }

src/comm/MAVLinkSimulationMAV.cc

@@ -4,32 +4,36 @@
 #include "MAVLinkSimulationMAV.h"
 
 MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid) :
-    QObject(parent),
-    link(parent),
-    planner(parent, systemid),
-    systemid(systemid),
-    timer25Hz(0),
-    timer10Hz(0),
-    timer1Hz(0),
-    latitude(47.376389),
-    longitude(8.548056),
-    altitude(0.0),
-    x(8.548056),
-    y(47.376389),
-    z(550),
-    roll(0.0),
-    pitch(0.0),
-    yaw(0.0),
-    globalNavigation(true),
-    firstWP(false),
-    previousSPX(8.548056),
-    previousSPY(47.376389),
-    previousSPZ(550),
-    previousSPYaw(0.0),
-    nextSPX(8.548056),
-    nextSPY(47.376389),
-    nextSPZ(550),
-    nextSPYaw(0.0)
+        QObject(parent),
+        link(parent),
+        planner(parent, systemid),
+        systemid(systemid),
+        timer25Hz(0),
+        timer10Hz(0),
+        timer1Hz(0),
+        latitude(47.376389),
+        longitude(8.548056),
+        altitude(0.0),
+        x(8.548056),
+        y(47.376389),
+        z(550),
+        roll(0.0),
+        pitch(0.0),
+        yaw(0.0),
+        globalNavigation(true),
+        firstWP(false),
+        previousSPX(8.548056),
+        previousSPY(47.376389),
+        previousSPZ(550),
+        previousSPYaw(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,9 +46,9 @@ 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)
 
     // 1 Hz execution
     if (timer1Hz <= 0)
@@ -61,8 +65,8 @@ void MAVLinkSimulationMAV::mainloop()
     {
         if (!firstWP)
         {
-            double radPer100ms = 0.0002;
-            double altPer100ms = 0.1;
+            double radPer100ms = 0.00006;
+            double altPer100ms = 1.0;
             double xm = (nextSPX - x);
             double ym = (nextSPY - y);
             double zm = (nextSPZ - z);
@@ -71,14 +75,35 @@ void MAVLinkSimulationMAV::mainloop()
 
             //float trueyaw = atan2f(xm, ym);
 
-            yaw = yaw*0.9 + 0.1*atan2f(xm, ym);
+            float newYaw = atan2f(xm, ym);
+            // Choose shortest direction
+//            if (yaw - newYaw < -180.0f)
+//            {
+//                yaw = newYaw + 180.0f;
+//            }
 
-            qDebug() << "SIMULATION MAV: x:" << xm << "y:" << ym << "z:" << zm << "yaw:" << yaw;
+//            if (yaw - newYaw > 180.0f)
+//            {
+//                yaw = newYaw - 180.0f;
+//            }
 
-            if (sqrt(xm*xm+ym*ym) > 0.0001)
+
+            if (fabs(yaw - newYaw) < 90)
+            {
+                yaw = yaw*0.7 + 0.3*newYaw;
+            }
+            else
             {
-                x += cos(yaw)*radPer100ms;
-                y += sin(yaw)*radPer100ms;
+                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;
             }
 
@@ -94,6 +119,7 @@ void MAVLinkSimulationMAV::mainloop()
         }
 
 
+        // GLOBAL POSITION
         mavlink_message_t msg;
         mavlink_global_position_int_t pos;
         pos.alt = z*1000.0;
@@ -105,15 +131,27 @@ void MAVLinkSimulationMAV::mainloop()
         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;
 
-        qDebug() << "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);
         timer10Hz = 5;
     }
 
@@ -136,6 +174,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;

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

@@ -463,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)
@@ -584,7 +584,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
 
                                 // FIXME big hack for simulation!
                                 //float oneDegreeOfLatMeters = 111131.745f;
-                                float orbit = 0.0001;
+                                float orbit = 0.00008;
 
                                 // compare current position (given in message) with current waypoint
                                 //float orbit = wp->param1;
@@ -1009,7 +1009,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/uas/UAS.cc

@@ -146,7 +146,16 @@ void UAS::updateState()
 
 void UAS::setSelected()
 {
-    UASManager::instance()->setActiveUAS(this);
+    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)

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/ui/MapWidget.cc

@@ -36,7 +36,7 @@ MapWidget::MapWidget(QWidget *parent) :
         m_ui(new Ui::MapWidget)
 {
     m_ui->setupUi(this);
-    mc = new qmapcontrol::MapControl(QSize(320, 240));
+    mc = new qmapcontrol::MapControl(this->size());
 
     //   VISUAL MAP STYLE
     QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)} QAbstractButton:checked { border: 2px solid #379AC3; }");
@@ -254,26 +254,26 @@ MapWidget::MapWidget(QWidget *parent) :
 void MapWidget::goTo()
 {
     bool ok;
-         QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
-                                              tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
-                                              QString("%1,%2").arg(mc->currentCoordinate().x()).arg(mc->currentCoordinate().y()), &ok);
-         if (ok && !text.isEmpty())
-         {
-             QStringList split = text.split(",");
-             if (split.length() == 2)
-             {
-                 bool convert;
-                 double latitude = split.first().toDouble(&convert);
-                 ok &= convert;
-                 double longitude = split.last().toDouble(&convert);
-                 ok &= convert;
-
-                 if (ok)
-                 {
-                     mc->setView(QPointF(latitude, longitude));
-                 }
-             }
-         }
+    QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
+                                         tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
+                                         QString("%1,%2").arg(mc->currentCoordinate().x()).arg(mc->currentCoordinate().y()), &ok);
+    if (ok && !text.isEmpty())
+    {
+        QStringList split = text.split(",");
+        if (split.length() == 2)
+        {
+            bool convert;
+            double latitude = split.first().toDouble(&convert);
+            ok &= convert;
+            double longitude = split.last().toDouble(&convert);
+            ok &= convert;
+
+            if (ok)
+            {
+                mc->setView(QPointF(latitude, longitude));
+            }
+        }
+    }
 }
 
 
@@ -290,7 +290,7 @@ void MapWidget::mapproviderSelected(QAction* action)
         l->setMapAdapter(mapadapter);
         geomLayer->setMapAdapter(mapadapter);
 
-        mc->updateRequestNew();
+        if (isVisible()) mc->updateRequestNew();
         mc->setZoom(zoom);
         //        yahooActionOverlay->setEnabled(false);
         overlay->setVisible(false);
@@ -306,7 +306,7 @@ void MapWidget::mapproviderSelected(QAction* action)
         l->setMapAdapter(mapadapter);
         geomLayer->setMapAdapter(mapadapter);
 
-        mc->updateRequestNew();
+        if (isVisible()) mc->updateRequestNew();
         mc->setZoom(zoom);
         //        yahooActionOverlay->setEnabled(false);
         overlay->setVisible(false);
@@ -321,7 +321,7 @@ void MapWidget::mapproviderSelected(QAction* action)
         mapadapter = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=1.7&t=a&s=256&x=%2&y=%3&z=%1");
         l->setMapAdapter(mapadapter);
 
-        mc->updateRequestNew();
+        if (isVisible()) mc->updateRequestNew();
         mc->setZoom(zoom);
         //        yahooActionOverlay->setEnabled(true);
     }
@@ -333,7 +333,7 @@ void MapWidget::mapproviderSelected(QAction* action)
         l->setMapAdapter(mapadapter);
         geomLayer->setMapAdapter(mapadapter);
 
-        mc->updateRequestNew();
+        if (isVisible()) mc->updateRequestNew();
         mc->setZoom(zoom);
         //        yahooActionOverlay->setEnabled(false);
         overlay->setVisible(false);
@@ -347,7 +347,7 @@ void MapWidget::mapproviderSelected(QAction* action)
         l->setMapAdapter(mapadapter);
         geomLayer->setMapAdapter(mapadapter); 
 
-        mc->updateRequestNew();
+        if (isVisible()) mc->updateRequestNew();
         mc->setZoom(zoom);
         //        yahooActionOverlay->setEnabled(false);
         overlay->setVisible(false);
@@ -426,7 +426,7 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina
             waypointPath->addPoint(tempPoint);
 
             // Refresh the screen
-            mc->updateRequest(tempPoint->boundingBox().toRect());
+            if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect());
         }
 
         // emit signal mouse was clicked
@@ -445,10 +445,11 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
     if (uas == this->mav->getUASID())
     {
         int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getIndexOf(wp);
+        if (wpindex == -1) return;
         // Create waypoint name
-        QString str = QString("%1").arg(wpindex);
+        //QString str = QString("%1").arg(wpindex);
         // Check if wp exists yet
-        if (!(wps.count() > wpindex))
+        if (!(wpIcons.count() > wpindex))
         {
             QPointF coordinate;
             coordinate.setX(wp->getX());
@@ -494,7 +495,7 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
 
                     //point2Find = dynamic_cast <Point*> (mc->layer("Waypoints")->get_Geometry(wpindex));
                     //point2Find->setCoordinate(coordinate);
-                    if (updateView) mc->updateRequest(waypoint->boundingBox().toRect());
+                    if (updateView) if (isVisible()) mc->updateRequest(waypoint->boundingBox().toRect());
                 }
             }
         }
@@ -537,7 +538,7 @@ void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate)
         qDebug()<<"Funcion createWaypointGraphAtMap WP= "<<str<<" -> x= "<<tempPoint->latitude()<<" y= "<<tempPoint->longitude();
 
         // Refresh the screen
-        mc->updateRequest(tempPoint->boundingBox().toRect());
+        if (isVisible()) if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect());
     }
 
     ////    // emit signal mouse was clicked
@@ -570,7 +571,7 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
     waypointIsDrag = true;
 
     // Refresh the screen
-    mc->updateRequest(geom->boundingBox().toRect());
+    if (isVisible()) mc->updateRequest(geom->boundingBox().toRect());
 
     int temp = 0;
 
@@ -578,40 +579,35 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
     bool wpIndexOk;
     int index = geom->name().toInt(&wpIndexOk);
 
-    qmapcontrol::Point* point2Find;
-    point2Find = wps.at(geom->name().toInt());
+    Waypoint2DIcon* point2Find = dynamic_cast <Waypoint2DIcon*> (geom);
 
-    if (point2Find)
+    if (wpIndexOk && point2Find && wps.count() > index)
     {
+        // Update visual
         point2Find->setCoordinate(coordinate);
+        waypointPath->points().at(index)->setCoordinate(coordinate);
+        if (isVisible()) mc->updateRequest(waypointPath->boundingBox().toRect());
 
-        point2Find = dynamic_cast <qmapcontrol::Point*> (geom);
-        if (point2Find)
+        // Update waypoint data storage
+        if (mav)
         {
-            point2Find->setCoordinate(coordinate);
+            QVector<Waypoint*> wps = mav->getWaypointManager()->getWaypointList();
 
-            if (wpIndexOk)
+            if (wps.size() > index)
             {
-                mc->updateRequest(point2Find->boundingBox().toRect());
-                if (mav)
-                {
-                        QVector<Waypoint*> wps = mav->getWaypointManager()->getWaypointList();
-
-                    if (wps.size() > index)
-                    {
-                        wps.at(index)->setX(coordinate.x());
-                        wps.at(index)->setY(coordinate.y());
-                        mav->getWaypointManager()->notifyOfChange(wps.at(index));
-                    }
-                }
+                wps.at(index)->setX(coordinate.x());
+                wps.at(index)->setY(coordinate.y());
+                mav->getWaypointManager()->notifyOfChange(wps.at(index));
             }
-            // qDebug() << geom->name();
-            temp = geom->get_myIndex();
-            //qDebug() << temp;
-            emit sendGeometryEndDrag(coordinate,temp);
         }
+
+        // qDebug() << geom->name();
+        temp = geom->get_myIndex();
+        //qDebug() << temp;
+        emit sendGeometryEndDrag(coordinate,temp);
     }
 
+    waypointIsDrag = false;
 }
 
 void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate)
@@ -670,21 +666,20 @@ void MapWidget::updateWaypointList(int uas)
         }
 
         // Delete now unused wps
-        if (wps.count() > wpList.count())
+        if (waypointPath->points().count() > wpList.count())
         {
-            for (int i = wpList.count(); i < wps.count(); ++i)
+            int overSize = waypointPath->points().count() - wpList.count();
+            for (int i = 0; i < overSize; ++i)
             {
-                wps.removeAt(i);
-                mc->layer("Waypoints")->removeGeometry(wpIcons.at(i));
-                waypointPath->points().removeAt(i);
-                //Point* linesegment = waypointPath->points().at(mav->getWaypointManager()->getWaypointList().indexOf(wp));
-
-
+                wps.removeLast();
+                mc->layer("Waypoints")->removeGeometry(wpIcons.last());
+                wpIcons.removeLast();
+                waypointPath->points().removeLast();
             }
         }
 
         // Update view
-        mc->updateRequest(updateRect);
+        if (isVisible()) mc->updateRequest(updateRect);
     }
 }
 
@@ -734,6 +729,21 @@ void MapWidget::activeUASSet(UASInterface* uas)
         connect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
         connect(mav->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
         connect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));
+
+        updateSelectedSystem(mav->getUASID());
+    }
+}
+
+void MapWidget::updateSelectedSystem(int uas)
+{
+    foreach (qmapcontrol::Point* p, uasIcons.values())
+    {
+        MAV2DIcon* icon = dynamic_cast<MAV2DIcon*>(p);
+        if (icon)
+        {
+            // Set as selected if ids match
+            icon->setSelectedUAS((icon->getUASId() == uas));
+        }
     }
 }
 
@@ -787,63 +797,62 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
         // Icon
         //QPen* pointpen = new QPen(uasColor);
         qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__;
-        //p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, mavPens.value(uas->getUASID()));
-        p = new MAV2DIcon(lat, lon, 50, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
+        p = new MAV2DIcon(uas, 50, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
         uasIcons.insert(uas->getUASID(), p);
         mc->layer("Waypoints")->addGeometry(p);
 
         // Line
         // A QPen also can use transparency
 
-//        QList<qmapcontrol::Point*> points;
-//        points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
-//        QPen* linepen = new QPen(uasColor.darker());
-//        linepen->setWidth(2);
+        //        QList<qmapcontrol::Point*> points;
+        //        points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
+        //        QPen* linepen = new QPen(uasColor.darker());
+        //        linepen->setWidth(2);
 
-//        // Create tracking line string
-//        qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen);
-//        uasTrails.insert(uas->getUASID(), ls);
+        //        // Create tracking line string
+        //        qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen);
+        //        uasTrails.insert(uas->getUASID(), ls);
 
-//        // Add the LineString to the layer
-//        mc->layer("Waypoints")->addGeometry(ls);
+        //        // Add the LineString to the layer
+        //        mc->layer("Waypoints")->addGeometry(ls);
     }
     else
     {
-//        p = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID()));
-//        if (p)
-//        {
-         p = uasIcons.value(uas->getUASID());
-            p->setCoordinate(QPointF(lat, lon));
-            //p->setYaw(uas->getYaw());
-//        }
+        //        p = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID()));
+        //        if (p)
+        //        {
+        p = uasIcons.value(uas->getUASID());
+        p->setCoordinate(QPointF(lat, lon));
+        //p->setYaw(uas->getYaw());
+        //        }
         // Extend trail
-//        uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
+        //        uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
     }
 
-mc->updateRequest(p->boundingBox().toRect());
+    if (isVisible()) mc->updateRequest(p->boundingBox().toRect());
 
-    //mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());
+    //if (isVisible()) mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());
 
-    if (uas->getUASID() == this->mav->getUASID())
-{
-    // Limit the position update rate
-    quint64 currTime = MG::TIME::getGroundTimeNow();
-    if (currTime - lastUpdate > 120)
+    if (this->mav && uas->getUASID() == this->mav->getUASID())
     {
-        lastUpdate = currTime;
-        // Sets the view to the interesting area
-        if (followgps->isChecked())
+        // Limit the position update rate
+        quint64 currTime = MG::TIME::getGroundTimeNow();
+        if (currTime - lastUpdate > 120)
         {
-            updatePosition(0, lat, lon);
-        }
-        else
-        {
-            // Refresh the screen
-            //mc->updateRequestNew();
+            lastUpdate = currTime;
+            // Sets the view to the interesting area
+            if (followgps->isChecked())
+            {
+                updatePosition(0, lat, lon);
+            }
+            else
+            {
+                // Refresh the screen
+                //if (isVisible()) mc->updateRequestNew();
+            }
         }
     }
 }
-}
 
 /**
  * Center the view on this position
@@ -953,7 +962,7 @@ void MapWidget::clearWaypoints(int uas)
     //waypointPath = new
     //mc->layer("Waypoints")->addGeometry(waypointPath);
     //wpIndex.clear();
-    mc->updateRequest(box);//(waypointPath->boundingBox().toRect());
+    if (isVisible()) mc->updateRequest(box);//(waypointPath->boundingBox().toRect());
 
     if(createPath->isChecked())
     {
@@ -975,7 +984,7 @@ void MapWidget::clearPath(int uas)
         mc->layer("Tracking")->addGeometry(lsNew);
     }
     // FIXME update this with update request only for bounding box of trails
-    mc->updateRequestNew();//(QRect(0, 0, width(), height()));
+    if (isVisible()) mc->updateRequestNew();//(QRect(0, 0, width(), height()));
 }
 
 void MapWidget::updateCameraPosition(double radio, double bearing, QString dir)
@@ -1012,14 +1021,14 @@ void MapWidget::updateCameraPosition(double radio, double bearing, QString dir)
 
         mc->layer("Camera")->addGeometry(camBorder);
         // mc->layer("Camera")->addGeometry(camLine);
-        mc->updateRequestNew();
+        if (isVisible()) mc->updateRequestNew();
 
     }
     else
     {
         //clear camera borders
         mc->layer("Camera")->clearGeometries();
-        mc->updateRequestNew();
+        if (isVisible()) mc->updateRequestNew();
 
     }
 

src/ui/MapWidget.h

@@ -68,6 +68,8 @@ 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);

src/ui/WaypointView.ui

@@ -235,7 +235,7 @@ QProgressBar::chunk#thrustBar {
          </sizepolicy>
         </property>
         <property name="focusPolicy">
-         <enum>Qt::ClickFocus</enum>
+         <enum>Qt::WheelFocus</enum>
         </property>
         <property name="toolTip">
          <string>Position X coordinate</string>
@@ -275,7 +275,7 @@ QProgressBar::chunk#thrustBar {
          </sizepolicy>
         </property>
         <property name="focusPolicy">
-         <enum>Qt::StrongFocus</enum>
+         <enum>Qt::WheelFocus</enum>
         </property>
         <property name="toolTip">
          <string>Position Y coordinate</string>
@@ -309,7 +309,7 @@ QProgressBar::chunk#thrustBar {
          </sizepolicy>
         </property>
         <property name="focusPolicy">
-         <enum>Qt::StrongFocus</enum>
+         <enum>Qt::WheelFocus</enum>
         </property>
         <property name="toolTip">
          <string>Position Z coordinate (negative)</string>
@@ -340,7 +340,7 @@ QProgressBar::chunk#thrustBar {
          </sizepolicy>
         </property>
         <property name="focusPolicy">
-         <enum>Qt::StrongFocus</enum>
+         <enum>Qt::WheelFocus</enum>
         </property>
         <property name="prefix">
          <string>lat </string>
@@ -371,7 +371,7 @@ QProgressBar::chunk#thrustBar {
          </sizepolicy>
         </property>
         <property name="focusPolicy">
-         <enum>Qt::StrongFocus</enum>
+         <enum>Qt::WheelFocus</enum>
         </property>
         <property name="prefix">
          <string>lon </string>

src/ui/map/MAV2DIcon.cc

@@ -3,13 +3,16 @@
 
 #include <qmath.h>
 
-MAV2DIcon::MAV2DIcon(qreal x, qreal y, int radius, int type, const QColor& color, QString name, Alignment alignment, QPen* pen)
-    : Point(x, y, name, alignment),
+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)
+    iconColor(color),
+    selected(uas->getSelected()),
+    uasid(uas->getUASID())
 {
+    //connect
     size = QSize(radius, radius);
     drawIcon(pen);
 }
@@ -18,7 +21,9 @@ MAV2DIcon::MAV2DIcon(qreal x, qreal y, QString name, Alignment alignment, QPen*
     : Point(x, y, name, alignment),
     radius(20),
     type(0),
-    iconColor(Qt::yellow)
+    iconColor(Qt::yellow),
+    selected(false),
+    uasid(0)
 {
     size = QSize(radius, radius);
     drawIcon(pen);
@@ -35,6 +40,12 @@ void MAV2DIcon::setPen(QPen* pen)
     drawIcon(pen);
 }
 
+void MAV2DIcon::setSelectedUAS(bool selected)
+{
+    this->selected = selected;
+    drawIcon(mypen);
+}
+
 /**
  * Yaw changes of less than ±15 degrees are ignored.
  *
@@ -43,22 +54,54 @@ void MAV2DIcon::setPen(QPen* pen)
 void MAV2DIcon::setYaw(float yaw)
 {
     //qDebug() << "MAV2Icon" << yaw;
-    //float diff = fabs(yaw - this->yaw);
-//    while (diff > M_PI)
-//    {
-//        diff -= M_PI;
-//    }
-
-//    if (diff > 0.25)
-//    {
+    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)
 {
     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);
+        painter.setPen(Qt::yellow);
+        painter.drawEllipse(QPoint(0, 0), radius/2-1, radius/2-1);
+        //qDebug() << "Painting ellipse" << radius/2-width << width;
+        //selPen->deleteLater();
+    }
+
+    float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f+180.0f;
+
+    painter.rotate(yawRotate);
+
+    if (this->name() == "1") qDebug() << "uas icon" << name() << "yaw in:" << yaw << "rotate:" << yawRotate;
+
     switch (type)
     {
     case MAV_ICON_GENERIC:
@@ -67,20 +110,9 @@ void MAV2DIcon::drawIcon(QPen* pen)
     case MAV_ICON_ROTARY_WING:
     default:
         {
-            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);
-            painter.rotate(((yaw/(float)M_PI)+1.0f)*360.0f);
-
             // DRAW AIRPLANE
 
-            qDebug() << "ICON SIZE:" << radius;
+            //qDebug() << "ICON SIZE:" << radius;
 
             float iconSize = radius*0.9f;
             QPolygonF poly(24);
@@ -134,6 +166,9 @@ void MAV2DIcon::drawIcon(QPen* pen)
 //                painter.setPen(pen2);
 //            }
             painter.setBrush(QBrush(iconColor));
+            QPen iconPen(Qt::black);
+            iconPen.setWidthF(1.0f);
+            painter.setPen(iconPen);
 
             painter.drawPolygon(poly);
         }

src/ui/map/MAV2DIcon.h

@@ -4,6 +4,8 @@
 #include <QGraphicsItem>
 #include "qmapcontrol.h"
 
+#include "UASInterface.h"
+
 class MAV2DIcon : public qmapcontrol::Point
 {
 public:
@@ -14,27 +16,29 @@ public:
         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, int type=0, const QColor& color=QColor(Qt::red), 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
@@ -45,8 +49,13 @@ 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:
@@ -54,6 +63,8 @@ protected:
     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/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>