FixedWingLandingComplexItem.cc 26.5 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 "FixedWingLandingComplexItem.h"
#include "JsonHelper.h"
#include "MissionController.h"
#include "QGCGeo.h"
#include "QGroundControlQmlGlobal.h"
15
#include "SimpleMissionItem.h"
16 17 18 19 20 21 22

#include <QPolygonF>

QGC_LOGGING_CATEGORY(FixedWingLandingComplexItemLog, "FixedWingLandingComplexItemLog")

const char* FixedWingLandingComplexItem::jsonComplexItemTypeValue = "fwLandingPattern";

23 24 25 26 27
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";
28
const char* FixedWingLandingComplexItem::glideSlopeName =           "GlideSlope";
29

30 31 32 33
const char* FixedWingLandingComplexItem::_jsonLoiterCoordinateKey =         "loiterCoordinate";
const char* FixedWingLandingComplexItem::_jsonLoiterRadiusKey =             "loiterRadius";
const char* FixedWingLandingComplexItem::_jsonLoiterClockwiseKey =          "loiterClockwise";
const char* FixedWingLandingComplexItem::_jsonLandingCoordinateKey =        "landCoordinate";
34 35 36 37
const char* FixedWingLandingComplexItem::_jsonValueSetIsDistanceKey =       "valueSetIsDistance";
const char* FixedWingLandingComplexItem::_jsonAltitudesAreRelativeKey =     "altitudesAreRelative";

// Usage deprecated
38
const char* FixedWingLandingComplexItem::_jsonFallRateKey =                 "fallRate";
39 40
const char* FixedWingLandingComplexItem::_jsonLandingAltitudeRelativeKey =  "landAltitudeRelative";
const char* FixedWingLandingComplexItem::_jsonLoiterAltitudeRelativeKey =   "loiterAltitudeRelative";
41

42
FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObject* parent)
43 44 45 46 47 48 49 50 51 52 53
    : ComplexMissionItem        (vehicle, parent)
    , _sequenceNumber           (0)
    , _dirty                    (false)
    , _landingCoordSet          (false)
    , _ignoreRecalcSignals      (false)
    , _metaDataMap              (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/FWLandingPattern.FactMetaData.json"), this))
    , _landingDistanceFact      (_metaDataMap[loiterToLandDistanceName])
    , _loiterAltitudeFact       (_metaDataMap[loiterAltitudeName])
    , _loiterRadiusFact         (_metaDataMap[loiterRadiusName])
    , _landingHeadingFact       (_metaDataMap[landingHeadingName])
    , _landingAltitudeFact      (_metaDataMap[landingAltitudeName])
54
    , _glideSlopeFact           (_metaDataMap[glideSlopeName])
55
    , _loiterClockwise          (true)
56 57
    , _altitudesAreRelative     (true)
    , _valueSetIsDistance       (true)
58 59 60
{
    _editorQml = "qrc:/qml/FWLandingPatternEditor.qml";

61
    connect(&_loiterAltitudeFact,       &Fact::valueChanged,                                    this, &FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact);
62 63 64 65 66 67 68 69 70 71
    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);
72

73 74
    connect(&_glideSlopeFact,           &Fact::valueChanged,                                    this, &FixedWingLandingComplexItem::_glideSlopeChanged);

75 76 77 78 79 80 81 82
    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);
    connect(this,                       &FixedWingLandingComplexItem::loiterCoordinateChanged,          this, &FixedWingLandingComplexItem::_setDirty);
    connect(this,                       &FixedWingLandingComplexItem::landingCoordinateChanged,         this, &FixedWingLandingComplexItem::_setDirty);
    connect(this,                       &FixedWingLandingComplexItem::loiterClockwiseChanged,           this, &FixedWingLandingComplexItem::_setDirty);
83
    connect(this,                       &FixedWingLandingComplexItem::altitudesAreRelativeChanged,      this, &FixedWingLandingComplexItem::_setDirty);
84

85 86
    connect(this,                       &FixedWingLandingComplexItem::altitudesAreRelativeChanged,      this, &FixedWingLandingComplexItem::coordinateHasRelativeAltitudeChanged);
    connect(this,                       &FixedWingLandingComplexItem::altitudesAreRelativeChanged,      this, &FixedWingLandingComplexItem::exitCoordinateHasRelativeAltitudeChanged);
87 88 89 90
}

int FixedWingLandingComplexItem::lastSequenceNumber(void) const
{
91 92
    // land start, loiter, land
    return _sequenceNumber + 2;
93 94 95 96 97 98 99 100 101 102
}

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

