SurveyMissionItem.cc 48.2 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 19 20 21

#include <QPolygonF>

QGC_LOGGING_CATEGORY(SurveyMissionItemLog, "SurveyMissionItemLog")

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

Don Gagne's avatar
Don Gagne committed
24 25 26 27 28 29 30 31 32
const char* SurveyMissionItem::_jsonPolygonObjectKey =              "polygon";
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";
const char* SurveyMissionItem::_jsonTurnaroundDistKey =             "turnAroundDistance";
const char* SurveyMissionItem::_jsonCameraTriggerKey =              "cameraTrigger";
const char* SurveyMissionItem::_jsonCameraTriggerDistanceKey =      "cameraTriggerDistance";
33 34
const char* SurveyMissionItem::_jsonCameraTriggerInTurnaroundKey =  "cameraTriggerInTurnaround";
const char* SurveyMissionItem::_jsonHoverAndCaptureKey =            "hoverAndCapture";
Don Gagne's avatar
Don Gagne committed
35 36
const char* SurveyMissionItem::_jsonGroundResolutionKey =           "groundResolution";
const char* SurveyMissionItem::_jsonFrontalOverlapKey =             "imageFrontalOverlap";
37
const char* SurveyMissionItem::_jsonSideOverlapKey =                "imageSideOverlap";
Don Gagne's avatar
Don Gagne committed
38 39 40 41 42 43 44 45 46 47 48
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";
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";

49 50 51 52 53 54 55 56
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";
const char* SurveyMissionItem::turnaroundDistName =             "TurnaroundDist";
const char* SurveyMissionItem::cameraTriggerDistanceName =      "CameraTriggerDistance";
57 58
const char* SurveyMissionItem::cameraTriggerInTurnaroundName =  "CameraTriggerInTurnaround";
const char* SurveyMissionItem::hoverAndCaptureName =            "HoverAndCapture";
59 60 61 62 63 64 65 66 67 68 69 70
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";
71

72 73 74 75
SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent)
    : ComplexMissionItem(vehicle, parent)
    , _sequenceNumber(0)
    , _dirty(false)
76
    , _cameraOrientationFixed(false)
DonLakeFlyer's avatar
DonLakeFlyer committed
77
    , _missionCommandCount(0)
78 79 80
    , _surveyDistance(0.0)
    , _cameraShots(0)
    , _coveredArea(0.0)
81
    , _timeBetweenShots(0.0)
82 83 84 85 86 87 88 89 90
    , _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])
    , _turnaroundDistFact               (settingsGroup, _metaDataMap[turnaroundDistName])
    , _cameraTriggerFact                (settingsGroup, _metaDataMap[cameraTriggerName])
    , _cameraTriggerDistanceFact        (settingsGroup, _metaDataMap[cameraTriggerDistanceName])
91 92
    , _cameraTriggerInTurnaroundFact    (settingsGroup, _metaDataMap[cameraTriggerInTurnaroundName])
    , _hoverAndCaptureFact              (settingsGroup, _metaDataMap[hoverAndCaptureName])
93 94 95 96 97 98 99 100 101 102 103
    , _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])
104
{
105 106
    _editorQml = "qrc:/qml/SurveyItemEditor.qml";

107 108
    // NULL check since object creation during unit testing passes NULL for vehicle
    if (_vehicle && _vehicle->multiRotor()) {
109 110
        _turnaroundDistFact.setRawValue(0);
    }
Don Gagne's avatar
Don Gagne committed
111

112 113 114 115 116 117
    connect(&_gridSpacingFact,                  &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid);
    connect(&_gridAngleFact,                    &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid);
    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);
DonLakeFlyer's avatar
DonLakeFlyer committed
118
    connect(&_cameraTriggerFact,                &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid);
119

120
    connect(&_gridAltitudeFact,             &Fact::valueChanged, this, &SurveyMissionItem::_updateCoordinateAltitude);
121

122 123 124 125 126 127 128 129 130 131
    // 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);
132

Don Gagne's avatar
Don Gagne committed
133
    connect(&_cameraTriggerDistanceFact, &Fact::valueChanged, this, &SurveyMissionItem::timeBetweenShotsChanged);
134 135 136 137 138 139 140 141 142 143 144 145 146 147
}

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
148
        emit cameraShotsChanged(this->cameraShots());
149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174
    }
}

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


