FixedWingLandingComplexItem.cc 33.4 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9 10 11 12 13
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "FixedWingLandingComplexItem.h"
#include "JsonHelper.h"
#include "MissionController.h"
#include "QGCGeo.h"
14
#include "SimpleMissionItem.h"
15
#include "PlanMasterController.h"
16 17 18 19 20

#include <QPolygonF>

QGC_LOGGING_CATEGORY(FixedWingLandingComplexItemLog, "FixedWingLandingComplexItemLog")

21
const char* FixedWingLandingComplexItem::settingsGroup =            "FixedWingLanding";
22 23
const char* FixedWingLandingComplexItem::jsonComplexItemTypeValue = "fwLandingPattern";

24 25 26 27 28
const char* FixedWingLandingComplexItem::loiterToLandDistanceName = "LandingDistance";
const char* FixedWingLandingComplexItem::landingHeadingName =       "LandingHeading";
const char* FixedWingLandingComplexItem::loiterAltitudeName =       "LoiterAltitude";
const char* FixedWingLandingComplexItem::loiterRadiusName =         "LoiterRadius";
const char* FixedWingLandingComplexItem::landingAltitudeName =      "LandingAltitude";
29
const char* FixedWingLandingComplexItem::glideSlopeName =           "GlideSlope";
30 31
const char* FixedWingLandingComplexItem::stopTakingPhotosName =     "StopTakingPhotos";
const char* FixedWingLandingComplexItem::stopTakingVideoName =      "StopTakingVideo";
32
const char* FixedWingLandingComplexItem::valueSetIsDistanceName =   "ValueSetIsDistance";
33

34 35 36 37
const char* FixedWingLandingComplexItem::_jsonLoiterCoordinateKey =         "loiterCoordinate";
const char* FixedWingLandingComplexItem::_jsonLoiterRadiusKey =             "loiterRadius";
const char* FixedWingLandingComplexItem::_jsonLoiterClockwiseKey =          "loiterClockwise";
const char* FixedWingLandingComplexItem::_jsonLandingCoordinateKey =        "landCoordinate";
38 39
const char* FixedWingLandingComplexItem::_jsonValueSetIsDistanceKey =       "valueSetIsDistance";
const char* FixedWingLandingComplexItem::_jsonAltitudesAreRelativeKey =     "altitudesAreRelative";
40 41
const char* FixedWingLandingComplexItem::_jsonStopTakingPhotosKey =         "stopTakingPhotos";
const char* FixedWingLandingComplexItem::_jsonStopTakingVideoKey =          "stopVideoPhotos";
42 43

// Usage deprecated
44
const char* FixedWingLandingComplexItem::_jsonFallRateKey =                 "fallRate";
45 46
const char* FixedWingLandingComplexItem::_jsonLandingAltitudeRelativeKey =  "landAltitudeRelative";
const char* FixedWingLandingComplexItem::_jsonLoiterAltitudeRelativeKey =   "loiterAltitudeRelative";
47

48 49
FixedWingLandingComplexItem::FixedWingLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent)
    : ComplexMissionItem        (masterController, flyView, parent)
