SurveyComplexItem.cc 70 KB
Newer Older
1 2 3 4 5 6 7 8 9 10
/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/


11
#include "SurveyComplexItem.h"
12 13 14
#include "JsonHelper.h"
#include "MissionController.h"
#include "QGCGeo.h"
15
#include "QGroundControlQmlGlobal.h"
16
#include "QGCQGeoCoordinate.h"
17 18
#include "SettingsManager.h"
#include "AppSettings.h"
19 20 21

#include <QPolygonF>

22
QGC_LOGGING_CATEGORY(SurveyComplexItemLog, "SurveyComplexItemLog")
23
QGC_LOGGING_CATEGORY(PolygonDecomposeLog, "PolygonDecomposeLog")
24

25 26 27 28 29 30
const char* SurveyComplexItem::jsonComplexItemTypeValue =   "survey";
const char* SurveyComplexItem::jsonV3ComplexItemTypeValue = "survey";

const char* SurveyComplexItem::settingsGroup =              "Survey";
const char* SurveyComplexItem::gridAngleName =              "GridAngle";
const char* SurveyComplexItem::gridEntryLocationName =      "GridEntryLocation";
31
const char* SurveyComplexItem::flyAlternateTransectsName =  "FlyAlternateTransects";
32 33

const char* SurveyComplexItem::_jsonGridAngleKey =          "angle";
DonLakeFlyer's avatar
DonLakeFlyer committed
34
const char* SurveyComplexItem::_jsonEntryPointKey =         "entryLocation";
35 36 37 38 39 40

const char* SurveyComplexItem::_jsonV3GridObjectKey =                   "grid";
const char* SurveyComplexItem::_jsonV3GridAltitudeKey =                 "altitude";
const char* SurveyComplexItem::_jsonV3GridAltitudeRelativeKey =         "relativeAltitude";
const char* SurveyComplexItem::_jsonV3GridAngleKey =                    "angle";
const char* SurveyComplexItem::_jsonV3GridSpacingKey =                  "spacing";
DonLakeFlyer's avatar
DonLakeFlyer committed
41
const char* SurveyComplexItem::_jsonV3EntryPointKey =                   "entryLocation";
42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60
const char* SurveyComplexItem::_jsonV3TurnaroundDistKey =               "turnAroundDistance";
const char* SurveyComplexItem::_jsonV3CameraTriggerDistanceKey =        "cameraTriggerDistance";
const char* SurveyComplexItem::_jsonV3CameraTriggerInTurnaroundKey =    "cameraTriggerInTurnaround";
const char* SurveyComplexItem::_jsonV3HoverAndCaptureKey =              "hoverAndCapture";
const char* SurveyComplexItem::_jsonV3GroundResolutionKey =             "groundResolution";
const char* SurveyComplexItem::_jsonV3FrontalOverlapKey =               "imageFrontalOverlap";
const char* SurveyComplexItem::_jsonV3SideOverlapKey =                  "imageSideOverlap";
const char* SurveyComplexItem::_jsonV3CameraSensorWidthKey =            "sensorWidth";
const char* SurveyComplexItem::_jsonV3CameraSensorHeightKey =           "sensorHeight";
const char* SurveyComplexItem::_jsonV3CameraResolutionWidthKey =        "resolutionWidth";
const char* SurveyComplexItem::_jsonV3CameraResolutionHeightKey =       "resolutionHeight";
const char* SurveyComplexItem::_jsonV3CameraFocalLengthKey =            "focalLength";
const char* SurveyComplexItem::_jsonV3CameraMinTriggerIntervalKey =     "minTriggerInterval";
const char* SurveyComplexItem::_jsonV3CameraObjectKey =                 "camera";
const char* SurveyComplexItem::_jsonV3CameraNameKey =                   "name";
const char* SurveyComplexItem::_jsonV3ManualGridKey =                   "manualGrid";
const char* SurveyComplexItem::_jsonV3CameraOrientationLandscapeKey =   "orientationLandscape";
const char* SurveyComplexItem::_jsonV3FixedValueIsAltitudeKey =         "fixedValueIsAltitude";
const char* SurveyComplexItem::_jsonV3Refly90DegreesKey =               "refly90Degrees";
61
const char* SurveyComplexItem::_jsonFlyAlternateTransectsKey =          "flyAlternateTransects";
62

63
SurveyComplexItem::SurveyComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlFile, QObject* parent)
64
    : TransectStyleComplexItem  (vehicle, flyView, settingsGroup, parent)
65 66
    , _metaDataMap              (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/Survey.SettingsGroup.json"), this))
    , _gridAngleFact            (settingsGroup, _metaDataMap[gridAngleName])
67
    , _flyAlternateTransectsFact(settingsGroup, _metaDataMap[flyAlternateTransectsName])
DonLakeFlyer's avatar
DonLakeFlyer committed
68
    , _entryPoint               (EntryLocationTopLeft)
69
{
70 71
    _editorQml = "qrc:/qml/SurveyItemEditor.qml";

72
    // If the user hasn't changed turnaround from the default (which is a fixed wing default) and we are multi-rotor set the multi-rotor default.
73
    // NULL check since object creation during unit testing passes NULL for vehicle
74
    if (_vehicle && _vehicle->multiRotor() && _turnAroundDistanceFact.rawValue().toDouble() == _turnAroundDistanceFact.rawDefaultValue().toDouble()) {
75
        // Note this is set to 10 meters to work around a problem with PX4 Pro turnaround behavior. Don't change unless firmware gets better as well.
76
        _turnAroundDistanceFact.setRawValue(10);
77
    }
Don Gagne's avatar
Don Gagne committed
78

79 80 81
    // We override the altitude to the mission default
    if (_cameraCalc.isManualCamera() || !_cameraCalc.valueSetIsDistance()->rawValue().toBool()) {
        _cameraCalc.distanceToSurface()->setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue());
82 83
    }

84
    connect(&_gridAngleFact,            &Fact::valueChanged,                        this, &SurveyComplexItem::_setDirty);
85
    connect(&_flyAlternateTransectsFact,&Fact::valueChanged,                        this, &SurveyComplexItem::_setDirty);
86
    connect(this,                       &SurveyComplexItem::refly90DegreesChanged,  this, &SurveyComplexItem::_setDirty);
87

88
    connect(&_gridAngleFact,            &Fact::valueChanged,                        this, &SurveyComplexItem::_rebuildTransects);
89
    connect(&_flyAlternateTransectsFact,&Fact::valueChanged,                        this, &SurveyComplexItem::_rebuildTransects);
90
    connect(this,                       &SurveyComplexItem::refly90DegreesChanged,  this, &SurveyComplexItem::_rebuildTransects);
91

92 93 94
    // FIXME: Shouldn't these be in TransectStyleComplexItem? They are also in CorridorScanComplexItem constructur
    connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, &SurveyComplexItem::coordinateHasRelativeAltitudeChanged);
    connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, &SurveyComplexItem::exitCoordinateHasRelativeAltitudeChanged);
95 96 97 98 99 100

    if (!kmlFile.isEmpty()) {
        _surveyAreaPolygon.loadKMLFile(kmlFile);
        _surveyAreaPolygon.setDirty(false);
    }
    setDirty(false);
101 102
}

103
void SurveyComplexItem::save(QJsonArray&  planItems)
104
{
105
    QJsonObject saveObject;
106

107
    _save(saveObject);
108

109 110 111 112
    saveObject[JsonHelper::jsonVersionKey] =                    4;
    saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
    saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;
    saveObject[_jsonGridAngleKey] =                             _gridAngleFact.rawValue().toDouble();
113
    saveObject[_jsonFlyAlternateTransectsKey] =                 _flyAlternateTransectsFact.rawValue().toBool();
DonLakeFlyer's avatar
DonLakeFlyer committed
114
    saveObject[_jsonEntryPointKey] =                            _entryPoint;
115

116 117
    // Polygon shape
    _surveyAreaPolygon.saveToJson(saveObject);
118

119
    planItems.append(saveObject);
120 121
}

