SurveyMissionItem.cc 57.3 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14
/****************************************************************************
 *
 *   (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 "SurveyMissionItem.h"
#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 22 23

#include <QPolygonF>

QGC_LOGGING_CATEGORY(SurveyMissionItemLog, "SurveyMissionItemLog")

Don Gagne's avatar
Don Gagne committed
24 25
const char* SurveyMissionItem::jsonComplexItemTypeValue =           "survey";

Don Gagne's avatar
Don Gagne committed
26 27 28 29 30
const char* SurveyMissionItem::_jsonGridObjectKey =                 "grid";
const char* SurveyMissionItem::_jsonGridAltitudeKey =               "altitude";
const char* SurveyMissionItem::_jsonGridAltitudeRelativeKey =       "relativeAltitude";
const char* SurveyMissionItem::_jsonGridAngleKey =                  "angle";
const char* SurveyMissionItem::_jsonGridSpacingKey =                "spacing";
DonLakeFlyer's avatar
DonLakeFlyer committed
31
const char* SurveyMissionItem::_jsonGridEntryLocationKey =          "entryLocation";
Don Gagne's avatar
Don Gagne committed
32 33
const char* SurveyMissionItem::_jsonTurnaroundDistKey =             "turnAroundDistance";
const char* SurveyMissionItem::_jsonCameraTriggerDistanceKey =      "cameraTriggerDistance";
34 35
const char* SurveyMissionItem::_jsonCameraTriggerInTurnaroundKey =  "cameraTriggerInTurnaround";
const char* SurveyMissionItem::_jsonHoverAndCaptureKey =            "hoverAndCapture";
Don Gagne's avatar
Don Gagne committed
36 37
const char* SurveyMissionItem::_jsonGroundResolutionKey =           "groundResolution";
const char* SurveyMissionItem::_jsonFrontalOverlapKey =             "imageFrontalOverlap";
38
const char* SurveyMissionItem::_jsonSideOverlapKey =                "imageSideOverlap";
Don Gagne's avatar
Don Gagne committed
39 40 41 42 43
const char* SurveyMissionItem::_jsonCameraSensorWidthKey =          "sensorWidth";
const char* SurveyMissionItem::_jsonCameraSensorHeightKey =         "sensorHeight";
const char* SurveyMissionItem::_jsonCameraResolutionWidthKey =      "resolutionWidth";
const char* SurveyMissionItem::_jsonCameraResolutionHeightKey =     "resolutionHeight";
const char* SurveyMissionItem::_jsonCameraFocalLengthKey =          "focalLength";
44
const char* SurveyMissionItem::_jsonCameraMinTriggerIntervalKey =   "minTriggerInterval";
Don Gagne's avatar
Don Gagne committed
45 46 47 48 49
const char* SurveyMissionItem::_jsonCameraObjectKey =               "camera";
const char* SurveyMissionItem::_jsonCameraNameKey =                 "name";
const char* SurveyMissionItem::_jsonManualGridKey =                 "manualGrid";
const char* SurveyMissionItem::_jsonCameraOrientationLandscapeKey = "orientationLandscape";
const char* SurveyMissionItem::_jsonFixedValueIsAltitudeKey =       "fixedValueIsAltitude";
50
const char* SurveyMissionItem::_jsonRefly90DegreesKey =             "refly90Degrees";
Don Gagne's avatar
Don Gagne committed
51

52 53 54 55 56 57
const char* SurveyMissionItem::settingsGroup =                  "Survey";
const char* SurveyMissionItem::manualGridName =                 "ManualGrid";
const char* SurveyMissionItem::gridAltitudeName =               "GridAltitude";
const char* SurveyMissionItem::gridAltitudeRelativeName =       "GridAltitudeRelative";
const char* SurveyMissionItem::gridAngleName =                  "GridAngle";
const char* SurveyMissionItem::gridSpacingName =                "GridSpacing";
DonLakeFlyer's avatar
DonLakeFlyer committed
58
const char* SurveyMissionItem::gridEntryLocationName =          "GridEntryLocation";
59 60
const char* SurveyMissionItem::turnaroundDistName =             "TurnaroundDist";
const char* SurveyMissionItem::cameraTriggerDistanceName =      "CameraTriggerDistance";
61 62
const char* SurveyMissionItem::cameraTriggerInTurnaroundName =  "CameraTriggerInTurnaround";
const char* SurveyMissionItem::hoverAndCaptureName =            "HoverAndCapture";
63 64 65 66 67 68 69 70 71 72 73 74
const char* SurveyMissionItem::groundResolutionName =           "GroundResolution";
const char* SurveyMissionItem::frontalOverlapName =             "FrontalOverlap";
const char* SurveyMissionItem::sideOverlapName =                "SideOverlap";
const char* SurveyMissionItem::cameraSensorWidthName =          "CameraSensorWidth";
const char* SurveyMissionItem::cameraSensorHeightName =         "CameraSensorHeight";
const char* SurveyMissionItem::cameraResolutionWidthName =      "CameraResolutionWidth";
const char* SurveyMissionItem::cameraResolutionHeightName =     "CameraResolutionHeight";
const char* SurveyMissionItem::cameraFocalLengthName =          "CameraFocalLength";
const char* SurveyMissionItem::cameraTriggerName =              "CameraTrigger";
const char* SurveyMissionItem::cameraOrientationLandscapeName = "CameraOrientationLandscape";
const char* SurveyMissionItem::fixedValueIsAltitudeName =       "FixedValueIsAltitude";
const char* SurveyMissionItem::cameraName =                     "Camera";
75

76 77 78 79
SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent)
    : ComplexMissionItem(vehicle, parent)
    , _sequenceNumber(0)
    , _dirty(false)
80
    , _mapPolygon(this)
81
    , _cameraOrientationFixed(false)
DonLakeFlyer's avatar
DonLakeFlyer committed
82
    , _missionCommandCount(0)
83
    , _refly90Degrees(false)
84
    , _additionalFlightDelaySeconds(0)
85
    , _cameraMinTriggerInterval(0)
86
    , _ignoreRecalc(false)
87 88 89
    , _surveyDistance(0.0)
    , _cameraShots(0)
    , _coveredArea(0.0)
90
    , _timeBetweenShots(0.0)
91 92 93 94 95 96
    , _metaDataMap(FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/Survey.SettingsGroup.json"), this))
    , _manualGridFact                   (settingsGroup, _metaDataMap[manualGridName])
    , _gridAltitudeFact                 (settingsGroup, _metaDataMap[gridAltitudeName])
    , _gridAltitudeRelativeFact         (settingsGroup, _metaDataMap[gridAltitudeRelativeName])
    , _gridAngleFact                    (settingsGroup, _metaDataMap[gridAngleName])
    , _gridSpacingFact                  (settingsGroup, _metaDataMap[gridSpacingName])
DonLakeFlyer's avatar
DonLakeFlyer committed
97
    , _gridEntryLocationFact            (settingsGroup, _metaDataMap[gridEntryLocationName])
98 99
    , _turnaroundDistFact               (settingsGroup, _metaDataMap[turnaroundDistName])
    , _cameraTriggerDistanceFact        (settingsGroup, _metaDataMap[cameraTriggerDistanceName])
100 101
    , _cameraTriggerInTurnaroundFact    (settingsGroup, _metaDataMap[cameraTriggerInTurnaroundName])
    , _hoverAndCaptureFact              (settingsGroup, _metaDataMap[hoverAndCaptureName])
102 103 104 105 106 107 108 109 110 111 112
    , _groundResolutionFact             (settingsGroup, _metaDataMap[groundResolutionName])
    , _frontalOverlapFact               (settingsGroup, _metaDataMap[frontalOverlapName])
    , _sideOverlapFact                  (settingsGroup, _metaDataMap[sideOverlapName])
    , _cameraSensorWidthFact            (settingsGroup, _metaDataMap[cameraSensorWidthName])
    , _cameraSensorHeightFact           (settingsGroup, _metaDataMap[cameraSensorHeightName])
    , _cameraResolutionWidthFact        (settingsGroup, _metaDataMap[cameraResolutionWidthName])
    , _cameraResolutionHeightFact       (settingsGroup, _metaDataMap[cameraResolutionHeightName])
    , _cameraFocalLengthFact            (settingsGroup, _metaDataMap[cameraFocalLengthName])
    , _cameraOrientationLandscapeFact   (settingsGroup, _metaDataMap[cameraOrientationLandscapeName])
    , _fixedValueIsAltitudeFact         (settingsGroup, _metaDataMap[fixedValueIsAltitudeName])
    , _cameraFact                       (settingsGroup, _metaDataMap[cameraName])
113
{
114 115
    _editorQml = "qrc:/qml/SurveyItemEditor.qml";

116
    // 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.
117
    // NULL check since object creation during unit testing passes NULL for vehicle
118
    if (_vehicle && _vehicle->multiRotor() && _turnaroundDistFact.rawValue().toDouble() == _turnaroundDistFact.rawDefaultValue().toDouble()) {
119 120
        // 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.
        _turnaroundDistFact.setRawValue(10);
121
    }
Don Gagne's avatar
Don Gagne committed
122

123 124 125 126 127
    // We override the grid altitude to the mission default
    if (_manualGridFact.rawValue().toBool() || _fixedValueIsAltitudeFact.rawValue().toBool()) {
        _gridAltitudeFact.setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue());
    }

128 129
    connect(&_gridSpacingFact,                  &Fact::valueChanged,                        this, &SurveyMissionItem::_generateGrid);
    connect(&_gridAngleFact,                    &Fact::valueChanged,                        this, &SurveyMissionItem::_generateGrid);
DonLakeFlyer's avatar
DonLakeFlyer committed
130
    connect(&_gridEntryLocationFact,            &Fact::valueChanged,                        this, &SurveyMissionItem::_generateGrid);
131 132 133 134 135
    connect(&_turnaroundDistFact,               &Fact::valueChanged,                        this, &SurveyMissionItem::_generateGrid);
    connect(&_cameraTriggerDistanceFact,        &Fact::valueChanged,                        this, &SurveyMissionItem::_generateGrid);
    connect(&_cameraTriggerInTurnaroundFact,    &Fact::valueChanged,                        this, &SurveyMissionItem::_generateGrid);
    connect(&_hoverAndCaptureFact,              &Fact::valueChanged,                        this, &SurveyMissionItem::_generateGrid);
    connect(this,                               &SurveyMissionItem::refly90DegreesChanged,  this, &SurveyMissionItem::_generateGrid);
136

Don Gagne's avatar
Don Gagne committed
137 138 139
    connect(&_gridAltitudeFact,                 &Fact::valueChanged, this, &SurveyMissionItem::_updateCoordinateAltitude);

    connect(&_gridAltitudeRelativeFact,         &Fact::valueChanged, this, &SurveyMissionItem::_setDirty);
140

141 142 143 144 145 146 147 148 149 150
    // Signal to Qml when camera value changes so it can recalc
    connect(&_groundResolutionFact,             &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged);
    connect(&_frontalOverlapFact,               &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged);
    connect(&_sideOverlapFact,                  &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged);
    connect(&_cameraSensorWidthFact,            &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged);
    connect(&_cameraSensorHeightFact,           &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged);
    connect(&_cameraResolutionWidthFact,        &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged);
    connect(&_cameraResolutionHeightFact,       &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged);
    connect(&_cameraFocalLengthFact,            &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged);
    connect(&_cameraOrientationLandscapeFact,   &Fact::valueChanged, this, &SurveyMissionItem::_cameraValueChanged);
151

Don Gagne's avatar
Don Gagne committed
152
    connect(&_cameraTriggerDistanceFact, &Fact::valueChanged, this, &SurveyMissionItem::timeBetweenShotsChanged);
153 154 155

    connect(&_mapPolygon, &QGCMapPolygon::dirtyChanged, this, &SurveyMissionItem::_polygonDirtyChanged);
    connect(&_mapPolygon, &QGCMapPolygon::pathChanged,  this, &SurveyMissionItem::_generateGrid);
156 157 158 159 160 161 162 163 164 165 166 167 168 169
}

void SurveyMissionItem::_setSurveyDistance(double surveyDistance)
{
    if (!qFuzzyCompare(_surveyDistance, surveyDistance)) {
        _surveyDistance = surveyDistance;
        emit complexDistanceChanged(_surveyDistance);
    }
}

void SurveyMissionItem::_setCameraShots(int cameraShots)
{
    if (_cameraShots != cameraShots) {
        _cameraShots = cameraShots;
Don Gagne's avatar
Don Gagne committed
170
        emit cameraShotsChanged(this->cameraShots());
171 172 173 174 175 176 177 178 179 180 181
    }
}

void SurveyMissionItem::_setCoveredArea(double coveredArea)
{
    if (!qFuzzyCompare(_coveredArea, coveredArea)) {
        _coveredArea = coveredArea;
        emit coveredAreaChanged(_coveredArea);
    }
}

182
void SurveyMissionItem::_clearInternal(void)
183
{
184 185 186
    // Bug workaround
    while (_simpleGridPoints.count() > 1) {
        _simpleGridPoints.takeLast();
187
    }
188 189 190
    emit gridPointsChanged();
    _simpleGridPoints.clear();
    _transectSegments.clear();
191

192
    _missionCommandCount = 0;
193

194 195 196 197 198 199 200 201
    setDirty(true);

    emit specifiesCoordinateChanged();
    emit lastSequenceNumberChanged(lastSequenceNumber());
}

int SurveyMissionItem::lastSequenceNumber(void) const
{
DonLakeFlyer's avatar
DonLakeFlyer committed
202
    return _sequenceNumber + _missionCommandCount;
203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220
}

void SurveyMissionItem::setCoordinate(const QGeoCoordinate& coordinate)
{
    if (_coordinate != coordinate) {
        _coordinate = coordinate;
        emit coordinateChanged(_coordinate);
    }
}

void SurveyMissionItem::setDirty(bool dirty)
{
    if (_dirty != dirty) {
        _dirty = dirty;
        emit dirtyChanged(_dirty);
    }
}

221
void SurveyMissionItem::save(QJsonArray&  missionItems)
222
{
223 224
    QJsonObject saveObject;

Don Gagne's avatar
Don Gagne committed
225 226 227
    saveObject[JsonHelper::jsonVersionKey] =                    3;
    saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
    saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;
228 229
    saveObject[_jsonManualGridKey] =                            _manualGridFact.rawValue().toBool();
    saveObject[_jsonFixedValueIsAltitudeKey] =                  _fixedValueIsAltitudeFact.rawValue().toBool();
DonLakeFlyer's avatar
DonLakeFlyer committed
230
    saveObject[_jsonHoverAndCaptureKey] =                       _hoverAndCaptureFact.rawValue().toBool();
231
    saveObject[_jsonRefly90DegreesKey] =                        _refly90Degrees;
232
    saveObject[_jsonCameraTriggerDistanceKey] =                 _cameraTriggerDistanceFact.rawValue().toDouble();
233
    saveObject[_jsonCameraTriggerInTurnaroundKey] =             _cameraTriggerInTurnaroundFact.rawValue().toBool();
Don Gagne's avatar
Don Gagne committed
234 235 236

    QJsonObject gridObject;
    gridObject[_jsonGridAltitudeKey] =          _gridAltitudeFact.rawValue().toDouble();
237
    gridObject[_jsonGridAltitudeRelativeKey] =  _gridAltitudeRelativeFact.rawValue().toBool();
Don Gagne's avatar
Don Gagne committed
238 239
    gridObject[_jsonGridAngleKey] =             _gridAngleFact.rawValue().toDouble();
    gridObject[_jsonGridSpacingKey] =           _gridSpacingFact.rawValue().toDouble();
240
    gridObject[_jsonGridEntryLocationKey] =     _gridEntryLocationFact.rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
241 242 243 244
    gridObject[_jsonTurnaroundDistKey] =        _turnaroundDistFact.rawValue().toDouble();

    saveObject[_jsonGridObjectKey] = gridObject;

245
    if (!_manualGridFact.rawValue().toBool()) {
Don Gagne's avatar
Don Gagne committed
246
        QJsonObject cameraObject;
247 248
        cameraObject[_jsonCameraNameKey] =                  _cameraFact.rawValue().toString();
        cameraObject[_jsonCameraOrientationLandscapeKey] =  _cameraOrientationLandscapeFact.rawValue().toBool();
Don Gagne's avatar
Don Gagne committed
249 250 251 252 253
        cameraObject[_jsonCameraSensorWidthKey] =           _cameraSensorWidthFact.rawValue().toDouble();
        cameraObject[_jsonCameraSensorHeightKey] =          _cameraSensorHeightFact.rawValue().toDouble();
        cameraObject[_jsonCameraResolutionWidthKey] =       _cameraResolutionWidthFact.rawValue().toDouble();
        cameraObject[_jsonCameraResolutionHeightKey] =      _cameraResolutionHeightFact.rawValue().toDouble();
        cameraObject[_jsonCameraFocalLengthKey] =           _cameraFocalLengthFact.rawValue().toDouble();
254
        cameraObject[_jsonCameraMinTriggerIntervalKey] =    _cameraMinTriggerInterval;
Don Gagne's avatar
Don Gagne committed
255 256 257 258 259 260
        cameraObject[_jsonGroundResolutionKey] =            _groundResolutionFact.rawValue().toDouble();
        cameraObject[_jsonFrontalOverlapKey] =              _frontalOverlapFact.rawValue().toInt();
        cameraObject[_jsonSideOverlapKey] =                 _sideOverlapFact.rawValue().toInt();

        saveObject[_jsonCameraObjectKey] = cameraObject;
    }
261 262

    // Polygon shape
263
    _mapPolygon.saveToJson(saveObject);
264 265

    missionItems.append(saveObject);
266 267 268 269 270 271 272 273 274 275 276
}

void SurveyMissionItem::setSequenceNumber(int sequenceNumber)
{
    if (_sequenceNumber != sequenceNumber) {
        _sequenceNumber = sequenceNumber;
        emit sequenceNumberChanged(sequenceNumber);
        emit lastSequenceNumberChanged(lastSequenceNumber());
    }
}

Don Gagne's avatar
Don Gagne committed
277
bool SurveyMissionItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
278
{
Don Gagne's avatar
Don Gagne committed
279
    QJsonObject v2Object = complexObject;
Don Gagne's avatar
Don Gagne committed
280

Don Gagne's avatar
Don Gagne committed
281 282 283
    // We need to pull version first to determine what validation/conversion needs to be performed.
    QList<JsonHelper::KeyValidateInfo> versionKeyInfoList = {
        { JsonHelper::jsonVersionKey, QJsonValue::Double, true },
Don Gagne's avatar
Don Gagne committed
284
    };
Don Gagne's avatar
Don Gagne committed
285
    if (!JsonHelper::validateKeys(v2Object, versionKeyInfoList, errorString)) {
286 287
        return false;
    }
Don Gagne's avatar
Don Gagne committed
288 289 290

    int version = v2Object[JsonHelper::jsonVersionKey].toInt();
    if (version != 2 && version != 3) {
291
        errorString = tr("%1 does not support this version of survey items").arg(qgcApp()->applicationName());
292 293
        return false;
    }
Don Gagne's avatar
Don Gagne committed
294 295 296 297 298 299 300
    if (version == 2) {
        // Convert to v3
        if (v2Object.contains(VisualMissionItem::jsonTypeKey) && v2Object[VisualMissionItem::jsonTypeKey].toString() == QStringLiteral("survey")) {
            v2Object[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue;
            v2Object[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue;
        }
    }
301

Don Gagne's avatar
Don Gagne committed
302 303 304 305
    QList<JsonHelper::KeyValidateInfo> mainKeyInfoList = {
        { JsonHelper::jsonVersionKey,                   QJsonValue::Double, true },
        { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
        { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
306
        { QGCMapPolygon::jsonPolygonKey,                QJsonValue::Array,  true },
Don Gagne's avatar
Don Gagne committed
307 308
        { _jsonGridObjectKey,                           QJsonValue::Object, true },
        { _jsonCameraObjectKey,                         QJsonValue::Object, false },
309
        { _jsonCameraTriggerDistanceKey,                QJsonValue::Double, true },
Don Gagne's avatar
Don Gagne committed
310 311
        { _jsonManualGridKey,                           QJsonValue::Bool,   true },
        { _jsonFixedValueIsAltitudeKey,                 QJsonValue::Bool,   true },
DonLakeFlyer's avatar
DonLakeFlyer committed
312
        { _jsonHoverAndCaptureKey,                      QJsonValue::Bool,   false },
313
        { _jsonRefly90DegreesKey,                       QJsonValue::Bool,   false },
314
        { _jsonCameraTriggerInTurnaroundKey,            QJsonValue::Bool,   false },    // Should really be required, but it was missing from initial code due to bug
Don Gagne's avatar
Don Gagne committed
315 316
    };
    if (!JsonHelper::validateKeys(v2Object, mainKeyInfoList, errorString)) {
317 318
        return false;
    }
Don Gagne's avatar
Don Gagne committed
319 320 321 322

    QString itemType = v2Object[VisualMissionItem::jsonTypeKey].toString();
    QString complexType = v2Object[ComplexMissionItem::jsonComplexItemTypeKey].toString();
    if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) {
323
        errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
324 325 326
        return false;
    }

327 328
    _ignoreRecalc = true;

329
    _mapPolygon.clear();
Don Gagne's avatar
Don Gagne committed
330

Don Gagne's avatar
Don Gagne committed
331
    setSequenceNumber(sequenceNumber);
332

333 334 335 336 337
    _manualGridFact.setRawValue                 (v2Object[_jsonManualGridKey].toBool(true));
    _fixedValueIsAltitudeFact.setRawValue       (v2Object[_jsonFixedValueIsAltitudeKey].toBool(true));
    _gridAltitudeRelativeFact.setRawValue       (v2Object[_jsonGridAltitudeRelativeKey].toBool(true));
    _hoverAndCaptureFact.setRawValue            (v2Object[_jsonHoverAndCaptureKey].toBool(false));
    _cameraTriggerInTurnaroundFact.setRawValue  (v2Object[_jsonCameraTriggerInTurnaroundKey].toBool(true));
Don Gagne's avatar
Don Gagne committed
338

339 340
    _refly90Degrees = v2Object[_jsonRefly90DegreesKey].toBool(false);

Don Gagne's avatar
Don Gagne committed
341 342 343 344 345
    QList<JsonHelper::KeyValidateInfo> gridKeyInfoList = {
        { _jsonGridAltitudeKey,                 QJsonValue::Double, true },
        { _jsonGridAltitudeRelativeKey,         QJsonValue::Bool,   true },
        { _jsonGridAngleKey,                    QJsonValue::Double, true },
        { _jsonGridSpacingKey,                  QJsonValue::Double, true },
DonLakeFlyer's avatar
DonLakeFlyer committed
346
        { _jsonGridEntryLocationKey,            QJsonValue::Double, false },
Don Gagne's avatar
Don Gagne committed
347 348 349 350 351 352
        { _jsonTurnaroundDistKey,               QJsonValue::Double, true },
    };
    QJsonObject gridObject = v2Object[_jsonGridObjectKey].toObject();
    if (!JsonHelper::validateKeys(gridObject, gridKeyInfoList, errorString)) {
        return false;
    }
353 354 355 356 357
    _gridAltitudeFact.setRawValue           (gridObject[_jsonGridAltitudeKey].toDouble());
    _gridAngleFact.setRawValue              (gridObject[_jsonGridAngleKey].toDouble());
    _gridSpacingFact.setRawValue            (gridObject[_jsonGridSpacingKey].toDouble());
    _turnaroundDistFact.setRawValue         (gridObject[_jsonTurnaroundDistKey].toDouble());
    _cameraTriggerDistanceFact.setRawValue  (v2Object[_jsonCameraTriggerDistanceKey].toDouble());
DonLakeFlyer's avatar
DonLakeFlyer committed
358 359 360 361 362
    if (gridObject.contains(_jsonGridEntryLocationKey)) {
        _gridEntryLocationFact.setRawValue(gridObject[_jsonGridEntryLocationKey].toDouble());
    } else {
        _gridEntryLocationFact.setRawValue(_gridEntryLocationFact.rawDefaultValue());
    }
Don Gagne's avatar
Don Gagne committed
363

364
    if (!_manualGridFact.rawValue().toBool()) {
Don Gagne's avatar
Don Gagne committed
365
        if (!v2Object.contains(_jsonCameraObjectKey)) {
Don Gagne's avatar
Don Gagne committed
366 367 368 369
            errorString = tr("%1 but %2 object is missing").arg("manualGrid = false").arg("camera");
            return false;
        }

Don Gagne's avatar
Don Gagne committed
370 371
        QJsonObject cameraObject = v2Object[_jsonCameraObjectKey].toObject();

372 373 374 375 376 377 378
        // Older code had typo on "imageSideOverlap" incorrectly being "imageSizeOverlap"
        QString incorrectImageSideOverlap = "imageSizeOverlap";
        if (cameraObject.contains(incorrectImageSideOverlap)) {
            cameraObject[_jsonSideOverlapKey] = cameraObject[incorrectImageSideOverlap];
            cameraObject.remove(incorrectImageSideOverlap);
        }

Don Gagne's avatar
Don Gagne committed
379 380 381 382 383 384 385 386 387 388 389
        QList<JsonHelper::KeyValidateInfo> cameraKeyInfoList = {
            { _jsonGroundResolutionKey,             QJsonValue::Double, true },
            { _jsonFrontalOverlapKey,               QJsonValue::Double, true },
            { _jsonSideOverlapKey,                  QJsonValue::Double, true },
            { _jsonCameraSensorWidthKey,            QJsonValue::Double, true },
            { _jsonCameraSensorHeightKey,           QJsonValue::Double, true },
            { _jsonCameraResolutionWidthKey,        QJsonValue::Double, true },
            { _jsonCameraResolutionHeightKey,       QJsonValue::Double, true },
            { _jsonCameraFocalLengthKey,            QJsonValue::Double, true },
            { _jsonCameraNameKey,                   QJsonValue::String, true },
            { _jsonCameraOrientationLandscapeKey,   QJsonValue::Bool,   true },
390
            { _jsonCameraMinTriggerIntervalKey,     QJsonValue::Double, false },
Don Gagne's avatar
Don Gagne committed
391
        };
Don Gagne's avatar
Don Gagne committed
392 393 394 395
        if (!JsonHelper::validateKeys(cameraObject, cameraKeyInfoList, errorString)) {
            return false;
        }

396 397
        _cameraFact.setRawValue(cameraObject[_jsonCameraNameKey].toString());
        _cameraOrientationLandscapeFact.setRawValue(cameraObject[_jsonCameraOrientationLandscapeKey].toBool(true));
Don Gagne's avatar
Don Gagne committed
398 399 400 401 402 403 404 405 406

        _groundResolutionFact.setRawValue       (cameraObject[_jsonGroundResolutionKey].toDouble());
        _frontalOverlapFact.setRawValue         (cameraObject[_jsonFrontalOverlapKey].toInt());
        _sideOverlapFact.setRawValue            (cameraObject[_jsonSideOverlapKey].toInt());
        _cameraSensorWidthFact.setRawValue      (cameraObject[_jsonCameraSensorWidthKey].toDouble());
        _cameraSensorHeightFact.setRawValue     (cameraObject[_jsonCameraSensorHeightKey].toDouble());
        _cameraResolutionWidthFact.setRawValue  (cameraObject[_jsonCameraResolutionWidthKey].toDouble());
        _cameraResolutionHeightFact.setRawValue (cameraObject[_jsonCameraResolutionHeightKey].toDouble());
        _cameraFocalLengthFact.setRawValue      (cameraObject[_jsonCameraFocalLengthKey].toDouble());
407
        _cameraMinTriggerInterval =             cameraObject[_jsonCameraMinTriggerIntervalKey].toDouble(0);
Don Gagne's avatar
Don Gagne committed
408
    }
409 410

    // Polygon shape
411 412 413 414 415 416 417
    /// 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)
    if (!_mapPolygon.loadFromJson(v2Object, true /* required */, errorString)) {
        _mapPolygon.clear();
418 419
        return false;
    }