50 51 52 53 54
    , _sequenceNumber           (0)
    , _dirty                    (false)
    , _landingCoordSet          (false)
    , _ignoreRecalcSignals      (false)
    , _metaDataMap              (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/FWLandingPattern.FactMetaData.json"), this))
55 56 57 58 59 60
    , _landingDistanceFact      (settingsGroup, _metaDataMap[loiterToLandDistanceName])
    , _loiterAltitudeFact       (settingsGroup, _metaDataMap[loiterAltitudeName])
    , _loiterRadiusFact         (settingsGroup, _metaDataMap[loiterRadiusName])
    , _landingHeadingFact       (settingsGroup, _metaDataMap[landingHeadingName])
    , _landingAltitudeFact      (settingsGroup, _metaDataMap[landingAltitudeName])
    , _glideSlopeFact           (settingsGroup, _metaDataMap[glideSlopeName])
61 62
    , _stopTakingPhotosFact     (settingsGroup, _metaDataMap[stopTakingPhotosName])
    , _stopTakingVideoFact      (settingsGroup, _metaDataMap[stopTakingVideoName])
63
    , _valueSetIsDistanceFact   (settingsGroup, _metaDataMap[valueSetIsDistanceName])
64
    , _loiterClockwise          (true)
65
    , _altitudesAreRelative     (true)
66 67
{
    _editorQml = "qrc:/qml/FWLandingPatternEditor.qml";
68
    _isIncomplete = false;
69

70
    connect(&_loiterAltitudeFact,       &Fact::valueChanged,                                    this, &FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact);
71 72 73 74 75 76 77 78 79 80
    connect(&_landingAltitudeFact,      &Fact::valueChanged,                                    this, &FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact);

    connect(&_landingDistanceFact,      &Fact::valueChanged,                                    this, &FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange);
    connect(&_landingHeadingFact,       &Fact::valueChanged,                                    this, &FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange);

    connect(&_loiterRadiusFact,         &Fact::valueChanged,                                    this, &FixedWingLandingComplexItem::_recalcFromRadiusChange);
    connect(this,                       &FixedWingLandingComplexItem::loiterClockwiseChanged,   this, &FixedWingLandingComplexItem::_recalcFromRadiusChange);

    connect(this,                       &FixedWingLandingComplexItem::loiterCoordinateChanged,  this, &FixedWingLandingComplexItem::_recalcFromCoordinateChange);
    connect(this,                       &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFromCoordinateChange);
81

82 83
    connect(&_glideSlopeFact,           &Fact::valueChanged,                                    this, &FixedWingLandingComplexItem::_glideSlopeChanged);

84 85 86
    connect(&_stopTakingPhotosFact,     &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_signalLastSequenceNumberChanged);
    connect(&_stopTakingVideoFact,      &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_signalLastSequenceNumberChanged);

87 88 89 90 91
    connect(&_loiterAltitudeFact,       &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
    connect(&_landingAltitudeFact,      &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
    connect(&_landingDistanceFact,      &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
    connect(&_landingHeadingFact,       &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
    connect(&_loiterRadiusFact,         &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
92 93
    connect(&_stopTakingPhotosFact,     &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
    connect(&_stopTakingVideoFact,      &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
94
    connect(&_valueSetIsDistanceFact,   &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
95 96 97
    connect(this,                       &FixedWingLandingComplexItem::loiterCoordinateChanged,          this, &FixedWingLandingComplexItem::_setDirty);
    connect(this,                       &FixedWingLandingComplexItem::landingCoordinateChanged,         this, &FixedWingLandingComplexItem::_setDirty);
    connect(this,                       &FixedWingLandingComplexItem::loiterClockwiseChanged,           this, &FixedWingLandingComplexItem::_setDirty);
98
    connect(this,                       &FixedWingLandingComplexItem::altitudesAreRelativeChanged,      this, &FixedWingLandingComplexItem::_setDirty);
99
    connect(this,                       &FixedWingLandingComplexItem::valueSetIsDistanceChanged,        this, &FixedWingLandingComplexItem::_setDirty);
100

101 102
    connect(this,                       &FixedWingLandingComplexItem::altitudesAreRelativeChanged,      this, &FixedWingLandingComplexItem::coordinateHasRelativeAltitudeChanged);
    connect(this,                       &FixedWingLandingComplexItem::altitudesAreRelativeChanged,      this, &FixedWingLandingComplexItem::exitCoordinateHasRelativeAltitudeChanged);
103

DonLakeFlyer's avatar
DonLakeFlyer committed
104
    connect(this,                       &FixedWingLandingComplexItem::landingCoordSetChanged,           this, &FixedWingLandingComplexItem::readyForSaveStateChanged);
105 106
    connect(this,                       &FixedWingLandingComplexItem::wizardModeChanged,                this, &FixedWingLandingComplexItem::readyForSaveStateChanged);

107
    if (_masterController->controllerVehicle()->apmFirmware()) {
108 109 110 111 112
        // ArduPilot does not support camera commands
        _stopTakingVideoFact.setRawValue(false);
        _stopTakingPhotosFact.setRawValue(false);
    }

113 114 115 116 117 118
    if (_valueSetIsDistanceFact.rawValue().toBool()) {
        _recalcFromHeadingAndDistanceChange();
    } else {
        _glideSlopeChanged();
    }
    setDirty(false);
119 120 121 122
}

int FixedWingLandingComplexItem::lastSequenceNumber(void) const
{
123 124 125 126 127
    // Fixed items are:
    //  land start, loiter, land
    // Optional items are:
    //  stop photos/video
    return _sequenceNumber + 2 + (_stopTakingPhotosFact.rawValue().toBool() ? 2 : 0) + (_stopTakingVideoFact.rawValue().toBool() ? 1 : 0);
128 129 130 131 132 133 134 135 136 137
}

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

138
void FixedWingLandingComplexItem::save(QJsonArray&  missionItems)
139
{
140 141
    QJsonObject saveObject;

142
    saveObject[JsonHelper::jsonVersionKey] =                    2;
143 144 145
    saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
    saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;

146 147 148 149 150 151 152 153 154 155 156 157 158
    QGeoCoordinate coordinate;
    QJsonValue jsonCoordinate;

    coordinate = _loiterCoordinate;
    coordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
    JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate);
    saveObject[_jsonLoiterCoordinateKey] = jsonCoordinate;

    coordinate = _landingCoordinate;
    coordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble());
    JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate);
    saveObject[_jsonLandingCoordinateKey] = jsonCoordinate;

159
    saveObject[_jsonLoiterRadiusKey] =          _loiterRadiusFact.rawValue().toDouble();
160 161
    saveObject[_jsonStopTakingPhotosKey] =      _stopTakingPhotosFact.rawValue().toBool();
    saveObject[_jsonStopTakingVideoKey] =       _stopTakingVideoFact.rawValue().toBool();
162 163
    saveObject[_jsonLoiterClockwiseKey] =       _loiterClockwise;
    saveObject[_jsonAltitudesAreRelativeKey] =  _altitudesAreRelative;
164
    saveObject[_jsonValueSetIsDistanceKey] =    _valueSetIsDistanceFact.rawValue().toBool();
165 166

    missionItems.append(saveObject);
167 168 169 170 171 172 173 174 175 176 177 178 179
}

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

bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
{
180 181 182 183 184 185 186 187
    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
        { JsonHelper::jsonVersionKey,                   QJsonValue::Double, true },
        { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
        { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
        { _jsonLoiterCoordinateKey,                     QJsonValue::Array,  true },
        { _jsonLoiterRadiusKey,                         QJsonValue::Double, true },
        { _jsonLoiterClockwiseKey,                      QJsonValue::Bool,   true },
        { _jsonLandingCoordinateKey,                    QJsonValue::Array,  true },
188 189
        { _jsonStopTakingPhotosKey,                     QJsonValue::Bool,   false },
        { _jsonStopTakingVideoKey,                      QJsonValue::Bool,   false },
190 191 192 193
    };
    if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
        return false;
    }
194

195 196 197
    QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
    QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
    if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) {
198
        errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
199 200
        return false;
    }
201

202
    setSequenceNumber(sequenceNumber);
203

204 205
    _ignoreRecalcSignals = true;

206 207
    int version = complexObject[JsonHelper::jsonVersionKey].toInt();
    if (version == 1) {
DonLakeFlyer's avatar
DonLakeFlyer committed
208 209 210 211 212 213 214 215
        QList<JsonHelper::KeyValidateInfo> v1KeyInfoList = {
            { _jsonLoiterAltitudeRelativeKey,   QJsonValue::Bool,  true },
            { _jsonLandingAltitudeRelativeKey,  QJsonValue::Bool,  true },
        };
        if (!JsonHelper::validateKeys(complexObject, v1KeyInfoList, errorString)) {
            return false;
        }

216 217 218
        bool loiterAltitudeRelative = complexObject[_jsonLoiterAltitudeRelativeKey].toBool();
        bool landingAltitudeRelative = complexObject[_jsonLandingAltitudeRelativeKey].toBool();
        if (loiterAltitudeRelative != landingAltitudeRelative) {
219
            qgcApp()->showAppMessage(tr("Fixed Wing Landing Pattern: "
220 221 222 223 224 225
                                     "Setting the loiter and landing altitudes with different settings for altitude relative is no longer supported. "
                                     "Both have been set to altitude relative. Be sure to adjust/check your plan prior to flight."));
            _altitudesAreRelative = true;
        } else {
            _altitudesAreRelative = loiterAltitudeRelative;
        }
226
        _valueSetIsDistanceFact.setRawValue(true);
227 228 229
    } else if (version == 2) {
        QList<JsonHelper::KeyValidateInfo> v2KeyInfoList = {
            { _jsonAltitudesAreRelativeKey, QJsonValue::Bool,  true },
DonLakeFlyer's avatar
DonLakeFlyer committed
230
            { _jsonValueSetIsDistanceKey,   QJsonValue::Bool,  true },
231 232
        };
        if (!JsonHelper::validateKeys(complexObject, v2KeyInfoList, errorString)) {
DonLakeFlyer's avatar
DonLakeFlyer committed
233
            _ignoreRecalcSignals = false;
234 235
            return false;
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
236 237

        _altitudesAreRelative = complexObject[_jsonAltitudesAreRelativeKey].toBool();
238
        _valueSetIsDistanceFact.setRawValue(complexObject[_jsonValueSetIsDistanceKey].toBool());
239 240 241 242 243 244
    } else {
        errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version);
        _ignoreRecalcSignals = false;
        return false;
    }

245 246 247 248 249 250
    QGeoCoordinate coordinate;
    if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLoiterCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
        return false;
    }
    _loiterCoordinate = coordinate;
    _loiterAltitudeFact.setRawValue(coordinate.altitude());
251

252 253
    if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLandingCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
        return false;
254
    }
255 256 257 258
    _landingCoordinate = coordinate;
    _landingAltitudeFact.setRawValue(coordinate.altitude());

    _loiterRadiusFact.setRawValue(complexObject[_jsonLoiterRadiusKey].toDouble());
259
    _loiterClockwise = complexObject[_jsonLoiterClockwiseKey].toBool();
DonLakeFlyer's avatar
DonLakeFlyer committed
260

261 262 263 264 265 266 267 268 269 270 271
    if (complexObject.contains(_jsonStopTakingPhotosKey)) {
        _stopTakingPhotosFact.setRawValue(complexObject[_jsonStopTakingPhotosKey].toBool());
    } else {
        _stopTakingPhotosFact.setRawValue(false);
    }
    if (complexObject.contains(_jsonStopTakingVideoKey)) {
        _stopTakingVideoFact.setRawValue(complexObject[_jsonStopTakingVideoKey].toBool());
    } else {
        _stopTakingVideoFact.setRawValue(false);
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
272
    _calcGlideSlope();
273

274
    _landingCoordSet = true;
275 276

    _ignoreRecalcSignals = false;
277

278
    _recalcFromCoordinateChange();
279
    emit coordinateChanged(this->coordinate());    // This will kick off terrain query
280 281 282 283 284 285 286

    return true;
}

double FixedWingLandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
    return qMax(_loiterCoordinate.distanceTo(other),_landingCoordinate.distanceTo(other));
287 288 289 290 291 292 293
}

bool FixedWingLandingComplexItem::specifiesCoordinate(void) const
{
    return true;
}

294
MissionItem* FixedWingLandingComplexItem::createDoLandStartItem(int seqNum, QObject* parent)
295
{
296 297 298 299 300 301 302 303
    return new MissionItem(seqNum,                              // sequence number
                           MAV_CMD_DO_LAND_START,               // MAV_CMD
                           MAV_FRAME_MISSION,                   // MAV_FRAME
                           0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,   // param 1-7
                           true,                                // autoContinue
                           false,                               // isCurrentItem
                           parent);
}
304

305 306 307
MissionItem* FixedWingLandingComplexItem::createLoiterToAltItem(int seqNum, bool altRel, double loiterRadius, double lat, double lon, double alt, QObject* parent)
{
    return new MissionItem(seqNum,
308
                           MAV_CMD_NAV_LOITER_TO_ALT,
309
                           altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
310 311 312
                           1.0,                             // Heading required = true
                           loiterRadius,                    // Loiter radius
                           0.0,                             // param 3 - unused
Don Gagne's avatar
Don Gagne committed
313
                           1.0,                             // Exit crosstrack - tangent of loiter to land point
314
                           lat, lon, alt,
315 316
                           true,                            // autoContinue
                           false,                           // isCurrentItem
317 318
                           parent);
}
319

320 321 322
MissionItem* FixedWingLandingComplexItem::createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent)
{
    return new MissionItem(seqNum,
323
                           MAV_CMD_NAV_LAND,
324 325 326
                           altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
                           0.0, 0.0, 0.0, 0.0,
                           lat, lon, alt,
327 328
                           true,                               // autoContinue
                           false,                              // isCurrentItem
329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362
                           parent);

}

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

    // IMPORTANT NOTE: Any changes here must also be taken into account in scanForItem

    MissionItem* item = createDoLandStartItem(seqNum++, missionItemParent);
    items.append(item);


    if (_stopTakingPhotosFact.rawValue().toBool()) {
        CameraSection::appendStopTakingPhotos(items, seqNum, missionItemParent);
    }

    if (_stopTakingVideoFact.rawValue().toBool()) {
        CameraSection::appendStopTakingVideo(items, seqNum, missionItemParent);
    }

    double loiterRadius = _loiterRadiusFact.rawValue().toDouble() * (_loiterClockwise ? 1.0 : -1.0);
    item = createLoiterToAltItem(seqNum++,
                                 _altitudesAreRelative,
                                 loiterRadius,
                                 _loiterCoordinate.latitude(), _loiterCoordinate.longitude(), _loiterAltitudeFact.rawValue().toDouble(),
                                 missionItemParent);
    items.append(item);

    item = createLandItem(seqNum++,
                          _altitudesAreRelative,
                          _landingCoordinate.latitude(), _landingCoordinate.longitude(), _landingAltitudeFact.rawValue().toDouble(),
                          missionItemParent);
363
    items.append(item);
364 365
}

366
bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController)
367 368 369
{
    qCDebug(FixedWingLandingComplexItemLog) << "FixedWingLandingComplexItem::scanForItem count" << visualItems->count();

370
    if (visualItems->count() < 3) {
371 372 373
        return false;
    }

374 375 376 377 378 379
    // A valid fixed wing landing pattern is comprised of the follow commands in this order at the end of the item list:
    //  MAV_CMD_DO_LAND_START - required
    //  Stop taking photos sequence - optional
    //  Stop taking video sequence - optional
    //  MAV_CMD_NAV_LOITER_TO_ALT - required
    //  MAV_CMD_NAV_LAND - required
380

381 382 383 384 385 386 387 388
    // Start looking for the commands in reverse order from the end of the list

    int scanIndex = visualItems->count() - 1;

    if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
        return false;
    }
    SimpleMissionItem* item = visualItems->value<SimpleMissionItem*>(scanIndex--);
