FixedWingLandingComplexItem.cc 7.03 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
#include "FlightPathSegment.h"
17 18 19 20 21

#include <QPolygonF>

QGC_LOGGING_CATEGORY(FixedWingLandingComplexItemLog, "FixedWingLandingComplexItemLog")

22 23
const QString FixedWingLandingComplexItem::name(tr("Fixed Wing Landing"));

24 25
const char* FixedWingLandingComplexItem::settingsGroup                      = "FixedWingLanding";
const char* FixedWingLandingComplexItem::jsonComplexItemTypeValue           = "fwLandingPattern";
26

27 28
const char* FixedWingLandingComplexItem::glideSlopeName                     = "GlideSlope";
const char* FixedWingLandingComplexItem::valueSetIsDistanceName             = "ValueSetIsDistance";
29

30
const char* FixedWingLandingComplexItem::_jsonValueSetIsDistanceKey         = "valueSetIsDistance";
31

32
FixedWingLandingComplexItem::FixedWingLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent)
33
    : LandingComplexItem        (masterController, flyView, parent)
34
    , _metaDataMap              (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/FWLandingPattern.FactMetaData.json"), this))
35 36
    , _landingDistanceFact      (settingsGroup, _metaDataMap[finalApproachToLandDistanceName])
    , _finalApproachAltitudeFact(settingsGroup, _metaDataMap[finalApproachAltitudeName])
37
    , _loiterRadiusFact         (settingsGroup, _metaDataMap[loiterRadiusName])
38
    , _loiterClockwiseFact      (settingsGroup, _metaDataMap[loiterClockwiseName])
39 40 41
    , _landingHeadingFact       (settingsGroup, _metaDataMap[landingHeadingName])
    , _landingAltitudeFact      (settingsGroup, _metaDataMap[landingAltitudeName])
    , _glideSlopeFact           (settingsGroup, _metaDataMap[glideSlopeName])
42
    , _useLoiterToAltFact       (settingsGroup, _metaDataMap[useLoiterToAltName])
43 44
    , _stopTakingPhotosFact     (settingsGroup, _metaDataMap[stopTakingPhotosName])
    , _stopTakingVideoFact      (settingsGroup, _metaDataMap[stopTakingVideoName])
45
    , _valueSetIsDistanceFact   (settingsGroup, _metaDataMap[valueSetIsDistanceName])
46
{
47 48
    _editorQml      = "qrc:/qml/FWLandingPatternEditor.qml";
    _isIncomplete   = false;
49

50 51
    _init();

52 53
    connect(&_glideSlopeFact,           &Fact::valueChanged, this, &FixedWingLandingComplexItem::_glideSlopeChanged);
    connect(&_valueSetIsDistanceFact,   &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty);
54

55 56 57 58 59 60
    if (_valueSetIsDistanceFact.rawValue().toBool()) {
        _recalcFromHeadingAndDistanceChange();
    } else {
        _glideSlopeChanged();
    }
    setDirty(false);
61 62
}

63
void FixedWingLandingComplexItem::save(QJsonArray&  missionItems)
64
{
65
    QJsonObject saveObject = _save();
66

67 68 69 70
    saveObject[JsonHelper::jsonVersionKey]                  = 2;
    saveObject[VisualMissionItem::jsonTypeKey]              = VisualMissionItem::jsonTypeComplexItemValue;
    saveObject[ComplexMissionItem::jsonComplexItemTypeKey]  = jsonComplexItemTypeValue;
    saveObject[_jsonValueSetIsDistanceKey]                  = _valueSetIsDistanceFact.rawValue().toBool();
71 72

    missionItems.append(saveObject);
73 74 75 76
}

bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
{
77
    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
78
        { JsonHelper::jsonVersionKey, QJsonValue::Double, true },
79 80 81 82
    };
    if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
        return false;
    }
83

84 85
    int version = complexObject[JsonHelper::jsonVersionKey].toInt();
    if (version == 1) {
86
        _valueSetIsDistanceFact.setRawValue(true);
87 88
    } else if (version == 2) {
        QList<JsonHelper::KeyValidateInfo> v2KeyInfoList = {
DonLakeFlyer's avatar
DonLakeFlyer committed
89
            { _jsonValueSetIsDistanceKey,   QJsonValue::Bool,  true },
90 91
        };
        if (!JsonHelper::validateKeys(complexObject, v2KeyInfoList, errorString)) {
DonLakeFlyer's avatar
DonLakeFlyer committed
92
            _ignoreRecalcSignals = false;
93 94
            return false;
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
95

96
        _valueSetIsDistanceFact.setRawValue(complexObject[_jsonValueSetIsDistanceKey].toBool());
97 98 99 100 101 102
    } else {
        errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version);
        _ignoreRecalcSignals = false;
        return false;
    }

103
    return _load(complexObject, sequenceNumber, jsonComplexItemTypeValue, version == 1 /* useDeprecatedRelAltKeys */, errorString);
104
}
105

106
MissionItem* FixedWingLandingComplexItem::_createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent)
107 108
{
    return new MissionItem(seqNum,
109
                           MAV_CMD_NAV_LAND,
110 111 112
                           altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
                           0.0, 0.0, 0.0, 0.0,
                           lat, lon, alt,
113 114
                           true,                               // autoContinue
                           false,                              // isCurrentItem
115 116 117 118
                           parent);

}

119 120 121
void FixedWingLandingComplexItem::_glideSlopeChanged(void)
{
    if (!_ignoreRecalcSignals) {
122
        double landingAltDifference = _finalApproachAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble();
123 124 125 126 127 128 129
        double glideSlope = _glideSlopeFact.rawValue().toDouble();
        _landingDistanceFact.setRawValue(landingAltDifference / qTan(qDegreesToRadians(glideSlope)));
    }
}

void FixedWingLandingComplexItem::_calcGlideSlope(void)
{
130
    double landingAltDifference = _finalApproachAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble();
131 132 133 134
    double landingDistance = _landingDistanceFact.rawValue().toDouble();

    _glideSlopeFact.setRawValue(qRadiansToDegrees(qAtan(landingAltDifference / landingDistance)));
}
135

136
void FixedWingLandingComplexItem::moveLandingPosition(const QGeoCoordinate& coordinate)
137
{
138 139 140 141 142 143
    double savedHeading = landingHeading()->rawValue().toDouble();
    double savedDistance = landingDistance()->rawValue().toDouble();

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

146
bool FixedWingLandingComplexItem::_isValidLandItem(const MissionItem& missionItem)
147
{
148 149 150 151 152 153 154
    if (missionItem.command() != MAV_CMD_NAV_LAND ||
            !(missionItem.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItem.frame() == MAV_FRAME_GLOBAL) ||
            missionItem.param1() != 0 || missionItem.param2() != 0 || missionItem.param3() != 0 || missionItem.param4() != 0) {
        return false;
    } else {
        return true;
    }
155 156
}

157
bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController)
158
{
159
    return _scanForItem(visualItems, flyView, masterController, _isValidLandItem, _createItem);
160
}