420

421 422 423
    _ignoreRecalc = false;
    _generateGrid();

424 425 426 427 428 429
    return true;
}

double SurveyMissionItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
    double greatestDistance = 0.0;
430 431
    for (int i=0; i<_simpleGridPoints.count(); i++) {
        QGeoCoordinate currentCoord = _simpleGridPoints[i].value<QGeoCoordinate>();
432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449
        double distance = currentCoord.distanceTo(other);
        if (distance > greatestDistance) {
            greatestDistance = distance;
        }
    }
    return greatestDistance;
}

void SurveyMissionItem::_setExitCoordinate(const QGeoCoordinate& coordinate)
{
    if (_exitCoordinate != coordinate) {
        _exitCoordinate = coordinate;
        emit exitCoordinateChanged(coordinate);
    }
}

bool SurveyMissionItem::specifiesCoordinate(void) const
{
450
    return _mapPolygon.count() > 2;
451 452 453 454 455
}

void _calcCameraShots()
{

456 457
}

458 459 460 461 462 463 464 465 466 467 468
void SurveyMissionItem::_convertTransectToGeo(const QList<QList<QPointF>>& transectSegmentsNED, const QGeoCoordinate& tangentOrigin, QList<QList<QGeoCoordinate>>& transectSegmentsGeo)
{
    transectSegmentsGeo.clear();

    for (int i=0; i<transectSegmentsNED.count(); i++) {
        QList<QGeoCoordinate>   transectCoords;
        const QList<QPointF>&   transectPoints = transectSegmentsNED[i];

        for (int j=0; j<transectPoints.count(); j++) {
            QGeoCoordinate coord;
            const QPointF& point = transectPoints[j];
469
            convertNedToGeo(point.y(), point.x(), 0, tangentOrigin, &coord);
470 471 472 473 474 475
            transectCoords.append(coord);
        }
        transectSegmentsGeo.append(transectCoords);
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498
/// Reverse the order of the transects. First transect becomes last and so forth.
void SurveyMissionItem::_reverseTransectOrder(QList<QList<QGeoCoordinate>>& transects)
{
    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.
void SurveyMissionItem::_reverseInternalTransectPoints(QList<QList<QGeoCoordinate>>& transects)
{
    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
499 500 501 502 503
/// 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
void SurveyMissionItem::_optimizeTransectsForShortestDistance(const QGeoCoordinate& distanceCoord, QList<QList<QGeoCoordinate>>& transects)
504
{
505
    double rgTransectDistance[4];
DonLakeFlyer's avatar
DonLakeFlyer committed
506 507 508 509
    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);
510 511 512 513 514 515 516 517 518

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

520 521
    if (shortestIndex > 1) {
        // We need to reverse the order of segments
DonLakeFlyer's avatar
DonLakeFlyer committed
522
        _reverseTransectOrder(transects);
523 524 525
    }
    if (shortestIndex & 1) {
        // We need to reverse the points within each segment
DonLakeFlyer's avatar
DonLakeFlyer committed
526
        _reverseInternalTransectPoints(transects);
527 528
    }
}
529

530 531
void SurveyMissionItem::_appendGridPointsFromTransects(QList<QList<QGeoCoordinate>>& rgTransectSegments)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
532 533
    qCDebug(SurveyMissionItemLog) << "Entry point _appendGridPointsFromTransects" << rgTransectSegments.first().first();

534 535 536
    for (int i=0; i<rgTransectSegments.count(); i++) {
        _simpleGridPoints.append(QVariant::fromValue(rgTransectSegments[i].first()));
        _simpleGridPoints.append(QVariant::fromValue(rgTransectSegments[i].last()));
537 538 539
    }
}

540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603
qreal SurveyMissionItem::_ccw(QPointF pt1, QPointF pt2, QPointF pt3)
{
    return (pt2.x()-pt1.x())*(pt3.y()-pt1.y()) - (pt2.y()-pt1.y())*(pt3.x()-pt1.x());
}

qreal SurveyMissionItem::_dp(QPointF pt1, QPointF pt2)
{
    return (pt2.x()-pt1.x())/qSqrt((pt2.x()-pt1.x())*(pt2.x()-pt1.x()) + (pt2.y()-pt1.y())*(pt2.y()-pt1.y()));
}

void SurveyMissionItem::_swapPoints(QList<QPointF>& points, int index1, int index2)
{
    QPointF temp = points[index1];
    points[index1] = points[index2];
    points[index2] = temp;
}

QList<QPointF> SurveyMissionItem::_convexPolygon(const QList<QPointF>& polygon)
{
    // We use the Graham scan algorithem to convert the possibly concave polygon to a convex polygon
    // https://en.wikipedia.org/wiki/Graham_scan

    QList<QPointF> workPolygon(polygon);

    // First point must be lowest y-coordinate point
    for (int i=1; i<workPolygon.count(); i++) {
        if (workPolygon[i].y() < workPolygon[0].y()) {
            _swapPoints(workPolygon, i, 0);
        }
    }

    // Sort the points by angle with first point
    for (int i=1; i<workPolygon.count(); i++) {
        qreal angle = _dp(workPolygon[0], workPolygon[i]);
        for (int j=i+1; j<workPolygon.count(); j++) {
            if (_dp(workPolygon[0], workPolygon[j]) > angle) {
                _swapPoints(workPolygon, i, j);
                angle = _dp(workPolygon[0], workPolygon[j]);
            }
        }
    }

    // Perform the the Graham scan

    workPolygon.insert(0, workPolygon.last());  // Sentinel for algo stop
    int convexCount = 1;                        // Number of points on the convex hull.

    for (int i=2; i<=polygon.count(); i++) {
        while (_ccw(workPolygon[convexCount-1], workPolygon[convexCount], workPolygon[i]) <= 0) {
            if (convexCount > 1) {
                convexCount -= 1;
            } else if (i == polygon.count()) {
                break;
            } else {
                i++;
            }
        }
        convexCount++;
        _swapPoints(workPolygon, convexCount, i);
    }

    return workPolygon.mid(1, convexCount);
}

DonLakeFlyer's avatar
DonLakeFlyer committed
604 605 606 607 608 609 610 611 612 613 614 615 616 617
/// Returns true if the current grid angle generates north/south oriented transects
bool SurveyMissionItem::_gridAngleIsNorthSouthTransects()
{
    // 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);
}

void SurveyMissionItem::_adjustTransectsToEntryPointLocation(QList<QList<QGeoCoordinate>>& transects)
{
    if (transects.count() == 0) {
        return;
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
618 619 620
    int entryLocation = _gridEntryLocationFact.rawValue().toInt();
    bool reversePoints = false;
    bool reverseTransects = false;
DonLakeFlyer's avatar
DonLakeFlyer committed
621

DonLakeFlyer's avatar
DonLakeFlyer committed
622 623
    if (entryLocation == EntryLocationBottomLeft || entryLocation == EntryLocationBottomRight) {
        reversePoints = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
624
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
625 626
    if (entryLocation == EntryLocationTopRight || entryLocation == EntryLocationBottomRight) {
        reverseTransects = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
627
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
628

DonLakeFlyer's avatar
DonLakeFlyer committed
629 630 631 632 633
    if (reversePoints) {
        qCDebug(SurveyMissionItemLog) << "Reverse Points";
        _reverseInternalTransectPoints(transects);
    }
    if (reverseTransects) {
DonLakeFlyer's avatar
DonLakeFlyer committed
634 635
        qCDebug(SurveyMissionItemLog) << "Reverse Transects";
        _reverseTransectOrder(transects);
DonLakeFlyer's avatar
DonLakeFlyer committed
636
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
637

DonLakeFlyer's avatar
DonLakeFlyer committed
638 639 640
    qCDebug(SurveyMissionItemLog) << "Modified entry point" << transects.first().first();
}

641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662
int SurveyMissionItem::_calcMissionCommandCount(QList<QList<QGeoCoordinate>>& transectSegments)
{
    int missionCommandCount= 0;
    for (int i=0; i<transectSegments.count(); i++) {
        const QList<QGeoCoordinate>& transectSegment = transectSegments[i];

        missionCommandCount += transectSegment.count();    // This accounts for all waypoints
        if (_hoverAndCaptureEnabled()) {
            // Internal camera trigger points are entry point, plus all points before exit point
            missionCommandCount += transectSegment.count() - (_hasTurnaround() ? 2 : 0) - 1;
        } else if (_triggerCamera() && !_imagesEverywhere()) {
            // Camera on/off at entry/exit of each transect
            missionCommandCount += 2;
        }
    }
    if (transectSegments.count() && _triggerCamera() && _imagesEverywhere()) {
         // Camera on/off for entire survey
        missionCommandCount += 2;
    }

    return missionCommandCount;
}
663 664
void SurveyMissionItem::_generateGrid(void)
{
665 666 667 668
    if (_ignoreRecalc) {
        return;
    }

669 670
    if (_mapPolygon.count() < 3 || _gridSpacingFact.rawValue().toDouble() <= 0) {
        _clearInternal();
671 672 673
        return;
    }

674
    _simpleGridPoints.clear();
DonLakeFlyer's avatar
DonLakeFlyer committed
675
    _transectSegments.clear();
676
    _reflyTransectSegments.clear();
677
    _additionalFlightDelaySeconds = 0;
678

679
    QList<QPointF>          polygonPoints;
DonLakeFlyer's avatar
DonLakeFlyer committed
680
    QList<QList<QPointF>>   transectSegments;
681

682
    // Convert polygon to NED
DonLakeFlyer's avatar
DonLakeFlyer committed
683 684
    QGeoCoordinate tangentOrigin = _mapPolygon.pathModel().value<QGCQGeoCoordinate*>(0)->coordinate();
    qCDebug(SurveyMissionItemLog) << "Convert polygon to NED - tangentOrigin" << tangentOrigin;
685
    for (int i=0; i<_mapPolygon.count(); i++) {
686
        double y, x, down;
687
        QGeoCoordinate vertex = _mapPolygon.pathModel().value<QGCQGeoCoordinate*>(i)->coordinate();
DonLakeFlyer's avatar
DonLakeFlyer committed
688 689 690 691 692 693
        if (i == 0) {
            // This avoids a nan calculation that comes out of convertGeoToNed
            x = y = 0;
        } else {
            convertGeoToNed(vertex, tangentOrigin, &y, &x, &down);
        }
694
        polygonPoints += QPointF(x, y);
DonLakeFlyer's avatar
DonLakeFlyer committed
695
        qCDebug(SurveyMissionItemLog) << "vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y();
696 697
    }

698 699
    polygonPoints = _convexPolygon(polygonPoints);

700 701 702 703 704 705 706 707 708 709 710
    double coveredArea = 0.0;
    for (int i=0; i<polygonPoints.count(); i++) {
        if (i != 0) {
            coveredArea += polygonPoints[i - 1].x() * polygonPoints[i].y() - polygonPoints[i].x() * polygonPoints[i -1].y();
        } else {
            coveredArea += polygonPoints.last().x() * polygonPoints[i].y() - polygonPoints[i].x() * polygonPoints.last().y();
        }
    }
    _setCoveredArea(0.5 * fabs(coveredArea));

    // Generate grid
711
    int cameraShots = 0;
712
    cameraShots += _gridGenerator(polygonPoints, transectSegments, false /* refly */);
713
    _convertTransectToGeo(transectSegments, tangentOrigin, _transectSegments);
DonLakeFlyer's avatar
DonLakeFlyer committed
714
    _adjustTransectsToEntryPointLocation(_transectSegments);
715
    _appendGridPointsFromTransects(_transectSegments);
716 717 718 719
    if (_refly90Degrees) {
        QVariantList reflyPointsGeo;

        transectSegments.clear();
720
        cameraShots += _gridGenerator(polygonPoints, transectSegments, true /* refly */);
721
        _convertTransectToGeo(transectSegments, tangentOrigin, _reflyTransectSegments);
DonLakeFlyer's avatar
DonLakeFlyer committed
722
        _optimizeTransectsForShortestDistance(_transectSegments.last().last(), _reflyTransectSegments);
723
        _appendGridPointsFromTransects(_reflyTransectSegments);
724
    }
725

726
    // Calc survey distance
727
    double surveyDistance = 0.0;
728 729 730 731
    for (int i=1; i<_simpleGridPoints.count(); i++) {
        QGeoCoordinate coord1 = _simpleGridPoints[i-1].value<QGeoCoordinate>();
        QGeoCoordinate coord2 = _simpleGridPoints[i].value<QGeoCoordinate>();
        surveyDistance += coord1.distanceTo(coord2);
732 733
    }
    _setSurveyDistance(surveyDistance);
734

735
    if (cameraShots == 0 && _triggerCamera()) {
736 737 738
        cameraShots = (int)floor(surveyDistance / _triggerDistance());
        // Take into account immediate camera trigger at waypoint entry
        cameraShots++;
739
    }
740
    _setCameraShots(cameraShots);
741

742 743 744 745 746
    if (_hoverAndCaptureEnabled()) {
        _additionalFlightDelaySeconds = cameraShots * _hoverAndCaptureDelaySeconds;
    }
    emit additionalTimeDelayChanged(_additionalFlightDelaySeconds);

747
    emit gridPointsChanged();
DonLakeFlyer's avatar
DonLakeFlyer committed
748 749

    // Determine command count for lastSequenceNumber
750 751
    _missionCommandCount = _calcMissionCommandCount(_transectSegments);
    _missionCommandCount += _calcMissionCommandCount(_reflyTransectSegments);
752 753
    emit lastSequenceNumberChanged(lastSequenceNumber());

DonLakeFlyer's avatar
DonLakeFlyer committed
754
    // Set exit coordinate
755 756
    if (_simpleGridPoints.count()) {
        QGeoCoordinate coordinate = _simpleGridPoints.first().value<QGeoCoordinate>();
757 758
        coordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble());
        setCoordinate(coordinate);
759
        QGeoCoordinate exitCoordinate = _simpleGridPoints.last().value<QGeoCoordinate>();
760 761
        exitCoordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble());
        _setExitCoordinate(exitCoordinate);
762
    }
763 764

    setDirty(true);
765 766
}

767 768 769 770 771 772
void SurveyMissionItem::_updateCoordinateAltitude(void)
{
    _coordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble());
    _exitCoordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble());
    emit coordinateChanged(_coordinate);
    emit exitCoordinateChanged(_exitCoordinate);
Don Gagne's avatar
Don Gagne committed
773
    setDirty(true);
774 775
}

776 777 778
QPointF SurveyMissionItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle)
{
    QPointF rotated;
DonLakeFlyer's avatar
DonLakeFlyer committed
779
    double radians = (M_PI / 180.0) * -angle;
780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845

    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;
}

void SurveyMissionItem::_intersectLinesWithRect(const QList<QLineF>& lineList, const QRectF& boundRect, QList<QLineF>& resultLines)
{
    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;
        }
    }
}

