CorridorScanComplexItem.cc 22.2 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
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 19 20 21 22 23
 *
 * 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

DonLakeFlyer's avatar
DonLakeFlyer committed
54 55
    connect(&_corridorPolyline,     &QGCMapPolyline::isValidChanged,this, &CorridorScanComplexItem::readyForSaveStateChanged);

56 57 58 59 60
    if (!kmlFile.isEmpty()) {
        _corridorPolyline.loadKMLFile(kmlFile);
        _corridorPolyline.setDirty(false);
    }
    setDirty(false);
61 62
}

63
void CorridorScanComplexItem::save(QJsonArray&  planItems)
64 65 66
{
    QJsonObject saveObject;

Don Gagne's avatar
Don Gagne committed
67
    TransectStyleComplexItem::_save(saveObject);
68 69

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

    _corridorPolyline.saveToJson(saveObject);

77
    planItems.append(saveObject);
78 79 80 81
}

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

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

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

    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);
107
        _ignoreRecalc = false;
108 109 110 111
        return false;
    }

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

    setSequenceNumber(sequenceNumber);

Don Gagne's avatar
Don Gagne committed
120
    if (!_load(complexObject, false /* forPresets */, errorString)) {
121
        _ignoreRecalc = false;
122 123 124
        return false;
    }

125 126
    _corridorWidthFact.setRawValue      (complexObject[corridorWidthName].toDouble());

127
    _entryPoint = complexObject[_jsonEntryPointKey].toInt();
128

129 130
    _ignoreRecalc = false;

131 132 133 134 135 136
    _recalcComplexDistance();
    if (_cameraShots == 0) {
        // Shot count was possibly not available from plan file
        _recalcCameraShots();
    }

137 138 139 140 141 142 143 144 145 146 147
    return true;
}

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

int CorridorScanComplexItem::_transectCount(void) const
{
    double fullWidth = _corridorWidthFact.rawValue().toDouble();
148
    return fullWidth > 0.0 ? qCeil(fullWidth / _transectSpacing()) : 1;
149 150
}

151 152 153 154 155 156
void CorridorScanComplexItem::_appendLoadedMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
    qCDebug(CorridorScanComplexItemLog) << "_appendLoadedMissionItems";

    int seqNum = _sequenceNumber;

157
    for (const MissionItem* loadedMissionItem: _loadedMissionItems) {
158 159 160 161 162 163 164
        MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent);
        item->setSequenceNumber(seqNum++);
        items.append(item);
    }
}

void CorridorScanComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
165
{
166 167
    qCDebug(CorridorScanComplexItemLog) << "_buildAndAppendMissionItems";

168 169 170 171 172 173
    // Now build the mission items from the transect points

    MissionItem* item;
    int seqNum =                    _sequenceNumber;
    bool imagesEverywhere =         _cameraTriggerInTurnAroundFact.rawValue().toBool();
    bool addTriggerAtBeginning =    imagesEverywhere;
174
    bool firstOverallPoint =        true;
175 176 177

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

178
    //qDebug() << "_buildAndAppendMissionItems";
179
    for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
180
        bool transectEntry = true;
181 182

        //qDebug() << "start transect";
183
        for (const CoordInfo_t& transectCoordInfo: transect) {
184
            //qDebug() << transectCoordInfo.coordType;
185

186
            item = new MissionItem(seqNum++,
187 188 189 190 191 192 193 194 195 196 197
                                   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
198
                                   missionItemParent);
199 200
            items.append(item);

201
            if (triggerCamera() && firstOverallPoint && addTriggerAtBeginning) {
202
                // Start triggering
203
                addTriggerAtBeginning = false;
204 205 206 207 208 209 210 211 212 213 214 215
                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);
            }
216 217
            firstOverallPoint = false;

218
            // Possibly add trigger start/stop to survey area entrance/exit
Don Gagne's avatar
Don Gagne committed
219 220
            if (triggerCamera() && transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge) {
                if (transectEntry) {
221 222 223
                    // Start of transect, always start triggering. We do this even if we are taking images everywhere.
                    // This allows a restart of the mission in mid-air without losing images from the entire mission.
                    // At most you may lose part of a transect.
224 225 226 227 228 229 230 231 232 233 234
                    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);
235 236
                    transectEntry = false;
                } else if (!imagesEverywhere && !transectEntry){
237 238 239 240 241 242 243 244 245 246 247 248 249 250
                    // 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);
                }
            }
251
        }
252
    }
253 254

    if (imagesEverywhere) {
255
        // Stop triggering
256 257 258 259 260 261 262 263 264 265 266 267
        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);
    }
268 269
}

270 271 272 273 274 275 276 277 278 279 280
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);
    }
}

281 282
void CorridorScanComplexItem::applyNewAltitude(double newAltitude)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
283 284 285
    _cameraCalc.valueSetIsDistance()->setRawValue(true);
    _cameraCalc.distanceToSurface()->setRawValue(newAltitude);
    _cameraCalc.setDistanceToSurfaceRelative(true);
286 287 288 289 290 291 292 293 294 295 296
}

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

void CorridorScanComplexItem::rotateEntryPoint(void)
{
297 298 299
    _entryPoint++;
    if (_entryPoint > 3) {
        _entryPoint = 0;
300
    }
301

302
    _rebuildTransects();
303 304 305 306 307
}

