RallyPointController.cc 10.9 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
    //-- RallyPointController::supported() tests both the capability bit AND the protocol version.
75
    connect(_managerVehicle,    &Vehicle::capabilityBitsChanged,        this, &RallyPointController::supportedChanged);
76
    connect(_managerVehicle,    &Vehicle::requestProtocolVersion,       this, &RallyPointController::supportedChanged);
77 78

    emit supportedChanged(supported());
79 80
}

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

100 101
    QString errorStr;
    QString errorMessage = tr("Rally: %1");
102

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

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

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

120
    setDirty(false);
121
    _setFirstPointCurrent();
122 123

    return true;
124 125
}

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

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

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

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

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

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

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

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

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

Don Gagne's avatar
Don Gagne committed
204
void RallyPointController::_managerLoadComplete(void)
205
{
DonLakeFlyer's avatar
DonLakeFlyer committed
206
    // Fly view always reloads on _loadComplete
207 208 209 210
    // Plan view only reloads if:
    //  - Load was specifically requested
    //  - There is no current Plan
    if (_flyView || _itemsRequested || isEmpty()) {
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 247 248 249 250 251
        if(_masterController->controllerVehicle()->fixedWing()) {
            defaultAlt = qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue().toDouble();
        }
        else {
            defaultAlt = RallyPoint::getDefaultFactAltitude();
        }
252 253 254 255 256 257 258 259
    }
    point.setAltitude(defaultAlt);
    RallyPoint* newPoint = new RallyPoint(point, this);
    _points.append(newPoint);
    setCurrentRallyPoint(newPoint);
    setDirty(true);
}

260
bool RallyPointController::supported(void) const
261
{
262
    return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_managerVehicle->maxProtoVersion() >= 200);
263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279
}

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 {
280
        setCurrentRallyPoint(nullptr);
281 282 283 284 285 286 287 288 289 290 291 292 293
    }
}

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

void RallyPointController::_setFirstPointCurrent(void)
{
294
    setCurrentRallyPoint(_points.count() ? _points[0] : nullptr);
295
}
296 297 298 299 300 301 302 303 304 305 306

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

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

DonLakeFlyer's avatar
DonLakeFlyer committed
307
bool RallyPointController::showPlanFromManagerVehicle (void)
308
{
309
    qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle _flyView" << _flyView;
DonLakeFlyer's avatar
DonLakeFlyer committed
310 311
    if (_masterController->offline()) {
        qCWarning(RallyPointControllerLog) << "RallyPointController::showPlanFromManagerVehicle called while offline";
Patrick José Pereira's avatar
Patrick José Pereira committed
312
        return true;    // stops further propagation of showPlanFromManagerVehicle due to error
DonLakeFlyer's avatar
DonLakeFlyer committed
313 314
    } else {
        if (!_managerVehicle->initialPlanRequestComplete()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
315
            // The vehicle hasn't completed initial load, we can just wait for loadComplete to be signalled automatically
DonLakeFlyer's avatar
DonLakeFlyer committed
316 317 318 319 320 321 322
            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 {
323
            qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: sync complete";
DonLakeFlyer's avatar
DonLakeFlyer committed
324
            _itemsRequested = true;
Don Gagne's avatar
Don Gagne committed
325
            _managerLoadComplete();
DonLakeFlyer's avatar
DonLakeFlyer committed
326 327 328
            return false;
        }
    }
329
}
330 331 332 333 334

bool RallyPointController::isEmpty(void) const
{
    return _points.count() == 0;
}