void SurveyMissionItem::clearPolygon(void)
{
    // Bug workaround, see below
    while (_polygonPath.count() > 1) {
        _polygonPath.takeLast();
    }
    emit polygonPathChanged();

    // Although this code should remove the polygon from the map it doesn't. There appears
    // to be a bug in MapPolygon which causes it to not be redrawn if the list is empty. So
    // we work around it by using the code above to remove all but the last point which in turn
    // will cause the polygon to go away.
    _polygonPath.clear();

175 176
    _polygonModel.clearAndDeleteContents();

177 178 179 180 181 182 183 184 185
    _clearGrid();
    setDirty(true);

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

void SurveyMissionItem::addPolygonCoordinate(const QGeoCoordinate coordinate)
{
186 187
    _polygonModel.append(new QGCQGeoCoordinate(coordinate, this));

188 189 190 191 192 193 194 195 196 197 198 199 200 201 202
    _polygonPath << QVariant::fromValue(coordinate);
    emit polygonPathChanged();

    int pointCount = _polygonPath.count();
    if (pointCount >= 3) {
        if (pointCount == 3) {
            emit specifiesCoordinateChanged();
        }
        _generateGrid();
    }
    setDirty(true);
}

void SurveyMissionItem::adjustPolygonCoordinate(int vertexIndex, const QGeoCoordinate coordinate)
{
203 204 205 206 207 208
    if (vertexIndex < 0 && vertexIndex > _polygonPath.length() - 1) {
        qWarning() << "Call to adjustPolygonCoordinate with bad vertexIndex:count" << vertexIndex << _polygonPath.length();
        return;
    }

    _polygonModel.value<QGCQGeoCoordinate*>(vertexIndex)->setCoordinate(coordinate);
209 210 211 212 213 214
    _polygonPath[vertexIndex] = QVariant::fromValue(coordinate);
    emit polygonPathChanged();
    _generateGrid();
    setDirty(true);
}

215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268
void SurveyMissionItem::splitPolygonSegment(int vertexIndex)
{
    int nextIndex = vertexIndex + 1;
    if (nextIndex > _polygonPath.length() - 1) {
        nextIndex = 0;
    }

    QGeoCoordinate firstVertex = _polygonPath[vertexIndex].value<QGeoCoordinate>();
    QGeoCoordinate nextVertex = _polygonPath[nextIndex].value<QGeoCoordinate>();

    double distance = firstVertex.distanceTo(nextVertex);
    double azimuth = firstVertex.azimuthTo(nextVertex);
    QGeoCoordinate newVertex = firstVertex.atDistanceAndAzimuth(distance / 2, azimuth);

    if (nextIndex == 0) {
        addPolygonCoordinate(newVertex);
    } else {
        _polygonModel.insert(nextIndex, new QGCQGeoCoordinate(newVertex, this));
        _polygonPath.insert(nextIndex, QVariant::fromValue(newVertex));
        emit polygonPathChanged();

        int pointCount = _polygonPath.count();
        if (pointCount >= 3) {
            if (pointCount == 3) {
                emit specifiesCoordinateChanged();
            }
            _generateGrid();
        }
        setDirty(true);
    }
}

void SurveyMissionItem::removePolygonVertex(int vertexIndex)
{
    if (vertexIndex < 0 && vertexIndex > _polygonPath.length() - 1) {
        qWarning() << "Call to removePolygonCoordinate with bad vertexIndex:count" << vertexIndex << _polygonPath.length();
        return;
    }

    if (_polygonPath.length() <= 3) {
        // Don't allow the user to trash the polygon
        return;
    }

    QObject* coordObj = _polygonModel.removeAt(vertexIndex);
    coordObj->deleteLater();

    _polygonPath.removeAt(vertexIndex);
    emit polygonPathChanged();

    _generateGrid();
    setDirty(true);
}

269 270
int SurveyMissionItem::lastSequenceNumber(void) const
{
DonLakeFlyer's avatar
DonLakeFlyer committed
271
    return _sequenceNumber + _missionCommandCount;
272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289
}

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

290
void SurveyMissionItem::save(QJsonArray&  missionItems)
291
{
292 293
    QJsonObject saveObject;

Don Gagne's avatar
Don Gagne committed
294 295 296
    saveObject[JsonHelper::jsonVersionKey] =                    3;
    saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
    saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;
297 298 299
    saveObject[_jsonCameraTriggerKey] =                         _cameraTriggerFact.rawValue().toBool();
    saveObject[_jsonManualGridKey] =                            _manualGridFact.rawValue().toBool();
    saveObject[_jsonFixedValueIsAltitudeKey] =                  _fixedValueIsAltitudeFact.rawValue().toBool();
DonLakeFlyer's avatar
DonLakeFlyer committed
300
    saveObject[_jsonHoverAndCaptureKey] =                       _hoverAndCaptureFact.rawValue().toBool();
Don Gagne's avatar
Don Gagne committed
301

302
    if (_cameraTriggerFact.rawValue().toBool()) {
Don Gagne's avatar
Don Gagne committed
303 304 305 306 307
        saveObject[_jsonCameraTriggerDistanceKey] = _cameraTriggerDistanceFact.rawValue().toDouble();
    }

    QJsonObject gridObject;
    gridObject[_jsonGridAltitudeKey] =          _gridAltitudeFact.rawValue().toDouble();
308
    gridObject[_jsonGridAltitudeRelativeKey] =  _gridAltitudeRelativeFact.rawValue().toBool();
Don Gagne's avatar
Don Gagne committed
309 310 311 312 313 314
    gridObject[_jsonGridAngleKey] =             _gridAngleFact.rawValue().toDouble();
    gridObject[_jsonGridSpacingKey] =           _gridSpacingFact.rawValue().toDouble();
    gridObject[_jsonTurnaroundDistKey] =        _turnaroundDistFact.rawValue().toDouble();

    saveObject[_jsonGridObjectKey] = gridObject;

315
    if (!_manualGridFact.rawValue().toBool()) {
Don Gagne's avatar
Don Gagne committed
316
        QJsonObject cameraObject;
317 318
        cameraObject[_jsonCameraNameKey] =                  _cameraFact.rawValue().toString();
        cameraObject[_jsonCameraOrientationLandscapeKey] =  _cameraOrientationLandscapeFact.rawValue().toBool();
Don Gagne's avatar
Don Gagne committed
319 320 321 322 323 324 325 326 327 328 329
        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();
        cameraObject[_jsonGroundResolutionKey] =            _groundResolutionFact.rawValue().toDouble();
        cameraObject[_jsonFrontalOverlapKey] =              _frontalOverlapFact.rawValue().toInt();
        cameraObject[_jsonSideOverlapKey] =                 _sideOverlapFact.rawValue().toInt();

        saveObject[_jsonCameraObjectKey] = cameraObject;
    }
330 331 332

    // Polygon shape
    QJsonArray polygonArray;
333
    JsonHelper::savePolygon(_polygonModel, polygonArray);
Don Gagne's avatar
Don Gagne committed
334
    saveObject[_jsonPolygonObjectKey] = polygonArray;
335 336

    missionItems.append(saveObject);
337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354
}

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

void SurveyMissionItem::_clear(void)
{
    clearPolygon();
    _clearGrid();
}


Don Gagne's avatar
Don Gagne committed
355
bool SurveyMissionItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
356
{
Don Gagne's avatar
Don Gagne committed
357
    QJsonObject v2Object = complexObject;
Don Gagne's avatar
Don Gagne committed
358

Don Gagne's avatar
Don Gagne committed
359 360 361
    // 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
362
    };
Don Gagne's avatar
Don Gagne committed
363
    if (!JsonHelper::validateKeys(v2Object, versionKeyInfoList, errorString)) {
364 365
        return false;
    }
Don Gagne's avatar
Don Gagne committed
366 367 368

    int version = v2Object[JsonHelper::jsonVersionKey].toInt();
    if (version != 2 && version != 3) {
369
        errorString = tr("%1 does not support this version of survey items").arg(qgcApp()->applicationName());
370 371
        return false;
    }
Don Gagne's avatar
Don Gagne committed
372 373 374 375 376 377 378
    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;
        }
    }