389 390 391 392 393 394
    if (!item) {
        return false;
    }
    MissionItem& missionItemLand = item->missionItem();
    if (missionItemLand.command() != MAV_CMD_NAV_LAND ||
            !(missionItemLand.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItemLand.frame() == MAV_FRAME_GLOBAL) ||
Don Gagne's avatar
Don Gagne committed
395
            missionItemLand.param1() != 0 || missionItemLand.param2() != 0 || missionItemLand.param3() != 0 || missionItemLand.param4() != 0) {
396 397
        return false;
    }
398
    MAV_FRAME landPointFrame = missionItemLand.frame();
399

400 401 402 403
    if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
        return false;
    }
    item = visualItems->value<SimpleMissionItem*>(scanIndex);
404 405 406 407 408
    if (!item) {
        return false;
    }
    MissionItem& missionItemLoiter = item->missionItem();
    if (missionItemLoiter.command() != MAV_CMD_NAV_LOITER_TO_ALT ||
409
            missionItemLoiter.frame() != landPointFrame ||
410 411 412 413
            missionItemLoiter.param1() != 1.0 || missionItemLoiter.param3() != 0 || missionItemLoiter.param4() != 1.0) {
        return false;
    }

414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430
    scanIndex -= CameraSection::stopTakingVideoCommandCount();
    bool stopTakingVideo = CameraSection::scanStopTakingVideo(visualItems, scanIndex, false /* removeScannedItems */);
    if (!stopTakingVideo) {
        scanIndex += CameraSection::stopTakingVideoCommandCount();
    }

    scanIndex -= CameraSection::stopTakingPhotosCommandCount();
    bool stopTakingPhotos = CameraSection::scanStopTakingPhotos(visualItems, scanIndex, false /* removeScannedItems */);
    if (!stopTakingPhotos) {
        scanIndex += CameraSection::stopTakingPhotosCommandCount();
    }

    scanIndex--;
    if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
        return false;
    }
    item = visualItems->value<SimpleMissionItem*>(scanIndex);
