GeoFenceController.cc 13.1 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
/****************************************************************************
 *
 *   (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 "GeoFenceController.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
19 20
#include "ParameterManager.h"
#include "JsonHelper.h"
Don Gagne's avatar
Don Gagne committed
21 22

#ifndef __mobile__
23
#include "MainWindow.h"
24
#include "QGCFileDialog.h"
Don Gagne's avatar
Don Gagne committed
25
#endif
26 27

#include <QJsonDocument>
28 29 30

QGC_LOGGING_CATEGORY(GeoFenceControllerLog, "GeoFenceControllerLog")

31 32
const char* GeoFenceController::_jsonFileTypeValue =    "GeoFence";
const char* GeoFenceController::_jsonBreachReturnKey =  "breachReturn";
33

34 35 36 37
GeoFenceController::GeoFenceController(QObject* parent)
    : PlanElementController(parent)
    , _dirty(false)
{
38

39 40 41 42 43 44 45 46 47 48 49 50
}

GeoFenceController::~GeoFenceController()
{

}

void GeoFenceController::start(bool editMode)
{
    qCDebug(GeoFenceControllerLog) << "start editMode" << editMode;

    PlanElementController::start(editMode);
51 52 53 54 55 56
    _init();
}

void GeoFenceController::startStaticActiveVehicle(Vehicle* vehicle)
{
    qCDebug(GeoFenceControllerLog) << "startStaticActiveVehicle";
57

58 59 60 61 62 63
    PlanElementController::startStaticActiveVehicle(vehicle);
    _init();
}

void GeoFenceController::_init(void)
{
64
    connect(&_polygon, &QGCMapPolygon::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged);
65 66 67 68
}

void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint)
{
69 70 71
    if (_breachReturnPoint != breachReturnPoint) {
        _breachReturnPoint = breachReturnPoint;
        setDirty(true);
72 73 74 75
        emit breachReturnPointChanged(breachReturnPoint);
    }
}

76
void GeoFenceController::_signalAll(void)
77
{
78 79 80
    emit circleEnabledChanged(circleEnabled());
    emit polygonEnabledChanged(polygonEnabled());
    emit breachReturnEnabledChanged(breachReturnEnabled());
81
    emit breachReturnPointChanged(breachReturnPoint());
82 83 84
    emit circleRadiusChanged(circleRadius());
    emit paramsChanged(params());
    emit paramLabelsChanged(paramLabels());
85
    emit editorQmlChanged(editorQml());
86
    emit dirtyChanged(dirty());
87 88
}

89
void GeoFenceController::_activeVehicleBeingRemoved(void)
90
{
91
    _activeVehicle->geoFenceManager()->disconnect(this);
92 93 94 95
}

void GeoFenceController::_activeVehicleSet(void)
{
96
    GeoFenceManager* geoFenceManager = _activeVehicle->geoFenceManager();
97 98 99 100 101 102 103 104 105
    connect(geoFenceManager, &GeoFenceManager::polygonEnabledChanged,       this, &GeoFenceController::_setDirty);
    connect(geoFenceManager, &GeoFenceManager::circleEnabledChanged,        this, &GeoFenceController::circleEnabledChanged);
    connect(geoFenceManager, &GeoFenceManager::polygonEnabledChanged,       this, &GeoFenceController::polygonEnabledChanged);
    connect(geoFenceManager, &GeoFenceManager::breachReturnEnabledChanged,  this, &GeoFenceController::breachReturnEnabledChanged);
    connect(geoFenceManager, &GeoFenceManager::circleRadiusChanged,         this, &GeoFenceController::circleRadiusChanged);
    connect(geoFenceManager, &GeoFenceManager::paramsChanged,               this, &GeoFenceController::paramsChanged);
    connect(geoFenceManager, &GeoFenceManager::paramLabelsChanged,          this, &GeoFenceController::paramLabelsChanged);
    connect(geoFenceManager, &GeoFenceManager::loadComplete,                this, &GeoFenceController::_loadComplete);
    connect(geoFenceManager, &GeoFenceManager::inProgressChanged,           this, &GeoFenceController::syncInProgressChanged);
106

107 108 109
    if (!geoFenceManager->inProgress()) {
        _loadComplete(geoFenceManager->breachReturnPoint(), geoFenceManager->polygon());
    }
110

Don Gagne's avatar
Don Gagne committed
111
    _signalAll();
112
}
Don Gagne's avatar
Don Gagne committed
113

114 115 116 117
bool GeoFenceController::_loadJsonFile(QJsonDocument& jsonDoc, QString& errorString)
{
    QJsonObject json = jsonDoc.object();

Don Gagne's avatar
Don Gagne committed
118 119 120 121 122 123 124
    int fileVersion;
    if (!JsonHelper::validateQGCJsonFile(json,
                                         _jsonFileTypeValue,    // expected file type
                                         1,                     // minimum supported version
                                         1,                     // maximum supported version
                                         fileVersion,
                                         errorString)) {
125 126 127
        return false;
    }

128
    if (!_activeVehicle->parameterManager()->loadFromJson(json, false /* required */, errorString)) {
129 130 131
        return false;
    }