void SurveyMissionItem::_intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines)
{
846
    resultLines.clear();
847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875
    for (int i=0; i<lineList.count(); i++) {
        int foundCount = 0;
        QLineF intersectLine;
        const QLineF& line = lineList[i];

        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) {
                if (foundCount == 0) {
                    foundCount++;
                    intersectLine.setP1(intersectPoint);
                } else {
                    foundCount++;
                    intersectLine.setP2(intersectPoint);
                    break;
                }
            }
        }

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

/// Adjust the line segments such that they are all going the same direction with respect to going from P1->P2
void SurveyMissionItem::_adjustLineDirection(const QList<QLineF>& lineList, QList<QLineF>& resultLines)
{
876
    qreal firstAngle = 0;
877 878 879 880
    for (int i=0; i<lineList.count(); i++) {
        const QLineF& line = lineList[i];
        QLineF adjustedLine;

881 882 883 884 885
        if (i == 0) {
            firstAngle = line.angle();
        }

        if (qAbs(line.angle() - firstAngle) > 1.0) {
886 887 888 889 890 891 892 893 894 895
            adjustedLine.setP1(line.p2());
            adjustedLine.setP2(line.p1());
        } else {
            adjustedLine = line;
        }

        resultLines += adjustedLine;
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
896 897 898 899 900 901 902 903 904 905 906
double SurveyMissionItem::_clampGridAngle90(double gridAngle)
{
    // 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;
}

907
int SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints,  QList<QList<QPointF>>& transectSegments, bool refly)
908
{
909 910
    int cameraShots = 0;

DonLakeFlyer's avatar
DonLakeFlyer committed
911
    double gridAngle = _gridAngleFact.rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
912 913
    double gridSpacing = _gridSpacingFact.rawValue().toDouble();

DonLakeFlyer's avatar
DonLakeFlyer committed
914 915 916 917
    gridAngle = _clampGridAngle90(gridAngle);
    gridAngle += refly ? 90 : 0;
    qCDebug(SurveyMissionItemLog) << "Clamped grid angle" << gridAngle;

DonLakeFlyer's avatar
DonLakeFlyer committed
918
    qCDebug(SurveyMissionItemLog) << "SurveyMissionItem::_gridGenerator gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly;
919

DonLakeFlyer's avatar
DonLakeFlyer committed
920
    transectSegments.clear();
921 922 923 924 925 926 927 928 929 930

    // Convert polygon to bounding rect

    qCDebug(SurveyMissionItemLog) << "Polygon";
    QPolygonF polygon;
    for (int i=0; i<polygonPoints.count(); i++) {
        qCDebug(SurveyMissionItemLog) << polygonPoints[i];
        polygon << polygonPoints[i];
    }
    polygon << polygonPoints[0];
931 932 933
    QRectF boundingRect = polygon.boundingRect();
    QPointF boundingCenter = boundingRect.center();
    qCDebug(SurveyMissionItemLog) << "Bounding rect" << boundingRect.topLeft().x() << boundingRect.topLeft().y() << boundingRect.bottomRight().x() << boundingRect.bottomRight().y();
934 935 936

    // 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
937

938
    QList<QLineF> lineList;
DonLakeFlyer's avatar
DonLakeFlyer committed
939

940
    // 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
941
    // This way they will always be guaranteed to intersect with a polygon edge no matter what angle they are rotated to.
942
    // They are initially generated with the transects flowing from west to east and then points within the transect north to south.
943
    double maxWidth = qMax(boundingRect.width(), boundingRect.height()) + 2000.0;
944 945 946 947 948 949 950 951 952 953 954
    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;
    }

955 956 957 958 959 960 961 962 963
    // 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

964 965 966 967 968 969 970 971 972 973 974 975 976 977 978
    // 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);
    }