379

Don Gagne's avatar
Don Gagne committed
380 381 382 383 384 385 386 387 388 389 390
    QList<JsonHelper::KeyValidateInfo> mainKeyInfoList = {
        { JsonHelper::jsonVersionKey,                   QJsonValue::Double, true },
        { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
        { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
        { _jsonPolygonObjectKey,                        QJsonValue::Array,  true },
        { _jsonGridObjectKey,                           QJsonValue::Object, true },
        { _jsonCameraObjectKey,                         QJsonValue::Object, false },
        { _jsonCameraTriggerKey,                        QJsonValue::Bool,   true },
        { _jsonCameraTriggerDistanceKey,                QJsonValue::Double, false },
        { _jsonManualGridKey,                           QJsonValue::Bool,   true },
        { _jsonFixedValueIsAltitudeKey,                 QJsonValue::Bool,   true },
DonLakeFlyer's avatar
DonLakeFlyer committed
391
        { _jsonHoverAndCaptureKey,                      QJsonValue::Bool,   false },
Don Gagne's avatar
Don Gagne committed
392 393
    };
    if (!JsonHelper::validateKeys(v2Object, mainKeyInfoList, errorString)) {
394 395
        return false;
    }
Don Gagne's avatar
Don Gagne committed
396 397 398 399

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

Don Gagne's avatar
Don Gagne committed
404 405
    _clear();

Don Gagne's avatar
Don Gagne committed
406
    setSequenceNumber(sequenceNumber);
407

408 409 410 411
    _manualGridFact.setRawValue             (v2Object[_jsonManualGridKey].toBool(true));
    _cameraTriggerFact.setRawValue          (v2Object[_jsonCameraTriggerKey].toBool(false));
    _fixedValueIsAltitudeFact.setRawValue   (v2Object[_jsonFixedValueIsAltitudeKey].toBool(true));
    _gridAltitudeRelativeFact.setRawValue   (v2Object[_jsonGridAltitudeRelativeKey].toBool(true));
DonLakeFlyer's avatar
DonLakeFlyer committed
412
    _hoverAndCaptureFact.setRawValue        (v2Object[_jsonHoverAndCaptureKey].toBool(false));
Don Gagne's avatar
Don Gagne committed
413

Don Gagne's avatar
Don Gagne committed
414 415 416 417 418 419 420 421 422 423 424
    QList<JsonHelper::KeyValidateInfo> gridKeyInfoList = {
        { _jsonGridAltitudeKey,                 QJsonValue::Double, true },
        { _jsonGridAltitudeRelativeKey,         QJsonValue::Bool,   true },
        { _jsonGridAngleKey,                    QJsonValue::Double, true },
        { _jsonGridSpacingKey,                  QJsonValue::Double, true },
        { _jsonTurnaroundDistKey,               QJsonValue::Double, true },
    };
    QJsonObject gridObject = v2Object[_jsonGridObjectKey].toObject();
    if (!JsonHelper::validateKeys(gridObject, gridKeyInfoList, errorString)) {
        return false;
    }
Don Gagne's avatar
Don Gagne committed
425 426 427 428 429
    _gridAltitudeFact.setRawValue   (gridObject[_jsonGridAltitudeKey].toDouble());
    _gridAngleFact.setRawValue      (gridObject[_jsonGridAngleKey].toDouble());
    _gridSpacingFact.setRawValue    (gridObject[_jsonGridSpacingKey].toDouble());
    _turnaroundDistFact.setRawValue (gridObject[_jsonTurnaroundDistKey].toDouble());

430
    if (_cameraTriggerFact.rawValue().toBool()) {
Don Gagne's avatar
Don Gagne committed
431
        if (!v2Object.contains(_jsonCameraTriggerDistanceKey)) {
Don Gagne's avatar
Don Gagne committed
432 433 434
            errorString = tr("%1 but %2 is missing").arg("cameraTrigger = true").arg("cameraTriggerDistance");
            return false;
        }
Don Gagne's avatar
Don Gagne committed
435
        _cameraTriggerDistanceFact.setRawValue(v2Object[_jsonCameraTriggerDistanceKey].toDouble());
Don Gagne's avatar
Don Gagne committed
436 437
    }

438
    if (!_manualGridFact.rawValue().toBool()) {
Don Gagne's avatar
Don Gagne committed
439
        if (!v2Object.contains(_jsonCameraObjectKey)) {
Don Gagne's avatar
Don Gagne committed
440 441 442 443
            errorString = tr("%1 but %2 object is missing").arg("manualGrid = false").arg("camera");
            return false;
        }

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

446 447 448 449 450 451 452
        // 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
453 454 455 456 457 458 459 460 461 462 463 464
        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 },
        };
Don Gagne's avatar
Don Gagne committed
465 466 467 468
        if (!JsonHelper::validateKeys(cameraObject, cameraKeyInfoList, errorString)) {
            return false;
        }

469 470
        _cameraFact.setRawValue(cameraObject[_jsonCameraNameKey].toString());
        _cameraOrientationLandscapeFact.setRawValue(cameraObject[_jsonCameraOrientationLandscapeKey].toBool(true));
Don Gagne's avatar
Don Gagne committed
471 472 473 474 475 476 477 478 479 480

        _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());
    }
