CorridorScanComplexItem.cc 21.1 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 31
CorridorScanComplexItem::CorridorScanComplexItem(Vehicle* vehicle, bool flyView, QObject* parent)
    : 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
void CorridorScanComplexItem::save(QJsonArray&  planItems)
56 57 58
{
    QJsonObject saveObject;

59 60 61
    _save(saveObject);

    saveObject[JsonHelper::jsonVersionKey] =                    2;
62 63
    saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
    saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;
64
    saveObject[corridorWidthName] =                             _corridorWidthFact.rawValue().toDouble();
65
    saveObject[_jsonEntryPointKey] =                            _entryPoint;
66 67 68

    _corridorPolyline.saveToJson(saveObject);

69
    planItems.append(saveObject);
70 71 72 73
}

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

77 78 79 80
    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
        { JsonHelper::jsonVersionKey,                   QJsonValue::Double, true },
        { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
        { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
81
        { corridorWidthName,                            QJsonValue::Double, true },
82
        { _jsonEntryPointKey,                           QJsonValue::Double, true },
83 84 85
        { QGCMapPolyline::jsonPolylineKey,              QJsonValue::Array,  true },
    };
    if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
86
        _ignoreRecalc = false;
87 88 89
        return false;
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
90
    if (!_corridorPolyline.loadFromJson(complexObject, true, errorString)) {
91
        _ignoreRecalc = false;
DonLakeFlyer's avatar
DonLakeFlyer committed
92 93
        return false;
    }
94 95 96 97 98

    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);
99
        _ignoreRecalc = false;
100 101 102 103
        return false;
    }

    int version = complexObject[JsonHelper::jsonVersionKey].toInt();
104
    if (version != 2) {
105
        errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version);
106
        _ignoreRecalc = false;
107 108 109 110 111
        return false;
    }

    setSequenceNumber(sequenceNumber);

112
    if (!_load(complexObject, errorString)) {
113
        _ignoreRecalc = false;
114 115 116
        return false;
    }

117 118
    _corridorWidthFact.setRawValue      (complexObject[corridorWidthName].toDouble());

119
    _entryPoint = complexObject[_jsonEntryPointKey].toInt();
120

121
    _rebuildTransects();
122

123 124
    _ignoreRecalc = false;

125 126 127 128 129 130 131 132 133 134 135 136 137 138 139
    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;
}

140 141 142 143 144 145 146 147 148 149 150 151 152 153
void CorridorScanComplexItem::_appendLoadedMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
    qCDebug(CorridorScanComplexItemLog) << "_appendLoadedMissionItems";

    int seqNum = _sequenceNumber;

    foreach (const MissionItem* loadedMissionItem, _loadedMissionItems) {
        MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent);
        item->setSequenceNumber(seqNum++);
        items.append(item);
    }
}

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

157 158 159 160 161 162
    // Now build the mission items from the transect points

    MissionItem* item;
    int seqNum =                    _sequenceNumber;
    bool imagesEverywhere =         _cameraTriggerInTurnAroundFact.rawValue().toBool();
    bool addTriggerAtBeginning =    imagesEverywhere;
163
    bool firstOverallPoint =        true;
164 165 166

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

167 168 169 170 171 172 173
    //qDebug() << "_buildAndAppendMissionItems";
    foreach (const QList<TransectStyleComplexItem::CoordInfo_t>& transect, _transects) {
        bool entryPoint = true;

        //qDebug() << "start transect";
        foreach (const CoordInfo_t& transectCoordInfo, transect) {
            //qDebug() << transectCoordInfo.coordType;
174

175
            item = new MissionItem(seqNum++,
176 177 178 179 180 181 182 183 184 185 186
                                   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
187
                                   missionItemParent);
188 189
            items.append(item);

190
            if (firstOverallPoint && addTriggerAtBeginning) {
191
                // Start triggering
192
                addTriggerAtBeginning = false;
193 194 195 196 197 198 199 200 201 202 203 204
                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);
            }
205 206 207 208 209 210 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
            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;
            }
237
        }
238
    }
239 240

    if (imagesEverywhere) {
241
        // Stop triggering
242 243 244 245 246 247 248 249 250 251 252 253
        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);
    }
254 255
}

256 257 258 259 260 261 262 263 264 265 266
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);
    }
}

267 268
void CorridorScanComplexItem::applyNewAltitude(double newAltitude)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
269 270 271
    _cameraCalc.valueSetIsDistance()->setRawValue(true);
    _cameraCalc.distanceToSurface()->setRawValue(newAltitude);
    _cameraCalc.setDistanceToSurfaceRelative(true);
272 273 274 275 276 277 278 279 280 281 282
}

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