431 432 433 434 435
    if (!item) {
        return false;
    }
    MissionItem& missionItemDoLandStart = item->missionItem();
    if (missionItemDoLandStart.command() != MAV_CMD_DO_LAND_START ||
Don Gagne's avatar
Don Gagne committed
436
            missionItemDoLandStart.frame() != MAV_FRAME_MISSION ||
437
            missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0|| missionItemDoLandStart.param5() != 0|| missionItemDoLandStart.param6() != 0) {
438 439 440
        return false;
    }

441 442 443 444 445 446 447 448 449 450 451 452 453 454 455
    // We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission.
    // Since we have scanned it we need to remove the items for it fromt the list
    int deleteCount = 3;
    if (stopTakingPhotos) {
        deleteCount += CameraSection::stopTakingPhotosCommandCount();
    }
    if (stopTakingVideo) {
        deleteCount += CameraSection::stopTakingVideoCommandCount();
    }
    int firstItem = visualItems->count() - deleteCount;
    while (deleteCount--) {
        visualItems->removeAt(firstItem)->deleteLater();
    }

    // Now stuff all the scanned information into the item
456

457
    FixedWingLandingComplexItem* complexItem = new FixedWingLandingComplexItem(masterController, flyView, visualItems);
458 459 460

    complexItem->_ignoreRecalcSignals = true;