481 482

    // Polygon shape
Don Gagne's avatar
Don Gagne committed
483
    QJsonArray polygonArray(v2Object[_jsonPolygonObjectKey].toArray());
484 485 486 487 488 489
    if (!JsonHelper::loadPolygon(polygonArray, _polygonModel, this, errorString)) {
        _clear();
        return false;
    }
    for (int i=0; i<_polygonModel.count(); i++) {
        _polygonPath << QVariant::fromValue(_polygonModel.value<QGCQGeoCoordinate*>(i)->coordinate());
490 491 492 493 494 495 496 497 498 499
    }

    _generateGrid();

    return true;
}

double SurveyMissionItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
    double greatestDistance = 0.0;
500 501
    for (int i=0; i<_simpleGridPoints.count(); i++) {
        QGeoCoordinate currentCoord = _simpleGridPoints[i].value<QGeoCoordinate>();
502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525
        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
{
    return _polygonPath.count() > 2;
}

void SurveyMissionItem::_clearGrid(void)
{
    // Bug workaround
526 527
    while (_simpleGridPoints.count() > 1) {
        _simpleGridPoints.takeLast();
528 529
    }
    emit gridPointsChanged();
530
    _simpleGridPoints.clear();
DonLakeFlyer's avatar
DonLakeFlyer committed
531 532 533
    _transectSegments.clear();

    _missionCommandCount = 0;
534 535 536 537 538
}

void _calcCameraShots()
{

539 540 541 542
}

void SurveyMissionItem::_generateGrid(void)
{
543
    if (_polygonPath.count() < 3 || _gridSpacingFact.rawValue().toDouble() <= 0) {
544 545 546 547
        _clearGrid();
        return;
    }

548
    _simpleGridPoints.clear();
DonLakeFlyer's avatar
DonLakeFlyer committed
549
    _transectSegments.clear();
550

551 552
    QList<QPointF>          polygonPoints;
    QList<QPointF>          gridPoints;
DonLakeFlyer's avatar
DonLakeFlyer committed
553
    QList<QList<QPointF>>   transectSegments;
554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575

    // Convert polygon to Qt coordinate system (y positive is down)
    qCDebug(SurveyMissionItemLog) << "Convert polygon";
    QGeoCoordinate tangentOrigin = _polygonPath[0].value<QGeoCoordinate>();
    for (int i=0; i<_polygonPath.count(); i++) {
        double y, x, down;
        convertGeoToNed(_polygonPath[i].value<QGeoCoordinate>(), tangentOrigin, &y, &x, &down);
        polygonPoints += QPointF(x, -y);
        qCDebug(SurveyMissionItemLog) << _polygonPath[i].value<QGeoCoordinate>() << polygonPoints.last().x() << polygonPoints.last().y();
    }

    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
DonLakeFlyer's avatar
DonLakeFlyer committed
576
    _gridGenerator(polygonPoints, gridPoints, transectSegments);
577

DonLakeFlyer's avatar
DonLakeFlyer committed
578
    // Convert simple grid to QGeoCoordinates
579 580 581 582 583 584 585 586 587 588
    double surveyDistance = 0.0;
    for (int i=0; i<gridPoints.count(); i++) {
        QPointF& point = gridPoints[i];

        if (i != 0) {
            surveyDistance += sqrt(pow((gridPoints[i] - gridPoints[i - 1]).x(),2.0) + pow((gridPoints[i] - gridPoints[i - 1]).y(),2.0));
        }

        QGeoCoordinate geoCoord;
        convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &geoCoord);
589
        _simpleGridPoints += QVariant::fromValue(geoCoord);
590 591
    }
    _setSurveyDistance(surveyDistance);
592

DonLakeFlyer's avatar
DonLakeFlyer committed
593 594 595 596 597 598 599 600 601 602 603 604 605 606
    // Convert transect segments to QGeoCoordinate
    for (int i=0; i<transectSegments.count(); i++) {
        QList<QGeoCoordinate>   transectCoords;
        const QList<QPointF>&   transectPoints = transectSegments[i];

        for (int j=0; j<transectPoints.count(); j++) {
            QGeoCoordinate coord;
            const QPointF& point = transectPoints[j];
            convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &coord);
            transectCoords.append(coord);
        }
        _transectSegments.append(transectCoords);
    }