979 980 981 982 983
    // 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);

984
    // Calc camera shots here if there are no images in turnaround
DonLakeFlyer's avatar
DonLakeFlyer committed
985
    if (_triggerCamera() && !_imagesEverywhere()) {
986
        for (int i=0; i<resultLines.count(); i++) {
987 988 989
            cameraShots += (int)floor(resultLines[i].length() / _triggerDistance());
            // Take into account immediate camera trigger at waypoint entry
            cameraShots++;
990 991 992 993
        }
    }

    // Turn into a path
994
    for (int i=0; i<resultLines.count(); i++) {
DonLakeFlyer's avatar
DonLakeFlyer committed
995 996 997
        QLineF          transectLine;
        QList<QPointF>  transectPoints;
        const QLineF&   line = resultLines[i];
998

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

1001
        if (i & 1) {
DonLakeFlyer's avatar
DonLakeFlyer committed
1002
            transectLine = QLineF(line.p2(), line.p1());
1003
        } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024
            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());
                qCDebug(SurveyMissionItemLog) << "innerPoints" << innerPoints;
                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
1025
            }
1026
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
1027 1028 1029 1030 1031 1032 1033 1034 1035

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

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

        transectSegments.append(transectPoints);
1036
    }
1037 1038

    return cameraShots;
