Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
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();
}