607
    // Set camera shots here if taking images in turnaround (otherwise it's in _gridGenerator)
DonLakeFlyer's avatar
DonLakeFlyer committed
608 609 610
    if (_triggerCamera()) {
        if (_imagesEverywhere()) {
            _setCameraShots((int)ceil(surveyDistance / _triggerDistance()));
611
        }
612 613 614
    } else {
        _setCameraShots(0);
    }
615 616

    emit gridPointsChanged();
DonLakeFlyer's avatar
DonLakeFlyer committed
617 618 619 620 621 622 623 624 625 626 627 628 629 630 631

    // Determine command count for lastSequenceNumber

    _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()) {
            _missionCommandCount += 2;                          // Camera on/off at entry/exit
        }
    }
632 633
    emit lastSequenceNumberChanged(lastSequenceNumber());

DonLakeFlyer's avatar
DonLakeFlyer committed
634
    // Set exit coordinate
635 636
    if (_simpleGridPoints.count()) {
        QGeoCoordinate coordinate = _simpleGridPoints.first().value<QGeoCoordinate>();
637 638
        coordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble());
        setCoordinate(coordinate);
639
        QGeoCoordinate exitCoordinate = _simpleGridPoints.last().value<QGeoCoordinate>();
640 641
        exitCoordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble());
        _setExitCoordinate(exitCoordinate);
