PlanMasterController.cc 20.3 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12
/****************************************************************************
 *
 *   (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 "PlanMasterController.h"
#include "QGCApplication.h"
#include "MultiVehicleManager.h"
13
#include "SettingsManager.h"
14 15
#include "AppSettings.h"
#include "JsonHelper.h"
16
#include "MissionManager.h"
17
#include "KML.h"
18

19
#include <QDomDocument>
20
#include <QJsonDocument>
Don Gagne's avatar
Don Gagne committed
21
#include <QFileInfo>
22

DonLakeFlyer's avatar
DonLakeFlyer committed
23 24
QGC_LOGGING_CATEGORY(PlanMasterControllerLog, "PlanMasterControllerLog")

25 26 27 28 29 30 31
const int   PlanMasterController::_planFileVersion =            1;
const char* PlanMasterController::_planFileType =               "Plan";
const char* PlanMasterController::_jsonMissionObjectKey =       "mission";
const char* PlanMasterController::_jsonGeoFenceObjectKey =      "geoFence";
const char* PlanMasterController::_jsonRallyPointsObjectKey =   "rallyPoints";

PlanMasterController::PlanMasterController(QObject* parent)
32 33 34 35 36 37 38 39 40 41 42 43 44 45
    : QObject               (parent)
    , _multiVehicleMgr      (qgcApp()->toolbox()->multiVehicleManager())
    , _controllerVehicle    (new Vehicle((MAV_AUTOPILOT)qgcApp()->toolbox()->settingsManager()->appSettings()->offlineEditingFirmwareType()->rawValue().toInt(), (MAV_TYPE)qgcApp()->toolbox()->settingsManager()->appSettings()->offlineEditingVehicleType()->rawValue().toInt(), qgcApp()->toolbox()->firmwarePluginManager()))
    , _managerVehicle       (_controllerVehicle)
    , _flyView              (true)
    , _offline              (true)
    , _missionController    (this)
    , _geoFenceController   (this)
    , _rallyPointController (this)
    , _loadGeoFence         (false)
    , _loadRallyPoints      (false)
    , _sendGeoFence         (false)
    , _sendRallyPoints      (false)
    , _syncInProgress       (false)
46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64
{
    connect(&_missionController,    &MissionController::dirtyChanged,       this, &PlanMasterController::dirtyChanged);
    connect(&_geoFenceController,   &GeoFenceController::dirtyChanged,      this, &PlanMasterController::dirtyChanged);
    connect(&_rallyPointController, &RallyPointController::dirtyChanged,    this, &PlanMasterController::dirtyChanged);

    connect(&_missionController,    &MissionController::containsItemsChanged,       this, &PlanMasterController::containsItemsChanged);
    connect(&_geoFenceController,   &GeoFenceController::containsItemsChanged,      this, &PlanMasterController::containsItemsChanged);
    connect(&_rallyPointController, &RallyPointController::containsItemsChanged,    this, &PlanMasterController::containsItemsChanged);

    connect(&_missionController,    &MissionController::syncInProgressChanged,      this, &PlanMasterController::syncInProgressChanged);
    connect(&_geoFenceController,   &GeoFenceController::syncInProgressChanged,     this, &PlanMasterController::syncInProgressChanged);
    connect(&_rallyPointController, &RallyPointController::syncInProgressChanged,   this, &PlanMasterController::syncInProgressChanged);
}

PlanMasterController::~PlanMasterController()
{

}

65
void PlanMasterController::start(bool flyView)
66
{
67 68 69 70
    _flyView = flyView;
    _missionController.start(_flyView);
    _geoFenceController.start(_flyView);
    _rallyPointController.start(_flyView);
71 72 73 74 75 76 77

    connect(_multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &PlanMasterController::_activeVehicleChanged);
    _activeVehicleChanged(_multiVehicleMgr->activeVehicle());
}

void PlanMasterController::startStaticActiveVehicle(Vehicle* vehicle)
{
78 79 80 81
    _flyView = true;
    _missionController.start(_flyView);
    _geoFenceController.start(_flyView);
    _rallyPointController.start(_flyView);
82 83 84 85 86
    _activeVehicleChanged(vehicle);
}

void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle)
{
87 88 89
    if (_managerVehicle == activeVehicle) {
        // We are already setup for this vehicle
        return;
90 91
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
92 93
    qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged" << activeVehicle;

94 95 96 97 98 99 100 101 102 103
    if (_managerVehicle) {
        // Disconnect old vehicle
        disconnect(_managerVehicle->missionManager(),       &MissionManager::newMissionItemsAvailable,  this, &PlanMasterController::_loadMissionComplete);
        disconnect(_managerVehicle->geoFenceManager(),      &GeoFenceManager::loadComplete,             this, &PlanMasterController::_loadGeoFenceComplete);
        disconnect(_managerVehicle->rallyPointManager(),    &RallyPointManager::loadComplete,           this, &PlanMasterController::_loadRallyPointsComplete);
        disconnect(_managerVehicle->missionManager(),       &MissionManager::sendComplete,              this, &PlanMasterController::_sendMissionComplete);
        disconnect(_managerVehicle->geoFenceManager(),      &GeoFenceManager::sendComplete,             this, &PlanMasterController::_sendGeoFenceComplete);
        disconnect(_managerVehicle->rallyPointManager(),    &RallyPointManager::sendComplete,           this, &PlanMasterController::_sendRallyPointsComplete);
    }

104 105 106 107 108
    bool newOffline = false;
    if (activeVehicle == NULL) {
        // Since there is no longer an active vehicle we use the offline controller vehicle as the manager vehicle
        _managerVehicle = _controllerVehicle;
        newOffline = true;
109
    } else {
110
        newOffline = false;
111 112 113 114 115 116 117 118
        _managerVehicle = activeVehicle;

        // Update controllerVehicle to the currently connected vehicle
        AppSettings* appSettings = qgcApp()->toolbox()->settingsManager()->appSettings();
        appSettings->offlineEditingFirmwareType()->setRawValue(AppSettings::offlineEditingFirmwareTypeFromFirmwareType(_managerVehicle->firmwareType()));
        appSettings->offlineEditingVehicleType()->setRawValue(AppSettings::offlineEditingVehicleTypeFromVehicleType(_managerVehicle->vehicleType()));

        // We use these signals to sequence upload and download to the multiple controller/managers
DonLakeFlyer's avatar
DonLakeFlyer committed
119 120 121 122 123 124
        connect(_managerVehicle->missionManager(),      &MissionManager::newMissionItemsAvailable,  this, &PlanMasterController::_loadMissionComplete);
        connect(_managerVehicle->geoFenceManager(),     &GeoFenceManager::loadComplete,             this, &PlanMasterController::_loadGeoFenceComplete);
        connect(_managerVehicle->rallyPointManager(),   &RallyPointManager::loadComplete,           this, &PlanMasterController::_loadRallyPointsComplete);
        connect(_managerVehicle->missionManager(),      &MissionManager::sendComplete,              this, &PlanMasterController::_sendMissionComplete);
        connect(_managerVehicle->geoFenceManager(),     &GeoFenceManager::sendComplete,             this, &PlanMasterController::_sendGeoFenceComplete);
        connect(_managerVehicle->rallyPointManager(),   &RallyPointManager::sendComplete,           this, &PlanMasterController::_sendRallyPointsComplete);
125 126 127 128
    }
    if (newOffline != _offline) {
        _offline = newOffline;
        emit offlineEditingChanged(newOffline);
129 130
    }

131 132 133
    _missionController.managerVehicleChanged(_managerVehicle);
    _geoFenceController.managerVehicleChanged(_managerVehicle);
    _rallyPointController.managerVehicleChanged(_managerVehicle);
134

135
    if (!_flyView) {
DonLakeFlyer's avatar
DonLakeFlyer committed
136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154
        if (!offline()) {
            // We are in Plan view and we have a newly connected vehicle:
            //  - If there is no plan available in Plan view show the one from the vehicle
            //  - Otherwise leave the current plan alone
            if (!containsItems()) {
                qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged: Plan view is empty so loading from manager";
                _showPlanFromManagerVehicle();
            }
        }
    } else {
        if (offline()) {
            // No more active vehicle, clear mission
            qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged: Fly view is offline clearing plan";
            removeAll();
        } else {
            // Fly view has changed to a new active vehicle, update to show correct mission
            qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged: Fly view is online so loading from manager";
            _showPlanFromManagerVehicle();
        }
155
    }
156 157 158 159
}

void PlanMasterController::loadFromVehicle(void)
{
160 161 162 163 164
    if (_managerVehicle->highLatencyLink()) {
        qgcApp()->showMessage(tr("Download not supported on high latency links."));
        return;
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
165 166
    if (offline()) {
        qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called while offline";
167
    } else if (_flyView) {
DonLakeFlyer's avatar
DonLakeFlyer committed
168 169 170 171
        qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called from Fly view";
    } else if (syncInProgress()) {
        qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called while syncInProgress";
    } else {
172
        _loadGeoFence = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
173
        _syncInProgress = true;
174
        emit syncInProgressChanged(true);
DonLakeFlyer's avatar
DonLakeFlyer committed
175
        qCDebug(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle _missionController.loadFromVehicle";
176 177 178
        _missionController.loadFromVehicle();
        setDirty(false);
    }
179 180
}

181

DonLakeFlyer's avatar
DonLakeFlyer committed
182
void PlanMasterController::_loadMissionComplete(void)
183
{
184
    if (!_flyView && _loadGeoFence) {
185 186
        _loadGeoFence = false;
        _loadRallyPoints = true;
187 188 189 190 191 192 193 194
        if (_geoFenceController.supported()) {
            qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete _geoFenceController.loadFromVehicle";
            _geoFenceController.loadFromVehicle();
        } else {
            qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete GeoFence not supported skipping";
            _geoFenceController.removeAll();
            _loadGeoFenceComplete();
        }
195
        setDirty(false);
DonLakeFlyer's avatar
DonLakeFlyer committed
196 197 198 199 200
    }
}

void PlanMasterController::_loadGeoFenceComplete(void)
{
201
    if (!_flyView && _loadRallyPoints) {
DonLakeFlyer's avatar
DonLakeFlyer committed
202
        _loadRallyPoints = false;
203 204 205 206 207 208 209 210
        if (_rallyPointController.supported()) {
            qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadGeoFenceComplete _rallyPointController.loadFromVehicle";
            _rallyPointController.loadFromVehicle();
        } else {
            qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete Rally Points not supported skipping";
            _rallyPointController.removeAll();
            _loadRallyPointsComplete();
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
211 212 213 214 215 216
        setDirty(false);
    }
}

void PlanMasterController::_loadRallyPointsComplete(void)
{
217
    if (!_flyView) {
DonLakeFlyer's avatar
DonLakeFlyer committed
218 219 220 221 222 223 224
        _syncInProgress = false;
        emit syncInProgressChanged(false);
    }
}

void PlanMasterController::_sendMissionComplete(void)
{
225
    if (_sendGeoFence) {
226 227
        _sendGeoFence = false;
        _sendRallyPoints = true;
228 229 230 231 232 233 234
        if (_geoFenceController.supported()) {
            qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start GeoFence sendToVehicle";
            _geoFenceController.sendToVehicle();
        } else {
            qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle GeoFence not supported skipping";
            _sendGeoFenceComplete();
        }
235 236 237 238
        setDirty(false);
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
239
void PlanMasterController::_sendGeoFenceComplete(void)
240
{
241
    if (_sendRallyPoints) {
242
        _sendRallyPoints = false;
243 244 245 246 247 248 249
        if (_rallyPointController.supported()) {
            qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start rally sendToVehicle";
            _rallyPointController.sendToVehicle();
        } else {
            qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle Rally Points not support skipping";
            _sendRallyPointsComplete();
        }
250 251 252
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
253 254
void PlanMasterController::_sendRallyPointsComplete(void)
{
255
    if (_syncInProgress) {
256
        qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle Rally Point send complete";
DonLakeFlyer's avatar
DonLakeFlyer committed
257 258 259 260 261
        _syncInProgress = false;
        emit syncInProgressChanged(false);
    }
}

262 263
void PlanMasterController::sendToVehicle(void)
{
264 265 266 267 268
    if (_managerVehicle->highLatencyLink()) {
        qgcApp()->showMessage(tr("Upload not supported on high latency links."));
        return;
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
269 270 271 272 273 274
    if (offline()) {
        qCWarning(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called while offline";
    } else if (syncInProgress()) {
        qCWarning(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called while syncInProgress";
    } else {
        qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start mission sendToVehicle";
275
        _sendGeoFence = true;
276 277
        _syncInProgress = true;
        emit syncInProgressChanged(true);
278 279 280
        _missionController.sendToVehicle();
        setDirty(false);
    }
281 282 283 284 285
}

void PlanMasterController::loadFromFile(const QString& filename)
{
    QString errorString;
286
    QString errorMessage = tr("Error loading Plan file (%1). %2").arg(filename).arg("%1");
287 288 289 290 291 292 293 294 295

    if (filename.isEmpty()) {
        return;
    }

    QFile file(filename);

    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        errorString = file.errorString() + QStringLiteral(" ") + filename;
296
        qgcApp()->showMessage(errorMessage.arg(errorString));
297 298 299
        return;
    }

300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340
    QString fileExtension(".%1");
    if (filename.endsWith(fileExtension.arg(AppSettings::planFileExtension))) {
        QJsonDocument   jsonDoc;
        QByteArray      bytes = file.readAll();

        if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) {
            qgcApp()->showMessage(errorMessage.arg(errorString));
            return;
        }

        int version;
        QJsonObject json = jsonDoc.object();
        if (!JsonHelper::validateQGCJsonFile(json, _planFileType, _planFileVersion, _planFileVersion, version, errorString)) {
            qgcApp()->showMessage(errorMessage.arg(errorString));
            return;
        }

        QList<JsonHelper::KeyValidateInfo> rgKeyInfo = {
            { _jsonMissionObjectKey,        QJsonValue::Object, true },
            { _jsonGeoFenceObjectKey,       QJsonValue::Object, true },
            { _jsonRallyPointsObjectKey,    QJsonValue::Object, true },
        };
        if (!JsonHelper::validateKeys(json, rgKeyInfo, errorString)) {
            qgcApp()->showMessage(errorMessage.arg(errorString));
            return;
        }

        if (!_missionController.load(json[_jsonMissionObjectKey].toObject(), errorString) ||
                !_geoFenceController.load(json[_jsonGeoFenceObjectKey].toObject(), errorString) ||
                !_rallyPointController.load(json[_jsonRallyPointsObjectKey].toObject(), errorString)) {
            qgcApp()->showMessage(errorMessage.arg(errorString));
        }
    } else if (filename.endsWith(fileExtension.arg(AppSettings::missionFileExtension))) {
        if (!_missionController.loadJsonFile(file, errorString)) {
            qgcApp()->showMessage(errorMessage.arg(errorString));
        }
    } else if (filename.endsWith(fileExtension.arg(AppSettings::waypointsFileExtension)) ||
               filename.endsWith(fileExtension.arg(QStringLiteral("txt")))) {
        if (!_missionController.loadTextFile(file, errorString)) {
            qgcApp()->showMessage(errorMessage.arg(errorString));
        }
341 342
    }

343
    if (!offline()) {
344
        setDirty(true);
345 346 347
    }
}

Gus Grubba's avatar
Gus Grubba committed
348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363
QJsonDocument PlanMasterController::saveToJson()
{
    QJsonObject planJson;
    QJsonObject missionJson;
    QJsonObject fenceJson;
    QJsonObject rallyJson;
    JsonHelper::saveQGCJsonFileHeader(planJson, _planFileType, _planFileVersion);
    _missionController.save(missionJson);
    _geoFenceController.save(fenceJson);
    _rallyPointController.save(rallyJson);
    planJson[_jsonMissionObjectKey] = missionJson;
    planJson[_jsonGeoFenceObjectKey] = fenceJson;
    planJson[_jsonRallyPointsObjectKey] = rallyJson;
    return QJsonDocument(planJson);
}

364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379
void PlanMasterController::saveToFile(const QString& filename)
{
    if (filename.isEmpty()) {
        return;
    }

    QString planFilename = filename;
    if (!QFileInfo(filename).fileName().contains(".")) {
        planFilename += QString(".%1").arg(fileExtension());
    }

    QFile file(planFilename);

    if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
        qgcApp()->showMessage(tr("Plan save error %1 : %2").arg(filename).arg(file.errorString()));
    } else {
Gus Grubba's avatar
Gus Grubba committed
380
        QJsonDocument saveDoc = saveToJson();
381 382 383
        file.write(saveDoc.toJson());
    }

384 385
    // Only clear dirty bit if we are offline
    if (offline()) {
386 387 388 389
        setDirty(false);
    }
}

390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413
void PlanMasterController::saveToKml(const QString& filename)
{
    if (filename.isEmpty()) {
        return;
    }

    QString kmlFilename = filename;
    if (!QFileInfo(filename).fileName().contains(".")) {
        kmlFilename += QString(".%1").arg(kmlFileExtension());
    }

    QFile file(kmlFilename);

    if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
        qgcApp()->showMessage(tr("KML save error %1 : %2").arg(filename).arg(file.errorString()));
    } else {
        QDomDocument domDocument;
        _missionController.convertToKMLDocument(domDocument);
        QTextStream stream(&file);
        stream << domDocument.toString();
        file.close();
    }
}

414 415
void PlanMasterController::removeAll(void)
{
416 417 418
    _missionController.removeAll();
    _geoFenceController.removeAll();
    _rallyPointController.removeAll();
419 420 421 422 423
    if (_offline) {
        _missionController.setDirty(false);
        _geoFenceController.setDirty(false);
        _rallyPointController.setDirty(false);
    }
424 425 426 427
}

void PlanMasterController::removeAllFromVehicle(void)
{
428 429
    if (!offline()) {
        _missionController.removeAllFromVehicle();
430 431 432 433 434 435
        if (_geoFenceController.supported()) {
            _geoFenceController.removeAllFromVehicle();
        }
        if (_rallyPointController.supported()) {
            _rallyPointController.removeAllFromVehicle();
        }
436
        setDirty(false);
437 438 439
    } else {
        qWarning() << "PlanMasterController::removeAllFromVehicle called while offline";
    }
440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463
}

bool PlanMasterController::containsItems(void) const
{
    return _missionController.containsItems() || _geoFenceController.containsItems() || _rallyPointController.containsItems();
}

bool PlanMasterController::dirty(void) const
{
    return _missionController.dirty() || _geoFenceController.dirty() || _rallyPointController.dirty();
}

void PlanMasterController::setDirty(bool dirty)
{
    _missionController.setDirty(dirty);
    _geoFenceController.setDirty(dirty);
    _rallyPointController.setDirty(dirty);
}

QString PlanMasterController::fileExtension(void) const
{
    return AppSettings::planFileExtension;
}

464 465 466 467 468
QString PlanMasterController::kmlFileExtension(void) const
{
    return AppSettings::kmlFileExtension;
}

469 470 471 472
QStringList PlanMasterController::loadNameFilters(void) const
{
    QStringList filters;

Don Gagne's avatar
Don Gagne committed
473
    filters << tr("Supported types (*.%1 *.%2 *.%3 *.%4)").arg(AppSettings::planFileExtension).arg(AppSettings::missionFileExtension).arg(AppSettings::waypointsFileExtension).arg("txt") <<
474 475 476 477 478 479 480 481 482 483 484 485 486
               tr("All Files (*.*)");
    return filters;
}


QStringList PlanMasterController::saveNameFilters(void) const
{
    QStringList filters;

    filters << tr("Plan Files (*.%1)").arg(fileExtension()) << tr("All Files (*.*)");
    return filters;
}

487 488 489 490 491 492 493 494
QStringList PlanMasterController::saveKmlFilters(void) const
{
    QStringList filters;

    filters << tr("KML Files (*.%1)").arg(kmlFileExtension()) << tr("All Files (*.*)");
    return filters;
}

495 496 497 498 499 500
void PlanMasterController::sendPlanToVehicle(Vehicle* vehicle, const QString& filename)
{
    // Use a transient PlanMasterController to accomplish this
    PlanMasterController* controller = new PlanMasterController();
    controller->startStaticActiveVehicle(vehicle);
    controller->loadFromFile(filename);
501
    controller->sendToVehicle();
502 503
    delete controller;
}
DonLakeFlyer's avatar
DonLakeFlyer committed
504 505 506

void PlanMasterController::_showPlanFromManagerVehicle(void)
{
507 508 509 510 511 512 513 514
    if (!_managerVehicle->initialPlanRequestComplete() &&
            !_missionController.syncInProgress() &&
            !_geoFenceController.syncInProgress() &&
            !_rallyPointController.syncInProgress()) {
        // Something went wrong with initial load. All controllers are idle, so just force it off
        _managerVehicle->forceInitialPlanRequestComplete();
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
515 516 517 518 519 520 521
    // The crazy if structure is to handle the load propogating by itself through the system
    if (!_missionController.showPlanFromManagerVehicle()) {
        if (!_geoFenceController.showPlanFromManagerVehicle()) {
            _rallyPointController.showPlanFromManagerVehicle();
        }
    }
}