122
bool SurveyComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
123
{
124 125 126 127 128 129
    // We need to pull version first to determine what validation/conversion needs to be performed
    QList<JsonHelper::KeyValidateInfo> versionKeyInfoList = {
        { JsonHelper::jsonVersionKey, QJsonValue::Double, true },
    };
    if (!JsonHelper::validateKeys(complexObject, versionKeyInfoList, errorString)) {
        return false;
130 131
    }

132 133 134 135
    int version = complexObject[JsonHelper::jsonVersionKey].toInt();
    if (version < 2 || version > 4) {
        errorString = tr("Survey items do not support version %1").arg(version);
        return false;
136
    }
137

138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154
    if (version == 4) {
        if (!_loadV4(complexObject, sequenceNumber, errorString)) {
            return false;
        }
    } else {
        // Must be v2 or v3
        QJsonObject v3ComplexObject = complexObject;
        if (version == 2) {
            // Convert to v3
            if (v3ComplexObject.contains(VisualMissionItem::jsonTypeKey) && v3ComplexObject[VisualMissionItem::jsonTypeKey].toString() == QStringLiteral("survey")) {
                v3ComplexObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue;
                v3ComplexObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue;
            }
        }
        if (!_loadV3(complexObject, sequenceNumber, errorString)) {
            return false;
        }
Don Gagne's avatar
Don Gagne committed
155
    }
156

157
    _rebuildTransects();
158

159
    return true;
160 161
}

162
bool SurveyComplexItem::_loadV4(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
163
{
164 165 166
    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
        { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
        { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
167
        { _jsonEntryPointKey,                           QJsonValue::Double, true },
168
        { _jsonGridAngleKey,                            QJsonValue::Double, true },
169
        { _jsonFlyAlternateTransectsKey,                QJsonValue::Bool,   false },
170 171 172
    };
    if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
        return false;
173
    }
Don Gagne's avatar
Don Gagne committed
174

175 176 177 178
    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);
179 180
        return false;
    }
Don Gagne's avatar
Don Gagne committed
181

182 183 184 185 186 187
    _ignoreRecalc = true;

    setSequenceNumber(sequenceNumber);

    if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) {
        _surveyAreaPolygon.clear();
188 189
        return false;
    }
190 191 192 193

    if (!_load(complexObject, errorString)) {
        _ignoreRecalc = false;
        return false;
Don Gagne's avatar
Don Gagne committed
194
    }
195

196 197 198
    _gridAngleFact.setRawValue              (complexObject[_jsonGridAngleKey].toDouble());
    _flyAlternateTransectsFact.setRawValue  (complexObject[_jsonFlyAlternateTransectsKey].toBool(false));

DonLakeFlyer's avatar
DonLakeFlyer committed
199
    _entryPoint = complexObject[_jsonEntryPointKey].toInt();
200 201 202 203 204 205 206 207

    _ignoreRecalc = false;

    return true;
}

