Merge pull request #5472 from DonLakeFlyer/ADSBVehicle

Add support for ADSB vehicle display

qgcresources.qrc

@@ -113,6 +113,7 @@
         <file alias="scale_end.png">src/FlightMap/Images/scale_end.png</file>
         <file alias="scaleLight.png">src/FlightMap/Images/scaleLight.png</file>
         <file alias="scale_endLight.png">src/FlightMap/Images/scale_endLight.png</file>
+        <file alias="adsbVehicle.svg">src/FlightMap/Images/adsbVehicle.svg</file>
         <file alias="vehicleArrowOutline.svg">src/FlightMap/Images/vehicleArrowOutline.svg</file>
         <file alias="vehicleArrowOpaque.svg">src/FlightMap/Images/vehicleArrowOpaque.svg</file>
         <file alias="ZoomPlus.svg">src/FlightMap/Images/ZoomPlus.svg</file>

qgroundcontrol.pro

@@ -836,6 +836,7 @@ HEADERS+= \
     src/FirmwarePlugin/CameraMetaData.h \
     src/FirmwarePlugin/FirmwarePlugin.h \
     src/FirmwarePlugin/FirmwarePluginManager.h \
+    src/Vehicle/ADSBVehicle.h \
     src/Vehicle/MultiVehicleManager.h \
     src/Vehicle/GPSRTKFactGroup.h \
     src/Vehicle/Vehicle.h \
@@ -861,6 +862,7 @@ SOURCES += \
     src/FirmwarePlugin/CameraMetaData.cc \
     src/FirmwarePlugin/FirmwarePlugin.cc \
     src/FirmwarePlugin/FirmwarePluginManager.cc \
+    src/Vehicle/ADSBVehicle.cc \
     src/Vehicle/MultiVehicleManager.cc \
     src/Vehicle/GPSRTKFactGroup.cc \
     src/Vehicle/Vehicle.cc \

src/FlightDisplay/FlightDisplayViewMap.qml

@@ -184,12 +184,27 @@ FlightMap {
         delegate: VehicleMapItem {
             vehicle:        object
             coordinate:     object.coordinate
-            isSatellite:    flightMap.isSatelliteMap
+            map:            flightMap
             size:           _mainIsMap ? ScreenTools.defaultFontPixelHeight * 3 : ScreenTools.defaultFontPixelHeight
             z:              QGroundControl.zOrderVehicles
         }
     }
 
