Add vehicle icon to flight display

qgroundcontrol.qrc

@@ -136,6 +136,7 @@
         <file alias="QGroundControl/FlightControls/QGCWaypointEditor.qml">src/ui/qmlcommon/QGCWaypointEditor.qml</file>
         <file alias="QGroundControl/FlightControls/qmldir">src/ui/qmlcommon/qmldir</file>
         <file alias="QGroundControl/FlightControls/QGCWaypoint.qml">src/ui/qmlcommon/QGCWaypoint.qml</file>
+        <file alias="QGroundControl/FlightControls/VehicleMapItem.qml">src/ui/qmlcommon/VehicleMapItem.qml</file>
     </qresource>
     <qresource prefix="/res">
         <file alias="LeftArrow">resources/LeftArrow.svg</file>

src/Vehicle/MultiVehicleManager.cc

@@ -229,3 +229,14 @@ QList<Vehicle*> MultiVehicleManager::vehicles(void)
     
     return list;
 }
+
+QVariantList MultiVehicleManager::vehiclesAsVariants(void)
+{
+    QVariantList list;
+    
+    foreach (Vehicle* vehicle, _vehicleMap) {
+        list += QVariant::fromValue(vehicle);
+    }
+    
+    return list;
+}

src/Vehicle/MultiVehicleManager.h

@@ -42,6 +42,7 @@ public:
     Q_PROPERTY(bool activeVehicleAvailable          READ activeVehicleAvailable                         NOTIFY activeVehicleAvailableChanged)
     Q_PROPERTY(bool parameterReadyVehicleAvailable  READ parameterReadyVehicleAvailable                 NOTIFY parameterReadyVehicleAvailableChanged)
     Q_PROPERTY(Vehicle* activeVehicle               READ activeVehicle          WRITE setActiveVehicle  NOTIFY activeVehicleChanged)
+    Q_PROPERTY(QVariantList vehicles                READ vehiclesAsVariants                                    CONSTANT)
     
     // Property accessors
     bool activeVehicleAvailable(void) { return _activeVehicleAvailable; }
@@ -64,6 +65,7 @@ public:
     UAS* activeUas(void) { return _activeVehicle ? _activeVehicle->uas() : NULL; }
     
     QList<Vehicle*> vehicles(void);
+    QVariantList vehiclesAsVariants(void);
     
     UASWaypointManager* activeWaypointManager(void);
 

src/Vehicle/Vehicle.cc

@@ -45,6 +45,14 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType)
     
     _uas = new UAS(MAVLinkProtocol::instance(), this);
     
+    setLatitude(_uas->getLatitude());
+    setLongitude(_uas->getLongitude());
+    _setYaw(_uas->getYaw());
+    
+    connect(_uas, &UAS::latitudeChanged, this, &Vehicle::setLatitude);
+    connect(_uas, &UAS::longitudeChanged, this, &Vehicle::setLongitude);
+    connect(_uas, &UAS::yawChanged, this, &Vehicle::_setYaw);
+    
     _firmwarePlugin = FirmwarePluginManager::instance()->firmwarePluginForAutopilot(firmwareType);
     _autopilotPlugin = AutoPilotPluginManager::instance()->newAutopilotPluginForVehicle(this);
 }
@@ -139,3 +147,20 @@ QList<LinkInterface*> Vehicle::links(void)
     
     return list;
 }
+
+void Vehicle::setLatitude(double latitude)
+{
+    _geoCoordinate.setLatitude(latitude);
+    emit coordinateChanged(_geoCoordinate);
+}
+
+void Vehicle::setLongitude(double longitude){
+    _geoCoordinate.setLongitude(longitude);
+    emit coordinateChanged(_geoCoordinate);
+}
+
+void Vehicle::_setYaw(double yaw)
+{
+    _heading = yaw * (180.0 / M_PI);
+    emit headingChanged(_heading);
+}
\ No newline at end of file

src/Vehicle/Vehicle.h

@@ -28,6 +28,7 @@
 #define Vehicle_H
 
 #include <QObject>
+#include <QGeoCoordinate>
 
 #include "LinkInterface.h"
 #include "QGCMAVLink.h"
@@ -48,6 +49,10 @@ public:
     Q_PROPERTY(int id READ id CONSTANT)
     Q_PROPERTY(AutoPilotPlugin* autopilot MEMBER _autopilotPlugin CONSTANT)
     
+    Q_PROPERTY(QGeoCoordinate coordinate MEMBER _geoCoordinate NOTIFY coordinateChanged)
+    
+    Q_PROPERTY(double heading MEMBER _heading NOTIFY headingChanged)
+    
     // Property accesors
     int id(void) { return _id; }
     MAV_AUTOPILOT firmwareType(void) { return _firmwareType; }
@@ -63,8 +68,14 @@ public:
     
     QList<LinkInterface*> links(void);
     
+public slots:
+    void setLatitude(double latitude);
+    void setLongitude(double longitude);
+    
 signals:
     void allLinksDisconnected(void);
