SurveyMissionItem.cc 56.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
}

void SurveyMissionItem::_setSurveyDistance(double surveyDistance)
{
    if (!qFuzzyCompare(_surveyDistance, surveyDistance)) {
        _surveyDistance = surveyDistance;
162
        emit complexDistanceChanged();
163 164 165 166 167 168 169
    }
}

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

DonLakeFlyer's avatar
DonLakeFlyer committed
557 558 559 560 561 562 563 564 565 566 567 568 569 570
/// 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
571 572 573
    int entryLocation = _gridEntryLocationFact.rawValue().toInt();
    bool reversePoints = false;
    bool reverseTransects = false;
DonLakeFlyer's avatar
DonLakeFlyer committed
574

DonLakeFlyer's avatar
DonLakeFlyer committed
575 576
    if (entryLocation == EntryLocationBottomLeft || entryLocation == EntryLocationBottomRight) {
        reversePoints = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
577
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
578 579
    if (entryLocation == EntryLocationTopRight || entryLocation == EntryLocationBottomRight) {
        reverseTransects = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
580
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
581

DonLakeFlyer's avatar
DonLakeFlyer committed
582 583 584 585 586
    if (reversePoints) {
        qCDebug(SurveyMissionItemLog) << "Reverse Points";
        _reverseInternalTransectPoints(transects);
    }
    if (reverseTransects) {
DonLakeFlyer's avatar
DonLakeFlyer committed
587 588
        qCDebug(SurveyMissionItemLog) << "Reverse Transects";
        _reverseTransectOrder(transects);
DonLakeFlyer's avatar
DonLakeFlyer committed
589
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
590

DonLakeFlyer's avatar
DonLakeFlyer committed
591 592 593
    qCDebug(SurveyMissionItemLog) << "Modified entry point" << transects.first().first();
}

594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615
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;
}
616 617
void SurveyMissionItem::_generateGrid(void)
{
618 619 620 621
    if (_ignoreRecalc) {
        return;
    }

622 623
    if (_mapPolygon.count() < 3 || _gridSpacingFact.rawValue().toDouble() <= 0) {
        _clearInternal();
624 625 626
        return;
    }

627
    _simpleGridPoints.clear();
DonLakeFlyer's avatar
DonLakeFlyer committed
628
    _transectSegments.clear();
629
    _reflyTransectSegments.clear();
630
    _additionalFlightDelaySeconds = 0;
631

632
    QList<QPointF>          polygonPoints;
DonLakeFlyer's avatar
DonLakeFlyer committed
633
    QList<QList<QPointF>>   transectSegments;
634

635
    // Convert polygon to NED
DonLakeFlyer's avatar
DonLakeFlyer committed
636 637
    QGeoCoordinate tangentOrigin = _mapPolygon.pathModel().value<QGCQGeoCoordinate*>(0)->coordinate();
    qCDebug(SurveyMissionItemLog) << "Convert polygon to NED - tangentOrigin" << tangentOrigin;
638
    for (int i=0; i<_mapPolygon.count(); i++) {
639
        double y, x, down;
640
        QGeoCoordinate vertex = _mapPolygon.pathModel().value<QGCQGeoCoordinate*>(i)->coordinate();
DonLakeFlyer's avatar
DonLakeFlyer committed
641 642 643 644 645 646
        if (i == 0) {
            // This avoids a nan calculation that comes out of convertGeoToNed
            x = y = 0;
        } else {
            convertGeoToNed(vertex, tangentOrigin, &y, &x, &down);
        }
647
        polygonPoints += QPointF(x, y);
DonLakeFlyer's avatar
DonLakeFlyer committed
648
        qCDebug(SurveyMissionItemLog) << "vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y();
649 650 651 652 653 654 655 656 657 658 659 660 661
    }

    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
662
    int cameraShots = 0;
663
    cameraShots += _gridGenerator(polygonPoints, transectSegments, false /* refly */);
664
    _convertTransectToGeo(transectSegments, tangentOrigin, _transectSegments);
DonLakeFlyer's avatar
DonLakeFlyer committed
665
    _adjustTransectsToEntryPointLocation(_transectSegments);
666
    _appendGridPointsFromTransects(_transectSegments);
667 668
    if (_refly90Degrees) {
        transectSegments.clear();
669
        cameraShots += _gridGenerator(polygonPoints, transectSegments, true /* refly */);
670
        _convertTransectToGeo(transectSegments, tangentOrigin, _reflyTransectSegments);
DonLakeFlyer's avatar
DonLakeFlyer committed
671
        _optimizeTransectsForShortestDistance(_transectSegments.last().last(), _reflyTransectSegments);
672
        _appendGridPointsFromTransects(_reflyTransectSegments);
673
    }
674

675
    // Calc survey distance
676
    double surveyDistance = 0.0;
677 678 679 680
    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);
681 682
    }
    _setSurveyDistance(surveyDistance);
