FollowMe.cc 6.51 KB
Newer Older
1 2 3 4 5 6 7 8
/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/
Jimmy Johnson's avatar
Jimmy Johnson committed
9 10 11 12 13

#include <QElapsedTimer>
#include <cmath>

#include "MultiVehicleManager.h"
14
#include "FirmwarePlugin.h"
Jimmy Johnson's avatar
Jimmy Johnson committed
15 16 17
#include "MAVLinkProtocol.h"
#include "FollowMe.h"
#include "Vehicle.h"
Jimmy Johnson's avatar
Jimmy Johnson committed
18
#include "PositionManager.h"
19 20
#include "SettingsManager.h"
#include "AppSettings.h"
Jimmy Johnson's avatar
Jimmy Johnson committed
21

DonLakeFlyer's avatar
DonLakeFlyer committed
22 23
QGC_LOGGING_CATEGORY(FollowMeLog, "FollowMeLog")

24
FollowMe::FollowMe(QGCApplication* app, QGCToolbox* toolbox)
25
    : QGCTool(app, toolbox)
Jimmy Johnson's avatar
Jimmy Johnson committed
26
{
Jimmy Johnson's avatar
Jimmy Johnson committed
27
    _gcsMotionReportTimer.setSingleShot(false);
28 29 30 31 32
}

void FollowMe::setToolbox(QGCToolbox* toolbox)
{
    QGCTool::setToolbox(toolbox);
Jimmy Johnson's avatar
Jimmy Johnson committed
33

34 35 36 37
    connect(&_gcsMotionReportTimer,                                     &QTimer::timeout,       this, &FollowMe::_sendGCSMotionReport);
    connect(toolbox->settingsManager()->appSettings()->followTarget(),  &Fact::rawValueChanged, this, &FollowMe::_settingsChanged);

    _settingsChanged();
38
}
Jimmy Johnson's avatar
Jimmy Johnson committed
39

40
void FollowMe::_settingsChanged()
41
{
42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59
    _currentMode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt();

    switch (_currentMode) {
    case MODE_NEVER:
        disconnect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded,     this, &FollowMe::_vehicleAdded);
        disconnect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved,   this, &FollowMe::_vehicleRemoved);
        _disableFollowSend();
        break;
    case MODE_ALWAYS:
        connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded,    this, &FollowMe::_vehicleAdded);
        connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved,  this, &FollowMe::_vehicleRemoved);
        _enableFollowSend();
        break;
    case MODE_FOLLOWME:
        connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleAdded,    this, &FollowMe::_vehicleAdded);
        connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::vehicleRemoved,  this, &FollowMe::_vehicleRemoved);
        _enableIfVehicleInFollow();
        break;
60
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
61 62
}

63
void FollowMe::_enableFollowSend()
Jimmy Johnson's avatar
Jimmy Johnson committed
64
{
65
    if (!_gcsMotionReportTimer.isActive()) {
Don Gagne's avatar
Don Gagne committed
66
        _gcsMotionReportTimer.setInterval(qMin(_toolbox->qgcPositionManager()->updateInterval(), 250));
67 68
        _gcsMotionReportTimer.start();
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
69 70
}

71
void FollowMe::_disableFollowSend()
Jimmy Johnson's avatar
Jimmy Johnson committed
72
{
73 74 75
    if (_gcsMotionReportTimer.isActive()) {
        _gcsMotionReportTimer.stop();
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
76 77
}

78
void FollowMe::_sendGCSMotionReport()
Jimmy Johnson's avatar
Jimmy Johnson committed
79
{
80 81 82
    QGeoPositionInfo    geoPositionInfo =   _toolbox->qgcPositionManager()->geoPositionInfo();
    QGeoCoordinate      gcsCoordinate =     geoPositionInfo.coordinate();

83
    if (!geoPositionInfo.isValid()) {
84 85
        return;
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
86

87 88
    GCSMotionReport motionReport = {};
    uint8_t         estimatation_capabilities = 0;
Jimmy Johnson's avatar
Jimmy Johnson committed
89

90
    // get the current location coordinates
Jimmy Johnson's avatar
Jimmy Johnson committed
91

92 93 94 95
    motionReport.lat_int =          static_cast<int>(gcsCoordinate.latitude()  * 1e7);
    motionReport.lon_int =          static_cast<int>(gcsCoordinate.longitude() * 1e7);
    motionReport.altMetersAMSL =    gcsCoordinate.altitude();
    estimatation_capabilities |=    (1 << POS);
Jimmy Johnson's avatar
Jimmy Johnson committed
96

97
    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) == true) {
98
        estimatation_capabilities |= (1 << HEADING);
99 100
        motionReport.headingDegrees = geoPositionInfo.attribute(QGeoPositionInfo::Direction);
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
101

102
    // get the current eph and epv
Jimmy Johnson's avatar
Jimmy Johnson committed
103

104 105 106
    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) {
        motionReport.pos_std_dev[0] = motionReport.pos_std_dev[1] = geoPositionInfo.attribute(QGeoPositionInfo::HorizontalAccuracy);
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
107

108 109 110
    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) {
        motionReport.pos_std_dev[2] = geoPositionInfo.attribute(QGeoPositionInfo::VerticalAccuracy);
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
111

112 113 114 115 116
    // calculate z velocity if it's available

    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalSpeed)) {
        motionReport.vzMetersPerSec = geoPositionInfo.attribute(QGeoPositionInfo::VerticalSpeed);
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
117

118
    // calculate x,y velocity if it's available
Jimmy Johnson's avatar
Jimmy Johnson committed
119

120 121
    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) && geoPositionInfo.hasAttribute(QGeoPositionInfo::GroundSpeed) == true) {
        estimatation_capabilities |= (1 << VEL);
Jimmy Johnson's avatar
Jimmy Johnson committed
122

123 124
        qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction));
        qreal velocity  = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);
