APMRallyPointManager.cc 5.69 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33
/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/

#include "APMRallyPointManager.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
#include "ParameterManager.h"

const char* APMRallyPointManager::_rallyTotalParam = "RALLY_TOTAL";

APMRallyPointManager::APMRallyPointManager(Vehicle* vehicle)
    : RallyPointManager(vehicle)
    , _readTransactionInProgress(false)
    , _writeTransactionInProgress(false)
{
    connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMRallyPointManager::_mavlinkMessageReceived);
}

APMRallyPointManager::~APMRallyPointManager()
{

}

void APMRallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& rgPoints)
{
34
    if (_vehicle->isOfflineEditingVehicle() || !rallyPointsSupported()) {
35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55
        return;
    }

    if (_readTransactionInProgress) {
        _sendError(InternalError, QStringLiteral("Rally Point write attempted while read in progress."));
        return;
    }

    _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->setRawValue(rgPoints.count());

    // FIXME: No validation of correct point received
    _rgPoints = rgPoints;
    for (uint8_t index=0; index<_rgPoints.count(); index++) {
        _sendRallyPoint(index);
    }

    emit loadComplete(_rgPoints);
}

void APMRallyPointManager::loadFromVehicle(void)
{
56
    if (_vehicle->isOfflineEditingVehicle() || !rallyPointsSupported() || _readTransactionInProgress) {
57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92
        return;
    }

    _rgPoints.clear();

    _cReadRallyPoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->rawValue().toInt();
    qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << _cReadRallyPoints;
    if (_cReadRallyPoints == 0) {
        emit loadComplete(_rgPoints);
        return;
    }

    _currentRallyPoint = 0;
    _readTransactionInProgress = true;
    _requestRallyPoint(_currentRallyPoint);
}

/// Called when a new mavlink message for out vehicle is received
void APMRallyPointManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
    if (message.msgid == MAVLINK_MSG_ID_RALLY_POINT) {
        mavlink_rally_point_t rallyPoint;

        mavlink_msg_rally_point_decode(&message, &rallyPoint);
        qCDebug(RallyPointManagerLog) << "From vehicle rally_point: idx:lat:lng:alt" << rallyPoint.idx << rallyPoint.lat << rallyPoint.lng << rallyPoint.alt;

        if (rallyPoint.idx != _currentRallyPoint) {
            // FIXME: Protocol out of whack
            _readTransactionInProgress = false;
            emit inProgressChanged(inProgress());
            qCWarning(RallyPointManagerLog) << "Indices out of sync" << rallyPoint.idx << _currentRallyPoint;
            return;
        }

        QGeoCoordinate point((float)rallyPoint.lat / 1e7, (float)rallyPoint.lng / 1e7, rallyPoint.alt);
        _rgPoints.append(point);
Don Gagne's avatar
Don Gagne committed
93
        if (rallyPoint.idx < _cReadRallyPoints - 1) {
94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111
            // Still more points to request
            _requestRallyPoint(++_currentRallyPoint);
        } else {
            // We've finished collecting rally points
            qCDebug(RallyPointManagerLog) << "Rally point load complete";
            _readTransactionInProgress = false;
            emit loadComplete(_rgPoints);
            emit inProgressChanged(inProgress());
        }
    }
}

void APMRallyPointManager::_requestRallyPoint(uint8_t pointIndex)
{
    mavlink_message_t   msg;
    MAVLinkProtocol*    mavlink = qgcApp()->toolbox()->mavlinkProtocol();

    qCDebug(RallyPointManagerLog) << "APMRallyPointManager::_requestRallyPoint" << pointIndex;
112 113 114 115 116 117 118 119
    mavlink_msg_rally_fetch_point_pack_chan(mavlink->getSystemId(),
                                            mavlink->getComponentId(),
                                            _vehicle->priorityLink()->mavlinkChannel(),
                                            &msg,
                                            _vehicle->id(),
                                            _vehicle->defaultComponentId(),
                                            pointIndex);
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
120 121 122 123 124 125 126 127
}

void APMRallyPointManager::_sendRallyPoint(uint8_t pointIndex)
{
    mavlink_message_t   msg;
    MAVLinkProtocol*    mavlink = qgcApp()->toolbox()->mavlinkProtocol();

    QGeoCoordinate point = _rgPoints[pointIndex];
128 129 130 131 132 133 134 135 136 137 138 139 140
    mavlink_msg_rally_point_pack_chan(mavlink->getSystemId(),
                                      mavlink->getComponentId(),
                                      _vehicle->priorityLink()->mavlinkChannel(),
                                      &msg,
                                      _vehicle->id(),
                                      _vehicle->defaultComponentId(),
                                      pointIndex,
                                      _rgPoints.count(),
                                      point.latitude() * 1e7,
                                      point.longitude() * 1e7,
                                      point.altitude(),
                                      0, 0, 0);          //  break_alt, land_dir, flags
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
141 142 143 144 145 146
}

bool APMRallyPointManager::inProgress(void) const
{
    return _readTransactionInProgress || _writeTransactionInProgress;
}
147 148 149 150 151

bool APMRallyPointManager::rallyPointsSupported(void) const
{
    return _vehicle->parameterManager()->parameterExists(_vehicle->defaultComponentId(), _rallyTotalParam);
}