bool SurveyComplexItem::_loadV3(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
{
Don Gagne's avatar
Don Gagne committed
208 209 210
    QList<JsonHelper::KeyValidateInfo> mainKeyInfoList = {
        { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
        { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
211
        { QGCMapPolygon::jsonPolygonKey,                QJsonValue::Array,  true },
212 213 214 215 216 217 218 219
        { _jsonV3GridObjectKey,                         QJsonValue::Object, true },
        { _jsonV3CameraObjectKey,                       QJsonValue::Object, false },
        { _jsonV3CameraTriggerDistanceKey,              QJsonValue::Double, true },
        { _jsonV3ManualGridKey,                         QJsonValue::Bool,   true },
        { _jsonV3FixedValueIsAltitudeKey,               QJsonValue::Bool,   true },
        { _jsonV3HoverAndCaptureKey,                    QJsonValue::Bool,   false },
        { _jsonV3Refly90DegreesKey,                     QJsonValue::Bool,   false },
        { _jsonV3CameraTriggerInTurnaroundKey,          QJsonValue::Bool,   false },    // Should really be required, but it was missing from initial code due to bug
Don Gagne's avatar
Don Gagne committed
220
    };
221
    if (!JsonHelper::validateKeys(complexObject, mainKeyInfoList, errorString)) {
222 223
        return false;
    }
Don Gagne's avatar
Don Gagne committed
224

225 226 227
    QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
    QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
    if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonV3ComplexItemTypeValue) {
228
        errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
229 230 231
        return false;
    }

232 233
    _ignoreRecalc = true;

Don Gagne's avatar
Don Gagne committed
234
    setSequenceNumber(sequenceNumber);
235

236 237 238 239 240 241
    _hoverAndCaptureFact.setRawValue            (complexObject[_jsonV3HoverAndCaptureKey].toBool(false));
    _refly90DegreesFact.setRawValue             (complexObject[_jsonV3Refly90DegreesKey].toBool(false));
    _cameraTriggerInTurnAroundFact.setRawValue  (complexObject[_jsonV3CameraTriggerInTurnaroundKey].toBool(true));

    _cameraCalc.valueSetIsDistance()->setRawValue   (complexObject[_jsonV3FixedValueIsAltitudeKey].toBool(true));
    _cameraCalc.setDistanceToSurfaceRelative        (complexObject[_jsonV3GridAltitudeRelativeKey].toBool(true));
Don Gagne's avatar
Don Gagne committed
242

243
    bool manualGrid = complexObject[_jsonV3ManualGridKey].toBool(true);
244

Don Gagne's avatar
Don Gagne committed
245
    QList<JsonHelper::KeyValidateInfo> gridKeyInfoList = {
246 247 248 249
        { _jsonV3GridAltitudeKey,           QJsonValue::Double, true },
        { _jsonV3GridAltitudeRelativeKey,   QJsonValue::Bool,   true },
        { _jsonV3GridAngleKey,              QJsonValue::Double, true },
        { _jsonV3GridSpacingKey,            QJsonValue::Double, true },
DonLakeFlyer's avatar
DonLakeFlyer committed
250
        { _jsonEntryPointKey,      QJsonValue::Double, false },
251
        { _jsonV3TurnaroundDistKey,         QJsonValue::Double, true },
Don Gagne's avatar
Don Gagne committed
252
    };
253
    QJsonObject gridObject = complexObject[_jsonV3GridObjectKey].toObject();
Don Gagne's avatar
Don Gagne committed
254
    if (!JsonHelper::validateKeys(gridObject, gridKeyInfoList, errorString)) {
255
        _ignoreRecalc = false;
Don Gagne's avatar
Don Gagne committed
256 257
        return false;
    }
258 259 260 261

    _gridAngleFact.setRawValue          (gridObject[_jsonV3GridAngleKey].toDouble());
    _turnAroundDistanceFact.setRawValue (gridObject[_jsonV3TurnaroundDistKey].toDouble());

DonLakeFlyer's avatar
DonLakeFlyer committed
262 263
    if (gridObject.contains(_jsonEntryPointKey)) {
        _entryPoint = gridObject[_jsonEntryPointKey].toDouble();
DonLakeFlyer's avatar
DonLakeFlyer committed
264
    } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
265
        _entryPoint = EntryLocationTopRight;
DonLakeFlyer's avatar
DonLakeFlyer committed
266
    }
Don Gagne's avatar
Don Gagne committed
267

268 269 270 271 272
    _cameraCalc.distanceToSurface()->setRawValue        (gridObject[_jsonV3GridAltitudeKey].toDouble());
    _cameraCalc.adjustedFootprintSide()->setRawValue    (gridObject[_jsonV3GridSpacingKey].toDouble());
    _cameraCalc.adjustedFootprintFrontal()->setRawValue (complexObject[_jsonV3CameraTriggerDistanceKey].toDouble());

    if (manualGrid) {
273
        _cameraCalc.cameraName()->setRawValue(_cameraCalc.manualCameraName());
274 275
    } else {
        if (!complexObject.contains(_jsonV3CameraObjectKey)) {
Don Gagne's avatar
Don Gagne committed
276
            errorString = tr("%1 but %2 object is missing").arg("manualGrid = false").arg("camera");
277
            _ignoreRecalc = false;
Don Gagne's avatar
Don Gagne committed
278 279 280
            return false;
        }

281
        QJsonObject cameraObject = complexObject[_jsonV3CameraObjectKey].toObject();
Don Gagne's avatar
Don Gagne committed
282

283 284 285
        // Older code had typo on "imageSideOverlap" incorrectly being "imageSizeOverlap"
        QString incorrectImageSideOverlap = "imageSizeOverlap";
        if (cameraObject.contains(incorrectImageSideOverlap)) {
286
            cameraObject[_jsonV3SideOverlapKey] = cameraObject[incorrectImageSideOverlap];
287 288 289
            cameraObject.remove(incorrectImageSideOverlap);
        }

Don Gagne's avatar
Don Gagne committed
290
        QList<JsonHelper::KeyValidateInfo> cameraKeyInfoList = {
291 292 293 294 295 296 297 298 299 300 301
            { _jsonV3GroundResolutionKey,           QJsonValue::Double, true },
            { _jsonV3FrontalOverlapKey,             QJsonValue::Double, true },
            { _jsonV3SideOverlapKey,                QJsonValue::Double, true },
            { _jsonV3CameraSensorWidthKey,          QJsonValue::Double, true },
            { _jsonV3CameraSensorHeightKey,         QJsonValue::Double, true },
            { _jsonV3CameraResolutionWidthKey,      QJsonValue::Double, true },
            { _jsonV3CameraResolutionHeightKey,     QJsonValue::Double, true },
            { _jsonV3CameraFocalLengthKey,          QJsonValue::Double, true },
            { _jsonV3CameraNameKey,                 QJsonValue::String, true },
            { _jsonV3CameraOrientationLandscapeKey, QJsonValue::Bool,   true },
            { _jsonV3CameraMinTriggerIntervalKey,   QJsonValue::Double, false },
Don Gagne's avatar
Don Gagne committed
302
        };
Don Gagne's avatar
Don Gagne committed
303
        if (!JsonHelper::validateKeys(cameraObject, cameraKeyInfoList, errorString)) {
304
            _ignoreRecalc = false;
Don Gagne's avatar
Don Gagne committed
305 306 307
            return false;
        }

308
        _cameraCalc.cameraName()->setRawValue           (cameraObject[_jsonV3CameraNameKey].toString());
309 310 311 312 313 314 315 316 317 318 319
        _cameraCalc.landscape()->setRawValue            (cameraObject[_jsonV3CameraOrientationLandscapeKey].toBool(true));
        _cameraCalc.frontalOverlap()->setRawValue       (cameraObject[_jsonV3FrontalOverlapKey].toInt());
        _cameraCalc.sideOverlap()->setRawValue          (cameraObject[_jsonV3SideOverlapKey].toInt());
        _cameraCalc.sensorWidth()->setRawValue          (cameraObject[_jsonV3CameraSensorWidthKey].toDouble());
        _cameraCalc.sensorHeight()->setRawValue         (cameraObject[_jsonV3CameraSensorHeightKey].toDouble());
        _cameraCalc.focalLength()->setRawValue          (cameraObject[_jsonV3CameraFocalLengthKey].toDouble());
        _cameraCalc.imageWidth()->setRawValue           (cameraObject[_jsonV3CameraResolutionWidthKey].toInt());
        _cameraCalc.imageHeight()->setRawValue          (cameraObject[_jsonV3CameraResolutionHeightKey].toInt());
        _cameraCalc.minTriggerInterval()->setRawValue   (cameraObject[_jsonV3CameraMinTriggerIntervalKey].toDouble(0));
        _cameraCalc.imageDensity()->setRawValue         (cameraObject[_jsonV3GroundResolutionKey].toDouble());
        _cameraCalc.fixedOrientation()->setRawValue     (false);
Don Gagne's avatar
Don Gagne committed
320
    }
321 322

    // Polygon shape
323 324 325 326 327
    /// Load a polygon from json
    ///     @param json Json object to load from
    ///     @param required true: no polygon in object will generate error
    ///     @param errorString Error string if return is false
    /// @return true: success, false: failure (errorString set)
328 329 330
    if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) {
        _surveyAreaPolygon.clear();
        _ignoreRecalc = false;
331 332
        return false;
    }
333

334 335
    _ignoreRecalc = false;

336 337 338
    return true;
}

DonLakeFlyer's avatar
DonLakeFlyer committed
339
/// Reverse the order of the transects. First transect becomes last and so forth.
340
void SurveyComplexItem::_reverseTransectOrder(QList<QList<QGeoCoordinate>>& transects)
DonLakeFlyer's avatar
DonLakeFlyer committed
341 342 343 344 345 346 347 348 349
{
    QList<QList<QGeoCoordinate>> rgReversedTransects;
    for (int i=transects.count() - 1; i>=0; i--) {
        rgReversedTransects.append(transects[i]);
    }
    transects = rgReversedTransects;
}

