StructureScanComplexItem.cc 35 KB
Newer Older
1 2
/****************************************************************************
 *
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "StructureScanComplexItem.h"
#include "JsonHelper.h"
#include "MissionController.h"
#include "QGCGeo.h"
#include "QGroundControlQmlGlobal.h"
#include "QGCQGeoCoordinate.h"
#include "SettingsManager.h"
#include "AppSettings.h"
#include "QGCQGeoCoordinate.h"
19 20
#include "PlanMasterController.h"
#include "FlightPathSegment.h"
21 22 23 24 25

#include <QPolygonF>

QGC_LOGGING_CATEGORY(StructureScanComplexItemLog, "StructureScanComplexItemLog")

26 27
const QString StructureScanComplexItem::name(tr("Structure Scan"));

28
const char* StructureScanComplexItem::settingsGroup =               "StructureScan";
29
const char* StructureScanComplexItem::_entranceAltName =            "EntranceAltitude";
30
const char* StructureScanComplexItem::scanBottomAltName =           "ScanBottomAlt";
31 32
const char* StructureScanComplexItem::structureHeightName =         "StructureHeight";
const char* StructureScanComplexItem::layersName =                  "Layers";
33
const char* StructureScanComplexItem::gimbalPitchName =             "GimbalPitch";
34
const char* StructureScanComplexItem::startFromTopName =            "StartFromTop";
35

36 37
const char* StructureScanComplexItem::jsonComplexItemTypeValue =    "StructureScan";
const char* StructureScanComplexItem::_jsonCameraCalcKey =          "CameraCalc";
38

39 40
StructureScanComplexItem::StructureScanComplexItem(PlanMasterController* masterController, bool flyView, const QString& kmlOrShpFile, QObject* parent)
    : ComplexMissionItem        (masterController, flyView, parent)
41
    , _metaDataMap              (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/StructureScan.SettingsGroup.json"), this /* QObject parent */))
42 43 44 45 46
    , _sequenceNumber           (0)
    , _entryVertex              (0)
    , _ignoreRecalc             (false)
    , _scanDistance             (0.0)
    , _cameraShots              (0)
47
    , _cameraCalc               (masterController, settingsGroup)
48
    , _scanBottomAltFact        (settingsGroup, _metaDataMap[scanBottomAltName])
49 50
    , _structureHeightFact      (settingsGroup, _metaDataMap[structureHeightName])
    , _layersFact               (settingsGroup, _metaDataMap[layersName])
51
    , _gimbalPitchFact          (settingsGroup, _metaDataMap[gimbalPitchName])
52
    , _startFromTopFact         (settingsGroup, _metaDataMap[startFromTopName])
53
    , _entranceAltFact          (settingsGroup, _metaDataMap[_entranceAltName])
54 55 56
{
    _editorQml = "qrc:/qml/StructureScanEditor.qml";

57
    _entranceAltFact.setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue());
58