461
    complexItem->_altitudesAreRelative = landPointFrame == MAV_FRAME_GLOBAL_RELATIVE_ALT;
462 463
    complexItem->_loiterRadiusFact.setRawValue(qAbs(missionItemLoiter.param2()));
    complexItem->_loiterClockwise = missionItemLoiter.param2() > 0;
464
    complexItem->setLoiterCoordinate(QGeoCoordinate(missionItemLoiter.param5(), missionItemLoiter.param6()));
465 466 467 468 469 470
    complexItem->_loiterAltitudeFact.setRawValue(missionItemLoiter.param7());

    complexItem->_landingCoordinate.setLatitude(missionItemLand.param5());
    complexItem->_landingCoordinate.setLongitude(missionItemLand.param6());
    complexItem->_landingAltitudeFact.setRawValue(missionItemLand.param7());

471 472 473
    complexItem->_stopTakingPhotosFact.setRawValue(stopTakingPhotos);
    complexItem->_stopTakingVideoFact.setRawValue(stopTakingVideo);

DonLakeFlyer's avatar
DonLakeFlyer committed
474 475
    complexItem->_calcGlideSlope();

476 477 478 479 480 481 482 483 484 485 486
    complexItem->_landingCoordSet = true;

    complexItem->_ignoreRecalcSignals = false;
    complexItem->_recalcFromCoordinateChange();
    complexItem->setDirty(false);

    visualItems->append(complexItem);

    return true;
}