642 643 644
    }
}

645 646 647 648 649 650 651 652
void SurveyMissionItem::_updateCoordinateAltitude(void)
{
    _coordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble());
    _exitCoordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble());
    emit coordinateChanged(_coordinate);
    emit exitCoordinateChanged(_exitCoordinate);
}

653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 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
QPointF SurveyMissionItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle)
{
    QPointF rotated;
    double radians = (M_PI / 180.0) * angle;

    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)
{
    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)
{
    for (int i=0; i<lineList.count(); i++) {
        const QLineF& line = lineList[i];
        QLineF adjustedLine;

        if (line.angle() > 180.0) {
            adjustedLine.setP1(line.p2());
            adjustedLine.setP2(line.p1());
        } else {
            adjustedLine = line;
        }

        resultLines += adjustedLine;
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
767
void SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints,  QList<QPointF>& simpleGridPoints, QList<QList<QPointF>>& transectSegments)
768 769
{
    double gridAngle = _gridAngleFact.rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
770 771 772
    double gridSpacing = _gridSpacingFact.rawValue().toDouble();

    qCDebug(SurveyMissionItemLog) << "SurveyMissionItem::_gridGenerator gridSpacing:gridAngle" << gridSpacing << gridAngle;
773

774
    simpleGridPoints.clear();
DonLakeFlyer's avatar
DonLakeFlyer committed
775
    transectSegments.clear();
776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802

    // 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];
    QRectF smallBoundRect = polygon.boundingRect();
    QPointF center = smallBoundRect.center();
    qCDebug(SurveyMissionItemLog) << "Bounding rect" << smallBoundRect.topLeft().x() << smallBoundRect.topLeft().y() << smallBoundRect.bottomRight().x() << smallBoundRect.bottomRight().y();

    // Rotate the bounding rect around it's center to generate the larger bounding rect
    QPolygonF boundPolygon;
    boundPolygon << _rotatePoint(smallBoundRect.topLeft(),       center, gridAngle);
    boundPolygon << _rotatePoint(smallBoundRect.topRight(),      center, gridAngle);
    boundPolygon << _rotatePoint(smallBoundRect.bottomRight(),   center, gridAngle);
    boundPolygon << _rotatePoint(smallBoundRect.bottomLeft(),    center, gridAngle);
    boundPolygon << boundPolygon[0];
    QRectF largeBoundRect = boundPolygon.boundingRect();
    qCDebug(SurveyMissionItemLog) << "Rotated bounding rect" << largeBoundRect.topLeft().x() << largeBoundRect.topLeft().y() << largeBoundRect.bottomRight().x() << largeBoundRect.bottomRight().y();

    // Create set of rotated parallel lines within the expanded bounding rect. Make the lines larger than the
    // bounding box to guarantee intersection.
    QList<QLineF> lineList;
803
    float x = largeBoundRect.topLeft().x() - (gridSpacing / 2);
804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827
    while (x < largeBoundRect.bottomRight().x()) {
        float yTop =    largeBoundRect.topLeft().y() - 100.0;
        float yBottom = largeBoundRect.bottomRight().y() + 100.0;

        lineList += QLineF(_rotatePoint(QPointF(x, yTop), center, gridAngle), _rotatePoint(QPointF(x, yBottom), center, gridAngle));
        qCDebug(SurveyMissionItemLog) << "line(" << lineList.last().x1() << ", " << lineList.last().y1() << ")-(" << lineList.last().x2() <<", " << lineList.last().y2() << ")";

        x += gridSpacing;
    }

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

    // 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);

828
    // Calc camera shots here if there are no images in turnaround
DonLakeFlyer's avatar
DonLakeFlyer committed
829
    if (_triggerCamera() && !_imagesEverywhere()) {
830 831
        int cameraShots = 0;
        for (int i=0; i<resultLines.count(); i++) {
DonLakeFlyer's avatar
DonLakeFlyer committed
832
            cameraShots += (int)ceil(resultLines[i].length() / _triggerDistance());
833 834 835 836 837
        }
        _setCameraShots(cameraShots);
    }

    // Turn into a path
838
    for (int i=0; i<resultLines.count(); i++) {
DonLakeFlyer's avatar
DonLakeFlyer committed
839 840 841
        QLineF          transectLine;
        QList<QPointF>  transectPoints;
        const QLineF&   line = resultLines[i];
842

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

845
        if (i & 1) {
DonLakeFlyer's avatar
DonLakeFlyer committed
846
            transectLine = QLineF(line.p2(), line.p1());
847
        } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868
            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
869
            }
870
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
871 872 873 874 875 876 877 878 879 880 881 882

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

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

        simpleGridPoints.append(transectPoints[0]);
        simpleGridPoints.append(transectPoints[transectPoints.count() - 1]);

        transectSegments.append(transectPoints);
883 884 885 886 887
    }
}

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

    qCDebug(SurveyMissionItemLog) << "_appendWaypointToMission seq:trigger" << seqNum << (cameraTrigger != CameraTriggerNone);