+    void coordinateChanged(QGeoCoordinate coordinate);
+    void headingChanged(double heading);
     
     /// Used internally to move sendMessage call to main thread
     void _sendMessageOnThread(mavlink_message_t message);
@@ -73,6 +84,7 @@ private slots:
     void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
     void _linkDisconnected(LinkInterface* link);
     void _sendMessage(mavlink_message_t message);
+    void _setYaw(double yaw);
     
 private:
     bool _containsLink(LinkInterface* link);
@@ -90,6 +102,10 @@ private:
     QList<SharedLinkInterface> _links;
     
     UAS* _uas;
+    
+    QGeoCoordinate  _geoCoordinate;
+    
+    double _heading;
 };
 
 #endif

src/ui/qmlcommon/QGCMapBackground.qml

@@ -36,6 +36,8 @@ import QGroundControl.Controls 1.0
 import QGroundControl.FlightControls        1.0
 import QGroundControl.ScreenTools           1.0
 import QGroundControl.MavManager            1.0
+import QGroundControl.MultiVehicleManager   1.0
+import QGroundControl.Vehicle               1.0
 
 Item {
     id:     root
@@ -47,6 +49,7 @@ Item {
     property real   heading:            0
     property bool   alwaysNorth:        true
     property bool   interactive:        true
+    property bool   showVehicles:       true
     property bool   showWaypoints:      false
     property string mapName:            'defaultMap'
     property alias  mapItem:            map
@@ -58,6 +61,9 @@ Item {
         map.zoomLevel   = 18
         map.markers     = []
         mapTypeMenu.update();
+        if (showVehicles) {
+            addExistingVehicles()
+        }
     }
 
     //-- Menu to select supported map types
@@ -153,6 +159,44 @@ Item {
         }
     }
 
+    property var vehicles: []           ///< List of known vehicles
+    property var vehicleMapItems: []    ///< List of know vehicle map items
+
+    function addVehicle(vehicle) {
+            var qmlItemTemplate = "VehicleMapItem { " +
+                                        "coordinate:    vehicles[%1].coordinate; " +
+                                        "heading:       vehicles[%1].heading " +
+                                    "}"
+
+            var i = vehicles.length
+            qmlItemTemplate = qmlItemTemplate.replace("%1", i)
+            qmlItemTemplate = qmlItemTemplate.replace("%1", i)
+
+            vehicles.push(vehicle)
+            var mapItem = Qt.createQmlObject (qmlItemTemplate, map)
+            vehicleMapItems.push(mapItem)
+
+            mapItem.z = map.z + 1
+            map.addMapItem(mapItem)
+    }
+
+    function removeVehicle(vehicle) {
+            for (var i=0; i<vehicles.length; i++) {
+                if (vehicles[i] == vehicle) {
+                    vehicle[i] = undefined
+                    map.removeMapItem(vehicleMapItems[i])
+                    vehicleMapItems[i] = undefined
+                    break
+                }
+            }
+    }
+
+    function addExistingVehicles() {
+        for (var i=0; i<multiVehicleManager.vehicles.length; i++) {
+            addVehicle(multiVehicleManager.vehicles[i])
+        }
+    }
+
     Plugin {
         id:   mapPlugin
         name: "QGroundControl"
@@ -165,6 +209,13 @@ Item {
         }
     }
 
+    Connections {
+        target: multiVehicleManager
+
+        onVehicleAdded: addVehicle(vehicle)
+        onVehicleRemoved: removeVehicle(vehicle)
+    }
+
     onShowWaypointsChanged: {
         root.updateWaypoints();
     }

src/ui/qmlcommon/VehicleMapItem.qml

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL 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.
+
+    QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/// @file
+///     @author Don Gagne <don@thegagnes.com>
+
+import QtQuick      2.4
+import QtLocation   5.3
+
+import QGroundControl.ScreenTools 1.0
+
+/// Marker for displaying a vehicle location on the map
+MapQuickItem {
+    property real heading: 0
+
+    anchorPoint.x: vehicleIcon.width  / 2
+    anchorPoint.y: vehicleIcon.height / 2
+
+    sourceItem: Image {
+        id:         vehicleIcon
+        source:     "/qmlimages/compassInstrumentAirplane.svg"
+        mipmap:     true
+        width:      ScreenTools.defaultFontPixelHeight * 4
+        fillMode:   Image.PreserveAspectFit
+
+        transform: Rotation {
+            origin.x:   vehicleIcon.width  / 2
+            origin.y:   vehicleIcon.height / 2
+            angle:      heading
+        }
+    }
+}

src/ui/qmlcommon/qmldir

@@ -17,3 +17,4 @@ QGCSpeedWidget          1.0 QGCSpeedWidget.qml
 QGCVideoBackground      1.0 QGCVideoBackground.qml
 QGCWaypoint             1.0 QGCWaypoint.qml
 QGCWaypointEditor       1.0 QGCWaypointEditor.qml
+VehicleMapItem          1.0 VehicleMapItem.qml