/// Reverse the order of all points withing each transect, First point becomes last and so forth.
350
void SurveyComplexItem::_reverseInternalTransectPoints(QList<QList<QGeoCoordinate>>& transects)
DonLakeFlyer's avatar
DonLakeFlyer committed
351 352 353 354 355 356 357 358 359 360 361
{
    for (int i=0; i<transects.count(); i++) {
        QList<QGeoCoordinate> rgReversedCoords;
        QList<QGeoCoordinate>& rgOriginalCoords = transects[i];
        for (int j=rgOriginalCoords.count()-1; j>=0; j--) {
            rgReversedCoords.append(rgOriginalCoords[j]);
        }
        transects[i] = rgReversedCoords;
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
362 363 364 365
/// Reorders the transects such that the first transect is the shortest distance to the specified coordinate
/// and the first point within that transect is the shortest distance to the specified coordinate.
///     @param distanceCoord Coordinate to measure distance against
///     @param transects Transects to test and reorder
366
void SurveyComplexItem::_optimizeTransectsForShortestDistance(const QGeoCoordinate& distanceCoord, QList<QList<QGeoCoordinate>>& transects)
367
{
368
    double rgTransectDistance[4];
DonLakeFlyer's avatar
DonLakeFlyer committed
369 370 371 372
    rgTransectDistance[0] = transects.first().first().distanceTo(distanceCoord);
    rgTransectDistance[1] = transects.first().last().distanceTo(distanceCoord);
    rgTransectDistance[2] = transects.last().first().distanceTo(distanceCoord);
    rgTransectDistance[3] = transects.last().last().distanceTo(distanceCoord);
373 374 375 376 377 378 379 380 381

    int shortestIndex = 0;
    double shortestDistance = rgTransectDistance[0];
    for (int i=1; i<3; i++) {
        if (rgTransectDistance[i] < shortestDistance) {
            shortestIndex = i;
            shortestDistance = rgTransectDistance[i];
        }
    }
382

383 384
    if (shortestIndex > 1) {
        // We need to reverse the order of segments
DonLakeFlyer's avatar
DonLakeFlyer committed
385
        _reverseTransectOrder(transects);
386 387 388
    }
    if (shortestIndex & 1) {
        // We need to reverse the points within each segment
DonLakeFlyer's avatar
DonLakeFlyer committed
389
        _reverseInternalTransectPoints(transects);
390 391
    }
}
392

393
qreal SurveyComplexItem::_ccw(QPointF pt1, QPointF pt2, QPointF pt3)
394 395 396 397
{
    return (pt2.x()-pt1.x())*(pt3.y()-pt1.y()) - (pt2.y()-pt1.y())*(pt3.x()-pt1.x());
}

398
qreal SurveyComplexItem::_dp(QPointF pt1, QPointF pt2)
399 400 401 402
{
    return (pt2.x()-pt1.x())/qSqrt((pt2.x()-pt1.x())*(pt2.x()-pt1.x()) + (pt2.y()-pt1.y())*(pt2.y()-pt1.y()));
}

403
void SurveyComplexItem::_swapPoints(QList<QPointF>& points, int index1, int index2)
404 405 406 407 408 409
{
    QPointF temp = points[index1];
    points[index1] = points[index2];
    points[index2] = temp;
}

DonLakeFlyer's avatar
DonLakeFlyer committed
410
/// Returns true if the current grid angle generates north/south oriented transects
411
bool SurveyComplexItem::_gridAngleIsNorthSouthTransects()
DonLakeFlyer's avatar
DonLakeFlyer committed
412 413 414 415 416 417
{
    // Grid angle ranges from -360<->360
    double gridAngle = qAbs(_gridAngleFact.rawValue().toDouble());
    return gridAngle < 45.0 || (gridAngle > 360.0 - 45.0) || (gridAngle > 90.0 + 45.0 && gridAngle < 270.0 - 45.0);
}

418
void SurveyComplexItem::_adjustTransectsToEntryPointLocation(QList<QList<QGeoCoordinate>>& transects)
DonLakeFlyer's avatar
DonLakeFlyer committed
419 420 421 422 423
{
    if (transects.count() == 0) {
        return;
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
424 425
    bool reversePoints = false;
    bool reverseTransects = false;
DonLakeFlyer's avatar
DonLakeFlyer committed
426

DonLakeFlyer's avatar
DonLakeFlyer committed
427
    if (_entryPoint == EntryLocationBottomLeft || _entryPoint == EntryLocationBottomRight) {
DonLakeFlyer's avatar
DonLakeFlyer committed
428
        reversePoints = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
429
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
430
    if (_entryPoint == EntryLocationTopRight || _entryPoint == EntryLocationBottomRight) {
DonLakeFlyer's avatar
DonLakeFlyer committed
431
        reverseTransects = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
432
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
433

DonLakeFlyer's avatar
DonLakeFlyer committed
434
    if (reversePoints) {
435
        qCDebug(SurveyComplexItemLog) << "_adjustTransectsToEntryPointLocation Reverse Points";
DonLakeFlyer's avatar
DonLakeFlyer committed
436 437 438
        _reverseInternalTransectPoints(transects);
    }
    if (reverseTransects) {
439
        qCDebug(SurveyComplexItemLog) << "_adjustTransectsToEntryPointLocation Reverse Transects";
DonLakeFlyer's avatar
DonLakeFlyer committed
440
        _reverseTransectOrder(transects);
DonLakeFlyer's avatar
DonLakeFlyer committed
441
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
442

DonLakeFlyer's avatar
DonLakeFlyer committed
443
    qCDebug(SurveyComplexItemLog) << "_adjustTransectsToEntryPointLocation Modified entry point:entryLocation" << transects.first().first() << _entryPoint;
DonLakeFlyer's avatar
DonLakeFlyer committed
444 445
}

446
QPointF SurveyComplexItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle)
447 448
{
    QPointF rotated;
DonLakeFlyer's avatar
DonLakeFlyer committed
449
    double radians = (M_PI / 180.0) * -angle;
450 451 452 453 454 455 456

    rotated.setX(((point.x() - origin.x()) * cos(radians)) - ((point.y() - origin.y()) * sin(radians)) + origin.x());
    rotated.setY(((point.x() - origin.x()) * sin(radians)) + ((point.y() - origin.y()) * cos(radians)) + origin.y());

    return rotated;
}

457
void SurveyComplexItem::_intersectLinesWithRect(const QList<QLineF>& lineList, const QRectF& boundRect, QList<QLineF>& resultLines)
458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513
{
    QLineF topLine      (boundRect.topLeft(),       boundRect.topRight());
    QLineF bottomLine   (boundRect.bottomLeft(),    boundRect.bottomRight());
    QLineF leftLine     (boundRect.topLeft(),       boundRect.bottomLeft());
    QLineF rightLine    (boundRect.topRight(),      boundRect.bottomRight());

    for (int i=0; i<lineList.count(); i++) {
        QPointF intersectPoint;
        QLineF intersectLine;
        const QLineF& line = lineList[i];

        int foundCount = 0;
        if (line.intersect(topLine, &intersectPoint) == QLineF::BoundedIntersection) {
            intersectLine.setP1(intersectPoint);
            foundCount++;
        }
        if (line.intersect(rightLine, &intersectPoint) == QLineF::BoundedIntersection) {
            if (foundCount == 0) {
                intersectLine.setP1(intersectPoint);
            } else {
                if (foundCount != 1) {
                    qWarning() << "Found more than two intersecting points";
                }
                intersectLine.setP2(intersectPoint);
            }
            foundCount++;
        }
        if (line.intersect(bottomLine, &intersectPoint) == QLineF::BoundedIntersection) {
            if (foundCount == 0) {
                intersectLine.setP1(intersectPoint);
            } else {
                if (foundCount != 1) {
                    qWarning() << "Found more than two intersecting points";
                }
                intersectLine.setP2(intersectPoint);
            }
            foundCount++;
        }
        if (line.intersect(leftLine, &intersectPoint) == QLineF::BoundedIntersection) {
            if (foundCount == 0) {
                intersectLine.setP1(intersectPoint);
            } else {
                if (foundCount != 1) {
                    qWarning() << "Found more than two intersecting points";
                }
                intersectLine.setP2(intersectPoint);
            }
            foundCount++;
        }

        if (foundCount == 2) {
            resultLines += intersectLine;
        }
    }
}

514
void SurveyComplexItem::_intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines)
515
{
516
    resultLines.clear();
517

518 519
    for (int i=0; i<lineList.count(); i++) {
        const QLineF& line = lineList[i];
520
        QList<QPointF> intersections;
521

522
        // Intersect the line with all the polygon edges
523 524 525 526
        for (int j=0; j<polygon.count()-1; j++) {
            QPointF intersectPoint;
            QLineF polygonLine = QLineF(polygon[j], polygon[j+1]);
            if (line.intersect(polygonLine, &intersectPoint) == QLineF::BoundedIntersection) {
527 528 529
                if (!intersections.contains(intersectPoint)) {
                    intersections.append(intersectPoint);
                }
530 531 532
            }
        }

533 534 535 536 537 538 539 540 541 542
        // We now have one or more intersection points all along the same line. Find the two
        // which are furthest away from each other to form the transect.
        if (intersections.count() > 1) {
            QPointF firstPoint;
            QPointF secondPoint;
            double currentMaxDistance = 0;

            for (int i=0; i<intersections.count(); i++) {
                for (int j=0; j<intersections.count(); j++) {
                    QLineF lineTest(intersections[i], intersections[j]);
543
                    \
544 545 546 547 548 549 550 551 552 553
                    double newMaxDistance = lineTest.length();
                    if (newMaxDistance > currentMaxDistance) {
                        firstPoint = intersections[i];
                        secondPoint = intersections[j];
                        currentMaxDistance = newMaxDistance;
                    }
                }
            }

            resultLines += QLineF(firstPoint, secondPoint);
554 555 556 557 558
        }
    }
}

/// Adjust the line segments such that they are all going the same direction with respect to going from P1->P2
559
void SurveyComplexItem::_adjustLineDirection(const QList<QLineF>& lineList, QList<QLineF>& resultLines)
560
{
561
    qreal firstAngle = 0;
562 563 564 565
    for (int i=0; i<lineList.count(); i++) {
        const QLineF& line = lineList[i];
        QLineF adjustedLine;

566 567 568 569 570
        if (i == 0) {
            firstAngle = line.angle();
        }

        if (qAbs(line.angle() - firstAngle) > 1.0) {
571 572 573 574 575 576 577 578 579 580
            adjustedLine.setP1(line.p2());
            adjustedLine.setP2(line.p1());
        } else {
            adjustedLine = line;
        }

        resultLines += adjustedLine;
    }
}

581
double SurveyComplexItem::_clampGridAngle90(double gridAngle)
DonLakeFlyer's avatar
DonLakeFlyer committed
582 583 584 585 586 587 588 589 590 591
{
    // Clamp grid angle to -90<->90. This prevents transects from being rotated to a reversed order.
    if (gridAngle > 90.0) {
        gridAngle -= 180.0;
    } else if (gridAngle < -90.0) {
        gridAngle += 180;
    }
    return gridAngle;
}

592
#if 0
593
int SurveyComplexItem::_gridGenerator(const QList<QPointF>& polygonPoints,  QList<QList<QPointF>>& transectSegments, bool refly)
594
{
595 596
    int cameraShots = 0;

DonLakeFlyer's avatar
DonLakeFlyer committed
597
    double gridAngle = _gridAngleFact.rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
598 599
    double gridSpacing = _gridSpacingFact.rawValue().toDouble();

DonLakeFlyer's avatar
DonLakeFlyer committed
600 601
    gridAngle = _clampGridAngle90(gridAngle);
    gridAngle += refly ? 90 : 0;
602
    qCDebug(SurveyComplexItemLog) << "Clamped grid angle" << gridAngle;
DonLakeFlyer's avatar
DonLakeFlyer committed
603

604
    qCDebug(SurveyComplexItemLog) << "SurveyComplexItem::_gridGenerator gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly;
605

DonLakeFlyer's avatar
DonLakeFlyer committed
606
    transectSegments.clear();
607 608 609

    // Convert polygon to bounding rect

610
    qCDebug(SurveyComplexItemLog) << "Polygon";
611 612
    QPolygonF polygon;
    for (int i=0; i<polygonPoints.count(); i++) {
613
        qCDebug(SurveyComplexItemLog) << polygonPoints[i];
614 615 616
        polygon << polygonPoints[i];
    }
    polygon << polygonPoints[0];
617 618
    QRectF boundingRect = polygon.boundingRect();
    QPointF boundingCenter = boundingRect.center();
619
    qCDebug(SurveyComplexItemLog) << "Bounding rect" << boundingRect.topLeft().x() << boundingRect.topLeft().y() << boundingRect.bottomRight().x() << boundingRect.bottomRight().y();
620 621 622

    // Create set of rotated parallel lines within the expanded bounding rect. Make the lines larger than the
    // bounding box to guarantee intersection.
DonLakeFlyer's avatar
DonLakeFlyer committed
623

624
    QList<QLineF> lineList;
DonLakeFlyer's avatar
DonLakeFlyer committed
625

626
    // Transects are generated to be as long as the largest width/height of the bounding rect plus some fudge factor.
DonLakeFlyer's avatar
DonLakeFlyer committed
627
    // This way they will always be guaranteed to intersect with a polygon edge no matter what angle they are rotated to.
628
    // They are initially generated with the transects flowing from west to east and then points within the transect north to south.
629
    double maxWidth = qMax(boundingRect.width(), boundingRect.height()) + 2000.0;
630 631 632 633 634 635 636 637 638 639 640
    double halfWidth = maxWidth / 2.0;
    double transectX = boundingCenter.x() - halfWidth;
    double transectXMax = transectX + maxWidth;
    while (transectX < transectXMax) {
        double transectYTop = boundingCenter.y() - halfWidth;
        double transectYBottom = boundingCenter.y() + halfWidth;

        lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle));
        transectX += gridSpacing;
    }

641 642 643 644 645 646 647 648 649
    // Now intersect the lines with the polygon
    QList<QLineF> intersectLines;
#if 1
    _intersectLinesWithPolygon(lineList, polygon, intersectLines);
#else
    // This is handy for debugging grid problems, not for release
    intersectLines = lineList;
#endif

650 651 652 653 654 655 656 657 658 659 660 661 662 663 664
    // Less than two transects intersected with the polygon:
    //      Create a single transect which goes through the center of the polygon
    //      Intersect it with the polygon
    if (intersectLines.count() < 2) {
        _mapPolygon.center();
        QLineF firstLine = lineList.first();
        QPointF lineCenter = firstLine.pointAt(0.5);
        QPointF centerOffset = boundingCenter - lineCenter;
        firstLine.translate(centerOffset);
        lineList.clear();
        lineList.append(firstLine);
        intersectLines = lineList;
        _intersectLinesWithPolygon(lineList, polygon, intersectLines);
    }

665 666 667 668 669
    // Make sure all lines are going to same direction. Polygon intersection leads to line which
    // can be in varied directions depending on the order of the intesecting sides.
    QList<QLineF> resultLines;
    _adjustLineDirection(intersectLines, resultLines);

670
    // Calc camera shots here if there are no images in turnaround
DonLakeFlyer's avatar
DonLakeFlyer committed
671
    if (_triggerCamera() && !_imagesEverywhere()) {
672
        for (int i=0; i<resultLines.count(); i++) {
673 674 675
            cameraShots += (int)floor(resultLines[i].length() / _triggerDistance());
            // Take into account immediate camera trigger at waypoint entry
            cameraShots++;
676 677 678 679
        }
    }

    // Turn into a path
680
    for (int i=0; i<resultLines.count(); i++) {
DonLakeFlyer's avatar
DonLakeFlyer committed
681 682 683
        QLineF          transectLine;
        QList<QPointF>  transectPoints;
        const QLineF&   line = resultLines[i];
684

DonLakeFlyer's avatar
DonLakeFlyer committed
685
        float turnaroundPosition = _turnaroundDistance() / line.length();
Andreas Bircher's avatar
Andreas Bircher committed
686

687
        if (i & 1) {
DonLakeFlyer's avatar
DonLakeFlyer committed
688
            transectLine = QLineF(line.p2(), line.p1());
689
        } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705
            transectLine = QLineF(line.p1(), line.p2());
        }

        // Build the points along the transect

        if (_hasTurnaround()) {
            transectPoints.append(transectLine.pointAt(-turnaroundPosition));
        }

        // Polygon entry point
        transectPoints.append(transectLine.p1());

        // For hover and capture we need points for each camera location
        if (_triggerCamera() && _hoverAndCaptureEnabled()) {
            if (_triggerDistance() < transectLine.length()) {
                int innerPoints = floor(transectLine.length() / _triggerDistance());
706
                qCDebug(SurveyComplexItemLog) << "innerPoints" << innerPoints;
DonLakeFlyer's avatar
DonLakeFlyer committed
707 708 709 710
                float transectPositionIncrement = _triggerDistance() / transectLine.length();
                for (int i=0; i<innerPoints; i++) {
                    transectPoints.append(transectLine.pointAt(transectPositionIncrement * (i + 1)));
                }
Andreas Bircher's avatar
Andreas Bircher committed
711
            }
712
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
713 714 715 716 717 718 719 720 721

        // Polygon exit point
        transectPoints.append(transectLine.p2());

        if (_hasTurnaround()) {
            transectPoints.append(transectLine.pointAt(1 + turnaroundPosition));
        }

        transectSegments.append(transectPoints);
722
    }
723 724

    return cameraShots;
725
}
726
#endif
727

728
#if 0
729
int SurveyComplexItem::_appendWaypointToMission(QList<MissionItem*>& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent)
730
{
DonLakeFlyer's avatar
DonLakeFlyer committed
731 732 733
    double  altitude =          _gridAltitudeFact.rawValue().toDouble();
    bool    altitudeRelative =  _gridAltitudeRelativeFact.rawValue().toBool();

734
    qCDebug(SurveyComplexItemLog) << "_appendWaypointToMission seq:trigger" << seqNum << (cameraTrigger != CameraTriggerNone);
735 736 737 738

    MissionItem* item = new MissionItem(seqNum++,
                                        MAV_CMD_NAV_WAYPOINT,
                                        altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
739
                                        cameraTrigger == CameraTriggerHoverAndCapture ? _hoverAndCaptureDelaySeconds : 0,  // Hold time (delay for hover and capture to settle vehicle before image is taken)
740 741
                                        0.0, 0.0,
                                        std::numeric_limits<double>::quiet_NaN(),   // Yaw unchanged
742 743 744
                                        coord.latitude(),
                                        coord.longitude(),
                                        altitude,
745 746
                                        true,                                       // autoContinue
                                        false,                                      // isCurrentItem
747 748 749
                                        missionItemParent);
    items.append(item);

DonLakeFlyer's avatar
DonLakeFlyer committed
750 751 752 753 754 755 756
    switch (cameraTrigger) {
    case CameraTriggerOff:
    case CameraTriggerOn:
        item = new MissionItem(seqNum++,
                               MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                               MAV_FRAME_MISSION,
                               cameraTrigger == CameraTriggerOn ? _triggerDistance() : 0,
757 758 759 760 761
                               0,                                           // shutter integration (ignore)
                               cameraTrigger == CameraTriggerOn ? 1 : 0,    // trigger immediately when starting
                               0, 0, 0, 0,                                  // param 4-7 unused
                               true,                                        // autoContinue
                               false,                                       // isCurrentItem
DonLakeFlyer's avatar
DonLakeFlyer committed
762
                               missionItemParent);
763
        items.append(item);
DonLakeFlyer's avatar
DonLakeFlyer committed
764 765 766 767 768
        break;
    case CameraTriggerHoverAndCapture:
        item = new MissionItem(seqNum++,
                               MAV_CMD_IMAGE_START_CAPTURE,
                               MAV_FRAME_MISSION,
Gus Grubba's avatar
Gus Grubba committed
769
                               0,                           // Reserved (Set to 0)
770 771
                               0,                           // Interval (none)
                               1,                           // Take 1 photo
772
                               NAN, NAN, NAN, NAN,          // param 4-7 reserved
773 774
                               true,                        // autoContinue
                               false,                       // isCurrentItem
DonLakeFlyer's avatar
DonLakeFlyer committed
775 776
                               missionItemParent);
        items.append(item);
777 778
#if 0
        // This generates too many commands. Pulling out for now, to see if image quality is still high enough.
DonLakeFlyer's avatar
DonLakeFlyer committed
779 780 781 782 783 784 785 786 787 788
        item = new MissionItem(seqNum++,
                               MAV_CMD_NAV_DELAY,
                               MAV_FRAME_MISSION,
                               0.5,                // Delay in seconds, give some time for image to be taken
                               -1, -1, -1,         // No time
                               0, 0, 0,            // Param 5-7 unused
                               true,               // autoContinue
                               false,              // isCurrentItem
                               missionItemParent);
        items.append(item);
789
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
790 791
    default:
        break;
792
    }
793 794

    return seqNum;
795
}
796
#endif
797

798
bool SurveyComplexItem::_nextTransectCoord(const QList<QGeoCoordinate>& transectPoints, int pointIndex, QGeoCoordinate& coord)
DonLakeFlyer's avatar
DonLakeFlyer committed
799 800 801 802 803 804 805 806 807 808
{
    if (pointIndex > transectPoints.count()) {
        qWarning() << "Bad grid generation";
        return false;
    }

    coord = transectPoints[pointIndex];
    return true;
}

809 810 811 812 813 814 815 816 817
void SurveyComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
    qCDebug(SurveyComplexItemLog) << "_buildAndAppendMissionItems";

    // Now build the mission items from the transect points

    MissionItem* item;
    int seqNum =                    _sequenceNumber;
    bool imagesEverywhere =         _cameraTriggerInTurnAroundFact.rawValue().toBool();
DonLakeFlyer's avatar
DonLakeFlyer committed
818
    bool addTriggerAtBeginning =    !hoverAndCaptureEnabled() && imagesEverywhere;
819 820 821 822
    bool firstOverallPoint =        true;

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

823
    for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
824 825
        bool entryPoint = true;

826
        for (const CoordInfo_t& transectCoordInfo: transect) {
827 828 829
            item = new MissionItem(seqNum++,
                                   MAV_CMD_NAV_WAYPOINT,
                                   mavFrame,
DonLakeFlyer's avatar
DonLakeFlyer committed
830 831 832 833 834
                                   hoverAndCaptureEnabled() ?
                                       _hoverAndCaptureDelaySeconds : 0,        // Hold time (delay for hover and capture to settle vehicle before image is taken)
                                   0.0,                                         // No acceptance radius specified
                                   0.0,                                         // Pass through waypoint
                                   std::numeric_limits<double>::quiet_NaN(),    // Yaw unchanged
835 836 837
                                   transectCoordInfo.coord.latitude(),
                                   transectCoordInfo.coord.longitude(),
                                   transectCoordInfo.coord.altitude(),
DonLakeFlyer's avatar
DonLakeFlyer committed
838 839
                                   true,                                        // autoContinue
                                   false,                                       // isCurrentItem
840 841
                                   missionItemParent);
            items.append(item);
DonLakeFlyer's avatar
DonLakeFlyer committed
842 843 844 845 846 847 848 849 850 851 852 853 854
            if (hoverAndCaptureEnabled()) {
                item = new MissionItem(seqNum++,
                                       MAV_CMD_IMAGE_START_CAPTURE,
                                       MAV_FRAME_MISSION,
                                       0,                           // Reserved (Set to 0)
                                       0,                           // Interval (none)
                                       1,                           // Take 1 photo
                                       NAN, NAN, NAN, NAN,          // param 4-7 reserved
                                       true,                        // autoContinue
                                       false,                       // isCurrentItem
                                       missionItemParent);
                items.append(item);
            }
855 856 857 858 859 860 861

            if (firstOverallPoint && addTriggerAtBeginning) {
                // Start triggering
                addTriggerAtBeginning = false;
                item = new MissionItem(seqNum++,
                                       MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                       MAV_FRAME_MISSION,
DonLakeFlyer's avatar
DonLakeFlyer committed
862 863 864 865 866 867
                                       triggerDistance(),   // trigger distance
                                       0,                   // shutter integration (ignore)
                                       1,                   // trigger immediately when starting
                                       0, 0, 0, 0,          // param 4-7 unused
                                       true,                // autoContinue
                                       false,               // isCurrentItem
868 869 870 871 872
                                       missionItemParent);
                items.append(item);
            }
            firstOverallPoint = false;

DonLakeFlyer's avatar
DonLakeFlyer committed
873
            if (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge && triggerCamera() && !hoverAndCaptureEnabled() && !imagesEverywhere) {
874 875 876 877 878
                if (entryPoint) {
                    // Start of transect, start triggering
                    item = new MissionItem(seqNum++,
                                           MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                           MAV_FRAME_MISSION,
DonLakeFlyer's avatar
DonLakeFlyer committed
879 880 881 882 883 884
                                           triggerDistance(),   // trigger distance
                                           0,                   // shutter integration (ignore)
                                           1,                   // trigger immediately when starting
                                           0, 0, 0, 0,          // param 4-7 unused
                                           true,                // autoContinue
                                           false,               // isCurrentItem
885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905
                                           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;
            }
        }
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
906
    if (triggerCamera() && !hoverAndCaptureEnabled() &&  imagesEverywhere) {
907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923
        // Stop triggering
        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);
    }
}

#if 0
bool SurveyComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
924
{
925
    int seqNum = _sequenceNumber;
926 927
    bool firstWaypointTrigger = false;

928 929 930 931 932 933
#if 1
    // FIXME: Hack
    bool hasRefly = false;
    bool buildRefly = false;
#endif

934
    qCDebug(SurveyComplexItemLog) << QStringLiteral("hasTurnaround(%1) triggerCamera(%2) hoverAndCapture(%3) imagesEverywhere(%4) hasRefly(%5) buildRefly(%6) ").arg(_hasTurnaround()).arg(_triggerCamera()).arg(_hoverAndCaptureEnabled()).arg(_imagesEverywhere()).arg(hasRefly).arg(buildRefly);
935

936
#if 0
937
    QList<QList<QGeoCoordinate>>& transectSegments = buildRefly ? _reflyTransectSegments : _transectSegments;
938
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
939

940
    if (!buildRefly && _imagesEverywhere()) {
941
        firstWaypointTrigger = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
942 943
    }

944
    for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
DonLakeFlyer's avatar
DonLakeFlyer committed
945 946
        int pointIndex = 0;
        QGeoCoordinate coord;
947
        CameraTriggerCode cameraTrigger;
DonLakeFlyer's avatar
DonLakeFlyer committed
948

949
        qCDebug(SurveyComplexItemLog) << "transect.count" << transect.count();
DonLakeFlyer's avatar
DonLakeFlyer committed
950 951 952

        if (_hasTurnaround()) {
            // Add entry turnaround point
953 954
            if (!_nextTransectCoord(segment, pointIndex++, coord)) {
                return false;
DonLakeFlyer's avatar
DonLakeFlyer committed
955
            }
956 957
            seqNum = _appendWaypointToMission(items, seqNum, coord, firstWaypointTrigger ? CameraTriggerOn : CameraTriggerNone, missionItemParent);
            firstWaypointTrigger = false;
DonLakeFlyer's avatar
DonLakeFlyer committed
958
        }
959

DonLakeFlyer's avatar
DonLakeFlyer committed
960
        // Add polygon entry point
961 962
        if (!_nextTransectCoord(segment, pointIndex++, coord)) {
            return false;
DonLakeFlyer's avatar
DonLakeFlyer committed
963
        }
964 965 966 967 968
        if (firstWaypointTrigger) {
            cameraTrigger = CameraTriggerOn;
        } else {
            cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerHoverAndCapture : CameraTriggerOn);
        }
969
        seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent);
970
        firstWaypointTrigger = false;
971

DonLakeFlyer's avatar
DonLakeFlyer committed
972 973
        // Add internal hover and capture points
        if (_hoverAndCaptureEnabled()) {
974
            int lastHoverAndCaptureIndex = segment.count() - 1 - (_hasTurnaround() ? 1 : 0);
975
            qCDebug(SurveyComplexItemLog) << "lastHoverAndCaptureIndex" << lastHoverAndCaptureIndex;
DonLakeFlyer's avatar
DonLakeFlyer committed
976
            for (; pointIndex < lastHoverAndCaptureIndex; pointIndex++) {
977 978
                if (!_nextTransectCoord(segment, pointIndex, coord)) {
                    return false;
DonLakeFlyer's avatar
DonLakeFlyer committed
979 980
                }
                seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerHoverAndCapture, missionItemParent);
981 982
            }
        }
