FollowMe.cc 7.06 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

#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

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 66 67 68
    if (!_gcsMotionReportTimer.isActive()) {
        _gcsMotionReportTimer.setInterval(qMin(_toolbox->qgcPositionManager()->updateInterval(), 250));
        _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 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102
        return;
    }

    // First check to see if any vehicles need follow me updates
    bool needFollowMe = false;
    if (_currentMode == MODE_ALWAYS) {
        needFollowMe = true;
    } else if (_currentMode == MODE_FOLLOWME) {
        QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles();
        for (int i=0; i<vehicles->count(); i++) {
            Vehicle* vehicle = vehicles->value<Vehicle*>(i);
            if (_isFollowFlightMode(vehicle, vehicle->flightMode())) {
                needFollowMe = true;
            }
        }
    }
    if (!needFollowMe) {
        return;
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
103

104 105
    GCSMotionReport motionReport = {};
    uint8_t         estimatation_capabilities = 0;
Jimmy Johnson's avatar
Jimmy Johnson committed
106

107
    // get the current location coordinates
Jimmy Johnson's avatar
Jimmy Johnson committed
108

109 110 111 112
    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
113

114 115 116 117
    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) == true) {
        estimatation_capabilities |= (1 << HEADING);
        motionReport.headingDegrees = geoPositionInfo.attribute(QGeoPositionInfo::Direction);
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
118

119
    // get the current eph and epv
Jimmy Johnson's avatar
Jimmy Johnson committed
120

121 122 123
    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
124

125 126 127
    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) {
        motionReport.pos_std_dev[2] = geoPositionInfo.attribute(QGeoPositionInfo::VerticalAccuracy);
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
128

129
    // calculate z velocity if it's available
Jimmy Johnson's avatar
Jimmy Johnson committed
130

131 132 133 134 135
    if (geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalSpeed)) {
        motionReport.vzMetersPerSec = geoPositionInfo.attribute(QGeoPositionInfo::VerticalSpeed);
    }

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

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

140 141
        qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction));
        qreal velocity  = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);
Jimmy Johnson's avatar
Jimmy Johnson committed
142

143 144 145 146 147 148
        motionReport.vxMetersPerSec = cos(direction)*velocity;
        motionReport.vyMetersPerSec = sin(direction)*velocity;
    } else {
        motionReport.vxMetersPerSec = 0;
        motionReport.vyMetersPerSec = 0;
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
149

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

152 153 154 155 156
    for (int i=0; i<vehicles->count(); i++) {
        Vehicle* vehicle = vehicles->value<Vehicle*>(i);
        if (_currentMode == MODE_ALWAYS || _isFollowFlightMode(vehicle, vehicle->flightMode())) {
            qCDebug(FollowMeLog) << "sendGCSMotionReport latInt:lonInt:altMetersAMSL" << motionReport.lat_int << motionReport.lon_int << motionReport.altMetersAMSL;
            vehicle->firmwarePlugin()->sendGCSMotionReport(vehicle, motionReport, estimatation_capabilities);
Jimmy Johnson's avatar
Jimmy Johnson committed
157 158 159 160
        }
    }
}

161
double FollowMe::_degreesToRadian(double deg)
Jimmy Johnson's avatar
Jimmy Johnson committed
162
{
163 164
    return deg * M_PI / 180.0;
}
165

166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181
void FollowMe::_vehicleAdded(Vehicle* vehicle)
{
    connect(vehicle, &Vehicle::flightModeChanged, this, &FollowMe::_enableIfVehicleInFollow);
    _enableIfVehicleInFollow();
}

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

void FollowMe::_enableIfVehicleInFollow(void)
{
    if (_currentMode == MODE_ALWAYS) {
        // System always enabled
182 183 184
        return;
    }

185 186 187 188 189 190 191 192
    // 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
193 194
        }
    }
195 196

    _disableFollowSend();
Jimmy Johnson's avatar
Jimmy Johnson committed
197 198
}

199
bool FollowMe::_isFollowFlightMode (Vehicle* vehicle, const QString& flightMode)
Jimmy Johnson's avatar
Jimmy Johnson committed
200
{
201
    return flightMode.compare(vehicle->followFlightMode()) == 0;
Jimmy Johnson's avatar
Jimmy Johnson committed
202
}