683

684
    if (cameraShots == 0 && _triggerCamera()) {
685 686 687
        cameraShots = (int)floor(surveyDistance / _triggerDistance());
        // Take into account immediate camera trigger at waypoint entry
        cameraShots++;
688
    }
689
    _setCameraShots(cameraShots);
690

691 692 693
    if (_hoverAndCaptureEnabled()) {
        _additionalFlightDelaySeconds = cameraShots * _hoverAndCaptureDelaySeconds;
    }
694
    emit additionalTimeDelayChanged();
695

696
    emit gridPointsChanged();
DonLakeFlyer's avatar
DonLakeFlyer committed
697 698

    // Determine command count for lastSequenceNumber
699 700
    _missionCommandCount = _calcMissionCommandCount(_transectSegments);
    _missionCommandCount += _calcMissionCommandCount(_reflyTransectSegments);
701 702
    emit lastSequenceNumberChanged(lastSequenceNumber());

DonLakeFlyer's avatar
DonLakeFlyer committed
703
    // Set exit coordinate
704 705
    if (_simpleGridPoints.count()) {
        QGeoCoordinate coordinate = _simpleGridPoints.first().value<QGeoCoordinate>();
706 707
        coordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble());
        setCoordinate(coordinate);
708
        QGeoCoordinate exitCoordinate = _simpleGridPoints.last().value<QGeoCoordinate>();
709 710
        exitCoordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble());
        _setExitCoordinate(exitCoordinate);
711
    }
712 713

    setDirty(true);
714 715
}

716 717 718 719 720 721
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
722
    setDirty(true);
723 724
}

725 726 727
QPointF SurveyMissionItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle)
{
    QPointF rotated;
DonLakeFlyer's avatar
DonLakeFlyer committed
728
    double radians = (M_PI / 180.0) * -angle;
729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794

    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)
{
795
    resultLines.clear();
796

797 798
    for (int i=0; i<lineList.count(); i++) {
        const QLineF& line = lineList[i];
799
        QList<QPointF> intersections;
800

801
        // Intersect the line with all the polygon edges
802 803 804 805
        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) {
806
                intersections.append(intersectPoint);
807 808 809
            }
        }

810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830
        // 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]);
\
                    double newMaxDistance = lineTest.length();
                    if (newMaxDistance > currentMaxDistance) {
                        firstPoint = intersections[i];
                        secondPoint = intersections[j];
                        currentMaxDistance = newMaxDistance;
                    }
                }
            }

            resultLines += QLineF(firstPoint, secondPoint);
831 832 833 834 835 836 837
        }
    }
}