1039 1040 1041 1042
}

int SurveyMissionItem::_appendWaypointToMission(QList<MissionItem*>& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1043 1044 1045 1046
    double  altitude =          _gridAltitudeFact.rawValue().toDouble();
    bool    altitudeRelative =  _gridAltitudeRelativeFact.rawValue().toBool();

    qCDebug(SurveyMissionItemLog) << "_appendWaypointToMission seq:trigger" << seqNum << (cameraTrigger != CameraTriggerNone);
1047 1048 1049 1050

    MissionItem* item = new MissionItem(seqNum++,
                                        MAV_CMD_NAV_WAYPOINT,
                                        altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
1051
                                        cameraTrigger == CameraTriggerHoverAndCapture ? _hoverAndCaptureDelaySeconds : 0,  // Hold time (delay for hover and capture to settle vehicle before image is taken)
1052 1053
                                        0.0, 0.0,
                                        std::numeric_limits<double>::quiet_NaN(),   // Yaw unchanged
1054 1055 1056
                                        coord.latitude(),
                                        coord.longitude(),
                                        altitude,
1057 1058
                                        true,                                       // autoContinue
                                        false,                                      // isCurrentItem
1059 1060 1061
                                        missionItemParent);
    items.append(item);

DonLakeFlyer's avatar
DonLakeFlyer committed
1062 1063 1064 1065 1066 1067 1068
    switch (cameraTrigger) {
    case CameraTriggerOff:
    case CameraTriggerOn:
        item = new MissionItem(seqNum++,
                               MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                               MAV_FRAME_MISSION,
                               cameraTrigger == CameraTriggerOn ? _triggerDistance() : 0,
1069 1070 1071 1072 1073
                               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
1074
                               missionItemParent);
1075
        items.append(item);
DonLakeFlyer's avatar
DonLakeFlyer committed
1076 1077 1078 1079 1080
        break;
    case CameraTriggerHoverAndCapture:
        item = new MissionItem(seqNum++,
                               MAV_CMD_IMAGE_START_CAPTURE,
                               MAV_FRAME_MISSION,
Gus Grubba's avatar
Gus Grubba committed
1081
                               0,                           // Reserved (Set to 0)
1082 1083
                               0,                           // Interval (none)
                               1,                           // Take 1 photo
1084
                               NAN, NAN, NAN, NAN,          // param 4-7 reserved
1085 1086
                               true,                        // autoContinue
                               false,                       // isCurrentItem
DonLakeFlyer's avatar
DonLakeFlyer committed
1087 1088
                               missionItemParent);
        items.append(item);
1089 1090
#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
1091 1092 1093 1094 1095 1096 1097 1098 1099 1100
        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);
1101
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
1102 1103
    default:
        break;
1104
    }
