CorridorScanComplexItem.cc 21.5 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 23
/****************************************************************************
 *
 *   (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 "CorridorScanComplexItem.h"
#include "JsonHelper.h"
#include "MissionController.h"
#include "QGCGeo.h"
#include "QGroundControlQmlGlobal.h"
#include "QGCQGeoCoordinate.h"
#include "SettingsManager.h"
#include "AppSettings.h"
#include "QGCQGeoCoordinate.h"

#include <QPolygonF>

QGC_LOGGING_CATEGORY(CorridorScanComplexItemLog, "CorridorScanComplexItemLog")

24 25
const char* CorridorScanComplexItem::settingsGroup =            "CorridorScan";
const char* CorridorScanComplexItem::corridorWidthName =        "CorridorWidth";
26
const char* CorridorScanComplexItem::_jsonEntryPointKey =       "EntryPoint";
27

28
const char* CorridorScanComplexItem::jsonComplexItemTypeValue = "CorridorScan";
29

30
CorridorScanComplexItem::CorridorScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlFile, QObject* parent)
31
    : TransectStyleComplexItem  (vehicle, flyView, settingsGroup, parent)
32 33 34
    , _entryPoint               (0)
    , _metaDataMap              (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CorridorScan.SettingsGroup.json"), this))
    , _corridorWidthFact        (settingsGroup, _metaDataMap[corridorWidthName])
35 36 37
{
    _editorQml = "qrc:/qml/CorridorScanEditor.qml";

DonLakeFlyer's avatar
DonLakeFlyer committed
38 39 40 41 42
    // We override the altitude to the mission default
    if (_cameraCalc.isManualCamera() || !_cameraCalc.valueSetIsDistance()->rawValue().toBool()) {
        _cameraCalc.distanceToSurface()->setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue());
    }

43 44 45
    connect(&_corridorWidthFact,    &Fact::valueChanged,                                this, &CorridorScanComplexItem::_setDirty);
    connect(&_corridorPolyline,     &QGCMapPolyline::pathChanged,                       this, &CorridorScanComplexItem::_setDirty);

46 47
    connect(&_cameraCalc,           &CameraCalc::distanceToSurfaceRelativeChanged, this, &CorridorScanComplexItem::coordinateHasRelativeAltitudeChanged);
    connect(&_cameraCalc,           &CameraCalc::distanceToSurfaceRelativeChanged, this, &CorridorScanComplexItem::exitCoordinateHasRelativeAltitudeChanged);
48

49
    connect(&_corridorPolyline,     &QGCMapPolyline::dirtyChanged,  this, &CorridorScanComplexItem::_polylineDirtyChanged);
50

51 52
    connect(&_corridorPolyline,     &QGCMapPolyline::pathChanged,   this, &CorridorScanComplexItem::_rebuildCorridorPolygon);
    connect(&_corridorWidthFact,    &Fact::valueChanged,            this, &CorridorScanComplexItem::_rebuildCorridorPolygon);
53 54 55 56 57 58

    if (!kmlFile.isEmpty()) {
        _corridorPolyline.loadKMLFile(kmlFile);
        _corridorPolyline.setDirty(false);
    }
    setDirty(false);
59 60
}

61
void CorridorScanComplexItem::save(QJsonArray&  planItems)
62 63 64
{
    QJsonObject saveObject;

65 66 67
    _save(saveObject);

    saveObject[JsonHelper::jsonVersionKey] =                    2;
68 69
    saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
    saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;
70
    saveObject[corridorWidthName] =                             _corridorWidthFact.rawValue().toDouble();
71
    saveObject[_jsonEntryPointKey] =                            _entryPoint;
72 73 74

    _corridorPolyline.saveToJson(saveObject);

75
    planItems.append(saveObject);
76 77 78 79
}

bool CorridorScanComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
{
80 81 82
    // We don't recalc while loading since all the information we need is specified in the file
    _ignoreRecalc = true;

83 84 85 86
    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
        { JsonHelper::jsonVersionKey,                   QJsonValue::Double, true },
        { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
        { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
87
        { corridorWidthName,                            QJsonValue::Double, true },
88
        { _jsonEntryPointKey,                           QJsonValue::Double, true },
89 90 91
        { QGCMapPolyline::jsonPolylineKey,              QJsonValue::Array,  true },
    };
    if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
92
        _ignoreRecalc = false;
93 94 95
        return false;
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
96
    if (!_corridorPolyline.loadFromJson(complexObject, true, errorString)) {
97
        _ignoreRecalc = false;
DonLakeFlyer's avatar
DonLakeFlyer committed
98 99
        return false;
    }
100 101 102 103 104

    QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
    QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
    if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) {
        errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
105
        _ignoreRecalc = false;
106 107 108 109
        return false;
    }

    int version = complexObject[JsonHelper::jsonVersionKey].toInt();
110
    if (version != 2) {
111
        errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version);
112
        _ignoreRecalc = false;
113 114 115 116 117
        return false;
    }

    setSequenceNumber(sequenceNumber);

118
    if (!_load(complexObject, errorString)) {
119
        _ignoreRecalc = false;
120 121 122
        return false;
    }

123 124
    _corridorWidthFact.setRawValue      (complexObject[corridorWidthName].toDouble());

125
    _entryPoint = complexObject[_jsonEntryPointKey].toInt();
126

127
    _rebuildTransects();
128

129 130
    _ignoreRecalc = false;

131 132 133 134 135 136 137 138 139 140 141 142 143 144 145
    return true;
}

bool CorridorScanComplexItem::specifiesCoordinate(void) const
{
    return _corridorPolyline.count() > 1;
}

int CorridorScanComplexItem::_transectCount(void) const
{
    double transectSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble();
    double fullWidth = _corridorWidthFact.rawValue().toDouble();
    return fullWidth > 0.0 ? qCeil(fullWidth / transectSpacing) : 1;
}

146 147 148 149 150 151
void CorridorScanComplexItem::_appendLoadedMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
    qCDebug(CorridorScanComplexItemLog) << "_appendLoadedMissionItems";

    int seqNum = _sequenceNumber;

152
    for (const MissionItem* loadedMissionItem: _loadedMissionItems) {
153 154 155 156 157 158 159
        MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent);
        item->setSequenceNumber(seqNum++);
        items.append(item);
    }
}

void CorridorScanComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
160
{
161 162
    qCDebug(CorridorScanComplexItemLog) << "_buildAndAppendMissionItems";

163 164 165 166 167 168
    // Now build the mission items from the transect points

    MissionItem* item;
    int seqNum =                    _sequenceNumber;
    bool imagesEverywhere =         _cameraTriggerInTurnAroundFact.rawValue().toBool();
    bool addTriggerAtBeginning =    imagesEverywhere;
169
    bool firstOverallPoint =        true;
170 171 172

    MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT;

173
    //qDebug() << "_buildAndAppendMissionItems";
174
    for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
175 176 177
        bool entryPoint = true;

        //qDebug() << "start transect";
178
        for (const CoordInfo_t& transectCoordInfo: transect) {
179
            //qDebug() << transectCoordInfo.coordType;
180

181
            item = new MissionItem(seqNum++,
182 183 184 185 186 187 188 189 190 191 192
                                   MAV_CMD_NAV_WAYPOINT,
                                   mavFrame,
                                   0,                                          // No hold time
                                   0.0,                                        // No acceptance radius specified
                                   0.0,                                        // Pass through waypoint
                                   std::numeric_limits<double>::quiet_NaN(),   // Yaw unchanged
                                   transectCoordInfo.coord.latitude(),
                                   transectCoordInfo.coord.longitude(),
                                   transectCoordInfo.coord.altitude(),
                                   true,                                       // autoContinue
                                   false,                                      // isCurrentItem
193
                                   missionItemParent);
194 195
            items.append(item);

196
            if (firstOverallPoint && addTriggerAtBeginning) {
197
                // Start triggering
198
                addTriggerAtBeginning = false;
199 200 201 202 203 204 205 206 207 208 209 210
                item = new MissionItem(seqNum++,
                                       MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                       MAV_FRAME_MISSION,
                                       _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(),   // trigger distance
                                       0,                                                               // shutter integration (ignore)
                                       1,                                                               // trigger immediately when starting
                                       0, 0, 0, 0,                                                      // param 4-7 unused
                                       true,                                                            // autoContinue
                                       false,                                                           // isCurrentItem
                                       missionItemParent);
                items.append(item);
            }
211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242
            firstOverallPoint = false;

            if (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge && !imagesEverywhere) {
                if (entryPoint) {
                    // Start of transect, start triggering
                    item = new MissionItem(seqNum++,
                                           MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                           MAV_FRAME_MISSION,
                                           _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(),   // trigger distance
                                           0,                                                               // shutter integration (ignore)
                                           1,                                                               // trigger immediately when starting
                                           0, 0, 0, 0,                                                      // param 4-7 unused
                                           true,                                                            // autoContinue
                                           false,                                                           // isCurrentItem
                                           missionItemParent);
                    items.append(item);
                } else {
                    // End of transect, stop triggering
                    item = new MissionItem(seqNum++,
                                           MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                           MAV_FRAME_MISSION,
                                           0,           // stop triggering
                                           0,           // shutter integration (ignore)
                                           0,           // trigger immediately when starting
                                           0, 0, 0, 0,  // param 4-7 unused
                                           true,        // autoContinue
                                           false,       // isCurrentItem
                                           missionItemParent);
                    items.append(item);
                }
                entryPoint = !entryPoint;
            }
243
        }
244
    }
245 246

    if (imagesEverywhere) {
247
        // Stop triggering
248 249 250 251 252 253 254 255 256 257 258 259
        MissionItem* item = new MissionItem(seqNum++,
                                            MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                            MAV_FRAME_MISSION,
                                            0,           // stop triggering
                                            0,           // shutter integration (ignore)
                                            0,           // trigger immediately when starting
                                            0, 0, 0, 0,  // param 4-7 unused
                                            true,        // autoContinue
                                            false,       // isCurrentItem
                                            missionItemParent);
        items.append(item);
    }
260 261
}

262 263 264 265 266 267 268 269 270 271 272
void CorridorScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
    if (_loadedMissionItems.count()) {
        // We have mission items from the loaded plan, use those
        _appendLoadedMissionItems(items, missionItemParent);
    } else {
        // Build the mission items on the fly
        _buildAndAppendMissionItems(items, missionItemParent);
    }
}

273 274
void CorridorScanComplexItem::applyNewAltitude(double newAltitude)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
275 276 277
    _cameraCalc.valueSetIsDistance()->setRawValue(true);
    _cameraCalc.distanceToSurface()->setRawValue(newAltitude);
    _cameraCalc.setDistanceToSurfaceRelative(true);
278 279 280 281 282 283 284 285 286 287 288
}

void CorridorScanComplexItem::_polylineDirtyChanged(bool dirty)
{
    if (dirty) {
        setDirty(true);
    }
}

void CorridorScanComplexItem::rotateEntryPoint(void)
{
289 290 291
    _entryPoint++;
    if (_entryPoint > 3) {
        _entryPoint = 0;
292
    }
293

294
    _rebuildTransects();
295 296 297 298 299
}

void CorridorScanComplexItem::_rebuildCorridorPolygon(void)
{
    if (_corridorPolyline.count() < 2) {
300
        _surveyAreaPolygon.clear();
301 302 303 304 305 306 307 308
        return;
    }

    double halfWidth = _corridorWidthFact.rawValue().toDouble() / 2.0;

    QList<QGeoCoordinate> firstSideVertices = _corridorPolyline.offsetPolyline(halfWidth);
    QList<QGeoCoordinate> secondSideVertices = _corridorPolyline.offsetPolyline(-halfWidth);

309
    _surveyAreaPolygon.clear();
DonLakeFlyer's avatar
DonLakeFlyer committed
310 311

    QList<QGeoCoordinate> rgCoord;
312
    for (const QGeoCoordinate& vertex: firstSideVertices) {
DonLakeFlyer's avatar
DonLakeFlyer committed
313
        rgCoord.append(vertex);
314 315
    }
    for (int i=secondSideVertices.count() - 1; i >= 0; i--) {
DonLakeFlyer's avatar
DonLakeFlyer committed
316
        rgCoord.append(secondSideVertices[i]);
317
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
318
    _surveyAreaPolygon.appendVertices(rgCoord);
319 320
}

321
void CorridorScanComplexItem::_rebuildTransectsPhase1(void)
322
{
323 324 325 326 327 328 329 330 331 332 333
    if (_ignoreRecalc) {
        return;
    }

    // If the transects are getting rebuilt then any previsouly loaded mission items are now invalid
    if (_loadedMissionItemsParent) {
        _loadedMissionItems.clear();
        _loadedMissionItemsParent->deleteLater();
        _loadedMissionItemsParent = NULL;
    }

334
    _transects.clear();
335
    _transectsPathHeightInfo.clear();
336 337 338 339 340 341 342

    double transectSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble();
    double fullWidth = _corridorWidthFact.rawValue().toDouble();
    double halfWidth = fullWidth / 2.0;
    int transectCount = _transectCount();
    double normalizedTransectPosition = transectSpacing / 2.0;

343 344
    if (_corridorPolyline.count() >= 2) {
        // First build up the transects all going the same direction
345
        //qDebug() << "_rebuildTransectsPhase1";
346
        for (int i=0; i<transectCount; i++) {
347
            //qDebug() << "start transect";
348 349 350 351 352 353 354 355 356
            double offsetDistance;
            if (transectCount == 1) {
                // Single transect is flown over scan line
                offsetDistance = 0;
            } else {
                // Convert from normalized to absolute transect offset distance
                offsetDistance = halfWidth - normalizedTransectPosition;
            }

357 358 359 360 361 362 363 364 365 366 367 368 369
            // Turn transect into CoordInfo transect
            QList<TransectStyleComplexItem::CoordInfo_t> transect;
            QList<QGeoCoordinate> transectCoords = _corridorPolyline.offsetPolyline(offsetDistance);
            for (int j=1; j<transectCoords.count() - 1; j++) {
                TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords[j], CoordTypeInterior };
                transect.append(coordInfo);
            }
            TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords.first(), CoordTypeSurveyEdge };
            transect.prepend(coordInfo);
            coordInfo = { transectCoords.last(), CoordTypeSurveyEdge };
            transect.append(coordInfo);

            // Extend the transect ends for turnaround
370
            if (_hasTurnaround()) {
371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388
                 QGeoCoordinate turnaroundCoord;
                 double turnAroundDistance = _turnAroundDistanceFact.rawValue().toDouble();

                 double azimuth = transectCoords[0].azimuthTo(transectCoords[1]);
                 turnaroundCoord = transectCoords[0].atDistanceAndAzimuth(-turnAroundDistance, azimuth);
                 turnaroundCoord.setAltitude(qQNaN());
                 TransectStyleComplexItem::CoordInfo_t coordInfo = { turnaroundCoord, CoordTypeTurnaround };
                 transect.prepend(coordInfo);

                 azimuth = transectCoords.last().azimuthTo(transectCoords[transectCoords.count() - 2]);
                 turnaroundCoord = transectCoords.last().atDistanceAndAzimuth(-turnAroundDistance, azimuth);
                 turnaroundCoord.setAltitude(qQNaN());
                 coordInfo = { turnaroundCoord, CoordTypeTurnaround };
                 transect.append(coordInfo);
            }

#if 0
            qDebug() << "transect debug";
389
            for (const TransectStyleComplexItem::CoordInfo_t& coordInfo: transect) {
390
                qDebug() << coordInfo.coordType;
391
            }
392
#endif
393

394
            _transects.append(transect);
395
            normalizedTransectPosition += transectSpacing;
396 397
        }

398 399 400 401 402 403 404 405 406 407 408
        // Now deal with fixing up the entry point:
        //  0: Leave alone
        //  1: Start at same end, opposite side of center
        //  2: Start at opposite end, same side
        //  3: Start at opposite end, opposite side

        bool reverseTransects = false;
        bool reverseVertices = false;
        switch (_entryPoint) {
        case 0:
            reverseTransects = false;
409
            reverseVertices = false;
410 411 412 413 414 415 416 417 418 419 420
            break;
        case 1:
            reverseTransects = true;
            reverseVertices = false;
            break;
        case 2:
            reverseTransects = false;
            reverseVertices = true;
            break;
        case 3:
            reverseTransects = true;
421
            reverseVertices = true;
422
            break;
423
        }
424
        if (reverseTransects) {
425
            QList<QList<TransectStyleComplexItem::CoordInfo_t>> reversedTransects;
426
            for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
427 428
                reversedTransects.prepend(transect);
            }
429
            _transects = reversedTransects;
430 431
        }
        if (reverseVertices) {
432 433
            for (int i=0; i<_transects.count(); i++) {
                QList<TransectStyleComplexItem::CoordInfo_t> reversedVertices;
434
                for (const TransectStyleComplexItem::CoordInfo_t& vertex: _transects[i]) {
435 436
                    reversedVertices.prepend(vertex);
                }
437
                _transects[i] = reversedVertices;
438
            }
439 440
        }

441
        // Adjust to lawnmower pattern
442
        reverseVertices = false;
443
        for (int i=0; i<_transects.count(); i++) {
444
            // We must reverse the vertices for every other transect in order to make a lawnmower pattern
445
            QList<TransectStyleComplexItem::CoordInfo_t> transectVertices = _transects[i];
446 447
            if (reverseVertices) {
                reverseVertices = false;
448
                QList<TransectStyleComplexItem::CoordInfo_t> reversedVertices;
449 450 451 452 453 454 455
                for (int j=transectVertices.count()-1; j>=0; j--) {
                    reversedVertices.append(transectVertices[j]);
                }
                transectVertices = reversedVertices;
            } else {
                reverseVertices = true;
            }
456
            _transects[i] = transectVertices;
457
        }
458
    }
459
}
460

461 462
void CorridorScanComplexItem::_rebuildTransectsPhase2(void)
{
463 464
    // Calculate distance flown for complex item
    _complexDistance = 0;
465
    for (int i=0; i<_visualTransectPoints.count() - 1; i++) {
466
        _complexDistance += _visualTransectPoints[i].value<QGeoCoordinate>().distanceTo(_visualTransectPoints[i+1].value<QGeoCoordinate>());
467 468
    }

469 470 471
    double triggerDistance = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble();
    if (triggerDistance == 0) {
        _cameraShots = 0;
472
    } else {
473 474 475 476 477 478
        if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) {
            _cameraShots = qCeil(_complexDistance / triggerDistance);
        } else {
            int singleTransectImageCount = qCeil(_corridorPolyline.length() / triggerDistance);
            _cameraShots = singleTransectImageCount * _transectCount();
        }
479 480
    }

481 482
    _coordinate = _visualTransectPoints.count() ? _visualTransectPoints.first().value<QGeoCoordinate>() : QGeoCoordinate();
    _exitCoordinate = _visualTransectPoints.count() ? _visualTransectPoints.last().value<QGeoCoordinate>() : QGeoCoordinate();
483 484

    emit cameraShotsChanged();
485
    emit complexDistanceChanged();
486 487 488 489
    emit coordinateChanged(_coordinate);
    emit exitCoordinateChanged(_exitCoordinate);
}

490 491 492 493
bool CorridorScanComplexItem::readyForSave(void) const
{
    return TransectStyleComplexItem::readyForSave();
}
494 495 496

double CorridorScanComplexItem::timeBetweenShots(void)
{
497
    return _cruiseSpeed == 0 ? 0 : _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / _cruiseSpeed;
498
}