892 893 894 895

    MissionItem* item = new MissionItem(seqNum++,
                                        MAV_CMD_NAV_WAYPOINT,
                                        altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
DonLakeFlyer's avatar
DonLakeFlyer committed
896 897
                                        cameraTrigger == CameraTriggerHoverAndCapture ? 1 : 0,  // Hold time (1 second for hover and capture to settle vehicle before image is taken)
                                        0.0, 0.0, 0.0,                                          // param 2-4 unused
898 899 900
                                        coord.latitude(),
                                        coord.longitude(),
                                        altitude,
DonLakeFlyer's avatar
DonLakeFlyer committed
901 902
                                        true,                                                   // autoContinue
                                        false,                                                  // isCurrentItem
903 904 905
                                        missionItemParent);
    items.append(item);

DonLakeFlyer's avatar
DonLakeFlyer committed
906 907 908 909 910 911 912 913 914 915 916
    switch (cameraTrigger) {
    case CameraTriggerOff:
    case CameraTriggerOn:
        item = new MissionItem(seqNum++,
                               MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                               MAV_FRAME_MISSION,
                               cameraTrigger == CameraTriggerOn ? _triggerDistance() : 0,
                               0, 0, 0, 0, 0, 0,               // param 2-7 unused
                               true,                           // autoContinue
                               false,                          // isCurrentItem
                               missionItemParent);
917
        items.append(item);
DonLakeFlyer's avatar
DonLakeFlyer committed
918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947
        break;
    case CameraTriggerHoverAndCapture:
        item = new MissionItem(seqNum++,
                               MAV_CMD_IMAGE_START_CAPTURE,
                               MAV_FRAME_MISSION,
                               0,                  // Interval
                               1,                  // Take 1 photo
                               -1,                 // Mav resolution
                               0, 0,               // Param 4-5 unused
                               0,                  // Camera ID
                               7,                  // Param 7 unused
                               true,               // autoContinue
                               false,              // isCurrentItem
                               missionItemParent);
        items.append(item);
#if 0
        // Not yet supported by firmware
        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);
#endif
    default:
        break;
948
    }
949 950

    return seqNum;
951 952
}

DonLakeFlyer's avatar
DonLakeFlyer committed
953 954 955 956 957 958 959 960 961 962 963
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;
}

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

DonLakeFlyer's avatar
DonLakeFlyer committed
968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984
    qCDebug(SurveyMissionItemLog) << "hasTurnaround:triggerCamera:hoverAndCapture:imagesEverywhere" << _hasTurnaround() << _triggerCamera() << _hoverAndCaptureEnabled() << _imagesEverywhere();

    if (_imagesEverywhere()) {
        MissionItem* item = new MissionItem(seqNum++,
                                            MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                            MAV_FRAME_MISSION,
                                            _triggerDistance(),
                                            0, 0, 0, 0, 0, 0,       // param 2-7 unused
                                            true,                   // autoContinue
                                            false,                  // isCurrentItem
                                            missionItemParent);
        items.append(item);
    }

    for (int segmentIndex=0; segmentIndex<_transectSegments.count(); segmentIndex++) {
        int pointIndex = 0;
        QGeoCoordinate coord;
985
        CameraTriggerCode cameraTrigger;
DonLakeFlyer's avatar
DonLakeFlyer committed
986 987 988 989 990 991 992 993 994 995 996
        const QList<QGeoCoordinate>& transectSegment = _transectSegments[segmentIndex];

        qCDebug(SurveyMissionItemLog) << "transectSegment.count" << transectSegment.count();

        if (_hasTurnaround()) {
            // Add entry turnaround point
            if (!_nextTransectCoord(transectSegment, pointIndex++, coord)) {
                return;
            }
            seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent);
        }