1105 1106

    return seqNum;
1107 1108
}

DonLakeFlyer's avatar
DonLakeFlyer committed
1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119
bool SurveyMissionItem::_nextTransectCoord(const QList<QGeoCoordinate>& transectPoints, int pointIndex, QGeoCoordinate& coord)
{
    if (pointIndex > transectPoints.count()) {
        qWarning() << "Bad grid generation";
        return false;
    }

    coord = transectPoints[pointIndex];
    return true;
}

1120 1121 1122 1123 1124 1125 1126 1127
/// Appends the mission items for the survey
///     @param items Mission items are appended to this list
///     @param missionItemParent Parent object for newly created MissionItem objects
///     @param seqNum[in,out] Sequence number to start from
///     @param hasRefly true: misison has a refly section
///     @param buildRefly: true: build the refly section, false: build the first section
/// @return false: Generation failed
bool SurveyMissionItem::_appendMissionItemsWorker(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly)
1128
{
1129 1130
    bool firstWaypointTrigger = false;

1131
    qCDebug(SurveyMissionItemLog) << 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);
1132

1133
    QList<QList<QGeoCoordinate>>& transectSegments = buildRefly ? _reflyTransectSegments : _transectSegments;
DonLakeFlyer's avatar
DonLakeFlyer committed
1134