983

DonLakeFlyer's avatar
DonLakeFlyer committed
984
        // Add polygon exit point
985 986
        if (!_nextTransectCoord(segment, pointIndex++, coord)) {
            return false;
987
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
988
        cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerNone : CameraTriggerOff);
989 990
        seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent);

DonLakeFlyer's avatar
DonLakeFlyer committed
991 992
        if (_hasTurnaround()) {
            // Add exit turnaround point
993 994
            if (!_nextTransectCoord(segment, pointIndex++, coord)) {
                return false;
995
            }
DonLakeFlyer's avatar
DonLakeFlyer committed
996
            seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent);
997
        }
998

999
        qCDebug(SurveyComplexItemLog) << "last PointIndex" << pointIndex;
1000 1001
    }

1002
    if (((hasRefly && buildRefly) || !hasRefly) && _imagesEverywhere()) {
1003 1004 1005 1006 1007 1008 1009 1010
        // Turn off camera at end of survey
        MissionItem* item = new MissionItem(seqNum++,
                                            MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                            MAV_FRAME_MISSION,
                                            0.0,                    // trigger distance (off)
                                            0, 0, 0, 0, 0, 0,       // param 2-7 unused
                                            true,                   // autoContinue
                                            false,                  // isCurrentItem
1011 1012
                                            missionItemParent);
        items.append(item);
1013
    }
