RallyPointController.cc 10.5 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
/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/


/// @file
///     @author Don Gagne <don@thegagnes.com>

#include "RallyPointController.h"
#include "RallyPoint.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
#include "ParameterManager.h"
#include "JsonHelper.h"
#include "SimpleMissionItem.h"
23
#include "QGroundControlQmlGlobal.h"
24
#include "SettingsManager.h"
25
#include "AppSettings.h"
DonLakeFlyer's avatar
DonLakeFlyer committed
26
#include "PlanMasterController.h"
27 28

#ifndef __mobile__
29
#include "QGCQFileDialog.h"
30 31 32 33 34 35 36 37 38 39
#endif

#include <QJsonDocument>
#include <QJsonArray>

QGC_LOGGING_CATEGORY(RallyPointControllerLog, "RallyPointControllerLog")

const char* RallyPointController::_jsonFileTypeValue =  "RallyPoints";
const char* RallyPointController::_jsonPointsKey =      "points";

40 41 42
RallyPointController::RallyPointController(PlanMasterController* masterController, QObject* parent)
    : PlanElementController(masterController, parent)
    , _rallyPointManager(_managerVehicle->rallyPointManager())
43 44
    , _dirty(false)
    , _currentRallyPoint(NULL)
DonLakeFlyer's avatar
DonLakeFlyer committed
45
    , _itemsRequested(false)
46
{
47
    connect(&_points, &QmlObjectListModel::countChanged, this, &RallyPointController::_updateContainsItems);
48 49

    managerVehicleChanged(_managerVehicle);
50 51 52 53 54 55 56
}

RallyPointController::~RallyPointController()
{

}

57
void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle)
58
{
59 60
    if (_managerVehicle) {
        _rallyPointManager->disconnect(this);
61
        _managerVehicle->disconnect(this);
62 63 64
        _managerVehicle = NULL;
        _rallyPointManager = NULL;
    }
65

66 67 68 69 70 71 72
    _managerVehicle = managerVehicle;
    if (!_managerVehicle) {
        qWarning() << "RallyPointController::managerVehicleChanged managerVehicle=NULL";
        return;
    }

    _rallyPointManager = _managerVehicle->rallyPointManager();
DonLakeFlyer's avatar
DonLakeFlyer committed
73 74 75
    connect(_rallyPointManager, &RallyPointManager::loadComplete,       this, &RallyPointController::_managerLoadComplete);
    connect(_rallyPointManager, &RallyPointManager::sendComplete,       this, &RallyPointController::_managerSendComplete);
    connect(_rallyPointManager, &RallyPointManager::removeAllComplete,  this, &RallyPointController::_managerRemoveAllComplete);
76
    connect(_rallyPointManager, &RallyPointManager::inProgressChanged,  this, &RallyPointController::syncInProgressChanged);
77

78 79 80
    connect(_managerVehicle,    &Vehicle::capabilityBitsChanged,        this, &RallyPointController::supportedChanged);

    emit supportedChanged(supported());
81 82
}

83
bool RallyPointController::load(const QJsonObject& json, QString& errorString)
84
{
DonLakeFlyer's avatar
DonLakeFlyer committed
85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101
    removeAll();

    errorString.clear();

    if (json.contains(JsonHelper::jsonVersionKey) && json[JsonHelper::jsonVersionKey].toInt() == 1) {
        // We just ignore old version 1 data
        return true;
    }

    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
        { JsonHelper::jsonVersionKey,   QJsonValue::Double, true },
        { _jsonPointsKey,               QJsonValue::Array,  true },
    };
    if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) {
        return false;
    }

102 103
    QString errorStr;
    QString errorMessage = tr("Rally: %1");
104

DonLakeFlyer's avatar
DonLakeFlyer committed
105 106
    if (json[JsonHelper::jsonVersionKey].toInt() != _jsonCurrentVersion) {
        errorString = tr("Rally Points supports version %1").arg(_jsonCurrentVersion);
107 108 109 110
        return false;
    }

    QList<QGeoCoordinate> rgPoints;
111 112
    if (!JsonHelper::loadGeoCoordinateArray(json[_jsonPointsKey], true /* altitudeRequired */, rgPoints, errorStr)) {
        errorString = errorMessage.arg(errorStr);
113
        return false;
114
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
115

116 117 118 119 120 121
    QObjectList pointList;
    for (int i=0; i<rgPoints.count(); i++) {
        pointList.append(new RallyPoint(rgPoints[i], this));
    }
    _points.swapObjectList(pointList);

122
    setDirty(false);
123
    _setFirstPointCurrent();
124 125

    return true;
126 127
}