1135
    if (!buildRefly && _imagesEverywhere()) {
1136
        firstWaypointTrigger = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
1137 1138
    }

1139
    for (int segmentIndex=0; segmentIndex<transectSegments.count(); segmentIndex++) {
DonLakeFlyer's avatar
DonLakeFlyer committed
1140 1141
        int pointIndex = 0;
        QGeoCoordinate coord;
1142
        CameraTriggerCode cameraTrigger;
1143
        const QList<QGeoCoordinate>& segment = transectSegments[segmentIndex];
DonLakeFlyer's avatar
DonLakeFlyer committed
1144

1145
        qCDebug(SurveyMissionItemLog) << "segment.count" << segment.count();
DonLakeFlyer's avatar
DonLakeFlyer committed
1146 1147 1148

        if (_hasTurnaround()) {
            // Add entry turnaround point
1149 1150
            if (!_nextTransectCoord(segment, pointIndex++, coord)) {
                return false;
DonLakeFlyer's avatar
DonLakeFlyer committed
1151
            }
1152 1153
            seqNum = _appendWaypointToMission(items, seqNum, coord, firstWaypointTrigger ? CameraTriggerOn : CameraTriggerNone, missionItemParent);
            firstWaypointTrigger = false;
DonLakeFlyer's avatar
DonLakeFlyer committed
1154
        }
1155

DonLakeFlyer's avatar
DonLakeFlyer committed
1156
        // Add polygon entry point
1157 1158
        if (!_nextTransectCoord(segment, pointIndex++, coord)) {
            return false;
DonLakeFlyer's avatar
DonLakeFlyer committed
1159
        }
1160 1161 1162 1163 1164
        if (firstWaypointTrigger) {
            cameraTrigger = CameraTriggerOn;
        } else {
            cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerHoverAndCapture : CameraTriggerOn);
        }