void CorridorScanComplexItem::rotateEntryPoint(void)
{
283 284 285
    _entryPoint++;
    if (_entryPoint > 3) {
        _entryPoint = 0;
286
    }
287

288
    _rebuildTransects();
289 290 291 292 293
}

void CorridorScanComplexItem::_rebuildCorridorPolygon(void)
{
    if (_corridorPolyline.count() < 2) {
294
        _surveyAreaPolygon.clear();
295 296 297 298 299 300 301 302
        return;
    }

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

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

303
    _surveyAreaPolygon.clear();
DonLakeFlyer's avatar
DonLakeFlyer committed
304 305

    QList<QGeoCoordinate> rgCoord;
306
    foreach (const QGeoCoordinate& vertex, firstSideVertices) {
DonLakeFlyer's avatar
DonLakeFlyer committed
307
        rgCoord.append(vertex);
308 309
    }
    for (int i=secondSideVertices.count() - 1; i >= 0; i--) {
DonLakeFlyer's avatar
DonLakeFlyer committed
310
        rgCoord.append(secondSideVertices[i]);
311
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
312
    _surveyAreaPolygon.appendVertices(rgCoord);
313 314
}

315
void CorridorScanComplexItem::_rebuildTransectsPhase1(void)
316
{
317 318 319 320 321 322 323 324 325 326 327
    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;
    }

328
    _transects.clear();
329
    _transectsPathHeightInfo.clear();
330 331 332 333 334 335 336

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

337 338
    if (_corridorPolyline.count() >= 2) {
        // First build up the transects all going the same direction
339
        //qDebug() << "_rebuildTransectsPhase1";
340
        for (int i=0; i<transectCount; i++) {
341
            //qDebug() << "start transect";
342 343 344 345 346 347 348 349 350
            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;
            }

351 352 353 354 355 356 357 358 359 360 361 362 363
            // 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
364
            if (_hasTurnaround()) {
365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384
                 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";
            foreach (const TransectStyleComplexItem::CoordInfo_t& coordInfo, transect) {
                qDebug() << coordInfo.coordType;
385
            }
386
#endif
387

388
            _transects.append(transect);
389
            normalizedTransectPosition += transectSpacing;
390 391
        }

392 393 394 395 396 397 398 399 400 401 402
        // 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;
403
            reverseVertices = false;
404 405 406 407 408 409 410 411 412 413 414
            break;
        case 1:
            reverseTransects = true;
            reverseVertices = false;
            break;
        case 2:
            reverseTransects = false;
            reverseVertices = true;
            break;
        case 3:
            reverseTransects = true;
415
            reverseVertices = true;
416
            break;
417
        }
418
        if (reverseTransects) {
419 420
            QList<QList<TransectStyleComplexItem::CoordInfo_t>> reversedTransects;
            foreach (const QList<TransectStyleComplexItem::CoordInfo_t>& transect, _transects) {
421 422
                reversedTransects.prepend(transect);
            }
423
            _transects = reversedTransects;
424 425
        }
        if (reverseVertices) {
426 427 428
            for (int i=0; i<_transects.count(); i++) {
                QList<TransectStyleComplexItem::CoordInfo_t> reversedVertices;
                foreach (const TransectStyleComplexItem::CoordInfo_t& vertex, _transects[i]) {
429 430
                    reversedVertices.prepend(vertex);
                }
431
                _transects[i] = reversedVertices;
432
            }
433 434
        }

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

455 456
void CorridorScanComplexItem::_rebuildTransectsPhase2(void)
{
457 458
    // Calculate distance flown for complex item
    _complexDistance = 0;
459
    for (int i=0; i<_visualTransectPoints.count() - 1; i++) {
460
        _complexDistance += _visualTransectPoints[i].value<QGeoCoordinate>().distanceTo(_visualTransectPoints[i+1].value<QGeoCoordinate>());
461 462
    }

463
    if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) {
464
        _cameraShots = qCeil(_complexDistance / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble());
465 466 467
    } else {
        int singleTransectImageCount = qCeil(_corridorPolyline.length() / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble());
        _cameraShots = singleTransectImageCount * _transectCount();
468 469
    }

470 471
    _coordinate = _visualTransectPoints.count() ? _visualTransectPoints.first().value<QGeoCoordinate>() : QGeoCoordinate();
    _exitCoordinate = _visualTransectPoints.count() ? _visualTransectPoints.last().value<QGeoCoordinate>() : QGeoCoordinate();
472 473

    emit cameraShotsChanged();
474
    emit complexDistanceChanged();
475 476 477 478
    emit coordinateChanged(_coordinate);
    emit exitCoordinateChanged(_exitCoordinate);
}

479 480 481 482
bool CorridorScanComplexItem::readyForSave(void) const
{
    return TransectStyleComplexItem::readyForSave();
}