SimulatedPosition.cc 3.24 KB
Newer Older
1 2
/****************************************************************************
 *
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
Jimmy Johnson's avatar
Jimmy Johnson committed
9 10 11 12 13 14

#include <QtCore>
#include <QDateTime>
#include <QDate>

#include "SimulatedPosition.h"
15 16
#include "QGCApplication.h"
#include "MultiVehicleManager.h"
Jimmy Johnson's avatar
Jimmy Johnson committed
17 18

SimulatedPosition::SimulatedPosition()
19
    : QGeoPositionInfoSource(nullptr)
Jimmy Johnson's avatar
Jimmy Johnson committed
20
{
21 22 23 24 25 26 27 28
    _updateTimer.setSingleShot(false);

    // Initialize position to normal PX4 Gazebo home position
    _lastPosition.setTimestamp(QDateTime::currentDateTime());
    _lastPosition.setCoordinate(QGeoCoordinate(47.3977420, 8.5455941, 488));
    _lastPosition.setAttribute(QGeoPositionInfo::Attribute::Direction, _heading);
    _lastPosition.setAttribute(QGeoPositionInfo::Attribute::GroundSpeed, _horizontalVelocityMetersPerSec);
    _lastPosition.setAttribute(QGeoPositionInfo::Attribute::VerticalSpeed, _verticalVelocityMetersPerSec);
Jimmy Johnson's avatar
Jimmy Johnson committed
29

30 31
    // When a vehicle shows up we switch location to the vehicle home position
    connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &SimulatedPosition::_vehicleAdded);
Jimmy Johnson's avatar
Jimmy Johnson committed
32

33
    connect(&_updateTimer, &QTimer::timeout, this, &SimulatedPosition::_updatePosition);
Jimmy Johnson's avatar
Jimmy Johnson committed
34 35 36 37
}

QGeoPositionInfo SimulatedPosition::lastKnownPosition(bool /*fromSatellitePositioningMethodsOnly*/) const
{
38
    return _lastPosition;
Jimmy Johnson's avatar
Jimmy Johnson committed
39 40 41 42 43 44 45
}

SimulatedPosition::PositioningMethods SimulatedPosition::supportedPositioningMethods() const
{
    return AllPositioningMethods;
}

46
void SimulatedPosition::startUpdates(void)
Jimmy Johnson's avatar
Jimmy Johnson committed
47
{
48
    _updateTimer.start(qMax(updateInterval(), minimumUpdateInterval()));
Jimmy Johnson's avatar
Jimmy Johnson committed
49 50
}

51
void SimulatedPosition::stopUpdates(void)
Jimmy Johnson's avatar
Jimmy Johnson committed
52
{
53
    _updateTimer.stop();
Jimmy Johnson's avatar
Jimmy Johnson committed
54 55 56 57 58 59 60
}

void SimulatedPosition::requestUpdate(int /*timeout*/)
{
    emit updateTimeout();
}

61
void SimulatedPosition::_updatePosition(void)
Jimmy Johnson's avatar
Jimmy Johnson committed
62
{
63
    int intervalMsecs = _updateTimer.interval();
Jimmy Johnson's avatar
Jimmy Johnson committed
64

65 66 67
    QGeoCoordinate  coord =                 _lastPosition.coordinate();
    double          horizontalDistance =    _horizontalVelocityMetersPerSec * (1000.0 / static_cast<double>(intervalMsecs));
    double          verticalDistance =      _verticalVelocityMetersPerSec * (1000.0 / static_cast<double>(intervalMsecs));
Jimmy Johnson's avatar
Jimmy Johnson committed
68

69
    _lastPosition.setCoordinate(coord.atDistanceAndAzimuth(horizontalDistance, _heading, verticalDistance));
Jimmy Johnson's avatar
Jimmy Johnson committed
70

71
    emit positionUpdated(_lastPosition);
Jimmy Johnson's avatar
Jimmy Johnson committed
72 73 74 75 76 77 78
}

QGeoPositionInfoSource::Error SimulatedPosition::error() const
{
    return QGeoPositionInfoSource::NoError;
}

79 80 81 82 83 84 85 86 87 88 89 90
void SimulatedPosition::_vehicleAdded(Vehicle* vehicle)
{
    if (vehicle->homePosition().isValid()) {
        _lastPosition.setCoordinate(vehicle->homePosition());
    } else {
        connect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged);
    }
}

void SimulatedPosition::_vehicleHomePositionChanged(QGeoCoordinate homePosition)
{
    Vehicle* vehicle = qobject_cast<Vehicle*>(sender());
Jimmy Johnson's avatar
Jimmy Johnson committed
91

92 93 94 95 96
    if (homePosition.isValid()) {
        _lastPosition.setCoordinate(homePosition);
        disconnect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged);
    }
}