1014 1015 1016

    return true;
}
1017
#endif
1018

1019
#if 0
1020
void SurveyComplexItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
1021 1022
{
    int seqNum = _sequenceNumber;
1023
    bool refly = refly90Degrees()->rawValue().toBool();
1024

1025
    if (!_appendMissionItemsWorker(items, missionItemParent, seqNum, refly, false /* buildRefly */)) {
1026 1027 1028
        return;
    }

1029 1030
    if (refly) {
        _appendMissionItemsWorker(items, missionItemParent, seqNum, refly, true /* buildRefly */);
1031 1032
    }
}
1033
#endif
1034

1035
bool SurveyComplexItem::_hasTurnaround(void) const
1036
{
1037
    return _turnaroundDistance() > 0;
1038
}
DonLakeFlyer's avatar
DonLakeFlyer committed
1039

1040
double SurveyComplexItem::_turnaroundDistance(void) const
DonLakeFlyer's avatar
DonLakeFlyer committed
1041
{
1042
    return _turnAroundDistanceFact.rawValue().toDouble();
DonLakeFlyer's avatar
DonLakeFlyer committed
1043 1044
}

1045
#if 0
1046
bool SurveyComplexItem::_triggerCamera(void) const
DonLakeFlyer's avatar
DonLakeFlyer committed
1047
{
1048
    return _triggerDistance() > 0;
DonLakeFlyer's avatar
DonLakeFlyer committed
1049 1050
}

