Add support for ADSB vehicle display

8 years ago · e910070a21
12 changed files with 295 additions and 26 deletions
--- a/qgcresources.qrc
+++ b/qgcresources.qrc
@ -113,6 +113,7 @@
				@@ -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>
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@ -836,6 +836,7 @@ HEADERS+= \
				@@ -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 += \
				@@ -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 \
--- a/src/FlightDisplay/FlightDisplayViewMap.qml
+++ b/src/FlightDisplay/FlightDisplayViewMap.qml
@ -184,12 +184,27 @@ FlightMap {
				@@ -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
--- a/src/FlightMap/Images/adsbVehicle.svg
+++ b/src/FlightMap/Images/adsbVehicle.svg
@ -0,0 +1,66 @@
				@@ -0,0 +1,66 @@
+<?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>
--- a/src/FlightMap/MapItems/VehicleMapItem.qml
+++ b/src/FlightMap/MapItems/VehicleMapItem.qml
@ -7,38 +7,62 @@
				@@ -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 : ""
+
        }
    }
 }
--- a/src/PlanView/PlanView.qml
+++ b/src/PlanView/PlanView.qml
@ -366,7 +366,7 @@ QGCView {
				@@ -366,7 +366,7 @@ QGCView {
                    VehicleMapItem {
                    vehicle:        object
                    coordinate:     object.coordinate
-                    isSatellite:    editorMap.isSatelliteMap
+                    map:            editorMap
                    size:           ScreenTools.defaultFontPixelHeight * 3
                    z:              QGroundControl.zOrderMapItems - 1
                }
--- a/src/Vehicle/ADSBVehicle.cc
+++ b/src/Vehicle/ADSBVehicle.cc
@ -0,0 +1,63 @@
				@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   (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);
+    }
+}
--- a/src/Vehicle/ADSBVehicle.h
+++ b/src/Vehicle/ADSBVehicle.h
@ -0,0 +1,47 @@
				@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   (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;
+};
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@ -31,6 +31,7 @@
				@@ -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
				@@ -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)
				@@ -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);
+        }
+    }
+}
+
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------

--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@ -38,6 +38,7 @@ class ParameterManager;
				@@ -38,6 +38,7 @@ class ParameterManager;
 class JoystickManager;
 class UASMessage;
 class SettingsManager;
+class ADSBVehicle;

 Q_DECLARE_LOGGING_CATEGORY(VehicleLog)

@ -311,6 +312,7 @@ public:
				@@ -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:
				@@ -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:
				@@ -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:
				@@ -975,6 +979,9 @@ private:

    QmlObjectListModel  _cameraTriggerPoints;

+    QmlObjectListModel              _adsbVehicles;
+    QMap<uint32_t, ADSBVehicle*>    _adsbICAOMap;
+
    // Toolbox references
    FirmwarePluginManager*      _firmwarePluginManager;
    JoystickManager*            _joystickManager;
--- a/src/comm/MockLink.cc
+++ b/src/comm/MockLink.cc
@ -161,6 +161,7 @@ void MockLink::_run1HzTasks(void)
				@@ -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)
				@@ -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);
+}
--- a/src/comm/MockLink.h
+++ b/src/comm/MockLink.h
@ -192,6 +192,7 @@ private:
				@@ -192,6 +192,7 @@ private:
    void _sendRCChannels(void);
    void _paramRequestListWorker(void);
    void _logDownloadWorker(void);
+    void _sendADSBVehicles(void);

    static MockLink* _startMockLink(MockConfiguration* mockConfig);