void CorridorScanComplexItem::_rebuildCorridorPolygon(void)
{
    if (_corridorPolyline.count() < 2) {
308
        _surveyAreaPolygon.clear();
309 310 311 312 313 314 315 316
        return;
    }

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

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

317
    _surveyAreaPolygon.clear();
DonLakeFlyer's avatar
DonLakeFlyer committed
318 319

    QList<QGeoCoordinate> rgCoord;
320
    for (const QGeoCoordinate& vertex: firstSideVertices) {
DonLakeFlyer's avatar
DonLakeFlyer committed
321
        rgCoord.append(vertex);
322 323
    }
    for (int i=secondSideVertices.count() - 1; i >= 0; i--) {
DonLakeFlyer's avatar
DonLakeFlyer committed
324
        rgCoord.append(secondSideVertices[i]);
325
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
326
    _surveyAreaPolygon.appendVertices(rgCoord);
327 328
}

329
void CorridorScanComplexItem::_rebuildTransectsPhase1(void)
330
{
331 332 333 334 335 336 337 338
    if (_ignoreRecalc) {
        return;
    }

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

342
    _transects.clear();
343
    _transectsPathHeightInfo.clear();
344

345
    double transectSpacing = _transectSpacing();
346 347 348 349 350
    double fullWidth = _corridorWidthFact.rawValue().toDouble();
    double halfWidth = fullWidth / 2.0;
    int transectCount = _transectCount();
    double normalizedTransectPosition = transectSpacing / 2.0;

351 352
    if (_corridorPolyline.count() >= 2) {
        // First build up the transects all going the same direction
353
        //qDebug() << "_rebuildTransectsPhase1";
354
        for (int i=0; i<transectCount; i++) {
355
            //qDebug() << "start transect";
356 357 358 359 360 361 362 363 364
            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;
            }

365 366 367 368 369 370 371 372 373 374 375 376 377
            // 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
378
            if (_hasTurnaround()) {
379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396
                 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";
397
            for (const TransectStyleComplexItem::CoordInfo_t& coordInfo: transect) {
398
                qDebug() << coordInfo.coordType;
399
            }
400
#endif
401

402
            _transects.append(transect);
403
            normalizedTransectPosition += transectSpacing;
404 405
        }

406 407 408 409 410 411 412 413 414 415 416
        // 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;
417
            reverseVertices = false;
418 419 420 421 422 423 424 425 426 427 428
            break;
        case 1:
            reverseTransects = true;
            reverseVertices = false;
            break;
        case 2:
            reverseTransects = false;
            reverseVertices = true;
            break;
        case 3:
            reverseTransects = true;
429
            reverseVertices = true;
430
            break;
431
        }
432
        if (reverseTransects) {
433
            QList<QList<TransectStyleComplexItem::CoordInfo_t>> reversedTransects;
434
            for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
435 436
                reversedTransects.prepend(transect);
            }
437
            _transects = reversedTransects;
438 439
        }
        if (reverseVertices) {
440 441
            for (int i=0; i<_transects.count(); i++) {
                QList<TransectStyleComplexItem::CoordInfo_t> reversedVertices;
442
                for (const TransectStyleComplexItem::CoordInfo_t& vertex: _transects[i]) {
443 444
                    reversedVertices.prepend(vertex);
                }
445
                _transects[i] = reversedVertices;
446
            }
447 448
        }

449
        // Adjust to lawnmower pattern
450
        reverseVertices = false;
451
        for (int i=0; i<_transects.count(); i++) {
452
            // We must reverse the vertices for every other transect in order to make a lawnmower pattern
453
            QList<TransectStyleComplexItem::CoordInfo_t> transectVertices = _transects[i];
454 455
            if (reverseVertices) {
                reverseVertices = false;
456
                QList<TransectStyleComplexItem::CoordInfo_t> reversedVertices;
457 458 459 460 461 462 463
                for (int j=transectVertices.count()-1; j>=0; j--) {
                    reversedVertices.append(transectVertices[j]);
                }
                transectVertices = reversedVertices;
            } else {
                reverseVertices = true;
            }
464
            _transects[i] = transectVertices;
465
        }
466
    }
467
}
468

469
void CorridorScanComplexItem::_recalcComplexDistance(void)
470
{
471
    _complexDistance = 0;
472
    for (int i=0; i<_visualTransectPoints.count() - 1; i++) {
473
        _complexDistance += _visualTransectPoints[i].value<QGeoCoordinate>().distanceTo(_visualTransectPoints[i+1].value<QGeoCoordinate>());
474
    }
475 476
    emit complexDistanceChanged();
}
477

478 479
void CorridorScanComplexItem::_recalcCameraShots(void)
{
480 481 482
    double triggerDistance = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble();
    if (triggerDistance == 0) {
        _cameraShots = 0;
483
    } else {
484 485 486 487 488 489
        if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) {
            _cameraShots = qCeil(_complexDistance / triggerDistance);
        } else {
            int singleTransectImageCount = qCeil(_corridorPolyline.length() / triggerDistance);
            _cameraShots = singleTransectImageCount * _transectCount();
        }
490 491 492 493
    }
    emit cameraShotsChanged();
}

DonLakeFlyer's avatar
DonLakeFlyer committed
494
CorridorScanComplexItem::ReadyForSaveState CorridorScanComplexItem::readyForSaveState(void) const
495
{
DonLakeFlyer's avatar
DonLakeFlyer committed
496
    return TransectStyleComplexItem::readyForSaveState();
497
}
498 499 500

double CorridorScanComplexItem::timeBetweenShots(void)
{
501
    return _cruiseSpeed == 0 ? 0 : _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / _cruiseSpeed;
502
}
503 504 505 506 507 508 509 510 511 512 513 514 515

double CorridorScanComplexItem::_transectSpacing(void) const
{
    double transectSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble();
    if (transectSpacing < 0.5) {
        // We can't let spacing get too small otherwise we will end up with too many transects.
        // So we limit to 0.5 meter spacing as min and set to huge value which will cause a single
        // transect to be added.
        transectSpacing = 100000;
    }

    return transectSpacing;
}