+    // Add ADSB vehicles to the map
+    MapItemView {
+        model: _activeVehicle ? _activeVehicle.adsbVehicles : 0
+
+        property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
+
+        delegate: VehicleMapItem {
+            coordinate:     object.coordinate
+            altitude:       object.altitude
+            heading:        object.heading
+            map:            flightMap
+            z:              QGroundControl.zOrderVehicles
+        }
+    }
+
     // Add the mission item visuals to the map
     Repeater {
         model: _mainIsMap ? _missionController.visualItems : 0

src/FlightMap/Images/adsbVehicle.svg

+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Generator: Adobe Illustrator 21.0.0, SVG Export Plug-In . SVG Version: 6.00 Build 0)  -->
+
+<svg
+   xmlns:dc="http://purl.org/dc/elements/1.1/"
+   xmlns:cc="http://creativecommons.org/ns#"
+   xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+   xmlns:svg="http://www.w3.org/2000/svg"
+   xmlns="http://www.w3.org/2000/svg"
+   xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+   xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+   version="1.1"
+   id="Layer_1"
+   x="0px"
+   y="0px"
+   viewBox="0 0 72 72"
+   style="enable-background:new 0 0 72 72;"
+   xml:space="preserve"
+   inkscape:version="0.91 r13725"
+   sodipodi:docname="adsbVehicle.svg"><metadata
+     id="metadata23"><rdf:RDF><cc:Work
+         rdf:about=""><dc:format>image/svg+xml</dc:format><dc:type
+           rdf:resource="http://purl.org/dc/dcmitype/StillImage" /></cc:Work></rdf:RDF></metadata><defs
+     id="defs21" /><sodipodi:namedview
+     pagecolor="#ffffff"
+     bordercolor="#666666"
+     borderopacity="1"
+     objecttolerance="10"
+     gridtolerance="10"
+     guidetolerance="10"
+     inkscape:pageopacity="0"
+     inkscape:pageshadow="2"
+     inkscape:window-width="664"
+     inkscape:window-height="480"
+     id="namedview19"
+     showgrid="false"
+     inkscape:zoom="3.2777778"
+     inkscape:cx="36"
+     inkscape:cy="36"
+     inkscape:window-x="0"
+     inkscape:window-y="0"
+     inkscape:window-maximized="0"
+     inkscape:current-layer="g7" /><style
+     type="text/css"
+     id="style3">
+	.st0{fill:#C72B27;}
+	.st1{fill:#7F0036;}
+	.st2{fill:#EE3424;}
+</style><g
+     id="g5"><g
+       id="g7"><polygon
+         class="st0"
+         points="35.5,2.118 35.5,53.691 1.118,70.882   "
+         id="polygon9"
+         style="fill:#24d3ee;fill-opacity:1" /><path
+         class="st1"
+         d="M35,4.236v49.146L2.236,69.764L35,4.236 M36,0L0,72l36-18V0L36,0z"
+         id="path11" /></g><g
+       id="g13"><polygon
+         class="st2"
+         points="36.5,53.691 36.5,2.118 70.882,70.882   "
+         id="polygon15"
+         style="fill:#24d3ee;fill-opacity:1" /><path
+         class="st1"
+         d="M37,4.236l32.764,65.528L37,53.382V4.236 M36,0v54l36,18L36,0L36,0z"
+         id="path17" /></g></g></svg>
\ No newline at end of file

src/FlightMap/MapItems/VehicleMapItem.qml

@@ -7,38 +7,62 @@
  *
  ****************************************************************************/
 
-
-/// @file
-///     @author Don Gagne <don@thegagnes.com>
-
 import QtQuick          2.3
 import QtLocation       5.3
 import QtPositioning    5.3
 
+import QGroundControl               1.0
 import QGroundControl.ScreenTools   1.0
 import QGroundControl.Vehicle       1.0
+import QGroundControl.Controls      1.0
 
 /// Marker for displaying a vehicle location on the map
 MapQuickItem {
-    property var    vehicle                ///< Vehicle object
-    property bool   isSatellite:    false  ///< true: satellite map is showing
-    property real   size:           ScreenTools.defaultFontPixelHeight * 5
-
-    anchorPoint.x:  vehicleIcon.width  / 2
-    anchorPoint.y:  vehicleIcon.height / 2
-    visible:        vehicle && vehicle.coordinate.isValid
-
-    sourceItem: Image {
-        id:                 vehicleIcon
-        source:             isSatellite ? vehicle.vehicleImageOpaque : vehicle.vehicleImageOutline
-        mipmap:             true
-        width:              size
-        sourceSize.width:   size
-        fillMode:           Image.PreserveAspectFit
-        transform: Rotation {
-            origin.x:       vehicleIcon.width  / 2
-            origin.y:       vehicleIcon.height / 2
-            angle:          vehicle ? vehicle.heading.value : 0
+    property var    vehicle                                                         /// Vehicle object, undefined for ADSB vehicle
+    property var    map
+    property double altitude:       Number.NaN                                      ///< NAN to not show
+    property double heading:        vehicle ? vehicle.heading.value : Number.NaN    ///< Vehicle heading, NAN for none
+    property real   size:           _adsbVehicle ? _adsbSize : _uavSize             /// Size for icon
+
+    anchorPoint.x:  vehicleItem.width  / 2
+    anchorPoint.y:  vehicleItem.height / 2
+    visible:        coordinate.isValid
+
+    property bool   _adsbVehicle:   vehicle ? false : true
+    property real   _uavSize:       ScreenTools.defaultFontPixelHeight * 5
+    property real   _adsbSize:      ScreenTools.defaultFontPixelHeight * 1.5
+    property var    _map:           map
+
+    sourceItem: Item {
+        id:     vehicleItem
+        width:  vehicleIcon.width
+        height: vehicleIcon.height
+
+        Image {
+            id:                 vehicleIcon
+            source:             _adsbVehicle ? "/qmlimages/adsbVehicle.svg" : (map.isSatelliteMap ? vehicle.vehicleImageOpaque : vehicle.vehicleImageOutline)
+            mipmap:             true
+            width:              size
+            sourceSize.width:   size
+            fillMode:           Image.PreserveAspectFit
+
+            transform: Rotation {
+                origin.x:       vehicleIcon.width  / 2
+                origin.y:       vehicleIcon.height / 2
+                angle:          isNaN(heading) ? 0 : heading
+            }
+        }
+
+        QGCMapLabel {
+            anchors.top:                parent.bottom
+            anchors.horizontalCenter:   parent.horizontalCenter
+            map:                        _map
+            text:                       altText
+            font.pointSize:             ScreenTools.smallFontPointSize
+            visible:                    !isNaN(altitude)
+
+            property string altText: visible ? QGroundControl.metersToAppSettingsDistanceUnits(altitude).toFixed(0) + " " + QGroundControl.appSettingsDistanceUnitsString : ""
+
         }
     }
 }

src/PlanView/PlanView.qml

@@ -366,7 +366,7 @@ QGCView {
                     VehicleMapItem {
                     vehicle:        object
                     coordinate:     object.coordinate
-                    isSatellite:    editorMap.isSatelliteMap
+                    map:            editorMap
                     size:           ScreenTools.defaultFontPixelHeight * 3
                     z:              QGroundControl.zOrderMapItems - 1
                 }

src/Vehicle/ADSBVehicle.cc

+/****************************************************************************
+ *
+ *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "ADSBVehicle.h"
+
+#include <QDebug>
+#include <QtMath>
+
+ADSBVehicle::ADSBVehicle(mavlink_adsb_vehicle_t& adsbVehicle, QObject* parent)
+    : QObject       (parent)
+    , _icaoAddress  (adsbVehicle.ICAO_address)
+    , _altitude     (NAN)
+    , _heading      (NAN)
+{
+    if (!(adsbVehicle.flags | ADSB_FLAGS_VALID_COORDS)) {
+        qWarning() << "At least coords must be valid";
+        return;
+    }
+
+    update(adsbVehicle);
+}
+
+void ADSBVehicle::update(mavlink_adsb_vehicle_t& adsbVehicle)
+{
+    if (_icaoAddress != adsbVehicle.ICAO_address) {
+        qWarning() << "ICAO address mismatch expected:actual" << _icaoAddress << adsbVehicle.ICAO_address;
+        return;
+    }
+
+    if (!(adsbVehicle.flags | ADSB_FLAGS_VALID_COORDS)) {
+        return;
+    }
+
+    QGeoCoordinate newCoordinate(adsbVehicle.lat / qPow(10.0, 7.0), adsbVehicle.lon / qPow(10.0, 7.0));
+    if (newCoordinate != _coordinate) {
+        _coordinate = newCoordinate;
+        emit coordinateChanged(_coordinate);
+    }
+
+    double newAltitude = NAN;
+    if (adsbVehicle.flags | ADSB_FLAGS_VALID_ALTITUDE) {
+        newAltitude = (double)adsbVehicle.altitude / 1000.0;
+    }
+    if (!(qIsNaN(newAltitude) && qIsNaN(_altitude)) && !qFuzzyCompare(newAltitude, _altitude)) {
+        _altitude = newAltitude;
+        emit altitudeChanged(_altitude);
+    }
+
+    double newHeading = NAN;
+    if (adsbVehicle.flags | ADSB_FLAGS_VALID_HEADING) {
+        newHeading = (double)adsbVehicle.heading / 100.0;
+    }
+    if (!(qIsNaN(newHeading) && qIsNaN(_heading)) && !qFuzzyCompare(newHeading, _heading)) {
+        _heading = newHeading;
+        emit headingChanged(_heading);
+    }
+}

src/Vehicle/ADSBVehicle.h

+/****************************************************************************
+ *
+ *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <QObject>
+#include <QGeoCoordinate>
+
+#include "QGCMAVLink.h"
+
+class ADSBVehicle : public QObject
+{
+    Q_OBJECT
+
+public:
+    ADSBVehicle(mavlink_adsb_vehicle_t& adsbVehicle, QObject* parent = NULL);
+
+    Q_PROPERTY(int              icaoAddress READ icaoAddress    CONSTANT)
+    Q_PROPERTY(QGeoCoordinate   coordinate  READ coordinate     NOTIFY coordinateChanged)
+    Q_PROPERTY(double           altitude    READ altitude       NOTIFY altitudeChanged)     // NaN for not available
+    Q_PROPERTY(double           heading     READ heading        NOTIFY headingChanged)      // NaN for not available
+
+    int             icaoAddress (void) const { return _icaoAddress; }
+    QGeoCoordinate  coordinate  (void) const { return _coordinate; }
+    double          altitude    (void) const { return _altitude; }
+    double          heading     (void) const { return _heading; }
+
+    /// Update the vehicle with new information
+    void update(mavlink_adsb_vehicle_t& adsbVehicle);
+
+signals:
+    void coordinateChanged(QGeoCoordinate coordinate);
+    void altitudeChanged(double altitude);
+    void headingChanged(double heading);
+
+private:
+    uint32_t        _icaoAddress;
+    QGeoCoordinate  _coordinate;
+    double          _altitude;
+    double          _heading;
+};

src/Vehicle/Vehicle.cc

@@ -31,6 +31,7 @@
 #include "SettingsManager.h"
 #include "QGCQGeoCoordinate.h"
 #include "QGCCorePlugin.h"
+#include "ADSBVehicle.h"
 
 QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")
 
@@ -600,10 +601,12 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_CAMERA_FEEDBACK:
         _handleCameraFeedback(message);
         break;
-
     case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
         _handleCameraImageCaptured(message);
         break;
+    case MAVLINK_MSG_ID_ADSB_VEHICLE:
+        _handleADSBVehicle(message);
+        break;
 
     case MAVLINK_MSG_ID_SERIAL_CONTROL:
     {
@@ -2622,6 +2625,22 @@ bool Vehicle::autoDisarm(void)
     return false;
 }
 
+void Vehicle::_handleADSBVehicle(const mavlink_message_t& message)
+{
+    mavlink_adsb_vehicle_t adsbVehicle;
+
+    mavlink_msg_adsb_vehicle_decode(&message, &adsbVehicle);
+    if (adsbVehicle.flags | ADSB_FLAGS_VALID_COORDS) {
+        if (_adsbICAOMap.contains(adsbVehicle.ICAO_address)) {
+            _adsbICAOMap[adsbVehicle.ICAO_address]->update(adsbVehicle);
+        } else {
+            ADSBVehicle* vehicle = new ADSBVehicle(adsbVehicle, this);
+            _adsbICAOMap[adsbVehicle.ICAO_address] = vehicle;
+            _adsbVehicles.append(vehicle);
+        }
+    }
+}
+
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 

src/Vehicle/Vehicle.h

@@ -38,6 +38,7 @@ class ParameterManager;
 class JoystickManager;
 class UASMessage;
 class SettingsManager;
+class ADSBVehicle;
 
 Q_DECLARE_LOGGING_CATEGORY(VehicleLog)
 
@@ -311,6 +312,7 @@ public:
     Q_PROPERTY(int                  telemetryRNoise         READ telemetryRNoise                                        NOTIFY telemetryRNoiseChanged)
     Q_PROPERTY(QVariantList         toolBarIndicators       READ toolBarIndicators                                      CONSTANT)
     Q_PROPERTY(QVariantList         cameraList              READ cameraList                                             CONSTANT)
+    Q_PROPERTY(QmlObjectListModel*  adsbVehicles            READ adsbVehicles                                           CONSTANT)
 
     /// true: Vehicle is flying, false: Vehicle is on ground
     Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged)
@@ -527,6 +529,7 @@ public:
 
     QmlObjectListModel* trajectoryPoints(void) { return &_mapTrajectoryList; }
     QmlObjectListModel* cameraTriggerPoints(void) { return &_cameraTriggerPoints; }
+    QmlObjectListModel* adsbVehicles(void) { return &_adsbVehicles; }
 
     int  flowImageIndex() { return _flowImageIndex; }
 
@@ -836,6 +839,7 @@ private:
     void _handleScaledPressure3(mavlink_message_t& message);
     void _handleCameraFeedback(const mavlink_message_t& message);
     void _handleCameraImageCaptured(const mavlink_message_t& message);
+    void _handleADSBVehicle(const mavlink_message_t& message);
     void _missionManagerError(int errorCode, const QString& errorMsg);
     void _geoFenceManagerError(int errorCode, const QString& errorMsg);
     void _rallyPointManagerError(int errorCode, const QString& errorMsg);
@@ -975,6 +979,9 @@ private:
 
     QmlObjectListModel  _cameraTriggerPoints;
 
+    QmlObjectListModel              _adsbVehicles;
+    QMap<uint32_t, ADSBVehicle*>    _adsbICAOMap;
+
     // Toolbox references
     FirmwarePluginManager*      _firmwarePluginManager;
     JoystickManager*            _joystickManager;

src/comm/MockLink.cc

@@ -161,6 +161,7 @@ void MockLink::_run1HzTasks(void)
 {
     if (_mavlinkStarted && _connected) {
         _sendVibration();
+        _sendADSBVehicles();
         if (!qgcApp()->runningUnitTests()) {
             // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
             _sendRCChannels();
@@ -1268,3 +1269,26 @@ void MockLink::_logDownloadWorker(void)
         }
     }
 }
+
+void MockLink::_sendADSBVehicles(void)
+{
+    mavlink_message_t responseMsg;
+    mavlink_msg_adsb_vehicle_pack_chan(_vehicleSystemId,
+                                       _vehicleComponentId,
+                                       _mavlinkChannel,
+                                       &responseMsg,
+                                       12345,                           // ICAO address
+                                       (_vehicleLatitude + 0.001) * qPow(10.0, 7.0),
+                                       (_vehicleLongitude + 0.001) * qPow(10.0, 7.0),
+                                       ADSB_ALTITUDE_TYPE_GEOMETRIC,
+                                       100 * 1000,                      // Altitude in millimeters
+                                       10 * 100,                        // Heading in centidegress
+                                       0, 0,                            // Horizontal/Vertical velocity
+                                       "N1234500",                      // Callsign
+                                       ADSB_EMITTER_TYPE_ROTOCRAFT,
+                                       1,                               // Seconds since last communication
+                                       ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_HEADING | ADSB_FLAGS_VALID_CALLSIGN | ADSB_FLAGS_SIMULATED,
+                                       0);                              // Squawk code
+
+    respondWithMavlinkMessage(responseMsg);
+}

src/comm/MockLink.h

@@ -192,6 +192,7 @@ private:
     void _sendRCChannels(void);
     void _paramRequestListWorker(void);
     void _logDownloadWorker(void);
+    void _sendADSBVehicles(void);
 
     static MockLink* _startMockLink(MockConfiguration* mockConfig);