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

22 23
FollowMe::FollowMe(QGCApplication* app, QGCToolbox* toolbox)
    : QGCTool(app, toolbox), estimatation_capabilities(0)
Jimmy Johnson's avatar
Jimmy Johnson committed
24 25
{
    memset(&_motionReport, 0, sizeof(motionReport_s));
Jimmy Johnson's avatar
Jimmy Johnson committed
26 27
    runTime.start();
    _gcsMotionReportTimer.setSingleShot(false);
28 29 30 31 32
}

void FollowMe::setToolbox(QGCToolbox* toolbox)
{
    QGCTool::setToolbox(toolbox);
Jimmy Johnson's avatar
Jimmy Johnson committed
33
    connect(&_gcsMotionReportTimer, &QTimer::timeout, this, &FollowMe::_sendGCSMotionReport);
34 35 36 37 38
    connect(toolbox->settingsManager()->appSettings()->followTarget(), &Fact::rawValueChanged, this, &FollowMe::_settingsChanged);
    _currentMode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt();
    if(_currentMode == MODE_ALWAYS) {
        _enable();
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
39 40 41 42
}

void FollowMe::followMeHandleManager(const QString&)
{
43 44
    //-- If we are to change based on current flight mode
    if(_currentMode == MODE_FOLLOWME) {
45 46 47 48
        QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles();
        for (int i=0; i< vehicles.count(); i++) {
            Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
            if (vehicle->px4Firmware() && vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
49
                _enable();
50 51
                return;
            }
Jimmy Johnson's avatar
Jimmy Johnson committed
52
        }
53
        _disable();
Jimmy Johnson's avatar
Jimmy Johnson committed
54
    }
55
}
Jimmy Johnson's avatar
Jimmy Johnson committed
56

57
void FollowMe::_settingsChanged()
58
{
59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81
    uint32_t mode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt();
    if(_currentMode != mode) {
        _currentMode = mode;
        switch (mode) {
        case MODE_NEVER:
            if(_gcsMotionReportTimer.isActive()) {
                _disable();
            }
            break;
        case MODE_ALWAYS:
            if(!_gcsMotionReportTimer.isActive()) {
                _enable();
            }
            break;
        case MODE_FOLLOWME:
            if(!_gcsMotionReportTimer.isActive()) {
                followMeHandleManager(QString());
            }
            break;
        default:
            break;
        }
    }
Jimmy Johnson's avatar
Jimmy Johnson committed
82 83
}

84
void FollowMe::_enable()
Jimmy Johnson's avatar
Jimmy Johnson committed
85
{
Jimmy Johnson's avatar
Jimmy Johnson committed
86 87 88 89
    connect(_toolbox->qgcPositionManager(),
            SIGNAL(positionInfoUpdated(QGeoPositionInfo)),
            this,
            SLOT(_setGPSLocation(QGeoPositionInfo)));
90
    _gcsMotionReportTimer.setInterval(_toolbox->qgcPositionManager()->updateInterval());
Jimmy Johnson's avatar
Jimmy Johnson committed
91
    _gcsMotionReportTimer.start();
Jimmy Johnson's avatar
Jimmy Johnson committed
92 93
}

Jimmy Johnson's avatar
Jimmy Johnson committed
94
void FollowMe::_disable()
Jimmy Johnson's avatar
Jimmy Johnson committed
95
{
Jimmy Johnson's avatar
Jimmy Johnson committed
96 97 98 99 100
    disconnect(_toolbox->qgcPositionManager(),
               SIGNAL(positionInfoUpdated(QGeoPositionInfo)),
               this,
               SLOT(_setGPSLocation(QGeoPositionInfo)));
    _gcsMotionReportTimer.stop();
Jimmy Johnson's avatar
Jimmy Johnson committed
101 102 103 104
}

void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo)
{
105 106 107 108
    if (!geoPositionInfo.isValid()) {
        //-- Invalidate cached coordinates
        memset(&_motionReport, 0, sizeof(motionReport_s));
    } else {
Jimmy Johnson's avatar
Jimmy Johnson committed
109 110 111 112
        // get the current location coordinates

        QGeoCoordinate geoCoordinate = geoPositionInfo.coordinate();

113 114 115
        _motionReport.lat_int = geoCoordinate.latitude()  * 1e7;
        _motionReport.lon_int = geoCoordinate.longitude() * 1e7;
        _motionReport.alt     = geoCoordinate.altitude();
Jimmy Johnson's avatar
Jimmy Johnson committed
116

Jimmy Johnson's avatar
Jimmy Johnson committed
117 118
        estimatation_capabilities |= (1 << POS);

Jimmy Johnson's avatar
Jimmy Johnson committed
119 120 121 122 123 124 125 126
        // get the current eph and epv

        if(geoPositionInfo.hasAttribute(QGeoPositionInfo::HorizontalAccuracy) == true) {
            _motionReport.pos_std_dev[0] = _motionReport.pos_std_dev[1] = geoPositionInfo.attribute(QGeoPositionInfo::HorizontalAccuracy);
        }

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

Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
129
        // calculate z velocity if it's available
Jimmy Johnson's avatar
Jimmy Johnson committed
130 131 132 133 134

        if(geoPositionInfo.hasAttribute(QGeoPositionInfo::VerticalSpeed)) {
            _motionReport.vz = geoPositionInfo.attribute(QGeoPositionInfo::VerticalSpeed);
        }

Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
135
        // calculate x,y velocity if it's available
Jimmy Johnson's avatar
Jimmy Johnson committed
136 137 138 139

        if((geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction)   == true) &&
           (geoPositionInfo.hasAttribute(QGeoPositionInfo::GroundSpeed) == true)) {

Jimmy Johnson's avatar
Jimmy Johnson committed
140 141
            estimatation_capabilities |= (1 << VEL);

Jimmy Johnson's avatar
Jimmy Johnson committed
142
            qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction));
