RallyPointController.cc 10.4 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 29 30 31 32 33 34 35

#include <QJsonDocument>
#include <QJsonArray>

QGC_LOGGING_CATEGORY(RallyPointControllerLog, "RallyPointControllerLog")

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

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

    managerVehicleChanged(_managerVehicle);
46 47 48 49 50 51 52
}

RallyPointController::~RallyPointController()
{

}

53
void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle)
54
{
55 56
    if (_managerVehicle) {
        _rallyPointManager->disconnect(this);
57
        _managerVehicle->disconnect(this);
58 59
        _managerVehicle = nullptr;
        _rallyPointManager = nullptr;
60
    }
61

62 63 64 65 66 67 68
    _managerVehicle = managerVehicle;
    if (!_managerVehicle) {
        qWarning() << "RallyPointController::managerVehicleChanged managerVehicle=NULL";
        return;
    }

    _rallyPointManager = _managerVehicle->rallyPointManager();
DonLakeFlyer's avatar
DonLakeFlyer committed
69 70 71
    connect(_rallyPointManager, &RallyPointManager::loadComplete,       this, &RallyPointController::_managerLoadComplete);
    connect(_rallyPointManager, &RallyPointManager::sendComplete,       this, &RallyPointController::_managerSendComplete);
    connect(_rallyPointManager, &RallyPointManager::removeAllComplete,  this, &RallyPointController::_managerRemoveAllComplete);
72
    connect(_rallyPointManager, &RallyPointManager::inProgressChanged,  this, &RallyPointController::syncInProgressChanged);
73

74 75 76
    connect(_managerVehicle,    &Vehicle::capabilityBitsChanged,        this, &RallyPointController::supportedChanged);

    emit supportedChanged(supported());
77 78
}

79
bool RallyPointController::load(const QJsonObject& json, QString& errorString)
80
{
DonLakeFlyer's avatar
DonLakeFlyer committed
81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97
    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;
    }

98 99
    QString errorStr;
    QString errorMessage = tr("Rally: %1");
100

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

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

112 113 114 115 116 117
    QObjectList pointList;
    for (int i=0; i<rgPoints.count(); i++) {
        pointList.append(new RallyPoint(rgPoints[i], this));
    }
    _points.swapObjectList(pointList);

118
    setDirty(false);
119
    _setFirstPointCurrent();
120 121

    return true;
122 123
}

124
void RallyPointController::save(QJsonObject& json)
125
{
DonLakeFlyer's avatar
DonLakeFlyer committed
126
    json[JsonHelper::jsonVersionKey] = _jsonCurrentVersion;
127

128 129 130 131 132
    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);
133
    }
134
    json[_jsonPointsKey] = QJsonValue(rgPoints);
135 136 137 138 139 140
}

void RallyPointController::removeAll(void)
{
    _points.clearAndDeleteContents();
    setDirty(true);
141
    setCurrentRallyPoint(nullptr);
142 143
}

DonLakeFlyer's avatar
DonLakeFlyer committed
144 145 146 147 148 149 150 151 152 153 154
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();
    }
}

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

void RallyPointController::sendToVehicle(void)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
169 170 171 172 173
    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
174
        qCDebug(RallyPointControllerLog) << "RallyPointController::sendToVehicle";
175 176 177 178 179
        setDirty(false);
        QList<QGeoCoordinate> rgPoints;
        for (int i=0; i<_points.count(); i++) {
            rgPoints.append(qobject_cast<RallyPoint*>(_points[i])->coordinate());
        }
180
        _rallyPointManager->sendToVehicle(rgPoints);
181 182 183 184 185
    }
}

bool RallyPointController::syncInProgress(void) const
{
186
    return _rallyPointManager->inProgress();
187 188 189 190 191 192 193 194 195 196 197 198
}

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

QString RallyPointController::editorQml(void) const
{
199
    return _rallyPointManager->editorQml();
200 201
}

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

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

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

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

251
bool RallyPointController::supported(void) const
252
{
253
    return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_managerVehicle->maxProtoVersion() >= 200);
254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270
}

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 {
271
        setCurrentRallyPoint(nullptr);
272 273 274 275 276 277 278 279 280 281 282 283 284
    }
}

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

void RallyPointController::_setFirstPointCurrent(void)
{
285
    setCurrentRallyPoint(_points.count() ? _points[0] : nullptr);
286
}
287 288 289 290 291 292 293 294 295 296 297

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

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

DonLakeFlyer's avatar
DonLakeFlyer committed
298
bool RallyPointController::showPlanFromManagerVehicle (void)
299
{
300
    qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle _flyView" << _flyView;
DonLakeFlyer's avatar
DonLakeFlyer committed
301 302
    if (_masterController->offline()) {
        qCWarning(RallyPointControllerLog) << "RallyPointController::showPlanFromManagerVehicle called while offline";
Patrick José Pereira's avatar
Patrick José Pereira committed
303
        return true;    // stops further propagation of showPlanFromManagerVehicle due to error
DonLakeFlyer's avatar
DonLakeFlyer committed
304 305
    } else {
        if (!_managerVehicle->initialPlanRequestComplete()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
306
            // The vehicle hasn't completed initial load, we can just wait for loadComplete to be signalled automatically
DonLakeFlyer's avatar
DonLakeFlyer committed
307 308 309 310 311 312 313
            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 {
314
            qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: sync complete";
DonLakeFlyer's avatar
DonLakeFlyer committed
315
            _itemsRequested = true;
Don Gagne's avatar
Don Gagne committed
316
            _managerLoadComplete();
DonLakeFlyer's avatar
DonLakeFlyer committed
317 318 319
            return false;
        }
    }
320
}