132
    if (breachReturnEnabled()) {
133
        if (json.contains(_jsonBreachReturnKey)
134
                && !JsonHelper::loadGeoCoordinate(json[_jsonBreachReturnKey], false /* altitudeRequired */, _breachReturnPoint, errorString)) {
135 136 137 138 139 140
            return false;
        }
    } else {
        _breachReturnPoint = QGeoCoordinate();
    }

141
    if (polygonEnabled()) {
142 143 144 145 146
        if (!_polygon.loadFromJson(json, false /* reauired */, errorString)) {
            return false;
        }
    } else {
        _polygon.clear();
147
    }
148
    _polygon.setDirty(false);
149 150

    return true;
151 152
}

153 154 155
#if 0
// NYI
bool GeoFenceController::_loadTextFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString)
156
{
157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188
    bool addPlannedHomePosition = false;

    QString firstLine = stream.readLine();
    const QStringList& version = firstLine.split(" ");

    bool versionOk = false;
    if (version.size() == 3 && version[0] == "QGC" && version[1] == "WPL") {
        if (version[2] == "110") {
            // ArduPilot file, planned home position is already in position 0
            versionOk = true;
        } else if (version[2] == "120") {
            // Old QGC file, no planned home position
            versionOk = true;
            addPlannedHomePosition = true;
        }
    }

    if (versionOk) {
        while (!stream.atEnd()) {
            SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this);

            if (item->load(stream)) {
                visualItems->append(item);
            } else {
                errorString = QStringLiteral("The mission file is corrupted.");
                return false;
            }
        }
    } else {
        errorString = QStringLiteral("The mission file is not compatible with this version of QGroundControl.");
        return false;
    }
189

190 191 192 193 194 195 196 197 198 199 200 201 202
    if (addPlannedHomePosition || visualItems->count() == 0) {
        _addPlannedHomePosition(visualItems, true /* addToCenter */);

        // Update sequence numbers in DO_JUMP commands to take into account added home position in index 0
        for (int i=1; i<visualItems->count(); i++) {
            SimpleMissionItem* item = qobject_cast<SimpleMissionItem*>(visualItems->get(i));
            if (item && item->command() == MavlinkQmlSingleton::MAV_CMD_DO_JUMP) {
                item->missionItem().setParam1((int)item->missionItem().param1() + 1);
            }
        }
    }

    return true;
203
}
204
#endif
205 206 207

void GeoFenceController::loadFromFile(const QString& filename)
{
208 209 210 211 212 213 214 215 216
    QString errorString;

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

    QFile file(filename);

    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
Don Gagne's avatar
Don Gagne committed
217
        errorString = file.errorString() + QStringLiteral(" ") + filename;
218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234
    } else {
        QJsonDocument   jsonDoc;
        QByteArray      bytes = file.readAll();

        if (JsonHelper::isJsonFile(bytes, jsonDoc)) {
            _loadJsonFile(jsonDoc, errorString);
        } else {
            // FIXME: No MP file format support
            qgcApp()->showMessage("GeoFence file is in incorrect format.");
            return;
        }
    }

    if (!errorString.isEmpty()) {
        qgcApp()->showMessage(errorString);
    }

235 236
    _signalAll();
    setDirty(true);
237 238
}

239
void GeoFenceController::loadFromFilePicker(void)
240
{
241
#ifndef __mobile__
242
    QString filename = QGCFileDialog::getOpenFileName(MainWindow::instance(), "Select GeoFence File to load", QString(), "Fence file (*.fence);;All Files (*.*)");
243

244 245 246 247 248
    if (filename.isEmpty()) {
        return;
    }
    loadFromFile(filename);
#endif
249 250 251 252
}

void GeoFenceController::saveToFile(const QString& filename)
{
253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269
    if (filename.isEmpty()) {
        return;
    }

    QString fenceFilename = filename;
    if (!QFileInfo(filename).fileName().contains(".")) {
        fenceFilename += QString(".%1").arg(QGCApplication::fenceFileExtension);
    }

    QFile file(fenceFilename);

    if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
        qgcApp()->showMessage(file.errorString());
    } else {
        QJsonObject fenceFileObject;    // top level json object

        fenceFileObject[JsonHelper::jsonFileTypeKey] =      _jsonFileTypeValue;
Don Gagne's avatar
Don Gagne committed
270
        fenceFileObject[JsonHelper::jsonVersionKey] =       1;
271 272 273
        fenceFileObject[JsonHelper::jsonGroundStationKey] = JsonHelper::jsonGroundStationValue;

        QStringList         paramNames;
274
        ParameterManager*   paramMgr = _activeVehicle->parameterManager();
275 276 277 278 279 280 281 282 283 284
        GeoFenceManager*    fenceMgr = _activeVehicle->geoFenceManager();
        QVariantList        params = fenceMgr->params();

        for (int i=0; i< params.count(); i++) {
            paramNames.append(params[i].value<Fact*>()->name());
        }
        if (paramNames.count() > 0) {
            paramMgr->saveToJson(paramMgr->defaultComponentId(), paramNames, fenceFileObject);
        }

285
        if (breachReturnEnabled()) {
286
            QJsonValue jsonBreachReturn;
287
            JsonHelper::saveGeoCoordinate(_breachReturnPoint, false /* writeAltitude */, jsonBreachReturn);
288 289 290
            fenceFileObject[_jsonBreachReturnKey] = jsonBreachReturn;
        }

291
        if (polygonEnabled()) {
292 293
            _polygon.saveToJson(fenceFileObject);
        }
294 295 296 297 298 299 300 301 302 303 304

        QJsonDocument saveDoc(fenceFileObject);
        file.write(saveDoc.toJson());
    }

    setDirty(false);
}