1051
bool SurveyComplexItem::_imagesEverywhere(void) const
DonLakeFlyer's avatar
DonLakeFlyer committed
1052 1053 1054 1055
{
    return _triggerCamera() && _cameraTriggerInTurnaroundFact.rawValue().toBool();
}

1056
bool SurveyComplexItem::_hoverAndCaptureEnabled(void) const
DonLakeFlyer's avatar
DonLakeFlyer committed
1057 1058 1059
{
    return hoverAndCaptureAllowed() && !_imagesEverywhere() && _triggerCamera() && _hoverAndCaptureFact.rawValue().toBool();
}
1060
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
1061

1062
void SurveyComplexItem::_rebuildTransectsPhase1(void)
1063 1064 1065 1066 1067 1068 1069 1070
{
    _rebuildTransectsPhase1Worker(false /* refly */);
    if (_refly90DegreesFact.rawValue().toBool()) {
        _rebuildTransectsPhase1Worker(true /* refly */);
    }
}

void SurveyComplexItem::_rebuildTransectsPhase1Worker(bool refly)
DonLakeFlyer's avatar
DonLakeFlyer committed
1071
{
1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082
    if (_ignoreRecalc) {
        return;
    }

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

1083 1084 1085 1086 1087
    // First pass will clear old transect data, refly will append to existing data
    if (!refly) {
        _transects.clear();
        _transectsPathHeightInfo.clear();
    }
1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110

    if (_surveyAreaPolygon.count() < 3) {
        return;
    }

    // Convert polygon to NED

    QList<QPointF> polygonPoints;
    QGeoCoordinate tangentOrigin = _surveyAreaPolygon.pathModel().value<QGCQGeoCoordinate*>(0)->coordinate();
    qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 Convert polygon to NED - _surveyAreaPolygon.count():tangentOrigin" << _surveyAreaPolygon.count() << tangentOrigin;
    for (int i=0; i<_surveyAreaPolygon.count(); i++) {
        double y, x, down;
        QGeoCoordinate vertex = _surveyAreaPolygon.pathModel().value<QGCQGeoCoordinate*>(i)->coordinate();
        if (i == 0) {
            // This avoids a nan calculation that comes out of convertGeoToNed
            x = y = 0;
        } else {
            convertGeoToNed(vertex, tangentOrigin, &y, &x, &down);
        }
        polygonPoints += QPointF(x, y);
        qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y();
    }

1111 1112 1113 1114 1115 1116 1117
    // convert into QPolygonF
    QPolygonF polygon;
    for (int i=0; i<polygonPoints.count(); i++) {
        qCDebug(SurveyComplexItemLog) << "Vertex" << polygonPoints[i];
        polygon << polygonPoints[i];
    }

1118
    // Create list of separate polygons
1119
    QList<QPolygonF> polygons{};
1120
    qCDebug(PolygonDecomposeLog) << "*********_PolygonDecomposeConvex begin of recursion**************";
1121
    _PolygonDecomposeConvex(polygon, polygons);
1122
    qCDebug(PolygonDecomposeLog) << "polygons.size() " << polygons.size() ;
1123 1124

    // iterate over polygons
1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143
    for (auto p = polygons.begin(); p != polygons.end(); ++p) {
        QPointF* vMatch = nullptr;
        // find matching vertex in previous polygon
        if (p != polygons.begin()) {
            auto pLast = p - 1;
            for (auto& i : *p) {
                for (auto& j : *pLast) {
                   if (i == j) {
                       vMatch = &i;
                       break;
                   }
                   if (vMatch) break;
                }
            }
            if (nullptr == vMatch) qCDebug(PolygonDecomposeLog) << "no match found";

        }


1144
        // close polygon
1145
        *p << p->front();
1146 1147
        // build transects for this polygon
        // TODO figure out tangent origin
1148 1149 1150
        // TODO improve selection of entry points
//        qCDebug(SurveyComplexItemLog) << "Transects from polynom p " << p;
        _rebuildTranscetsFromPolygon(refly, *p, tangentOrigin, vMatch);
1151
    }
1152 1153
}