128
void RallyPointController::save(QJsonObject& json)
129
{
DonLakeFlyer's avatar
DonLakeFlyer committed
130
    json[JsonHelper::jsonVersionKey] = _jsonCurrentVersion;
131

132 133 134 135 136
    QJsonArray rgPoints;
    QJsonValue jsonPoint;
    for (int i=0; i<_points.count(); i++) {
        JsonHelper::saveGeoCoordinate(qobject_cast<RallyPoint*>(_points[i])->coordinate(), true /* writeAltitude */, jsonPoint);
        rgPoints.append(jsonPoint);
137
    }
138
    json[_jsonPointsKey] = QJsonValue(rgPoints);
139 140 141 142 143 144 145 146 147
}

void RallyPointController::removeAll(void)
{
    _points.clearAndDeleteContents();
    setDirty(true);
    setCurrentRallyPoint(NULL);
}

DonLakeFlyer's avatar
DonLakeFlyer committed
148 149 150 151 152 153 154 155 156 157 158
void RallyPointController::removeAllFromVehicle(void)
{
    if (_masterController->offline()) {
        qCWarning(RallyPointControllerLog) << "RallyPointController::removeAllFromVehicle called while offline";
    } else if (syncInProgress()) {
        qCWarning(RallyPointControllerLog) << "RallyPointController::removeAllFromVehicle called while syncInProgress";
    } else {
        _rallyPointManager->removeAll();
    }
}

159 160
void RallyPointController::loadFromVehicle(void)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
161 162 163 164
    if (_masterController->offline()) {
        qCWarning(RallyPointControllerLog) << "RallyPointController::loadFromVehicle called while offline";
    } else if (syncInProgress()) {
        qCWarning(RallyPointControllerLog) << "RallyPointController::loadFromVehicle called while syncInProgress";
165
    } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
166 167
        _itemsRequested = true;
        _rallyPointManager->loadFromVehicle();
168 169 170 171 172
    }
}

void RallyPointController::sendToVehicle(void)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
173 174 175 176 177
    if (_masterController->offline()) {
        qCWarning(RallyPointControllerLog) << "RallyPointController::sendToVehicle called while offline";
    } else if (syncInProgress()) {
        qCWarning(RallyPointControllerLog) << "RallyPointController::sendToVehicle called while syncInProgress";
    } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
178
        qCDebug(RallyPointControllerLog) << "RallyPointController::sendToVehicle";
179 180 181 182 183
        setDirty(false);
        QList<QGeoCoordinate> rgPoints;
        for (int i=0; i<_points.count(); i++) {
            rgPoints.append(qobject_cast<RallyPoint*>(_points[i])->coordinate());
        }
184
        _rallyPointManager->sendToVehicle(rgPoints);
185 186 187 188 189
    }
}

bool RallyPointController::syncInProgress(void) const
{
190
    return _rallyPointManager->inProgress();
191 192 193 194 195 196 197 198 199 200 201 202
}

void RallyPointController::setDirty(bool dirty)
{
    if (dirty != _dirty) {
        _dirty = dirty;
        emit dirtyChanged(dirty);
    }
}

QString RallyPointController::editorQml(void) const
{
203
    return _rallyPointManager->editorQml();
204 205
}

Don Gagne's avatar
Don Gagne committed
206
void RallyPointController::_managerLoadComplete(void)
207
{
DonLakeFlyer's avatar
DonLakeFlyer committed
208 209
    // Fly view always reloads on _loadComplete
    // Plan view only reloads on _loadComplete if specifically requested
210
    if (_flyView || _itemsRequested) {
DonLakeFlyer's avatar
DonLakeFlyer committed
211 212
        _points.clearAndDeleteContents();
        QObjectList pointList;
Don Gagne's avatar
Don Gagne committed
213 214
        for (int i=0; i<_rallyPointManager->points().count(); i++) {
            pointList.append(new RallyPoint(_rallyPointManager->points()[i], this));
DonLakeFlyer's avatar
DonLakeFlyer committed
215 216 217 218 219 220 221 222 223
        }
        _points.swapObjectList(pointList);
        setDirty(false);
        _setFirstPointCurrent();
        emit loadComplete();
    }
    _itemsRequested = false;
}

