FollowMe.cc 5.2 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 14 15 16 17

#include <QElapsedTimer>
#include <cmath>

#include "MultiVehicleManager.h"
#include "PX4FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "FollowMe.h"
#include "Vehicle.h"
Jimmy Johnson's avatar
Jimmy Johnson committed
18
#include "PositionManager.h"
Jimmy Johnson's avatar
Jimmy Johnson committed
19 20

FollowMe::FollowMe(QGCApplication* app)
Jimmy Johnson's avatar
Jimmy Johnson committed
21
    : QGCTool(app), estimatation_capabilities(0)
Jimmy Johnson's avatar
Jimmy Johnson committed
22 23
{
    memset(&_motionReport, 0, sizeof(motionReport_s));
Jimmy Johnson's avatar
Jimmy Johnson committed
24
    runTime.start();
Jimmy Johnson's avatar
Jimmy Johnson committed
25

Jimmy Johnson's avatar
Jimmy Johnson committed
26 27
    _gcsMotionReportTimer.setSingleShot(false);
    connect(&_gcsMotionReportTimer, &QTimer::timeout, this, &FollowMe::_sendGCSMotionReport);
Jimmy Johnson's avatar
Jimmy Johnson committed
28 29 30 31
}

FollowMe::~FollowMe()
{
Jimmy Johnson's avatar
Jimmy Johnson committed
32
    _disable();
Jimmy Johnson's avatar
Jimmy Johnson committed
33 34 35 36 37 38 39 40
}

void FollowMe::followMeHandleManager(const QString&)
{
    QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles();

    for (int i=0; i< vehicles.count(); i++) {
        Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
41
        if (vehicle->px4Firmware() && vehicle->flightMode().compare(PX4FirmwarePlugin::followMeFlightMode, Qt::CaseInsensitive) == 0) {
Jimmy Johnson's avatar
Jimmy Johnson committed
42
            _enable();
Jimmy Johnson's avatar
Jimmy Johnson committed
43 44 45 46
            return;
        }
    }

Jimmy Johnson's avatar
Jimmy Johnson committed
47
    _disable();
Jimmy Johnson's avatar
Jimmy Johnson committed
48 49
}

Jimmy Johnson's avatar
Jimmy Johnson committed
50
void FollowMe::_enable()
Jimmy Johnson's avatar
Jimmy Johnson committed
51
{
Jimmy Johnson's avatar
Jimmy Johnson committed
52 53 54 55 56 57 58
    connect(_toolbox->qgcPositionManager(),
            SIGNAL(positionInfoUpdated(QGeoPositionInfo)),
            this,
            SLOT(_setGPSLocation(QGeoPositionInfo)));

    _gcsMotionReportTimer.setInterval(_toolbox->qgcPositionManager()->updateInterval());
    _gcsMotionReportTimer.start();
Jimmy Johnson's avatar
Jimmy Johnson committed
59 60
}

Jimmy Johnson's avatar
Jimmy Johnson committed
61
void FollowMe::_disable()
Jimmy Johnson's avatar
Jimmy Johnson committed
62
{
Jimmy Johnson's avatar
Jimmy Johnson committed
63 64 65 66 67 68
    disconnect(_toolbox->qgcPositionManager(),
               SIGNAL(positionInfoUpdated(QGeoPositionInfo)),
               this,
               SLOT(_setGPSLocation(QGeoPositionInfo)));

    _gcsMotionReportTimer.stop();
Jimmy Johnson's avatar
Jimmy Johnson committed
69 70 71 72 73 74 75 76 77 78 79 80 81 82
}

void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo)
{
    if (geoPositionInfo.isValid())
    {
        // get the current location coordinates

        QGeoCoordinate geoCoordinate = geoPositionInfo.coordinate();

        _motionReport.lat_int = geoCoordinate.latitude()*1e7;
        _motionReport.lon_int = geoCoordinate.longitude()*1e7;
        _motionReport.alt = geoCoordinate.altitude();

Jimmy Johnson's avatar
Jimmy Johnson committed
83 84
        estimatation_capabilities |= (1 << POS);

Jimmy Johnson's avatar
Jimmy Johnson committed
85 86 87 88 89 90 91 92
        // 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
93
        }                
Jimmy Johnson's avatar
Jimmy Johnson committed
94

Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
95
        // calculate z velocity if it's available
Jimmy Johnson's avatar
Jimmy Johnson committed
96 97 98 99 100

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

Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
101
        // calculate x,y velocity if it's available
Jimmy Johnson's avatar
Jimmy Johnson committed
102 103 104 105

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

Jimmy Johnson's avatar
Jimmy Johnson committed
106 107
            estimatation_capabilities |= (1 << VEL);

Jimmy Johnson's avatar
Jimmy Johnson committed
108 109 110 111 112
            qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction));
            qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);

            _motionReport.vx = cos(direction)*velocity;
            _motionReport.vy = sin(direction)*velocity;
Jimmy Johnson's avatar
Jimmy Johnson committed
113 114 115 116

        } else {
            _motionReport.vx = 0.0f;
            _motionReport.vy = 0.0f;
Jimmy Johnson's avatar
Jimmy Johnson committed
117 118 119 120 121 122 123 124 125 126 127 128 129
        }
    }
}

void FollowMe::_sendGCSMotionReport(void)
{
    QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles();
    MAVLinkProtocol* mavlinkProtocol = _toolbox->mavlinkProtocol();
    mavlink_follow_target_t follow_target;

    memset(&follow_target, 0, sizeof(mavlink_follow_target_t));

    follow_target.timestamp = runTime.nsecsElapsed()*1e-6;
Jimmy Johnson's avatar
Jimmy Johnson committed
130
    follow_target.est_capabilities = estimatation_capabilities;
Jimmy Johnson's avatar
Jimmy Johnson committed
131 132 133 134 135
    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
136 137
    follow_target.vel[0] = _motionReport.vx;
    follow_target.vel[1] = _motionReport.vy;
Jimmy Johnson's avatar
Jimmy Johnson committed
138 139 140

    for (int i=0; i< vehicles.count(); i++) {
        Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
Jimmy Johnson's avatar
Jimmy Johnson committed
141
        if(vehicle->flightMode().compare(PX4FirmwarePlugin::followMeFlightMode, Qt::CaseInsensitive) == 0) {
Jimmy Johnson's avatar
Jimmy Johnson committed
142 143 144 145 146
            mavlink_message_t message;
            mavlink_msg_follow_target_encode(mavlinkProtocol->getSystemId(),
                                             mavlinkProtocol->getComponentId(),
                                             &message,
                                             &follow_target);
147
            vehicle->sendMessageOnPriorityLink(message);
Jimmy Johnson's avatar
Jimmy Johnson committed
148 149 150 151 152 153 154 155
        }
    }
}

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