CorridorScanComplexItem.cc 14.5 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
 *
 * 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 "QGCQGeoCoordinate.h"
#include "SettingsManager.h"
#include "AppSettings.h"
#include "QGCQGeoCoordinate.h"
18
#include "PlanMasterController.h"
19
#include "QGCApplication.h"
20 21 22 23 24

#include <QPolygonF>

QGC_LOGGING_CATEGORY(CorridorScanComplexItemLog, "CorridorScanComplexItemLog")

25 26
const QString CorridorScanComplexItem::name(tr("Corridor Scan"));

27 28
const char* CorridorScanComplexItem::settingsGroup =            "CorridorScan";
const char* CorridorScanComplexItem::corridorWidthName =        "CorridorWidth";
29
const char* CorridorScanComplexItem::_jsonEntryPointKey =       "EntryPoint";
30

31
const char* CorridorScanComplexItem::jsonComplexItemTypeValue = "CorridorScan";
32

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

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

DoinLakeFlyer's avatar
DoinLakeFlyer committed
46 47
    connect(&_corridorWidthFact,    &Fact::valueChanged,                            this, &CorridorScanComplexItem::_setDirty);
    connect(&_corridorPolyline,     &QGCMapPolyline::pathChanged,                   this, &CorridorScanComplexItem::_setDirty);
48

DoinLakeFlyer's avatar
DoinLakeFlyer committed
49
    connect(&_corridorPolyline,     &QGCMapPolyline::dirtyChanged,                  this, &CorridorScanComplexItem::_polylineDirtyChanged);
50

DoinLakeFlyer's avatar
DoinLakeFlyer committed
51 52
    connect(&_corridorPolyline,     &QGCMapPolyline::pathChanged,                   this, &CorridorScanComplexItem::_rebuildCorridorPolygon);
    connect(&_corridorWidthFact,    &Fact::valueChanged,                            this, &CorridorScanComplexItem::_rebuildCorridorPolygon);
53

DoinLakeFlyer's avatar
DoinLakeFlyer committed
54 55
    connect(&_corridorPolyline,     &QGCMapPolyline::isValidChanged,                this, &CorridorScanComplexItem::_updateWizardMode);
    connect(&_corridorPolyline,     &QGCMapPolyline::traceModeChanged,              this, &CorridorScanComplexItem::_updateWizardMode);
DonLakeFlyer's avatar
DonLakeFlyer committed
56

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

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

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

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

    _corridorPolyline.saveToJson(saveObject);

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

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

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

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

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

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

    setSequenceNumber(sequenceNumber);

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

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

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

130 131
    _ignoreRecalc = false;

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

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

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

146
int CorridorScanComplexItem::_calcTransectCount(void) const
147 148
{
    double fullWidth = _corridorWidthFact.rawValue().toDouble();
149
    return fullWidth > 0.0 ? qCeil(fullWidth / _calcTransectSpacing()) : 1;
150 151
}

152 153 154 155 156 157 158 159 160
void CorridorScanComplexItem::_polylineDirtyChanged(bool dirty)
{
    if (dirty) {
        setDirty(true);
    }
}

void CorridorScanComplexItem::rotateEntryPoint(void)
{
161 162 163
    _entryPoint++;
    if (_entryPoint > 3) {
        _entryPoint = 0;
164
    }
165

166
    _rebuildTransects();
167 168 169 170 171
}