1165
        seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent);
1166
        firstWaypointTrigger = false;
1167

DonLakeFlyer's avatar
DonLakeFlyer committed
1168 1169
        // Add internal hover and capture points
        if (_hoverAndCaptureEnabled()) {
1170
            int lastHoverAndCaptureIndex = segment.count() - 1 - (_hasTurnaround() ? 1 : 0);
DonLakeFlyer's avatar
DonLakeFlyer committed
1171 1172
            qCDebug(SurveyMissionItemLog) << "lastHoverAndCaptureIndex" << lastHoverAndCaptureIndex;
            for (; pointIndex < lastHoverAndCaptureIndex; pointIndex++) {
1173 1174
                if (!_nextTransectCoord(segment, pointIndex, coord)) {
                    return false;
DonLakeFlyer's avatar
DonLakeFlyer committed
1175 1176
                }
                seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerHoverAndCapture, missionItemParent);
1177 1178
            }
        }
1179

DonLakeFlyer's avatar
DonLakeFlyer committed
1180
        // Add polygon exit point
1181 1182
        if (!_nextTransectCoord(segment, pointIndex++, coord)) {
            return false;
1183
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
1184
        cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerNone : CameraTriggerOff);
1185 1186
        seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent);

DonLakeFlyer's avatar
DonLakeFlyer committed
1187 1188
        if (_hasTurnaround()) {
            // Add exit turnaround point
1189 1190
            if (!_nextTransectCoord(segment, pointIndex++, coord)) {
                return false;
1191
            }
DonLakeFlyer's avatar
DonLakeFlyer committed
1192
            seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent);
1193
        }
1194

DonLakeFlyer's avatar
DonLakeFlyer committed
1195
        qCDebug(SurveyMissionItemLog) << "last PointIndex" << pointIndex;
1196 1197
    }

1198
    if (((hasRefly && buildRefly) || !hasRefly) && _imagesEverywhere()) {
1199 1200 1201 1202 1203 1204 1205 1206
        // 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
1207 1208
                                            missionItemParent);
        items.append(item);
1209
    }
1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224

    return true;
}

void SurveyMissionItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
    int seqNum = _sequenceNumber;

    if (!_appendMissionItemsWorker(items, missionItemParent, seqNum, _refly90Degrees, false /* buildRefly */)) {
        return;
    }

    if (_refly90Degrees) {
        _appendMissionItemsWorker(items, missionItemParent, seqNum, _refly90Degrees, true /* buildRefly */);
    }
1225 1226
}

Don Gagne's avatar
Don Gagne committed
1227 1228
int SurveyMissionItem::cameraShots(void) const
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1229
    return _triggerCamera() ? _cameraShots : 0;
1230
}
Don Gagne's avatar
Don Gagne committed
1231 1232 1233 1234 1235

void SurveyMissionItem::_cameraValueChanged(void)
{
    emit cameraValueChanged();
}
1236 1237 1238

double SurveyMissionItem::timeBetweenShots(void) const
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1239
    return _cruiseSpeed == 0 ? 0 : _triggerDistance() / _cruiseSpeed;
1240 1241
}

1242
void SurveyMissionItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus)
1243
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1244 1245 1246
    ComplexMissionItem::setMissionFlightStatus(missionFlightStatus);
    if (!qFuzzyCompare(_cruiseSpeed, missionFlightStatus.vehicleSpeed)) {
        _cruiseSpeed = missionFlightStatus.vehicleSpeed;
1247 1248 1249
        emit timeBetweenShotsChanged();
    }
}
1250 1251 1252 1253 1254

void SurveyMissionItem::_setDirty(void)
{
    setDirty(true);
}
DonLakeFlyer's avatar
DonLakeFlyer committed
1255 1256 1257 1258 1259 1260 1261 1262 1263 1264 1265 1266

bool SurveyMissionItem::hoverAndCaptureAllowed(void) const
{
    return _vehicle->multiRotor() || _vehicle->vtol();
}

double SurveyMissionItem::_triggerDistance(void) const {
    return _cameraTriggerDistanceFact.rawValue().toDouble();
}

bool SurveyMissionItem::_triggerCamera(void) const
{
1267
    return _triggerDistance() > 0;
DonLakeFlyer's avatar
DonLakeFlyer committed
1268 1269 1270 1271 1272 1273 1274 1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285 1286 1287 1288
}

bool SurveyMissionItem::_imagesEverywhere(void) const
{
    return _triggerCamera() && _cameraTriggerInTurnaroundFact.rawValue().toBool();
}

bool SurveyMissionItem::_hoverAndCaptureEnabled(void) const
{
    return hoverAndCaptureAllowed() && !_imagesEverywhere() && _triggerCamera() && _hoverAndCaptureFact.rawValue().toBool();
}

bool SurveyMissionItem::_hasTurnaround(void) const
{
    return _turnaroundDistance() > 0;
}

double SurveyMissionItem::_turnaroundDistance(void) const
{
    return _turnaroundDistFact.rawValue().toDouble();
}
DonLakeFlyer's avatar
DonLakeFlyer committed
1289 1290 1291

void SurveyMissionItem::applyNewAltitude(double newAltitude)
{
1292
    _fixedValueIsAltitudeFact.setRawValue(true);
DonLakeFlyer's avatar
DonLakeFlyer committed
1293 1294
    _gridAltitudeFact.setRawValue(newAltitude);
}
1295 1296 1297 1298 1299 1300 1301 1302

void SurveyMissionItem::setRefly90Degrees(bool refly90Degrees)
{
    if (refly90Degrees != _refly90Degrees) {
        _refly90Degrees = refly90Degrees;
        emit refly90DegreesChanged(refly90Degrees);
    }
}
1303 1304 1305 1306 1307 1308 1309

void SurveyMissionItem::_polygonDirtyChanged(bool dirty)
{
    if (dirty) {
        setDirty(true);
    }
}