487 488
double FixedWingLandingComplexItem::complexDistance(void) const
{
489
    return _loiterCoordinate.distanceTo(_landingCoordinate);
490 491
}

492
void FixedWingLandingComplexItem::setLandingCoordinate(const QGeoCoordinate& coordinate)
493
{
494 495 496 497 498 499 500 501 502 503 504
    if (coordinate != _landingCoordinate) {
        _landingCoordinate = coordinate;
        if (_landingCoordSet) {
            emit exitCoordinateChanged(coordinate);
            emit landingCoordinateChanged(coordinate);
        } else {
            _ignoreRecalcSignals = true;
            emit exitCoordinateChanged(coordinate);
            emit landingCoordinateChanged(coordinate);
            _ignoreRecalcSignals = false;
            _landingCoordSet = true;
505
            _recalcFromHeadingAndDistanceChange();
506 507 508 509
            emit landingCoordSetChanged(true);
        }
    }
}
510

511 512 513 514 515 516 517 518
void FixedWingLandingComplexItem::setLoiterCoordinate(const QGeoCoordinate& coordinate)
{
    if (coordinate != _loiterCoordinate) {
        _loiterCoordinate = coordinate;
        emit coordinateChanged(coordinate);
        emit loiterCoordinateChanged(coordinate);
    }
}
519