void CorridorScanComplexItem::_rebuildCorridorPolygon(void)
{
    if (_corridorPolyline.count() < 2) {
172
        _surveyAreaPolygon.clear();
173 174 175 176 177 178 179 180
        return;
    }

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

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

181
    _surveyAreaPolygon.clear();
DonLakeFlyer's avatar
DonLakeFlyer committed
182 183

    QList<QGeoCoordinate> rgCoord;
184
    for (const QGeoCoordinate& vertex: firstSideVertices) {
DonLakeFlyer's avatar
DonLakeFlyer committed
185
        rgCoord.append(vertex);
186 187
    }
    for (int i=secondSideVertices.count() - 1; i >= 0; i--) {
DonLakeFlyer's avatar
DonLakeFlyer committed
188
        rgCoord.append(secondSideVertices[i]);
189
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
190
    _surveyAreaPolygon.appendVertices(rgCoord);
191 192
}

193
void CorridorScanComplexItem::_rebuildTransectsPhase1(void)
194
{
195 196 197 198 199 200 201 202
    if (_ignoreRecalc) {
        return;
    }

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

206
    _transects.clear();
207
    _transectsPathHeightInfo.clear();
208

209
    double transectSpacing = _calcTransectSpacing();
210 211
    double fullWidth = _corridorWidthFact.rawValue().toDouble();
    double halfWidth = fullWidth / 2.0;
212
    int transectCount = _calcTransectCount();
213 214
    double normalizedTransectPosition = transectSpacing / 2.0;

215 216
    if (_corridorPolyline.count() >= 2) {
        // First build up the transects all going the same direction
217
        //qDebug() << "_rebuildTransectsPhase1";
218
        for (int i=0; i<transectCount; i++) {
219
            //qDebug() << "start transect";
220 221 222 223 224 225 226 227 228
            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;
            }

229 230 231 232 233 234 235
            // 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);
            }
236
            TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords.first(), CoordTypeSurveyEntry };
237
            transect.prepend(coordInfo);
238
            coordInfo = { transectCoords.last(), CoordTypeSurveyExit };
239 240 241
            transect.append(coordInfo);

            // Extend the transect ends for turnaround
242
            if (_hasTurnaround()) {
243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260
                 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";
261
            for (const TransectStyleComplexItem::CoordInfo_t& coordInfo: transect) {
262
                qDebug() << coordInfo.coordType;
263
            }
264
#endif
265

266
            _transects.append(transect);
267
            normalizedTransectPosition += transectSpacing;
268 269
        }

270 271 272 273 274 275 276 277 278 279 280
        // 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;
281
            reverseVertices = false;
282 283 284 285 286 287 288 289 290 291 292
            break;
        case 1:
            reverseTransects = true;
            reverseVertices = false;
            break;
        case 2:
            reverseTransects = false;
            reverseVertices = true;
            break;
        case 3:
            reverseTransects = true;
293
            reverseVertices = true;
294
            break;
295
        }
296
        if (reverseTransects) {
297
            QList<QList<TransectStyleComplexItem::CoordInfo_t>> reversedTransects;
298
            for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
299 300
                reversedTransects.prepend(transect);
            }
301
            _transects = reversedTransects;
302 303
        }
        if (reverseVertices) {
304 305
            for (int i=0; i<_transects.count(); i++) {
                QList<TransectStyleComplexItem::CoordInfo_t> reversedVertices;
306
                for (const TransectStyleComplexItem::CoordInfo_t& vertex: _transects[i]) {
307 308
                    reversedVertices.prepend(vertex);
                }
309
                _transects[i] = reversedVertices;
310
            }
311 312
        }

313
        // Adjust to lawnmower pattern
314
        reverseVertices = false;
315
        for (int i=0; i<_transects.count(); i++) {
316
            // We must reverse the vertices for every other transect in order to make a lawnmower pattern
317
            QList<TransectStyleComplexItem::CoordInfo_t> transectVertices = _transects[i];
318 319
            if (reverseVertices) {
                reverseVertices = false;
320
                QList<TransectStyleComplexItem::CoordInfo_t> reversedVertices;
321 322 323 324 325 326 327
                for (int j=transectVertices.count()-1; j>=0; j--) {
                    reversedVertices.append(transectVertices[j]);
                }
                transectVertices = reversedVertices;
            } else {
                reverseVertices = true;
            }
328
            _transects[i] = transectVertices;
329
        }
330
    }
331
}
332

333 334
void CorridorScanComplexItem::_recalcCameraShots(void)
{
335 336 337
    double triggerDistance = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble();
    if (triggerDistance == 0) {
        _cameraShots = 0;
338
    } else {
339 340 341 342
        if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) {
            _cameraShots = qCeil(_complexDistance / triggerDistance);
        } else {
            int singleTransectImageCount = qCeil(_corridorPolyline.length() / triggerDistance);
343
            _cameraShots = singleTransectImageCount * _calcTransectCount();
344
        }
345 346 347 348
    }
    emit cameraShotsChanged();
}

DonLakeFlyer's avatar
DonLakeFlyer committed
349
CorridorScanComplexItem::ReadyForSaveState CorridorScanComplexItem::readyForSaveState(void) const
350
{
DonLakeFlyer's avatar
DonLakeFlyer committed
351
    return TransectStyleComplexItem::readyForSaveState();
352
}
353 354 355

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

359
double CorridorScanComplexItem::_calcTransectSpacing(void) const
360 361 362 363 364 365 366 367 368 369 370
{
    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;
}
DoinLakeFlyer's avatar
DoinLakeFlyer committed
371 372 373 374 375 376 377

void CorridorScanComplexItem::_updateWizardMode(void)
{
    if (_corridorPolyline.isValid() && !_corridorPolyline.traceMode()) {
        setWizardMode(false);
    }
}