224
void RallyPointController::_managerSendComplete(bool error)
DonLakeFlyer's avatar
DonLakeFlyer committed
225 226
{
    // Fly view always reloads after send
Don Gagne's avatar
Don Gagne committed
227
    if (!error && _flyView) {
DonLakeFlyer's avatar
DonLakeFlyer committed
228
        showPlanFromManagerVehicle();
229
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
230 231
}

232
void RallyPointController::_managerRemoveAllComplete(bool error)
DonLakeFlyer's avatar
DonLakeFlyer committed
233
{
234 235 236 237
    if (!error) {
        // Remove all from vehicle so we always update
        showPlanFromManagerVehicle();
    }
238 239 240 241 242 243 244 245
}

void RallyPointController::addPoint(QGeoCoordinate point)
{
    double defaultAlt;
    if (_points.count()) {
        defaultAlt = qobject_cast<RallyPoint*>(_points[_points.count() - 1])->coordinate().altitude();
    } else {
246
        defaultAlt = qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue().toDouble();
247 248 249 250 251 252 253 254
    }
    point.setAltitude(defaultAlt);
    RallyPoint* newPoint = new RallyPoint(point, this);
    _points.append(newPoint);
    setCurrentRallyPoint(newPoint);
    setDirty(true);
}

255
bool RallyPointController::supported(void) const
256
{
257
    return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_managerVehicle->maxProtoVersion() >= 200);
258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290
}

void RallyPointController::removePoint(QObject* rallyPoint)
{
    int foundIndex = 0;
    for (foundIndex=0; foundIndex<_points.count(); foundIndex++) {
        if (_points[foundIndex] == rallyPoint) {
            _points.removeOne(rallyPoint);
            rallyPoint->deleteLater();
        }
    }

    if (_points.count()) {
        int newIndex = qMin(foundIndex, _points.count() - 1);
        newIndex = qMax(newIndex, 0);
        setCurrentRallyPoint(_points[newIndex]);
    } else {
        setCurrentRallyPoint(NULL);
    }
}

void RallyPointController::setCurrentRallyPoint(QObject* rallyPoint)
{
    if (_currentRallyPoint != rallyPoint) {
        _currentRallyPoint = rallyPoint;
        emit currentRallyPointChanged(rallyPoint);
    }
}

void RallyPointController::_setFirstPointCurrent(void)
{
    setCurrentRallyPoint(_points.count() ? _points[0] : NULL);
}
291 292 293 294 295 296 297 298 299 300 301

bool RallyPointController::containsItems(void) const
{
    return _points.count() > 0;
}

void RallyPointController::_updateContainsItems(void)
{
    emit containsItemsChanged(containsItems());
}

DonLakeFlyer's avatar
DonLakeFlyer committed
302
bool RallyPointController::showPlanFromManagerVehicle (void)
303
{
304
    qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle _flyView" << _flyView;
DonLakeFlyer's avatar
DonLakeFlyer committed
305 306
    if (_masterController->offline()) {
        qCWarning(RallyPointControllerLog) << "RallyPointController::showPlanFromManagerVehicle called while offline";
Patrick José Pereira's avatar
Patrick José Pereira committed
307
        return true;    // stops further propagation of showPlanFromManagerVehicle due to error
DonLakeFlyer's avatar
DonLakeFlyer committed
308 309
    } else {
        if (!_managerVehicle->initialPlanRequestComplete()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
310
            // The vehicle hasn't completed initial load, we can just wait for loadComplete to be signalled automatically
DonLakeFlyer's avatar
DonLakeFlyer committed
311 312 313 314 315 316 317
            qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: !initialPlanRequestComplete, wait for signal";
            return true;
        } else if (syncInProgress()) {
            // If the sync is already in progress, _loadComplete will be called automatically when it is done. So no need to do anything.
            qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: syncInProgress wait for signal";
            return true;
        } else {
318
            qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: sync complete";
DonLakeFlyer's avatar
DonLakeFlyer committed
319
            _itemsRequested = true;
Don Gagne's avatar
Don Gagne committed
320
            _managerLoadComplete();
DonLakeFlyer's avatar
DonLakeFlyer committed
321 322 323
            return false;
        }
    }
324
}