/// 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)
{
838
    qreal firstAngle = 0;
839 840 841 842
    for (int i=0; i<lineList.count(); i++) {
        const QLineF& line = lineList[i];
        QLineF adjustedLine;

843 844 845 846 847
        if (i == 0) {
            firstAngle = line.angle();
        }

        if (qAbs(line.angle() - firstAngle) > 1.0) {
848 849 850 851 852 853 854 855 856 857
            adjustedLine.setP1(line.p2());
            adjustedLine.setP2(line.p1());
        } else {
            adjustedLine = line;
        }

        resultLines += adjustedLine;
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
858 859 860 861 862 863 864 865 866 867 868
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;
}

869
int SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints,  QList<QList<QPointF>>& transectSegments, bool refly)
870
{
871 872
    int cameraShots = 0;

DonLakeFlyer's avatar
DonLakeFlyer committed
873
    double gridAngle = _gridAngleFact.rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
874 875
    double gridSpacing = _gridSpacingFact.rawValue().toDouble();

DonLakeFlyer's avatar
DonLakeFlyer committed
876 877 878 879
    gridAngle = _clampGridAngle90(gridAngle);
    gridAngle += refly ? 90 : 0;
    qCDebug(SurveyMissionItemLog) << "Clamped grid angle" << gridAngle;

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

DonLakeFlyer's avatar
DonLakeFlyer committed
882
    transectSegments.clear();
883 884 885 886 887 888 889 890 891 892

    // 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];
893 894 895
    QRectF boundingRect = polygon.boundingRect();
    QPointF boundingCenter = boundingRect.center();
    qCDebug(SurveyMissionItemLog) << "Bounding rect" << boundingRect.topLeft().x() << boundingRect.topLeft().y() << boundingRect.bottomRight().x() << boundingRect.bottomRight().y();
896 897 898

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

900
    QList<QLineF> lineList;
DonLakeFlyer's avatar
DonLakeFlyer committed
901

902
    // 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
903
    // This way they will always be guaranteed to intersect with a polygon edge no matter what angle they are rotated to.
904
    // They are initially generated with the transects flowing from west to east and then points within the transect north to south.
905
    double maxWidth = qMax(boundingRect.width(), boundingRect.height()) + 2000.0;
906 907 908 909 910 911 912 913 914 915 916
    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;
    }

917 918 919 920 921 922 923 924 925
    // 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

926 927 928 929 930 931 932 933 934 935 936 937 938 939 940
    // 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);
    }

941 942 943 944 945
    // 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);

946
    // Calc camera shots here if there are no images in turnaround
DonLakeFlyer's avatar
DonLakeFlyer committed
947
    if (_triggerCamera() && !_imagesEverywhere()) {
948
        for (int i=0; i<resultLines.count(); i++) {
949 950 951
            cameraShots += (int)floor(resultLines[i].length() / _triggerDistance());
            // Take into account immediate camera trigger at waypoint entry
            cameraShots++;
952 953 954 955
        }
    }

    // Turn into a path
956
    for (int i=0; i<resultLines.count(); i++) {
DonLakeFlyer's avatar
DonLakeFlyer committed
957 958 959
        QLineF          transectLine;
        QList<QPointF>  transectPoints;
        const QLineF&   line = resultLines[i];
960

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

963
        if (i & 1) {
DonLakeFlyer's avatar
DonLakeFlyer committed
964
            transectLine = QLineF(line.p2(), line.p1());
965
        } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986
            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
987
            }
988
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
989 990 991 992 993 994 995 996 997

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

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

        transectSegments.append(transectPoints);
998
    }
999 1000

    return cameraShots;
1001 1002 1003 1004
}

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

    qCDebug(SurveyMissionItemLog) << "_appendWaypointToMission seq:trigger" << seqNum << (cameraTrigger != CameraTriggerNone);
1009 1010 1011 1012

    MissionItem* item = new MissionItem(seqNum++,
                                        MAV_CMD_NAV_WAYPOINT,
                                        altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
1013
                                        cameraTrigger == CameraTriggerHoverAndCapture ? _hoverAndCaptureDelaySeconds : 0,  // Hold time (delay for hover and capture to settle vehicle before image is taken)
1014 1015
                                        0.0, 0.0,
                                        std::numeric_limits<double>::quiet_NaN(),   // Yaw unchanged
1016 1017 1018
                                        coord.latitude(),
                                        coord.longitude(),
                                        altitude,
1019 1020
                                        true,                                       // autoContinue
                                        false,                                      // isCurrentItem
1021 1022 1023
                                        missionItemParent);
    items.append(item);