520
double FixedWingLandingComplexItem::_mathematicAngleToHeading(double angle)
521
{
522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551
    double heading = (angle - 90) * -1;
    if (heading < 0) {
        heading += 360;
    }

    return heading;
}

double FixedWingLandingComplexItem::_headingToMathematicAngle(double heading)
{
    return heading - 90 * -1;
}

void FixedWingLandingComplexItem::_recalcFromRadiusChange(void)
{
    // Fixed:
    //      land
    //      loiter tangent
    //      distance
    //      radius
    //      heading
    // Adjusted:
    //      loiter

    if (!_ignoreRecalcSignals) {
        // These are our known values
        double radius  = _loiterRadiusFact.rawValue().toDouble();
        double landToTangentDistance = _landingDistanceFact.rawValue().toDouble();
        double heading = _landingHeadingFact.rawValue().toDouble();

552 553 554 555
        double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
        if (landToLoiterDistance < radius) {
            // Degnenerate case: Move tangent to loiter point
            _loiterTangentCoordinate = _loiterCoordinate;
556

557
            double heading = _landingCoordinate.azimuthTo(_loiterTangentCoordinate);
558

559 560 561 562 563 564 565 566 567
            _ignoreRecalcSignals = true;
            _landingHeadingFact.setRawValue(heading);
            emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
            _ignoreRecalcSignals = false;
        } else {
            double landToLoiterDistance = qSqrt(qPow(radius, 2) + qPow(landToTangentDistance, 2));
            double angleLoiterToTangent = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? -1 : 1);

            _loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToLoiterDistance, heading + 180 + angleLoiterToTangent);
568
            _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
569 570 571

            _ignoreRecalcSignals = true;
            emit loiterCoordinateChanged(_loiterCoordinate);
572
            emit coordinateChanged(_loiterCoordinate);
573 574
            _ignoreRecalcSignals = false;
        }
575 576
    }
}
577

578 579 580 581 582 583 584 585 586 587
void FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange(void)
{
    // Fixed:
    //      land
    //      heading
    //      distance
    //      radius
    // Adjusted:
    //      loiter
    //      loiter tangent
588
    //      glide slope
589

590 591 592 593
    if (!_ignoreRecalcSignals && _landingCoordSet) {
        // These are our known values
        double radius = _loiterRadiusFact.rawValue().toDouble();
        double landToTangentDistance = _landingDistanceFact.rawValue().toDouble();
594 595
        double heading = _landingHeadingFact.rawValue().toDouble();

596 597 598 599 600 601 602 603
        // Calculate loiter tangent coordinate
        _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, heading + 180);

        // Calculate the distance and angle to the loiter coordinate
        QGeoCoordinate tangent = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, 0);
        QGeoCoordinate loiter = tangent.atDistanceAndAzimuth(radius, 90);
        double loiterDistance = _landingCoordinate.distanceTo(loiter);
        double loiterAzimuth = _landingCoordinate.azimuthTo(loiter) * (_loiterClockwise ? -1 : 1);
604

605 606
        // Use those values to get the new loiter point which takes heading into acount
        _loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(loiterDistance, heading + 180 + loiterAzimuth);
607
        _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
608 609

        _ignoreRecalcSignals = true;
610
        emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
611
        emit loiterCoordinateChanged(_loiterCoordinate);
612
        emit coordinateChanged(_loiterCoordinate);
613
        _calcGlideSlope();