void GeoFenceController::saveToFilePicker(void)
{
#ifndef __mobile__
305
    QString filename = QGCFileDialog::getSaveFileName(MainWindow::instance(), "Select file to save GeoFence to", QString(), "Fence file (*.fence);;All Files (*.*)");
306 307 308 309 310 311

    if (filename.isEmpty()) {
        return;
    }
    saveToFile(filename);
#endif
312 313 314 315
}

void GeoFenceController::removeAll(void)
{
316 317
    setBreachReturnPoint(QGeoCoordinate());
    _polygon.clear();
318 319 320 321
}

void GeoFenceController::loadFromVehicle(void)
{
322
    if (_activeVehicle->parameterManager()->parametersReady() && !syncInProgress()) {
323
        _activeVehicle->geoFenceManager()->loadFromVehicle();
324
    } else {
325
        qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle->parameterManager()->parametersReady() << syncInProgress();
326 327 328 329 330
    }
}

void GeoFenceController::sendToVehicle(void)
{
331
    if (_activeVehicle->parameterManager()->parametersReady() && !syncInProgress()) {
332
        _activeVehicle->geoFenceManager()->sendToVehicle(_breachReturnPoint, _polygon.coordinateList());
Don Gagne's avatar
Don Gagne committed
333 334
        _polygon.setDirty(false);
        setDirty(false);
335
    } else {
336
        qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle->parameterManager()->parametersReady() << syncInProgress();
337 338 339 340 341
    }
}

bool GeoFenceController::syncInProgress(void) const
{
342
    return _activeVehicle->geoFenceManager()->inProgress();
343 344 345 346
}

bool GeoFenceController::dirty(void) const
{
347
    return _dirty | _polygon.dirty();
348 349 350 351 352 353 354 355
}


void GeoFenceController::setDirty(bool dirty)
{
    if (dirty != _dirty) {
        _dirty = dirty;
        if (!dirty) {
356
            _polygon.setDirty(dirty);
357 358 359 360 361 362 363 364 365 366 367
        }
        emit dirtyChanged(dirty);
    }
}

void GeoFenceController::_polygonDirtyChanged(bool dirty)
{
    if (dirty) {
        setDirty(true);
    }
}
368

369
bool GeoFenceController::circleEnabled(void) const
370
{
371
    return _activeVehicle->geoFenceManager()->circleEnabled();
372 373
}

374
bool GeoFenceController::polygonEnabled(void) const
375
{
376
    return _activeVehicle->geoFenceManager()->polygonEnabled();
377 378
}

379
bool GeoFenceController::breachReturnEnabled(void) const
380
{
381
    return _activeVehicle->geoFenceManager()->breachReturnEnabled();
382 383 384 385 386 387 388
}

void GeoFenceController::_setDirty(void)
{
    setDirty(true);
}

389
void GeoFenceController::_setPolygonFromManager(const QList<QGeoCoordinate>& polygon)
390 391
{
    _polygon.setPath(polygon);
392 393 394 395 396 397 398 399
    _polygon.setDirty(false);
    emit polygonPathChanged(_polygon.path());
}

void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnPoint)
{
    _breachReturnPoint = breachReturnPoint;
    emit breachReturnPointChanged(_breachReturnPoint);
400 401 402 403
}

float GeoFenceController::circleRadius(void) const
{
404
    return _activeVehicle->geoFenceManager()->circleRadius();
405 406 407 408
}

QVariantList GeoFenceController::params(void) const
{
409
    return _activeVehicle->geoFenceManager()->params();
410 411 412 413
}

QStringList GeoFenceController::paramLabels(void) const
{
414
    return _activeVehicle->geoFenceManager()->paramLabels();
415
}
416 417 418

QString GeoFenceController::editorQml(void) const
{
419
    return _activeVehicle->geoFenceManager()->editorQml();
420
}
421 422 423 424 425 426

void GeoFenceController::_loadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon)
{
    _setReturnPointFromManager(breachReturn);
    _setPolygonFromManager(polygon);
    setDirty(false);
Don Gagne's avatar
Don Gagne committed
427
    emit loadComplete();
428
}
429 430 431 432 433

QString GeoFenceController::fileExtension(void) const
{
    return QGCApplication::fenceFileExtension;
}