RallyPointController.cc 11.2 KB
Newer Older
1 2
/****************************************************************************
 *
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#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"
19
#include "SettingsManager.h"
20
#include "AppSettings.h"
DonLakeFlyer's avatar
DonLakeFlyer committed
21
#include "PlanMasterController.h"
22 23 24 25 26 27 28 29 30

#include <QJsonDocument>
#include <QJsonArray>

QGC_LOGGING_CATEGORY(RallyPointControllerLog, "RallyPointControllerLog")

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

31
RallyPointController::RallyPointController(PlanMasterController* masterController, QObject* parent)
32 33 34
    : PlanElementController (masterController, parent)
    , _managerVehicle               (masterController->managerVehicle())
    , _rallyPointManager    (masterController->managerVehicle()->rallyPointManager())
35
{
36
    connect(&_points, &QmlObjectListModel::countChanged, this, &RallyPointController::_updateContainsItems);
37 38 39 40 41 42 43
}

RallyPointController::~RallyPointController()
{

}

44 45 46 47 48 49 50 51 52 53 54
void RallyPointController::start(bool flyView)
{
    qCDebug(GeoFenceControllerLog) << "start flyView" << flyView;

    _managerVehicleChanged(_masterController->managerVehicle());
    connect(_masterController, &PlanMasterController::managerVehicleChanged, this, &RallyPointController::_managerVehicleChanged);

    PlanElementController::start(flyView);
}

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

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

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

75
    //-- RallyPointController::supported() tests both the capability bit AND the protocol version.
76
    connect(_managerVehicle,    &Vehicle::capabilityBitsChanged,        this, &RallyPointController::supportedChanged);
77
    connect(_managerVehicle,    &Vehicle::requestProtocolVersion,       this, &RallyPointController::supportedChanged);
78 79

    emit supportedChanged(supported());
80 81
}

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

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

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

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

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

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

    return true;
125 126
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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