614 615
        _ignoreRecalcSignals = false;
    }
616 617 618 619 620 621 622 623 624 625 626 627
}

QPointF FixedWingLandingComplexItem::_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;
}
628

629
void FixedWingLandingComplexItem::_recalcFromCoordinateChange(void)
630
{
631 632 633 634 635 636 637 638
    // Fixed:
    //      land
    //      loiter
    //      radius
    // Adjusted:
    //      loiter tangent
    //      heading
    //      distance
639
    //      glide slope
640

641 642 643 644 645
    if (!_ignoreRecalcSignals && _landingCoordSet) {
        // These are our known values
        double radius = _loiterRadiusFact.rawValue().toDouble();
        double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
        double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate);
646

647 648 649 650 651 652 653 654
        double landToTangentDistance;
        if (landToLoiterDistance < radius) {
            // Degenerate case, set tangent to loiter coordinate
            _loiterTangentCoordinate = _loiterCoordinate;
            landToTangentDistance = _landingCoordinate.distanceTo(_loiterTangentCoordinate);
        } else {
            double loiterToTangentAngle = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? 1 : -1);
            landToTangentDistance = qSqrt(qPow(landToLoiterDistance, 2) - qPow(radius, 2));
655

656 657 658
            _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, landToLoiterHeading + loiterToTangentAngle);

        }
659

660
        double heading = _loiterTangentCoordinate.azimuthTo(_landingCoordinate);
661

662 663 664 665
        _ignoreRecalcSignals = true;
        _landingHeadingFact.setRawValue(heading);
        _landingDistanceFact.setRawValue(landToTangentDistance);
        emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
666
        _calcGlideSlope();
667 668 669
        _ignoreRecalcSignals = false;
    }
}
670 671 672 673

void FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact(void)
{
    _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
674 675
    emit loiterCoordinateChanged(_loiterCoordinate);
    emit coordinateChanged(_loiterCoordinate);
676 677 678 679 680
}

void FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void)
{
    _landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble());
681
    emit landingCoordinateChanged(_landingCoordinate);
682
}
683 684 685 686 687

void FixedWingLandingComplexItem::_setDirty(void)
{
    setDirty(true);
}
688

DonLakeFlyer's avatar
DonLakeFlyer committed
689 690 691 692
void FixedWingLandingComplexItem::applyNewAltitude(double newAltitude)
{
    _loiterAltitudeFact.setRawValue(newAltitude);
}
693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709

void FixedWingLandingComplexItem::_glideSlopeChanged(void)
{
    if (!_ignoreRecalcSignals) {
        double landingAltDifference = _loiterAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble();
        double glideSlope = _glideSlopeFact.rawValue().toDouble();
        _landingDistanceFact.setRawValue(landingAltDifference / qTan(qDegreesToRadians(glideSlope)));
    }
}

void FixedWingLandingComplexItem::_calcGlideSlope(void)
{
    double landingAltDifference = _loiterAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble();
    double landingDistance = _landingDistanceFact.rawValue().toDouble();

    _glideSlopeFact.setRawValue(qRadiansToDegrees(qAtan(landingAltDifference / landingDistance)));
}
710 711 712 713 714

void FixedWingLandingComplexItem::_signalLastSequenceNumberChanged(void)
{
    emit lastSequenceNumberChanged(lastSequenceNumber());
}
DonLakeFlyer's avatar
DonLakeFlyer committed
715 716 717

FixedWingLandingComplexItem::ReadyForSaveState FixedWingLandingComplexItem::readyForSaveState(void) const
{
718 719 720
    return _landingCoordSet && !_wizardMode ? ReadyForSave : NotReadyForSaveData;
}

721
void FixedWingLandingComplexItem::moveLandingPosition(const QGeoCoordinate& coordinate)
722
{
723 724 725 726 727 728
    double savedHeading = landingHeading()->rawValue().toDouble();
    double savedDistance = landingDistance()->rawValue().toDouble();

    setLandingCoordinate(coordinate);
    landingHeading()->setRawValue(savedHeading);
    landingDistance()->setRawValue(savedDistance);
DonLakeFlyer's avatar
DonLakeFlyer committed
729
}