997

DonLakeFlyer's avatar
DonLakeFlyer committed
998 999 1000 1001 1002
        // Add polygon entry point
        if (!_nextTransectCoord(transectSegment, pointIndex++, coord)) {
            return;
        }
        cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerHoverAndCapture : CameraTriggerOn);
1003 1004
        seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent);

DonLakeFlyer's avatar
DonLakeFlyer committed
1005 1006 1007 1008 1009 1010 1011 1012 1013
        // Add internal hover and capture points
        if (_hoverAndCaptureEnabled()) {
            int lastHoverAndCaptureIndex = transectSegment.count() - 1 - (_hasTurnaround() ? 1 : 0);
            qCDebug(SurveyMissionItemLog) << "lastHoverAndCaptureIndex" << lastHoverAndCaptureIndex;
            for (; pointIndex < lastHoverAndCaptureIndex; pointIndex++) {
                if (!_nextTransectCoord(transectSegment, pointIndex, coord)) {
                    return;
                }
                seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerHoverAndCapture, missionItemParent);
1014 1015
            }
        }
1016

DonLakeFlyer's avatar
DonLakeFlyer committed
1017 1018 1019
        // Add polygon exit point
        if (!_nextTransectCoord(transectSegment, pointIndex++, coord)) {
            return;
1020
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
1021
        cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerNone : CameraTriggerOff);
1022 1023
        seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent);

DonLakeFlyer's avatar
DonLakeFlyer committed
1024 1025 1026 1027
        if (_hasTurnaround()) {
            // Add exit turnaround point
            if (!_nextTransectCoord(transectSegment, pointIndex++, coord)) {
                return;
1028
            }
DonLakeFlyer's avatar
DonLakeFlyer committed
1029
            seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent);
1030
        }
1031

DonLakeFlyer's avatar
DonLakeFlyer committed
1032
        qCDebug(SurveyMissionItemLog) << "last PointIndex" << pointIndex;
1033 1034
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
1035
    if (_imagesEverywhere()) {
1036 1037 1038 1039 1040 1041 1042 1043
        // 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
1044 1045
                                            missionItemParent);
        items.append(item);
1046 1047 1048
    }
}

Don Gagne's avatar
Don Gagne committed
1049 1050
int SurveyMissionItem::cameraShots(void) const
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1051
    return _triggerCamera() ? _cameraShots : 0;
1052
}
Don Gagne's avatar
Don Gagne committed
1053 1054 1055 1056 1057

void SurveyMissionItem::_cameraValueChanged(void)
{
    emit cameraValueChanged();
}
1058 1059 1060

double SurveyMissionItem::timeBetweenShots(void) const
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1061
    return _cruiseSpeed == 0 ? 0 : _triggerDistance() / _cruiseSpeed;
1062 1063
}

DonLakeFlyer's avatar
DonLakeFlyer committed
1064
void SurveyMissionItem::setMissionFlightStatus  (MissionController::MissionFlightStatus_t& missionFlightStatus)
1065
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1066 1067 1068
    ComplexMissionItem::setMissionFlightStatus(missionFlightStatus);
    if (!qFuzzyCompare(_cruiseSpeed, missionFlightStatus.vehicleSpeed)) {
        _cruiseSpeed = missionFlightStatus.vehicleSpeed;
1069 1070 1071
        emit timeBetweenShotsChanged();
    }
}
1072 1073 1074 1075 1076

void SurveyMissionItem::_setDirty(void)
{
    setDirty(true);
}
DonLakeFlyer's avatar
DonLakeFlyer committed
1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110

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

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

bool SurveyMissionItem::_triggerCamera(void) const
{
    return _cameraTriggerFact.rawValue().toBool() && _triggerDistance() > 0;
}

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
1111 1112 1113 1114 1115

void SurveyMissionItem::applyNewAltitude(double newAltitude)
{
    _gridAltitudeFact.setRawValue(newAltitude);
}