From e910070a21da12f664f87785b00b8fcd77508c46 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 18 Jul 2017 20:15:56 -0700 Subject: [PATCH] Add support for ADSB vehicle display --- qgcresources.qrc | 1 + qgroundcontrol.pro | 2 + src/FlightDisplay/FlightDisplayViewMap.qml | 17 +++++- src/FlightMap/Images/adsbVehicle.svg | 66 ++++++++++++++++++++ src/FlightMap/MapItems/VehicleMapItem.qml | 70 +++++++++++++++------- src/PlanView/PlanView.qml | 2 +- src/Vehicle/ADSBVehicle.cc | 63 +++++++++++++++++++ src/Vehicle/ADSBVehicle.h | 47 +++++++++++++++ src/Vehicle/Vehicle.cc | 21 ++++++- src/Vehicle/Vehicle.h | 7 +++ src/comm/MockLink.cc | 24 ++++++++ src/comm/MockLink.h | 1 + 12 files changed, 295 insertions(+), 26 deletions(-) create mode 100644 src/FlightMap/Images/adsbVehicle.svg create mode 100644 src/Vehicle/ADSBVehicle.cc create mode 100644 src/Vehicle/ADSBVehicle.h diff --git a/qgcresources.qrc b/qgcresources.qrc index 4abea6ec7..b27361e6c 100644 --- a/qgcresources.qrc +++ b/qgcresources.qrc @@ -113,6 +113,7 @@ src/FlightMap/Images/scale_end.png src/FlightMap/Images/scaleLight.png src/FlightMap/Images/scale_endLight.png + src/FlightMap/Images/adsbVehicle.svg src/FlightMap/Images/vehicleArrowOutline.svg src/FlightMap/Images/vehicleArrowOpaque.svg src/FlightMap/Images/ZoomPlus.svg diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 91c979e92..dcd8c080d 100644 --- a/qgroundcontrol.pro +++ b/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 \ diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index c8c844f66..838c19d4c 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/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 diff --git a/src/FlightMap/Images/adsbVehicle.svg b/src/FlightMap/Images/adsbVehicle.svg new file mode 100644 index 000000000..b8c67e0f6 --- /dev/null +++ b/src/FlightMap/Images/adsbVehicle.svg @@ -0,0 +1,66 @@ + + + +image/svg+xml \ No newline at end of file diff --git a/src/FlightMap/MapItems/VehicleMapItem.qml b/src/FlightMap/MapItems/VehicleMapItem.qml index c0e729b98..4cdbc4343 100644 --- a/src/FlightMap/MapItems/VehicleMapItem.qml +++ b/src/FlightMap/MapItems/VehicleMapItem.qml @@ -7,38 +7,62 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - 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 : "" + } } } diff --git a/src/PlanView/PlanView.qml b/src/PlanView/PlanView.qml index cbc8136fa..673d930cf 100644 --- a/src/PlanView/PlanView.qml +++ b/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 } diff --git a/src/Vehicle/ADSBVehicle.cc b/src/Vehicle/ADSBVehicle.cc new file mode 100644 index 000000000..e51d10010 --- /dev/null +++ b/src/Vehicle/ADSBVehicle.cc @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "ADSBVehicle.h" + +#include +#include + +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); + } +} diff --git a/src/Vehicle/ADSBVehicle.h b/src/Vehicle/ADSBVehicle.h new file mode 100644 index 000000000..a1ee26c48 --- /dev/null +++ b/src/Vehicle/ADSBVehicle.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#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; +}; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 9ced59729..f5b6f6c59 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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); + } + } +} + //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index ddc6e7ef0..c099b88ed 100644 --- a/src/Vehicle/Vehicle.h +++ b/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 _adsbICAOMap; + // Toolbox references FirmwarePluginManager* _firmwarePluginManager; JoystickManager* _joystickManager; diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 0d2db89aa..c9628beee 100644 --- a/src/comm/MockLink.cc +++ b/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); +} diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 96ff3698e..28ec94e41 100644 --- a/src/comm/MockLink.h +++ b/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); -- 2.22.0