DonLakeFlyer's avatar
DonLakeFlyer committed
1024 1025 1026 1027 1028 1029 1030
    switch (cameraTrigger) {
    case CameraTriggerOff:
    case CameraTriggerOn:
        item = new MissionItem(seqNum++,
                               MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                               MAV_FRAME_MISSION,
                               cameraTrigger == CameraTriggerOn ? _triggerDistance() : 0,
1031 1032 1033 1034 1035
                               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
1036
                               missionItemParent);
1037
        items.append(item);
DonLakeFlyer's avatar
DonLakeFlyer committed
1038 1039 1040 1041 1042
        break;
    case CameraTriggerHoverAndCapture:
        item = new MissionItem(seqNum++,
                               MAV_CMD_IMAGE_START_CAPTURE,
                               MAV_FRAME_MISSION,
Gus Grubba's avatar
Gus Grubba committed
1043
                               0,                           // Reserved (Set to 0)
1044 1045
                               0,                           // Interval (none)
                               1,                           // Take 1 photo
1046
                               NAN, NAN, NAN, NAN,          // param 4-7 reserved
1047 1048
                               true,                        // autoContinue
                               false,                       // isCurrentItem
DonLakeFlyer's avatar
DonLakeFlyer committed
1049 1050
                               missionItemParent);
        items.append(item);
1051 1052
#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
1053 1054 1055 1056 1057 1058 1059 1060 1061 1062
        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);
1063
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
1064 1065
    default:
        break;
1066
    }
1067 1068

    return seqNum;
1069 1070
}

DonLakeFlyer's avatar
DonLakeFlyer committed
1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081
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;
}

1082 1083 1084 1085 1086 1087 1088 1089
/// 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)
1090
{
1091 1092
    bool firstWaypointTrigger = false;

1093
    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);
1094

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

1097
    if (!buildRefly && _imagesEverywhere()) {
1098
        firstWaypointTrigger = true;
DonLakeFlyer's avatar
DonLakeFlyer committed
1099 1100
    }

1101
    for (int segmentIndex=0; segmentIndex<transectSegments.count(); segmentIndex++) {
DonLakeFlyer's avatar
DonLakeFlyer committed
1102 1103
        int pointIndex = 0;
        QGeoCoordinate coord;
1104
        CameraTriggerCode cameraTrigger;
1105
        const QList<QGeoCoordinate>& segment = transectSegments[segmentIndex];
DonLakeFlyer's avatar
DonLakeFlyer committed
1106

1107
        qCDebug(SurveyMissionItemLog) << "segment.count" << segment.count();
DonLakeFlyer's avatar
DonLakeFlyer committed
1108 1109 1110

        if (_hasTurnaround()) {
            // Add entry turnaround point
1111 1112
            if (!_nextTransectCoord(segment, pointIndex++, coord)) {
                return false;
DonLakeFlyer's avatar
DonLakeFlyer committed
1113
            }
1114 1115
            seqNum = _appendWaypointToMission(items, seqNum, coord, firstWaypointTrigger ? CameraTriggerOn : CameraTriggerNone, missionItemParent);
            firstWaypointTrigger = false;
DonLakeFlyer's avatar
DonLakeFlyer committed
1116
        }
1117

DonLakeFlyer's avatar
DonLakeFlyer committed
1118
        // Add polygon entry point
1119 1120
        if (!_nextTransectCoord(segment, pointIndex++, coord)) {
            return false;
DonLakeFlyer's avatar
DonLakeFlyer committed
1121
        }
1122 1123 1124 1125 1126
        if (firstWaypointTrigger) {
            cameraTrigger = CameraTriggerOn;
        } else {
            cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerHoverAndCapture : CameraTriggerOn);
        }