103
void FixedWingLandingComplexItem::save(QJsonArray&  missionItems)
104
{
105 106
    QJsonObject saveObject;

107
    saveObject[JsonHelper::jsonVersionKey] =                    2;
108 109 110
    saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
    saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;

111 112 113 114 115 116 117 118 119 120 121 122 123
    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;

124 125 126 127
    saveObject[_jsonLoiterRadiusKey] =          _loiterRadiusFact.rawValue().toDouble();
    saveObject[_jsonLoiterClockwiseKey] =       _loiterClockwise;
    saveObject[_jsonAltitudesAreRelativeKey] =  _altitudesAreRelative;
    saveObject[_jsonValueSetIsDistanceKey] =    _valueSetIsDistance;
128 129

    missionItems.append(saveObject);
130 131 132 133 134 135 136 137 138 139 140 141 142
}

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)
{
143 144 145 146 147 148 149 150 151 152 153 154
    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 },
    };
    if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
        return false;
    }
155

156 157 158
    QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
    QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
    if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) {
159
        errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
160 161
        return false;
    }
162

163
    setSequenceNumber(sequenceNumber);
164

165 166
    _ignoreRecalcSignals = true;

167 168
    int version = complexObject[JsonHelper::jsonVersionKey].toInt();
    if (version == 1) {
DonLakeFlyer's avatar
DonLakeFlyer committed
169 170 171 172 173 174 175 176
        QList<JsonHelper::KeyValidateInfo> v1KeyInfoList = {
            { _jsonLoiterAltitudeRelativeKey,   QJsonValue::Bool,  true },
            { _jsonLandingAltitudeRelativeKey,  QJsonValue::Bool,  true },
        };
        if (!JsonHelper::validateKeys(complexObject, v1KeyInfoList, errorString)) {
            return false;
        }

177 178 179 180 181 182 183 184 185 186
        bool loiterAltitudeRelative = complexObject[_jsonLoiterAltitudeRelativeKey].toBool();
        bool landingAltitudeRelative = complexObject[_jsonLandingAltitudeRelativeKey].toBool();
        if (loiterAltitudeRelative != landingAltitudeRelative) {
            qgcApp()->showMessage(tr("Fixed Wing Landing Pattern: "
                                     "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;
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
187
        _valueSetIsDistance = true;
188 189 190
    } else if (version == 2) {
        QList<JsonHelper::KeyValidateInfo> v2KeyInfoList = {
            { _jsonAltitudesAreRelativeKey, QJsonValue::Bool,  true },
DonLakeFlyer's avatar
DonLakeFlyer committed
191
            { _jsonValueSetIsDistanceKey,   QJsonValue::Bool,  true },
192 193
        };
        if (!JsonHelper::validateKeys(complexObject, v2KeyInfoList, errorString)) {
DonLakeFlyer's avatar
DonLakeFlyer committed
194
            _ignoreRecalcSignals = false;
195 196
            return false;
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
197 198 199

        _altitudesAreRelative = complexObject[_jsonAltitudesAreRelativeKey].toBool();
        _valueSetIsDistance = complexObject[_jsonValueSetIsDistanceKey].toBool();
200 201 202 203 204 205
    } else {
        errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version);
        _ignoreRecalcSignals = false;
        return false;
    }

206 207 208 209 210 211
    QGeoCoordinate coordinate;
    if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLoiterCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
        return false;
    }
    _loiterCoordinate = coordinate;
    _loiterAltitudeFact.setRawValue(coordinate.altitude());
212

213 214
    if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLandingCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
        return false;
215
    }
216 217 218 219
    _landingCoordinate = coordinate;
    _landingAltitudeFact.setRawValue(coordinate.altitude());

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

    _calcGlideSlope();
223

224
    _landingCoordSet = true;
225 226 227

    _ignoreRecalcSignals = false;
    _recalcFromCoordinateChange();
228 229 230 231 232 233 234

    return true;
}

double FixedWingLandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
    return qMax(_loiterCoordinate.distanceTo(other),_landingCoordinate.distanceTo(other));
235 236 237 238 239 240 241
}

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

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

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

248 249
    MissionItem* item = new MissionItem(seqNum++,                           // sequence number
                                        MAV_CMD_DO_LAND_START,              // MAV_CMD
250
                                        MAV_FRAME_MISSION,                  // MAV_FRAME
251 252 253
                                        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,  // param 1-7
                                        true,                               // autoContinue
                                        false,                              // isCurrentItem
254 255
                                        missionItemParent);
    items.append(item);
