From 1c2e8fe9b74e0239fff9d048ea3860a85e392678 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sun, 22 Sep 2019 18:57:07 -0700 Subject: [PATCH] Better flight trajectory point algorithm --- qgroundcontrol.pro | 25 ++++----- src/FlightDisplay/FlightDisplayViewMap.qml | 30 +++++++---- src/QGCApplication.cc | 2 + src/Vehicle/TrajectoryPoints.cc | 60 ++++++++++++++++++++++ src/Vehicle/TrajectoryPoints.h | 49 ++++++++++++++++++ src/Vehicle/Vehicle.cc | 48 ++++------------- src/Vehicle/Vehicle.h | 25 +++------ src/uas/UASMessageHandler.h | 8 --- 8 files changed, 161 insertions(+), 86 deletions(-) create mode 100644 src/Vehicle/TrajectoryPoints.cc create mode 100644 src/Vehicle/TrajectoryPoints.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 84e17a884..6b0242d2e 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -416,7 +416,7 @@ INCLUDEPATH += \ src/QtLocationPlugin/QMLControl \ src/Settings \ src/Terrain \ - src/VehicleSetup \ + src/Vehicle \ src/ViewWidgets \ src/Audio \ src/comm \ @@ -663,7 +663,13 @@ HEADERS += \ src/SHPFileHelper.h \ src/Terrain/TerrainQuery.h \ src/TerrainTile.h \ + src/Vehicle/ADSBVehicle.h \ + src/Vehicle/GPSRTKFactGroup.h \ src/Vehicle/MAVLinkLogManager.h \ + src/Vehicle/MultiVehicleManager.h \ + src/Vehicle/TrajectoryPoints.h \ + src/Vehicle/Vehicle.h \ + src/Vehicle/VehicleObjectAvoidance.h \ src/VehicleSetup/JoystickConfigController.h \ src/comm/LinkConfiguration.h \ src/comm/LinkInterface.h \ @@ -881,7 +887,13 @@ SOURCES += \ src/SHPFileHelper.cc \ src/Terrain/TerrainQuery.cc \ src/TerrainTile.cc\ + src/Vehicle/ADSBVehicle.cc \ + src/Vehicle/GPSRTKFactGroup.cc \ src/Vehicle/MAVLinkLogManager.cc \ + src/Vehicle/MultiVehicleManager.cc \ + src/Vehicle/TrajectoryPoints.cc \ + src/Vehicle/Vehicle.cc \ + src/Vehicle/VehicleObjectAvoidance.cc \ src/VehicleSetup/JoystickConfigController.cc \ src/comm/LinkConfiguration.cc \ src/comm/LinkInterface.cc \ @@ -983,7 +995,6 @@ SOURCES += \ INCLUDEPATH += \ src/AutoPilotPlugins/Common \ src/FirmwarePlugin \ - src/Vehicle \ src/VehicleSetup \ HEADERS+= \ @@ -998,11 +1009,6 @@ 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 \ - src/Vehicle/VehicleObjectAvoidance.h \ src/VehicleSetup/VehicleComponent.h \ !MobileBuild { !NoSerialBuild { @@ -1025,11 +1031,6 @@ 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/Vehicle/VehicleObjectAvoidance.cc \ src/VehicleSetup/VehicleComponent.cc \ !MobileBuild { !NoSerialBuild { diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 15beb719a..5bf9539c5 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -49,6 +49,7 @@ FlightMap { property var _geoFenceController: missionController.geoFenceController property var _rallyPointController: missionController.rallyPointController + property var activeVehicle: QGroundControl.multiVehicleManager.activeVehicle property var _activeVehicleCoordinate: activeVehicle ? activeVehicle.coordinate : QtPositioning.coordinate() property real _toolButtonTopMargin: parent.height - mainWindow.height + (ScreenTools.defaultFontPixelHeight / 2) property bool _airspaceEnabled: QGroundControl.airmapSupported ? (QGroundControl.settingsManager.airMapSettings.enableAirMap.rawValue && QGroundControl.airspaceManager.connected): false @@ -184,17 +185,24 @@ FlightMap { property real leftToolWidth: toolStrip.x + toolStrip.width } - // Add trajectory points to the map - MapItemView { - model: mainIsMap ? activeVehicle ? activeVehicle.trajectoryPoints : 0 : 0 - delegate: MapPolyline { - line.width: 3 - line.color: "red" - z: QGroundControl.zOrderTrajectoryLines - path: [ - object.coordinate1, - object.coordinate2, - ] + // Add trajectory lines to the map + MapPolyline { + id: trajectoryPolyline + line.width: 3 + line.color: "red" + z: QGroundControl.zOrderTrajectoryLines + visible: true//mainIsMap + + Connections { + target: QGroundControl.multiVehicleManager + onActiveVehicleChanged: trajectoryPolyline.path = activeVehicle ? activeVehicle.trajectoryPoints.list() : [] + } + + Connections { + target: activeVehicle ? activeVehicle.trajectoryPoints : null + onPointAdded: trajectoryPolyline.addCoordinate(coordinate) + onUpdateLastPoint: trajectoryPolyline.replaceCoordinate(trajectoryPolyline.pathLength() - 1, coordinate) + onPointsCleared: trajectoryPolyline.path = [] } } diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 0d0d45e93..6425246ec 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -96,6 +96,7 @@ #include "GeoTagController.h" #include "LogReplayLink.h" #include "VehicleObjectAvoidance.h" +#include "TrajectoryPoints.h" #if defined(QGC_ENABLE_PAIRING) #include "PairingManager.h" @@ -508,6 +509,7 @@ void QGCApplication::_initCommon() qmlRegisterUncreatableType ("QGroundControl.FlightMap", 1, 0, "QGCMapPolygon", kRefOnly); qmlRegisterUncreatableType ("QGroundControl.FlightMap", 1, 0, "QGCGeoBoundingCube", kRefOnly); + qmlRegisterUncreatableType ("QGroundControl.FlightMap", 1, 0, "TrajectoryPoints", kRefOnly); qmlRegisterType ("QGroundControl.FlightMap", 1, 0, "QGCMapCircle"); diff --git a/src/Vehicle/TrajectoryPoints.cc b/src/Vehicle/TrajectoryPoints.cc new file mode 100644 index 000000000..49f6b1f5f --- /dev/null +++ b/src/Vehicle/TrajectoryPoints.cc @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * (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 "TrajectoryPoints.h" +#include "Vehicle.h" + +TrajectoryPoints::TrajectoryPoints(Vehicle* vehicle, QObject* parent) + : QObject (parent) + , _vehicle (vehicle) + , _lastAzimuth (qQNaN()) +{ +} + +void TrajectoryPoints::_vehicleCoordinateChanged(QGeoCoordinate coordinate) +{ + if (_lastPoint.isValid()) { + if (_lastPoint.distanceTo(coordinate) > _distanceTolerance) { + double newAzimuth = _lastPoint.azimuthTo(coordinate); + if (qIsNaN(_lastAzimuth) || qAbs(newAzimuth - _lastAzimuth) > _azimuthTolerance) { + _lastAzimuth = _lastPoint.azimuthTo(coordinate); + _lastPoint = coordinate; + _points.append(QVariant::fromValue(coordinate)); + emit pointAdded(coordinate); + } else { + _lastPoint = coordinate; + _points[_points.count() - 1] = QVariant::fromValue(coordinate); + emit updateLastPoint(coordinate); + } + } + } else { + _lastAzimuth = _lastPoint.azimuthTo(coordinate); + _lastPoint = coordinate; + _points.append(QVariant::fromValue(coordinate)); + emit pointAdded(coordinate); + } +} + +void TrajectoryPoints::start(void) +{ + clear(); + connect(_vehicle, &Vehicle::coordinateChanged, this, &TrajectoryPoints::_vehicleCoordinateChanged); +} + +void TrajectoryPoints::stop(void) +{ + qDebug() << "Stop" << _points.count(); + disconnect(_vehicle, &Vehicle::coordinateChanged, this, &TrajectoryPoints::_vehicleCoordinateChanged); +} + +void TrajectoryPoints::clear(void) +{ + _points.clear(); + emit pointsCleared(); +} diff --git a/src/Vehicle/TrajectoryPoints.h b/src/Vehicle/TrajectoryPoints.h new file mode 100644 index 000000000..85805f8c4 --- /dev/null +++ b/src/Vehicle/TrajectoryPoints.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * (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 "QmlObjectListModel.h" + +#include + +class Vehicle; + +class TrajectoryPoints : public QObject +{ + Q_OBJECT + +public: + TrajectoryPoints(Vehicle* vehicle, QObject* parent = nullptr); + + Q_INVOKABLE QVariantList list(void) const { return _points; } + + void start (void); + void stop (void); + +public slots: + void clear (void); + +signals: + void pointAdded (QGeoCoordinate coordinate); + void updateLastPoint(QGeoCoordinate coordinate); + void pointsCleared (void); + +private slots: + void _vehicleCoordinateChanged(QGeoCoordinate coordinate); + +private: + Vehicle* _vehicle; + QVariantList _points; + QGeoCoordinate _lastPoint; + double _lastAzimuth; + + static constexpr double _distanceTolerance = 1.0; + static constexpr double _azimuthTolerance = 1.5; +}; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 23bc05d47..1dc3153e1 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -42,6 +42,7 @@ #include "VideoSettings.h" #include "PositionManager.h" #include "VehicleObjectAvoidance.h" +#include "TrajectoryPoints.h" #if defined(QGC_AIRMAP_ENABLED) #include "AirspaceVehicleManager.h" @@ -167,6 +168,7 @@ Vehicle::Vehicle(LinkInterface* link, , _base_mode(0) , _custom_mode(0) , _nextSendMessageMultipleIndex(0) + , _trajectoryPoints(new TrajectoryPoints(this, this)) , _firmwarePluginManager(firmwarePluginManager) , _joystickManager(joystickManager) , _flowImageIndex(0) @@ -291,9 +293,6 @@ Vehicle::Vehicle(LinkInterface* link, _sendMultipleTimer.start(_sendMessageMultipleIntraMessageDelay); connect(&_sendMultipleTimer, &QTimer::timeout, this, &Vehicle::_sendMessageMultipleNext); - _mapTrajectoryTimer.setInterval(_mapTrajectoryMsecsBetweenPoints); - connect(&_mapTrajectoryTimer, &QTimer::timeout, this, &Vehicle::_addNewMapTrajectoryPoint); - connect(&_orbitTelemetryTimer, &QTimer::timeout, this, &Vehicle::_orbitTelemetryTimeout); // Create camera manager instance @@ -377,6 +376,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _base_mode(0) , _custom_mode(0) , _nextSendMessageMultipleIndex(0) + , _trajectoryPoints(new TrajectoryPoints(this, this)) , _firmwarePluginManager(firmwarePluginManager) , _joystickManager(nullptr) , _flowImageIndex(0) @@ -456,11 +456,12 @@ void Vehicle::_commonInit(void) connect(_missionManager, &MissionManager::error, this, &Vehicle::_missionManagerError); connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_missionLoadComplete); connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_clearCameraTriggerPoints); - connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_clearTrajectoryPoints); connect(_missionManager, &MissionManager::sendComplete, this, &Vehicle::_clearCameraTriggerPoints); - connect(_missionManager, &MissionManager::sendComplete, this, &Vehicle::_clearTrajectoryPoints); connect(_missionManager, &MissionManager::currentIndexChanged, this, &Vehicle::_updateHeadingToNextWP); + connect(_missionManager, &MissionManager::sendComplete, _trajectoryPoints, &TrajectoryPoints::clear); + connect(_missionManager, &MissionManager::newMissionItemsAvailable, _trajectoryPoints, &TrajectoryPoints::clear); + _parameterManager = new ParameterManager(this); connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady); @@ -1661,10 +1662,11 @@ void Vehicle::_updateArmed(bool armed) // We are transitioning to the armed state, begin tracking trajectory points for the map if (_armed) { - _mapTrajectoryStart(); + _trajectoryPoints->start(); + _flightTimerStart(); _clearCameraTriggerPoints(); } else { - _mapTrajectoryStop(); + _trajectoryPoints->stop(); // Also handle Video Streaming if(qgcApp()->toolbox()->videoManager()->videoReceiver()) { if(_settingsManager->videoSettings()->disableWhenDisarmed()->rawValue().toBool()) { @@ -2550,48 +2552,18 @@ void Vehicle::_rallyPointManagerError(int errorCode, const QString& errorMsg) qgcApp()->showMessage(tr("Rally Point transfer failed. Retry transfer. Error: %1").arg(errorMsg)); } -void Vehicle::_addNewMapTrajectoryPoint(void) -{ - if (_mapTrajectoryHaveFirstCoordinate) { - // Keep three minutes of trajectory on mobile due to perf impact of lines -#ifdef __mobile__ - if (_mapTrajectoryList.count() * _mapTrajectoryMsecsBetweenPoints > 3 * 1000 * 60) { - _mapTrajectoryList.removeAt(0)->deleteLater(); - } -#endif - _mapTrajectoryList.append(new CoordinateVector(_mapTrajectoryLastCoordinate, _coordinate, this)); - _flightDistanceFact.setRawValue(_flightDistanceFact.rawValue().toDouble() + _mapTrajectoryLastCoordinate.distanceTo(_coordinate)); - } - _mapTrajectoryHaveFirstCoordinate = true; - _mapTrajectoryLastCoordinate = _coordinate; - _flightTimeFact.setRawValue((double)_flightTimer.elapsed() / 1000.0); -} - -void Vehicle::_clearTrajectoryPoints(void) -{ - _mapTrajectoryList.clearAndDeleteContents(); -} - void Vehicle::_clearCameraTriggerPoints(void) { _cameraTriggerPoints.clearAndDeleteContents(); } -void Vehicle::_mapTrajectoryStart(void) +void Vehicle::_flightTimerStart(void) { - _mapTrajectoryHaveFirstCoordinate = false; - _clearTrajectoryPoints(); - _mapTrajectoryTimer.start(); _flightTimer.start(); _flightDistanceFact.setRawValue(0); _flightTimeFact.setRawValue(0); } -void Vehicle::_mapTrajectoryStop() -{ - _mapTrajectoryTimer.stop(); -} - void Vehicle::_startPlanRequest(void) { if (_missionManagerInitialRequestSent) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 2876044cc..a3d5c7a56 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -38,6 +38,7 @@ class ADSBVehicle; class QGCCameraManager; class Joystick; class VehicleObjectAvoidance; +class TrajectoryPoints; #if defined(QGC_AIRMAP_ENABLED) class AirspaceVehicleManager; @@ -548,7 +549,7 @@ public: Q_PROPERTY(QStringList flightModes READ flightModes NOTIFY flightModesChanged) Q_PROPERTY(QString flightMode READ flightMode WRITE setFlightMode NOTIFY flightModeChanged) Q_PROPERTY(bool hilMode READ hilMode WRITE setHilMode NOTIFY hilModeChanged) - Q_PROPERTY(QmlObjectListModel* trajectoryPoints READ trajectoryPoints CONSTANT) + Q_PROPERTY(TrajectoryPoints* trajectoryPoints MEMBER _trajectoryPoints CONSTANT) Q_PROPERTY(QmlObjectListModel* cameraTriggerPoints READ cameraTriggerPoints CONSTANT) Q_PROPERTY(float latitude READ latitude NOTIFY coordinateChanged) Q_PROPERTY(float longitude READ longitude NOTIFY coordinateChanged) @@ -877,9 +878,8 @@ public: QString prearmError(void) const { return _prearmError; } void setPrearmError(const QString& prearmError); - QmlObjectListModel* trajectoryPoints(void) { return &_mapTrajectoryList; } - QmlObjectListModel* cameraTriggerPoints(void) { return &_cameraTriggerPoints; } - QmlObjectListModel* adsbVehicles(void) { return &_adsbVehicles; } + QmlObjectListModel* cameraTriggerPoints (void) { return &_cameraTriggerPoints; } + QmlObjectListModel* adsbVehicles (void) { return &_adsbVehicles; } int flowImageIndex() { return _flowImageIndex; } @@ -1226,7 +1226,6 @@ private slots: void _linkInactiveOrDeleted(LinkInterface* link); void _sendMessageOnLink(LinkInterface* link, mavlink_message_t message); void _sendMessageMultipleNext(void); - void _addNewMapTrajectoryPoint(void); void _parametersReady(bool parametersReady); void _remoteControlRSSIChanged(uint8_t rssi); void _handleFlightModeChanged(const QString& flightMode); @@ -1246,7 +1245,6 @@ private slots: void _geoFenceLoadComplete(void); void _rallyPointLoadComplete(void); void _sendMavCommandAgain(void); - void _clearTrajectoryPoints(void); void _clearCameraTriggerPoints(void); void _updateDistanceHeadingToHome(void); void _updateHeadingToNextWP(void); @@ -1313,8 +1311,6 @@ private: void _missionManagerError(int errorCode, const QString& errorMsg); void _geoFenceManagerError(int errorCode, const QString& errorMsg); void _rallyPointManagerError(int errorCode, const QString& errorMsg); - void _mapTrajectoryStart(void); - void _mapTrajectoryStop(void); void _linkActiveChanged(LinkInterface* link, bool active, int vehicleID); void _say(const QString& text); QString _vehicleIdSpeech(void); @@ -1334,6 +1330,7 @@ private: void _handleUnsupportedRequestProtocolVersion(void); void _initializeCsv(); void _writeCsvLine(); + void _flightTimerStart(void); int _id; ///< Mavlink system id int _defaultComponentId; @@ -1463,15 +1460,9 @@ private: QTimer _sendMultipleTimer; int _nextSendMessageMultipleIndex; - QTime _flightTimer; - QTimer _mapTrajectoryTimer; - QmlObjectListModel _mapTrajectoryList; - QGeoCoordinate _mapTrajectoryLastCoordinate; - bool _mapTrajectoryHaveFirstCoordinate; - static const int _mapTrajectoryMsecsBetweenPoints = 1000; - - QmlObjectListModel _cameraTriggerPoints; - + QTime _flightTimer; + TrajectoryPoints* _trajectoryPoints; + QmlObjectListModel _cameraTriggerPoints; QmlObjectListModel _adsbVehicles; QMap _adsbICAOMap; QMap _trafficVehicleMap; diff --git a/src/uas/UASMessageHandler.h b/src/uas/UASMessageHandler.h index ae525741b..064e8240e 100644 --- a/src/uas/UASMessageHandler.h +++ b/src/uas/UASMessageHandler.h @@ -7,20 +7,12 @@ * ****************************************************************************/ - -/*! - * @file - * @brief Message Handler - * @author Gus Grubba - */ - #pragma once #include #include #include -#include "Vehicle.h" #include "QGCToolbox.h" class Vehicle; -- 2.22.0