1127
        seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent);
1128
        firstWaypointTrigger = false;
1129

DonLakeFlyer's avatar
DonLakeFlyer committed
1130 1131
        // Add internal hover and capture points
        if (_hoverAndCaptureEnabled()) {
1132
            int lastHoverAndCaptureIndex = segment.count() - 1 - (_hasTurnaround() ? 1 : 0);
DonLakeFlyer's avatar
DonLakeFlyer committed
1133 1134
            qCDebug(SurveyMissionItemLog) << "lastHoverAndCaptureIndex" << lastHoverAndCaptureIndex;
            for (; pointIndex < lastHoverAndCaptureIndex; pointIndex++) {
1135 1136
                if (!_nextTransectCoord(segment, pointIndex, coord)) {
                    return false;
DonLakeFlyer's avatar
DonLakeFlyer committed
1137 1138
                }
                seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerHoverAndCapture, missionItemParent);
1139 1140
            }
        }
1141

DonLakeFlyer's avatar
DonLakeFlyer committed
1142
        // Add polygon exit point
1143 1144
        if (!_nextTransectCoord(segment, pointIndex++, coord)) {
            return false;
1145
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
1146
        cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerNone : CameraTriggerOff);
1147 1148
        seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent);

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

DonLakeFlyer's avatar
DonLakeFlyer committed
1157
        qCDebug(SurveyMissionItemLog) << "last PointIndex" << pointIndex;
1158 1159
    }

1160
    if (((hasRefly && buildRefly) || !hasRefly) && _imagesEverywhere()) {
1161 1162 1163 1164 1165 1166 1167 1168
        // 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
1169 1170
                                            missionItemParent);
        items.append(item);
1171
    }
1172 1173 1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186

    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 */);
    }
1187 1188
}

Don Gagne's avatar
Don Gagne committed
1189 1190
int SurveyMissionItem::cameraShots(void) const
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1191
    return _triggerCamera() ? _cameraShots : 0;
1192
}
Don Gagne's avatar
Don Gagne committed
1193 1194 1195 1196 1197

void SurveyMissionItem::_cameraValueChanged(void)
{
    emit cameraValueChanged();
}
1198 1199 1200

double SurveyMissionItem::timeBetweenShots(void) const
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1201
    return _cruiseSpeed == 0 ? 0 : _triggerDistance() / _cruiseSpeed;
1202 1203
}

1204
void SurveyMissionItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus)
1205
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1206 1207 1208
    ComplexMissionItem::setMissionFlightStatus(missionFlightStatus);
    if (!qFuzzyCompare(_cruiseSpeed, missionFlightStatus.vehicleSpeed)) {
        _cruiseSpeed = missionFlightStatus.vehicleSpeed;
1209 1210 1211
        emit timeBetweenShotsChanged();
    }
}
1212 1213 1214 1215 1216

void SurveyMissionItem::_setDirty(void)
{
    setDirty(true);
}
DonLakeFlyer's avatar
DonLakeFlyer committed
1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228

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

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

bool SurveyMissionItem::_triggerCamera(void) const
{
1229
    return _triggerDistance() > 0;
DonLakeFlyer's avatar
DonLakeFlyer committed
1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250
}

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
1251 1252 1253

void SurveyMissionItem::applyNewAltitude(double newAltitude)
{
1254
    _fixedValueIsAltitudeFact.setRawValue(true);
DonLakeFlyer's avatar
DonLakeFlyer committed
1255 1256
    _gridAltitudeFact.setRawValue(newAltitude);
}
1257 1258 1259 1260 1261 1262 1263 1264

void SurveyMissionItem::setRefly90Degrees(bool refly90Degrees)
{
    if (refly90Degrees != _refly90Degrees) {
        _refly90Degrees = refly90Degrees;
        emit refly90DegreesChanged(refly90Degrees);
    }
}
1265 1266 1267 1268 1269 1270 1271

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