256

257
    float loiterRadius = _loiterRadiusFact.rawValue().toDouble() * (_loiterClockwise ? 1.0 : -1.0);
258 259
    item = new MissionItem(seqNum++,
                           MAV_CMD_NAV_LOITER_TO_ALT,
260
                           _altitudesAreRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
261 262 263
                           1.0,                             // Heading required = true
                           loiterRadius,                    // Loiter radius
                           0.0,                             // param 3 - unused
Don Gagne's avatar
Don Gagne committed
264
                           1.0,                             // Exit crosstrack - tangent of loiter to land point
265 266
                           _loiterCoordinate.latitude(),
                           _loiterCoordinate.longitude(),
267
                           _loiterAltitudeFact.rawValue().toDouble(),
268 269
                           true,                            // autoContinue
                           false,                           // isCurrentItem
270 271
                           missionItemParent);
    items.append(item);
272 273 274

    item = new MissionItem(seqNum++,
                           MAV_CMD_NAV_LAND,
275
                           _altitudesAreRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
276
                           0.0, 0.0, 0.0, 0.0,                 // param 1-4
277 278
                           _landingCoordinate.latitude(),
                           _landingCoordinate.longitude(),
279
                           _landingAltitudeFact.rawValue().toDouble(),
280 281
                           true,                               // autoContinue
                           false,                              // isCurrentItem
282 283
                           missionItemParent);
    items.append(item);
284 285
}

286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305
bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, Vehicle* vehicle)
{
    qCDebug(FixedWingLandingComplexItemLog) << "FixedWingLandingComplexItem::scanForItem count" << visualItems->count();

    if (visualItems->count() < 4) {
        return false;
    }

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

    SimpleMissionItem* item = visualItems->value<SimpleMissionItem*>(lastItem--);
    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) ||
            missionItemLand.param1() != 0 || missionItemLand.param2() != 0 || missionItemLand.param3() != 0 || missionItemLand.param4() == 1.0) {
        return false;
    }
306
    MAV_FRAME landPointFrame = missionItemLand.frame();
307 308 309 310 311 312 313

    item = visualItems->value<SimpleMissionItem*>(lastItem--);
    if (!item) {
        return false;
    }
    MissionItem& missionItemLoiter = item->missionItem();
    if (missionItemLoiter.command() != MAV_CMD_NAV_LOITER_TO_ALT ||
314
            missionItemLoiter.frame() != landPointFrame ||
315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334
            missionItemLoiter.param1() != 1.0 || missionItemLoiter.param3() != 0 || missionItemLoiter.param4() != 1.0) {
        return false;
    }

    item = visualItems->value<SimpleMissionItem*>(lastItem--);
    if (!item) {
        return false;
    }
    MissionItem& missionItemDoLandStart = item->missionItem();
    if (missionItemDoLandStart.command() != MAV_CMD_DO_LAND_START ||
            missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0|| missionItemDoLandStart.param5() != 0|| missionItemDoLandStart.param6() != 0|| missionItemDoLandStart.param6() != 0) {
        return false;
    }

    // We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission

    FixedWingLandingComplexItem* complexItem = new FixedWingLandingComplexItem(vehicle, visualItems);

    complexItem->_ignoreRecalcSignals = true;

335
    complexItem->_altitudesAreRelative = landPointFrame == MAV_FRAME_GLOBAL_RELATIVE_ALT;
336 337 338 339 340 341 342 343 344 345
    complexItem->_loiterRadiusFact.setRawValue(qAbs(missionItemLoiter.param2()));
    complexItem->_loiterClockwise = missionItemLoiter.param2() > 0;
    complexItem->_loiterCoordinate.setLatitude(missionItemLoiter.param5());
    complexItem->_loiterCoordinate.setLongitude(missionItemLoiter.param6());
    complexItem->_loiterAltitudeFact.setRawValue(missionItemLoiter.param7());

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

DonLakeFlyer's avatar
DonLakeFlyer committed
346 347
    complexItem->_calcGlideSlope();

348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363
    complexItem->_landingCoordSet = true;

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

    lastItem = visualItems->count() - 1;
    visualItems->removeAt(lastItem--)->deleteLater();
    visualItems->removeAt(lastItem--)->deleteLater();
    visualItems->removeAt(lastItem--)->deleteLater();

    visualItems->append(complexItem);

    return true;
}