143
            qreal velocity  = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);
Jimmy Johnson's avatar
Jimmy Johnson committed
144 145 146

            _motionReport.vx = cos(direction)*velocity;
            _motionReport.vy = sin(direction)*velocity;
Jimmy Johnson's avatar
Jimmy Johnson committed
147 148 149 150

        } else {
            _motionReport.vx = 0.0f;
            _motionReport.vy = 0.0f;
Jimmy Johnson's avatar
Jimmy Johnson committed
151 152 153 154
        }
    }
}

155
void FollowMe::_sendGCSMotionReport()
Jimmy Johnson's avatar
Jimmy Johnson committed
156
{
157 158 159 160 161 162 163

    //-- Do we have any real data?
    if(_motionReport.lat_int == 0 && _motionReport.lon_int == 0 && _motionReport.alt == 0) {
        return;
    }

    QmlObjectListModel & vehicles    = *_toolbox->multiVehicleManager()->vehicles();
Jimmy Johnson's avatar
Jimmy Johnson committed
164 165
    MAVLinkProtocol* mavlinkProtocol = _toolbox->mavlinkProtocol();
    mavlink_follow_target_t follow_target;
Don Gagne's avatar
Don Gagne committed
166
    memset(&follow_target, 0, sizeof(follow_target));
Jimmy Johnson's avatar
Jimmy Johnson committed
167

168
    follow_target.timestamp = runTime.nsecsElapsed() * 1e-6;
Jimmy Johnson's avatar
Jimmy Johnson committed
169
    follow_target.est_capabilities = estimatation_capabilities;
Jimmy Johnson's avatar
Jimmy Johnson committed
170 171 172 173 174
    follow_target.position_cov[0] = _motionReport.pos_std_dev[0];
    follow_target.position_cov[2] = _motionReport.pos_std_dev[2];
    follow_target.alt = _motionReport.alt;
    follow_target.lat = _motionReport.lat_int;
    follow_target.lon = _motionReport.lon_int;
Jimmy Johnson's avatar
Jimmy Johnson committed
175 176
    follow_target.vel[0] = _motionReport.vx;
    follow_target.vel[1] = _motionReport.vy;
Jimmy Johnson's avatar
Jimmy Johnson committed
177 178 179

    for (int i=0; i< vehicles.count(); i++) {
        Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
180
        if(_currentMode || vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
Jimmy Johnson's avatar
Jimmy Johnson committed
181
            mavlink_message_t message;
182 183 184 185 186 187
            mavlink_msg_follow_target_encode_chan(mavlinkProtocol->getSystemId(),
                                                  mavlinkProtocol->getComponentId(),
                                                  vehicle->priorityLink()->mavlinkChannel(),
                                                  &message,
                                                  &follow_target);
            vehicle->sendMessageOnLink(vehicle->priorityLink(), message);
Jimmy Johnson's avatar
Jimmy Johnson committed
188 189 190 191 192 193 194 195
        }
    }
}

double FollowMe::_degreesToRadian(double deg)
{
    return deg * M_PI / 180.0;
}