Better flight trajectory point algorithm

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 {

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
+    // Add trajectory lines to the map
-    MapItemView {
+    MapPolyline {
-        model: mainIsMap ? activeVehicle ? activeVehicle.trajectoryPoints : 0 : 0
+        id:         trajectoryPolyline
-        delegate: MapPolyline {
+        line.width: 3
-            line.width: 3
+        line.color: "red"
-            line.color: "red"
+        z:          QGroundControl.zOrderTrajectoryLines
-            z:          QGroundControl.zOrderTrajectoryLines
+        visible:    true//mainIsMap
-            path: [
+
-                object.coordinate1,
+        Connections {
-                object.coordinate2,
+            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 = []
         }
     }
 

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<QGCMapPolygon>       ("QGroundControl.FlightMap",            1, 0, "QGCMapPolygon",              kRefOnly);
     qmlRegisterUncreatableType<QGCGeoBoundingCube>  ("QGroundControl.FlightMap",            1, 0, "QGCGeoBoundingCube",         kRefOnly);
+    qmlRegisterUncreatableType<TrajectoryPoints>    ("QGroundControl.FlightMap",            1, 0, "TrajectoryPoints",           kRefOnly);
 
     qmlRegisterType<QGCMapCircle>                   ("QGroundControl.FlightMap",            1, 0, "QGCMapCircle");
 

src/Vehicle/TrajectoryPoints.cc

@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   (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 "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();
+}

src/Vehicle/TrajectoryPoints.h

@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   (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 "QmlObjectListModel.h"
+
+#include <QGeoCoordinate>
+
+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;
+};

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) {

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* cameraTriggerPoints(void) { return &_cameraTriggerPoints; }
+    QmlObjectListModel* adsbVehicles        (void) { return &_adsbVehicles; }
-    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;
+    QTime                           _flightTimer;
-    QTimer              _mapTrajectoryTimer;
+    TrajectoryPoints*               _trajectoryPoints;
-    QmlObjectListModel  _mapTrajectoryList;
+    QmlObjectListModel              _cameraTriggerPoints;
-    QGeoCoordinate      _mapTrajectoryLastCoordinate;
-    bool                _mapTrajectoryHaveFirstCoordinate;
-    static const int    _mapTrajectoryMsecsBetweenPoints = 1000;
-
-    QmlObjectListModel  _cameraTriggerPoints;
-
     QmlObjectListModel              _adsbVehicles;
     QMap<uint32_t, ADSBVehicle*>    _adsbICAOMap;
     QMap<QString, ADSBVehicle*>     _trafficVehicleMap;

src/uas/UASMessageHandler.h

@@ -7,20 +7,12 @@
  *
  ****************************************************************************/
 
-
-/*!
- * @file
- *   @brief Message Handler
- *   @author Gus Grubba <mavlink@grubba.com>
- */
-
 #pragma once
 
 #include <QObject>
 #include <QVector>
 #include <QMutex>
 
-#include "Vehicle.h"
 #include "QGCToolbox.h"
 
 class Vehicle;