364 365
double FixedWingLandingComplexItem::complexDistance(void) const
{
366
    return _loiterCoordinate.distanceTo(_landingCoordinate);
367 368
}

369
void FixedWingLandingComplexItem::setLandingCoordinate(const QGeoCoordinate& coordinate)
370
{
371 372 373 374 375 376 377 378 379 380 381
    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;
382
            _recalcFromHeadingAndDistanceChange();
383 384 385 386
            emit landingCoordSetChanged(true);
        }
    }
}
387

388 389 390 391 392 393 394 395
void FixedWingLandingComplexItem::setLoiterCoordinate(const QGeoCoordinate& coordinate)
{
    if (coordinate != _loiterCoordinate) {
        _loiterCoordinate = coordinate;
        emit coordinateChanged(coordinate);
        emit loiterCoordinateChanged(coordinate);
    }
}
396

397
double FixedWingLandingComplexItem::_mathematicAngleToHeading(double angle)
398
{
399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428
    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();

429 430 431 432
        double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
        if (landToLoiterDistance < radius) {
            // Degnenerate case: Move tangent to loiter point
            _loiterTangentCoordinate = _loiterCoordinate;
433

434
            double heading = _landingCoordinate.azimuthTo(_loiterTangentCoordinate);
435

436 437 438 439 440 441 442 443 444
            _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);
445
            _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
446 447 448

            _ignoreRecalcSignals = true;
            emit loiterCoordinateChanged(_loiterCoordinate);
449
            emit coordinateChanged(_loiterCoordinate);
450 451
            _ignoreRecalcSignals = false;
        }
452 453
    }
}
454

455 456 457 458 459 460 461 462 463 464
void FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange(void)
{
    // Fixed:
    //      land
    //      heading
    //      distance
    //      radius
    // Adjusted:
    //      loiter
    //      loiter tangent
465
    //      glide slope
466

467 468 469 470
    if (!_ignoreRecalcSignals && _landingCoordSet) {
        // These are our known values
        double radius = _loiterRadiusFact.rawValue().toDouble();
        double landToTangentDistance = _landingDistanceFact.rawValue().toDouble();
471 472
        double heading = _landingHeadingFact.rawValue().toDouble();

473 474 475 476 477 478 479 480
        // 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);
481

482 483
        // Use those values to get the new loiter point which takes heading into acount
        _loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(loiterDistance, heading + 180 + loiterAzimuth);
484
        _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
485 486

        _ignoreRecalcSignals = true;
487
        emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
488
        emit loiterCoordinateChanged(_loiterCoordinate);
489
        emit coordinateChanged(_loiterCoordinate);
490
        _calcGlideSlope();
491 492
        _ignoreRecalcSignals = false;
    }
493 494 495 496 497 498 499 500 501 502 503 504
}

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

506
void FixedWingLandingComplexItem::_recalcFromCoordinateChange(void)
507
{
508 509 510 511 512 513 514 515
    // Fixed:
    //      land
    //      loiter
    //      radius
    // Adjusted:
    //      loiter tangent
    //      heading
    //      distance
516
    //      glide slope
517

518 519 520 521 522
    if (!_ignoreRecalcSignals && _landingCoordSet) {
        // These are our known values
        double radius = _loiterRadiusFact.rawValue().toDouble();
        double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
        double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate);
523

524 525 526 527 528 529 530 531
        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));
532

533 534 535
            _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, landToLoiterHeading + loiterToTangentAngle);

        }
536

537
        double heading = _loiterTangentCoordinate.azimuthTo(_landingCoordinate);
538

539 540 541 542
        _ignoreRecalcSignals = true;
        _landingHeadingFact.setRawValue(heading);
        _landingDistanceFact.setRawValue(landToTangentDistance);
        emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
543
        _calcGlideSlope();
544 545 546
        _ignoreRecalcSignals = false;
    }
}
547 548 549 550

void FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact(void)
{
    _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
551 552
    emit loiterCoordinateChanged(_loiterCoordinate);
    emit coordinateChanged(_loiterCoordinate);
553 554 555 556 557
}

void FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void)
{
    _landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble());
558
    emit landingCoordinateChanged(_landingCoordinate);
559
}
560 561 562 563 564

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

DonLakeFlyer's avatar
DonLakeFlyer committed
566 567 568 569
void FixedWingLandingComplexItem::applyNewAltitude(double newAltitude)
{
    _loiterAltitudeFact.setRawValue(newAltitude);
}
570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586

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