1154
void SurveyComplexItem::_PolygonDecomposeConvex(const QPolygonF& polygon, QList<QPolygonF>& decomposedPolygons)
1155
{
1156 1157
//    qCDebug(SurveyComplexItemLog) << "_PolygonDecomposeConvex polygon.size() " << polygon.size();
    int decompSize = std::numeric_limits<int>::max();
1158
    if (polygon.size() < 3) return;
1159 1160 1161 1162 1163
    if (polygon.size() == 3) {
        decomposedPolygons << polygon;
//        qCDebug(PolygonDecomposeLog) << polygon << " polygon of 3";
        return;
    }
1164 1165 1166 1167 1168 1169

    QList<QPolygonF> decomposedPolygonsMin{};

    for (auto vertex = polygon.begin(); vertex != polygon.end(); ++vertex)
    {
        // is vertex reflex?
1170 1171
        bool vertexIsReflex = _VertexIsReflex(polygon, vertex);
//        qCDebug(SurveyComplexItemLog) << "area " << area << " vertexIsReflex " << vertexIsReflex;
1172 1173 1174 1175 1176

        if (!vertexIsReflex) continue;

        for (auto vertexOther = polygon.begin(); vertexOther != polygon.end(); ++vertexOther)
        {
1177 1178
            auto vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1;
            auto vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1;
1179
            if (vertexOther == vertex) continue;
1180 1181
            if (vertexAfter == vertexOther) continue;
            if (vertexBefore == vertexOther) continue;
1182
            bool canSee = _VertexCanSeeOther(polygon, vertex, vertexOther);
1183
//            qCDebug(SurveyComplexItemLog) << "canSee " << canSee;
1184 1185 1186 1187
            if (!canSee) continue;

            QPolygonF polyLeft;
            auto v = vertex;
1188
            auto polyLeftContainsReflex = false;
1189
            while ( v != vertexOther) {
1190 1191 1192
                if (v != vertex && _VertexIsReflex(polygon, v)) {
                    polyLeftContainsReflex = true;
                }
1193 1194 1195 1196 1197
                polyLeft << *v;
                ++v;
                if (v == polygon.end()) v = polygon.begin();
            }
            polyLeft << *vertexOther;
1198
            auto polyLeftValid = !(polyLeftContainsReflex && polyLeft.size() == 3);
1199 1200 1201

            QPolygonF polyRight;
            v = vertexOther;
1202
            auto polyRightContainsReflex = false;
1203
            while ( v != vertex) {
1204 1205 1206
                if (v != vertex && _VertexIsReflex(polygon, v)) {
                    polyRightContainsReflex = true;
                }
1207 1208 1209 1210 1211
                polyRight << *v;
                ++v;
                if (v == polygon.end()) v = polygon.begin();
            }
            polyRight << *vertex;
1212 1213 1214 1215 1216 1217
            auto polyRightValid = !(polyRightContainsReflex && polyRight.size() == 3);

            if (!polyLeftValid || ! polyRightValid) {
//                decompSize = std::numeric_limits<int>::max();
                continue;
            }
1218 1219 1220

            // recursion
            QList<QPolygonF> polyLeftDecomposed{};
1221
//            qCDebug(PolygonDecomposeLog) << " polyLeft "<< polyLeft;
1222
            _PolygonDecomposeConvex(polyLeft, polyLeftDecomposed);
1223

1224
            QList<QPolygonF> polyRightDecomposed{};
1225
//            qCDebug(PolygonDecomposeLog) << " polyRight "<< polyRight;
1226 1227 1228
            _PolygonDecomposeConvex(polyRight, polyRightDecomposed);

            // compositon
1229 1230 1231 1232 1233 1234 1235 1236 1237
            auto subSize = polyLeftDecomposed.size() + polyRightDecomposed.size();
            if ((polyLeftContainsReflex && polyLeftDecomposed.size() == 1)
                    || (polyRightContainsReflex && polyRightDecomposed.size() == 1))
            {
                // don't accept polygons that contian reflex vertices and were not split
                subSize = std::numeric_limits<int>::max();
            }
            if (subSize < decompSize) {
                decompSize = subSize;
1238
                decomposedPolygonsMin = polyLeftDecomposed + polyRightDecomposed;
1239 1240 1241 1242
//            qCDebug(PolygonDecomposeLog) << "_PolygonDecomposeConvex polygon " << polygon;
//            qCDebug(PolygonDecomposeLog) << "polyLeft.size() " << polyLeft.size() << " polyRight.size() " << polyRight.size() << " out of " << polygon.size();
//            qCDebug(PolygonDecomposeLog) << "vertex " << *vertex << " vertexOther " << *vertexOther << " vertexAfter " << *vertexAfter << " vertexBefore " << *vertexBefore;
//            qCDebug(SurveyComplexItemLog) << "changing decomposedPolygonsMin";
1243 1244
            }
            else {
1245
//                qCDebug(SurveyComplexItemLog) << "NOT changing decomposedPolygonsMin";
1246 1247 1248 1249 1250 1251 1252
            }
        }

    }

    // assemble output
    if (decomposedPolygonsMin.size() > 0) {
1253
//        qCDebug(SurveyComplexItemLog) << "use decomposed polygon, decomposedPolygonsMin.size() " << decomposedPolygonsMin.size();
1254
        decomposedPolygons << decomposedPolygonsMin;
1255
//        qCDebug(PolygonDecomposeLog) << decomposedPolygonsMin;
1256
    } else {
1257
//        qCDebug(SurveyComplexItemLog) << "use default polygon";
Thomas Gubler's avatar