Jimmy Johnson's avatar
Jimmy Johnson committed
125

126 127 128 129 130 131
        motionReport.vxMetersPerSec = cos(direction)*velocity;
        motionReport.vyMetersPerSec = sin(direction)*velocity;
    } else {
        motionReport.vxMetersPerSec = 0;
        motionReport.vyMetersPerSec = 0;
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
132

133
    QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles();
Jimmy Johnson's avatar
Jimmy Johnson committed
134

135 136 137
    for (int i=0; i<vehicles->count(); i++) {
        Vehicle* vehicle = vehicles->value<Vehicle*>(i);
        if (_currentMode == MODE_ALWAYS || (_currentMode == MODE_FOLLOWME && _isFollowFlightMode(vehicle, vehicle->flightMode()))) {
DonLakeFlyer's avatar
DonLakeFlyer committed
138
            qCDebug(FollowMeLog) << "sendGCSMotionReport latInt:lonInt:altMetersAMSL" << motionReport.lat_int << motionReport.lon_int << motionReport.altMetersAMSL;
139
            vehicle->firmwarePlugin()->sendGCSMotionReport(vehicle, motionReport, estimatation_capabilities);
Jimmy Johnson's avatar
Jimmy Johnson committed
140 141 142 143
        }
    }
}

144 145 146 147 148 149
double FollowMe::_degreesToRadian(double deg)
{
    return deg * M_PI / 180.0;
}

void FollowMe::_vehicleAdded(Vehicle* vehicle)
Jimmy Johnson's avatar
Jimmy Johnson committed
150
{
151 152 153 154 155 156 157 158 159
    connect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow);
    _enableIfVehicleInFollow();
}

void FollowMe::_vehicleRemoved(Vehicle* vehicle)
{
    disconnect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow);
    _enableIfVehicleInFollow();
}
160

161 162 163 164
void FollowMe::_enableIfVehicleInFollow(void)
{
    if (_currentMode == MODE_ALWAYS) {
        // System always enabled
165 166 167
        return;
    }

168 169 170 171 172 173 174 175
    // Any vehicle in follow mode will enable the system
    QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles();

    for (int i=0; i< vehicles->count(); i++) {
        Vehicle* vehicle = vehicles->value<Vehicle*>(i);
        if (_isFollowFlightMode(vehicle, vehicle->flightMode())) {
            _enableFollowSend();
            return;
Jimmy Johnson's avatar
Jimmy Johnson committed
176 177
        }
    }
178 179

    _disableFollowSend();
Jimmy Johnson's avatar
Jimmy Johnson committed
180 181
}

182
bool FollowMe::_isFollowFlightMode (Vehicle* vehicle, const QString& flightMode)
Jimmy Johnson's avatar
Jimmy Johnson committed
183
{
184
    return flightMode.compare(vehicle->followFlightMode()) == 0;
Jimmy Johnson's avatar
Jimmy Johnson committed
185
}