59 60 61 62 63
    connect(&_entranceAltFact,      &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
    connect(&_scanBottomAltFact,    &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
    connect(&_layersFact,           &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
    connect(&_gimbalPitchFact,      &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
    connect(&_startFromTopFact,     &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
64

65 66 67
    connect(&_startFromTopFact,     &Fact::valueChanged, this, &StructureScanComplexItem::_signalTopBottomAltChanged);
    connect(&_layersFact,           &Fact::valueChanged, this, &StructureScanComplexItem::_signalTopBottomAltChanged);

DonLakeFlyer's avatar
DonLakeFlyer committed
68
    connect(&_structureHeightFact,                  &Fact::valueChanged,    this, &StructureScanComplexItem::_recalcLayerInfo);
69
    connect(&_scanBottomAltFact,                    &Fact::valueChanged,    this, &StructureScanComplexItem::_recalcLayerInfo);
DonLakeFlyer's avatar
DonLakeFlyer committed
70 71
    connect(_cameraCalc.adjustedFootprintFrontal(), &Fact::valueChanged,    this, &StructureScanComplexItem::_recalcLayerInfo);

72
    connect(&_entranceAltFact, &Fact::valueChanged, this, &StructureScanComplexItem::_updateCoordinateAltitudes);
73

74 75
    connect(&_structurePolygon, &QGCMapPolygon::dirtyChanged,   this, &StructureScanComplexItem::_polygonDirtyChanged);
    connect(&_structurePolygon, &QGCMapPolygon::pathChanged,    this, &StructureScanComplexItem::_rebuildFlightPolygon);
76 77 78
    connect(&_structurePolygon, &QGCMapPolygon::isValidChanged, this, &StructureScanComplexItem::readyForSaveStateChanged);
    connect(&_structurePolygon, &QGCMapPolygon::isValidChanged,     this, &StructureScanComplexItem::_updateWizardMode);
    connect(&_structurePolygon, &QGCMapPolygon::traceModeChanged,   this, &StructureScanComplexItem::_updateWizardMode);
79

80 81 82
    connect(&_structurePolygon, &QGCMapPolygon::countChanged,   this, &StructureScanComplexItem::_updateLastSequenceNumber);
    connect(&_layersFact,       &Fact::valueChanged,            this, &StructureScanComplexItem::_updateLastSequenceNumber);

83 84
    connect(&_flightPolygon,    &QGCMapPolygon::pathChanged,    this, &StructureScanComplexItem::_flightPathChanged);

85
    connect(_cameraCalc.distanceToSurface(),    &Fact::valueChanged,                this, &StructureScanComplexItem::_rebuildFlightPolygon);
86 87 88 89

    connect(&_flightPolygon,                        &QGCMapPolygon::pathChanged,    this, &StructureScanComplexItem::_recalcCameraShots);
    connect(_cameraCalc.adjustedFootprintSide(),    &Fact::valueChanged,            this, &StructureScanComplexItem::_recalcCameraShots);
    connect(&_layersFact,                           &Fact::valueChanged,            this, &StructureScanComplexItem::_recalcCameraShots);
DonLakeFlyer's avatar
DonLakeFlyer committed
90

91 92
    connect(&_cameraCalc, &CameraCalc::isManualCameraChanged, this, &StructureScanComplexItem::_updateGimbalPitch);

93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123
    connect(&_layersFact,                           &Fact::valueChanged,            this, &StructureScanComplexItem::_recalcScanDistance);
    connect(&_flightPolygon,                        &QGCMapPolygon::pathChanged,    this, &StructureScanComplexItem::_recalcScanDistance);

    connect(this, &StructureScanComplexItem::wizardModeChanged, this, &StructureScanComplexItem::readyForSaveStateChanged);

    connect(_missionController, &MissionController::plannedHomePositionChanged,     this, &StructureScanComplexItem::_amslEntryAltChanged);
    connect(&_entranceAltFact,  &Fact::valueChanged,                                this, &StructureScanComplexItem::_amslEntryAltChanged);
    connect(this,               &StructureScanComplexItem::amslEntryAltChanged,     this, &StructureScanComplexItem::amslExitAltChanged);

    connect(_missionController, &MissionController::plannedHomePositionChanged,     this, &StructureScanComplexItem::_minAMSLAltChanged);
    connect(_missionController, &MissionController::plannedHomePositionChanged,     this, &StructureScanComplexItem::_maxAMSLAltChanged);
    connect(this,               &StructureScanComplexItem::topFlightAltChanged,     this, &StructureScanComplexItem::_minAMSLAltChanged);
    connect(this,               &StructureScanComplexItem::topFlightAltChanged,     this, &StructureScanComplexItem::_maxAMSLAltChanged);
    connect(this,               &StructureScanComplexItem::bottomFlightAltChanged,  this, &StructureScanComplexItem::_minAMSLAltChanged);
    connect(this,               &StructureScanComplexItem::bottomFlightAltChanged,  this, &StructureScanComplexItem::_maxAMSLAltChanged);
    connect(&_entranceAltFact,  &Fact::valueChanged,                                this, &StructureScanComplexItem::_minAMSLAltChanged);
    connect(&_entranceAltFact,  &Fact::valueChanged,                                this, &StructureScanComplexItem::_maxAMSLAltChanged);

    connect(&_flightPolygon,                        &QGCMapPolygon::pathChanged,                    this, &StructureScanComplexItem::_updateFlightPathSegmentsSignal);
    connect(&_startFromTopFact,                     &Fact::valueChanged,                            this, &StructureScanComplexItem::_updateFlightPathSegmentsSignal);
    connect(&_structureHeightFact,                  &Fact::valueChanged,                            this, &StructureScanComplexItem::_updateFlightPathSegmentsSignal);
    connect(&_scanBottomAltFact,                    &Fact::valueChanged,                            this, &StructureScanComplexItem::_updateFlightPathSegmentsSignal);
    connect(_cameraCalc.adjustedFootprintFrontal(), &Fact::valueChanged,                            this, &StructureScanComplexItem::_updateFlightPathSegmentsSignal);
    connect(&_entranceAltFact,                      &Fact::valueChanged,                            this, &StructureScanComplexItem::_updateFlightPathSegmentsSignal);
    connect(&_layersFact,                           &Fact::valueChanged,                            this, &StructureScanComplexItem::_updateFlightPathSegmentsSignal);
    connect(_missionController,                     &MissionController::plannedHomePositionChanged, this, &StructureScanComplexItem::_updateFlightPathSegmentsSignal);

    // The follow is used to compress multiple recalc calls in a row to into a single call.
    connect(this, &StructureScanComplexItem::_updateFlightPathSegmentsSignal, this, &StructureScanComplexItem::_updateFlightPathSegmentsDontCallDirectly,   Qt::QueuedConnection);
    qgcApp()->addCompressedSignal(QMetaMethod::fromSignal(&StructureScanComplexItem::_updateFlightPathSegmentsSignal));

DonLakeFlyer's avatar
DonLakeFlyer committed
124
    _recalcLayerInfo();
125

126 127
    if (!kmlOrShpFile.isEmpty()) {
        _structurePolygon.loadKMLOrSHPFile(kmlOrShpFile);
128 129 130 131
        _structurePolygon.setDirty(false);
    }

    setDirty(false);
132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149
}

void StructureScanComplexItem::_setCameraShots(int cameraShots)
{
    if (_cameraShots != cameraShots) {
        _cameraShots = cameraShots;
        emit cameraShotsChanged(this->cameraShots());
    }
}

void StructureScanComplexItem::_clearInternal(void)
{
    setDirty(true);

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

150
void StructureScanComplexItem::_updateLastSequenceNumber(void)
151 152 153 154 155 156
{
    emit lastSequenceNumberChanged(lastSequenceNumber());
}

int StructureScanComplexItem::lastSequenceNumber(void) const
{
157 158 159 160 161
    // Each structure layer contains:
    //  1 waypoint for each polygon vertex + 1 to go back to first polygon vertex for each layer
    //  Two commands for camera trigger start/stop
    int layerItemCount = _flightPolygon.count() + 1 + 2;

162
    // Take into account the number of layers
163 164
    int multiLayerItemCount = layerItemCount * _layersFact.rawValue().toInt();

165 166 167
    // +2 for ROI_WPNEXT_OFFSET and ROI_NONE commands
    // +2 for entrance/exit waypoints
    int itemCount = multiLayerItemCount + 2 + 2;
168 169

    return _sequenceNumber + itemCount - 1;
170 171 172 173 174 175 176 177 178 179
}

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

180
void StructureScanComplexItem::save(QJsonArray&  missionItems)
181 182 183
{
    QJsonObject saveObject;

184
    // Header
185
    saveObject[JsonHelper::jsonVersionKey] =                    3;
186 187 188
    saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
    saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;

189 190 191 192 193 194
    saveObject[_entranceAltName] =      _entranceAltFact.rawValue().toDouble();
    saveObject[scanBottomAltName] =     _scanBottomAltFact.rawValue().toDouble();
    saveObject[structureHeightName] =   _structureHeightFact.rawValue().toDouble();
    saveObject[layersName] =            _layersFact.rawValue().toDouble();
    saveObject[gimbalPitchName] =       _gimbalPitchFact.rawValue().toDouble();
    saveObject[startFromTopName] =      _startFromTopFact.rawValue().toBool();
195 196 197 198 199

    QJsonObject cameraCalcObject;
    _cameraCalc.save(cameraCalcObject);
    saveObject[_jsonCameraCalcKey] = cameraCalcObject;

200
    _structurePolygon.saveToJson(saveObject);
201 202 203 204 205 206 207 208 209 210 211 212 213 214 215

    missionItems.append(saveObject);
}

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

bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
{
216
    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
217 218 219 220
        { JsonHelper::jsonVersionKey,                   QJsonValue::Double, true },
        { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
        { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
        { QGCMapPolygon::jsonPolygonKey,                QJsonValue::Array,  true },
221
        { scanBottomAltName,                            QJsonValue::Double, true },
222 223
        { structureHeightName,                          QJsonValue::Double, true },
        { layersName,                                   QJsonValue::Double, true },
224
        { _jsonCameraCalcKey,                           QJsonValue::Object, true },
225 226 227
        { _entranceAltName,                             QJsonValue::Double, true },
        { gimbalPitchName,                              QJsonValue::Double, true },
        { startFromTopName,                             QJsonValue::Bool,   true },
228
    };
229
    if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
230 231 232
        return false;
    }

233 234 235 236
    _structurePolygon.clear();

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

242
    int version = complexObject[JsonHelper::jsonVersionKey].toInt();
243 244
    if (version != 3) {
        errorString = tr("%1 version %2 not supported").arg(jsonComplexItemTypeValue).arg(version);
245 246 247
        return false;
    }

248
    setSequenceNumber(sequenceNumber);
249

250 251 252 253 254
    // Load CameraCalc first since it will trigger camera name change which will trounce gimbal angles
    if (!_cameraCalc.load(complexObject[_jsonCameraCalcKey].toObject(), errorString)) {
        return false;
    }

255 256 257 258 259 260
    _scanBottomAltFact.setRawValue      (complexObject[scanBottomAltName].toDouble());
    _layersFact.setRawValue             (complexObject[layersName].toDouble());
    _structureHeightFact.setRawValue    (complexObject[structureHeightName].toDouble());
    _startFromTopFact.setRawValue       (complexObject[startFromTopName].toBool());
    _entranceAltFact.setRawValue        (complexObject[startFromTopName].toDouble());
    _gimbalPitchFact.setRawValue        (complexObject[gimbalPitchName].toDouble());
261

262
    if (!_structurePolygon.loadFromJson(complexObject, true /* required */, errorString)) {
263
        _structurePolygon.clear();
264 265 266 267 268 269
        return false;
    }

    return true;
}

270
void StructureScanComplexItem::_flightPathChanged(void)
271
{
272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292
    // Calc bounding cube
    double north = 0.0;
    double south = 180.0;
    double east  = 0.0;
    double west  = 360.0;
    double bottom = 100000.;
    double top = 0.;
    QList<QGeoCoordinate> vertices = _flightPolygon.coordinateList();
    for (int i = 0; i < vertices.count(); i++) {
        QGeoCoordinate vertex = vertices[i];
        double lat = vertex.latitude()  + 90.0;
        double lon = vertex.longitude() + 180.0;
        north   = fmax(north, lat);
        south   = fmin(south, lat);
        east    = fmax(east,  lon);
        west    = fmin(west,  lon);
        bottom  = fmin(bottom, vertex.altitude());
        top     = fmax(top, vertex.altitude());
    }
    //-- Update bounding cube for airspace management control
    _setBoundingCube(QGCGeoBoundingCube(
293 294
                         QGeoCoordinate(north - 90.0, west - 180.0, bottom),
                         QGeoCoordinate(south - 90.0, east - 180.0, top)));
295

296 297 298
    emit coordinateChanged(coordinate());
    emit exitCoordinateChanged(exitCoordinate());
    emit greatestDistanceToChanged();
299 300 301 302 303

    if (_isIncomplete) {
        _isIncomplete = false;
        emit isIncompleteChanged();
    }
304 305 306 307 308
}

double StructureScanComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
    double greatestDistance = 0.0;
309
    QList<QGeoCoordinate> vertices = _flightPolygon.coordinateList();
310 311 312 313 314 315 316 317 318 319 320 321 322 323

    for (int i=0; i<vertices.count(); i++) {
        QGeoCoordinate vertex = vertices[i];
        double distance = vertex.distanceTo(other);
        if (distance > greatestDistance) {
            greatestDistance = distance;
        }
    }

    return greatestDistance;
}

void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
324 325
    int     seqNum =        _sequenceNumber;
    bool    startFromTop =  _startFromTopFact.rawValue().toBool();
326
    double  startAltitude = (startFromTop ? _structureHeightFact.rawValue().toDouble() : _scanBottomAltFact.rawValue().toDouble());
327 328 329 330 331 332 333

    MissionItem* item = nullptr;

    // Entrance waypoint
    QGeoCoordinate entranceExitCoord = _flightPolygon.vertexCoordinate(_entryVertex);
    item = new MissionItem(seqNum++,
                           MAV_CMD_NAV_WAYPOINT,
334
                           MAV_FRAME_GLOBAL_RELATIVE_ALT,
335 336 337 338 339 340 341 342 343 344
                           0,                                          // No hold time
                           0.0,                                        // No acceptance radius specified
                           0.0,                                        // Pass through waypoint
                           std::numeric_limits<double>::quiet_NaN(),   // Yaw unchanged
                           entranceExitCoord.latitude(),
                           entranceExitCoord.longitude(),
                           _entranceAltFact.rawValue().toDouble(),
                           true,                                       // autoContinue
                           false,                                      // isCurrentItem
                           missionItemParent);
345
    items.append(item);
346

347 348 349 350 351 352 353 354 355 356 357 358 359
    // Point camera at structure
    item = new MissionItem(seqNum++,
                           MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET,
                           MAV_FRAME_MISSION,
                           0, 0, 0, 0,                             // param 1-4 not used
                           _gimbalPitchFact.rawValue().toDouble(),
                           0,                                      // Roll stays in standard orientation
                           90,                                     // 90 degreee yaw offset to point to structure
                           true,                                   // autoContinue
                           false,                                  // isCurrentItem
                           missionItemParent);
    items.append(item);

Don Gagne's avatar
Don Gagne committed
360 361 362 363 364 365 366 367 368
    // Set up for the first layer
    double  layerAltitude = startAltitude;
    double  halfLayerHeight = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0;
    if (startFromTop) {
        layerAltitude -= halfLayerHeight;
    } else {
        layerAltitude += halfLayerHeight;
    }

369
    for (int layer=0; layer<_layersFact.rawValue().toInt(); layer++) {
Don Gagne's avatar
Don Gagne committed
370
        bool addTriggerStart = true;
371

372 373 374 375 376 377 378 379
        bool done = false;
        int currentVertex = _entryVertex;
        int processedVertices = 0;
        do {
            QGeoCoordinate vertexCoord = _flightPolygon.vertexCoordinate(currentVertex);

            item = new MissionItem(seqNum++,
                                   MAV_CMD_NAV_WAYPOINT,
380
                                   MAV_FRAME_GLOBAL_RELATIVE_ALT,
381 382 383 384 385 386 387 388 389 390
                                   0,                                          // No hold time
                                   0.0,                                        // No acceptance radius specified
                                   0.0,                                        // Pass through waypoint
                                   std::numeric_limits<double>::quiet_NaN(),   // Yaw unchanged
                                   vertexCoord.latitude(),
                                   vertexCoord.longitude(),
                                   layerAltitude,
                                   true,                                       // autoContinue
                                   false,                                      // isCurrentItem
                                   missionItemParent);
391
            items.append(item);
392

393
            // Start camera triggering after first waypoint in layer
394 395 396 397 398 399 400 401 402 403 404 405 406 407
            if (addTriggerStart) {
                addTriggerStart = false;
                item = new MissionItem(seqNum++,
                                       MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                                       MAV_FRAME_MISSION,
                                       _cameraCalc.adjustedFootprintSide()->rawValue().toDouble(),  // trigger distance
                                       0,                                                           // shutter integration (ignore)
                                       1,                                                           // trigger immediately when starting
                                       0, 0, 0, 0,                                                  // param 4-7 unused
                                       true,                                                        // autoContinue
                                       false,                                                       // isCurrentItem
                                       missionItemParent);
                items.append(item);
            }
408

409 410 411 412 413 414 415 416 417 418
            // Move to next vertext
            currentVertex++;
            if (currentVertex >= _flightPolygon.count()) {
                currentVertex = 0;
            }

            // Have we processed all the vertices
            processedVertices++;
            done = processedVertices == _flightPolygon.count() + 1;
        } while (!done);
419

420
        // Stop camera triggering after last waypoint in layer
421 422 423 424 425 426 427 428 429 430
        item = new MissionItem(seqNum++,
                               MAV_CMD_DO_SET_CAM_TRIGG_DIST,
                               MAV_FRAME_MISSION,
                               0,           // stop triggering
                               0,           // shutter integration (ignore)
                               0,           // trigger immediately when starting
                               0, 0, 0, 0,  // param 4-7 unused
                               true,        // autoContinue
                               false,       // isCurrentItem
                               missionItemParent);
431
        items.append(item);
432 433 434 435 436 437 438

        // Move to next layer altitude
        if (startFromTop) {
            layerAltitude -= halfLayerHeight * 2;
        } else {
            layerAltitude += halfLayerHeight * 2;
        }
439
    }
440

441
    // Return camera to neutral position
442 443 444 445 446 447 448 449
    item = new MissionItem(seqNum++,
                           MAV_CMD_DO_SET_ROI_NONE,
                           MAV_FRAME_MISSION,
                           0, 0, 0,0, 0, 0, 0,                 // param 1-7 not used
                           true,                               // autoContinue
                           false,                              // isCurrentItem
                           missionItemParent);
    items.append(item);
450 451 452 453

    // Exit waypoint
    item = new MissionItem(seqNum++,
                           MAV_CMD_NAV_WAYPOINT,
454
                           MAV_FRAME_GLOBAL_RELATIVE_ALT,
455 456 457 458 459 460 461 462 463 464 465 466
                           0,                                          // No hold time
                           0.0,                                        // No acceptance radius specified
                           0.0,                                        // Pass through waypoint
                           std::numeric_limits<double>::quiet_NaN(),   // Yaw unchanged
                           entranceExitCoord.latitude(),
                           entranceExitCoord.longitude(),
                           _entranceAltFact.rawValue().toDouble(),
                           true,                                       // autoContinue
                           false,                                      // isCurrentItem
                           missionItemParent);
    items.append(item);

467 468 469 470
}

int StructureScanComplexItem::cameraShots(void) const
{
471
    return _cameraShots;
472 473
}

474
void StructureScanComplexItem::setMissionFlightStatus(const MissionController::MissionFlightStatus_t &missionFlightStatus)
475 476
{
    ComplexMissionItem::setMissionFlightStatus(missionFlightStatus);
477 478
    if (!QGC::fuzzyCompare(_vehicleSpeed, missionFlightStatus.vehicleSpeed)) {
        _vehicleSpeed = missionFlightStatus.vehicleSpeed;
479 480 481 482 483 484 485 486 487 488 489
        emit timeBetweenShotsChanged();
    }
}

void StructureScanComplexItem::_setDirty(void)
{
    setDirty(true);
}

void StructureScanComplexItem::applyNewAltitude(double newAltitude)
{
490
    _entranceAltFact.setRawValue(newAltitude);
491 492 493 494 495 496 497 498 499
}

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

500
double StructureScanComplexItem::timeBetweenShots(void)
501
{
502
    return _vehicleSpeed == 0 ? 0 : _cameraCalc.adjustedFootprintSide()->rawValue().toDouble() / _vehicleSpeed;
503 504 505 506
}

QGeoCoordinate StructureScanComplexItem::coordinate(void) const
{
507
    if (_flightPolygon.count() > 0) {
508 509
        QGeoCoordinate coord = _flightPolygon.vertexCoordinate(_entryVertex);
        return coord;
510 511 512 513 514 515 516 517 518 519 520 521 522 523
    } else {
        return QGeoCoordinate();
    }
}

void StructureScanComplexItem::_updateCoordinateAltitudes(void)
{
    emit coordinateChanged(coordinate());
    emit exitCoordinateChanged(exitCoordinate());
}

void StructureScanComplexItem::rotateEntryPoint(void)
{
    _entryVertex++;
524
    if (_entryVertex >= _flightPolygon.count()) {
525 526 527 528 529
        _entryVertex = 0;
    }
    emit coordinateChanged(coordinate());
    emit exitCoordinateChanged(exitCoordinate());
}
530 531 532

void StructureScanComplexItem::_rebuildFlightPolygon(void)
{
533 534 535 536 537
    // While this is happening all hell breaks loose signal-wise which can cause a bad vertex reference.
    // So we reset to a safe value first and then double check validity when putting it back
    int savedEntryVertex = _entryVertex;
    _entryVertex = 0;

538
    _flightPolygon = _structurePolygon;
539
    _flightPolygon.offset(_cameraCalc.distanceToSurface()->rawValue().toDouble());
540 541 542 543 544 545

    if (savedEntryVertex >= _flightPolygon.count()) {
        _entryVertex = 0;
    } else {
        _entryVertex = savedEntryVertex;
    }
546

547 548
    emit coordinateChanged(coordinate());
    emit exitCoordinateChanged(exitCoordinate());
549
}
550 551 552

void StructureScanComplexItem::_recalcCameraShots(void)
{
553 554 555 556 557 558
    double triggerDistance = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble();
    if (triggerDistance == 0) {
        _setCameraShots(0);
        return;
    }

559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575
    if (_flightPolygon.count() < 3) {
        _setCameraShots(0);
        return;
    }

    // Determine the distance for each polygon traverse
    double distance = 0;
    for (int i=0; i<_flightPolygon.count(); i++) {
        QGeoCoordinate coord1 = _flightPolygon.vertexCoordinate(i);
        QGeoCoordinate coord2 = _flightPolygon.vertexCoordinate(i + 1 == _flightPolygon.count() ? 0 : i + 1);
        distance += coord1.distanceTo(coord2);
    }
    if (distance == 0.0) {
        _setCameraShots(0);
        return;
    }

576
    int cameraShots = static_cast<int>(distance / triggerDistance);
577 578
    _setCameraShots(cameraShots * _layersFact.rawValue().toInt());
}
579

580
void StructureScanComplexItem::_recalcLayerInfo(void)
581
{
582 583 584 585 586 587 588 589 590 591
    double surfaceHeight = qMax(_structureHeightFact.rawValue().toDouble() - _scanBottomAltFact.rawValue().toDouble(), 0.0);

    // Layer count is calculated from surface and layer heights
    _layersFact.setRawValue(qMax(qCeil(surfaceHeight / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()), 1));
}

void StructureScanComplexItem::_updateGimbalPitch(void)
{
    if (!_cameraCalc.isManualCamera()) {
        _gimbalPitchFact.setRawValue(0);
592 593
    }
}
DonLakeFlyer's avatar
DonLakeFlyer committed
594

595
double StructureScanComplexItem::bottomFlightAlt(void) const
DonLakeFlyer's avatar
DonLakeFlyer committed
596
{
597 598 599 600
    if (_startFromTopFact.rawValue().toBool()) {
        // Structure Height minus the topmost layers
        double  layerIncrement = (_cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0) + ((_layersFact.rawValue().toInt() - 1) * _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble());
        return _structureHeightFact.rawValue().toDouble() - layerIncrement;
DonLakeFlyer's avatar
DonLakeFlyer committed
601
    } else {
602 603 604
        // Bottom alt plus half the height of a layer
        double  layerIncrement = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0;
        return _scanBottomAltFact.rawValue().toDouble() + layerIncrement;
DonLakeFlyer's avatar
DonLakeFlyer committed
605 606
    }
}
607

608
double StructureScanComplexItem:: topFlightAlt(void) const
609
{
610 611 612 613 614 615 616 617
    if (_startFromTopFact.rawValue().toBool()) {
        // Structure Height minus half the layer height
        double  layerIncrement = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0;
        return _structureHeightFact.rawValue().toDouble() - layerIncrement;
    } else {
        // Bottom alt plus all layers
        double  layerIncrement = (_cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0) + ((_layersFact.rawValue().toInt() - 1) * _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble());
        return _scanBottomAltFact.rawValue().toDouble() + layerIncrement;
618 619
    }
}
620

621
void StructureScanComplexItem::_signalTopBottomAltChanged(void)
622
{
623 624
    emit topFlightAltChanged();
    emit bottomFlightAltChanged();
625
}
626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 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 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782

void StructureScanComplexItem::_recalcScanDistance()
{
    double scanDistance = 0;

    if (_flightPolygon.count() > 2) {
        QList<QGeoCoordinate> vertices = _flightPolygon.coordinateList();
        for (int i=0; i<vertices.count() - 1; i++) {
            scanDistance += vertices[i].distanceTo(vertices[i+1]);
        }
        scanDistance += vertices.last().distanceTo(vertices.first());

        scanDistance *= _layersFact.rawValue().toInt();

        double surfaceHeight = qMax(_structureHeightFact.rawValue().toDouble() - _scanBottomAltFact.rawValue().toDouble(), 0.0);
        scanDistance += surfaceHeight;

        qCDebug(StructureScanComplexItemLog) << "StructureScanComplexItem--_recalcScanDistance layers: "
                                             << _layersFact.rawValue().toInt() << " structure height: " << surfaceHeight
                                             << " scanDistance: " << _scanDistance;
    }

    if (!QGC::fuzzyCompare(_scanDistance, scanDistance)) {
        _scanDistance = scanDistance;
        emit complexDistanceChanged();
    }

}

StructureScanComplexItem::ReadyForSaveState StructureScanComplexItem::readyForSaveState(void) const
{
    return _structurePolygon.isValid() && !_wizardMode ? ReadyForSave : NotReadyForSaveData;
}

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

double StructureScanComplexItem::amslEntryAlt(void) const
{
    return _entranceAltFact.rawValue().toDouble() + _missionController->plannedHomePosition().altitude();
}

// Never call this method directly. If you want to update the flight segments you emit _updateFlightPathSegmentsSignal()
void StructureScanComplexItem::_updateFlightPathSegmentsDontCallDirectly(void)
{
    // Generation of flight segments depends on the following values:
    //  _flightPolygon,
    //  _startFromTopFact
    //  _structureHeightFact,
    //  _scanBottomAltFact
    //  _cameraCalc.adjustedFootprintFrontal()
    //  _missionController->plannedHomePosition().altitude()
    //  _entranceAltFact
    //  _layersFact
    // Any changes to these values must rebuild the segments

    if (_cTerrainCollisionSegments != 0) {
        _cTerrainCollisionSegments = 0;
        emit terrainCollisionChanged(false);
        _structurePolygon.setShowAltColor(false);
    }

    _flightPathSegments.beginReset();
    _flightPathSegments.clearAndDeleteContents();

    if (_flightPolygon.count() > 2) {

        bool    startFromTop =  _startFromTopFact.rawValue().toBool();
        double  startAltitude = (startFromTop ? _structureHeightFact.rawValue().toDouble() : _scanBottomAltFact.rawValue().toDouble());

        // Set up for the first layer
        double  prevLayerAltitude = 0;
        double  layerAltitude = startAltitude;
        double  halfLayerHeight = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0;
        if (startFromTop) {
            layerAltitude -= halfLayerHeight;
        } else {
            layerAltitude += halfLayerHeight;
        }
        layerAltitude += _missionController->plannedHomePosition().altitude();

        QGeoCoordinate layerEntranceCoord = _flightPolygon.vertexCoordinate(_entryVertex);

        // Entrance to first layer entrance
        double entranceAlt = _entranceAltFact.rawValue().toDouble() + _missionController->plannedHomePosition().altitude();
        _appendFlightPathSegment(layerEntranceCoord, entranceAlt, layerEntranceCoord, layerAltitude);

        // Segments for each layer
        for (int layerIndex=0; layerIndex<_layersFact.rawValue().toInt(); layerIndex++) {
            // Move from one layer to the next
            if (layerIndex != 0) {
                _appendFlightPathSegment(layerEntranceCoord, prevLayerAltitude, layerEntranceCoord, layerAltitude);
            }

            QGeoCoordinate prevCoord = QGeoCoordinate();
            for (const QGeoCoordinate& coord: _flightPolygon.coordinateList()) {
                if (prevCoord.isValid()) {
                    _appendFlightPathSegment(prevCoord, layerAltitude, coord, layerAltitude);
                }
                prevCoord = coord;
            }
            _appendFlightPathSegment(_flightPolygon.coordinateList().last(), layerAltitude, _flightPolygon.coordinateList().first(), layerAltitude);

            // Move to next layer altitude
            prevLayerAltitude = layerAltitude;
            if (startFromTop) {
                layerAltitude -= halfLayerHeight * 2;
            } else {
                layerAltitude += halfLayerHeight * 2;
            }
        }

        // Last layer exit back to entrance
        _appendFlightPathSegment(layerEntranceCoord, prevLayerAltitude, layerEntranceCoord, entranceAlt);
    }

    _flightPathSegments.endReset();

    if (_cTerrainCollisionSegments != 0) {
        emit terrainCollisionChanged(true);
    }

    _masterController->missionController()->recalcTerrainProfile();
}

double StructureScanComplexItem::minAMSLAltitude(void) const
{
    double minAlt = qMin(bottomFlightAlt(), _entranceAltFact.rawValue().toDouble());
    return minAlt + _missionController->plannedHomePosition().altitude();
}

double StructureScanComplexItem::maxAMSLAltitude(void) const
{
    double maxAlt = qMax(topFlightAlt(), _entranceAltFact.rawValue().toDouble());
    return maxAlt + _missionController->plannedHomePosition().altitude();
}

void StructureScanComplexItem::_minAMSLAltChanged(void)
{
    emit minAMSLAltitudeChanged(minAMSLAltitude());
}

void StructureScanComplexItem::_maxAMSLAltChanged(void)
{
    emit maxAMSLAltitudeChanged(maxAMSLAltitude());
}

void StructureScanComplexItem::_segmentTerrainCollisionChanged(bool terrainCollision)
{
    ComplexMissionItem::_segmentTerrainCollisionChanged(terrainCollision);
    _structurePolygon.setShowAltColor(_cTerrainCollisionSegments != 0);
}