diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index c642fe9..99d7aac 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -12,6 +12,7 @@ #include "MissionController.h" #include "QGCGeo.h" #include "QGroundControlQmlGlobal.h" +#include "SimpleMissionItem.h" #include @@ -169,6 +170,8 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq setSequenceNumber(sequenceNumber); + _ignoreRecalcSignals = true; + QGeoCoordinate coordinate; if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLoiterCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { return false; @@ -188,7 +191,9 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq _landingAltitudeRelative = complexObject[_jsonLandingAltitudeRelativeKey].toBool(); _landingCoordSet = true; - _recalcFromHeadingAndDistanceChange(); + + _ignoreRecalcSignals = false; + _recalcFromCoordinateChange(); return true; } @@ -207,6 +212,8 @@ void FixedWingLandingComplexItem::appendMissionItems(QList& items, { int seqNum = _sequenceNumber; + // IMPORTANT NOTE: Any changes here must also be taken into account in scanForItem + MissionItem* item = new MissionItem(seqNum++, // sequence number MAV_CMD_DO_LAND_START, // MAV_CMD MAV_FRAME_MISSION, // MAV_FRAME @@ -245,6 +252,82 @@ void FixedWingLandingComplexItem::appendMissionItems(QList& items, items.append(item); } +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(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; + } + + item = visualItems->value(lastItem--); + if (!item) { + return false; + } + MissionItem& missionItemLoiter = item->missionItem(); + if (missionItemLoiter.command() != MAV_CMD_NAV_LOITER_TO_ALT || + !(missionItemLoiter.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItemLoiter.frame() == MAV_FRAME_GLOBAL) || + missionItemLoiter.param1() != 1.0 || missionItemLoiter.param3() != 0 || missionItemLoiter.param4() != 1.0) { + return false; + } + + item = visualItems->value(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; + + complexItem->_loiterAltitudeRelative = missionItemLoiter.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT; + 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->_landingAltitudeRelative = missionItemLand.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT; + complexItem->_landingCoordinate.setLatitude(missionItemLand.param5()); + complexItem->_landingCoordinate.setLongitude(missionItemLand.param6()); + complexItem->_landingAltitudeFact.setRawValue(missionItemLand.param7()); + + 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; +} + double FixedWingLandingComplexItem::complexDistance(void) const { return _loiterCoordinate.distanceTo(_landingCoordinate); @@ -437,3 +520,4 @@ void FixedWingLandingComplexItem::_setDirty(void) { setDirty(true); } + diff --git a/src/MissionManager/FixedWingLandingComplexItem.h b/src/MissionManager/FixedWingLandingComplexItem.h index e07de5b..ac20e2c 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.h +++ b/src/MissionManager/FixedWingLandingComplexItem.h @@ -49,6 +49,9 @@ public: void setLandingCoordinate (const QGeoCoordinate& coordinate); void setLoiterCoordinate (const QGeoCoordinate& coordinate); + /// Scans the loaded items for a landing pattern complex item + static bool scanForItem(QmlObjectListModel* visualItems, Vehicle* vehicle); + // Overrides from ComplexMissionItem double complexDistance (void) const final; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index a9d5479..cd14bbc 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -1547,6 +1547,9 @@ QString MissionController::fileExtension(void) const void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle) { + // First we look for a Fixed Wing Landing Pattern which is at the end + FixedWingLandingComplexItem::scanForItem(visualItems, vehicle); + int scanIndex = 0; while (scanIndex < visualItems->count()) { VisualMissionItem* visualItem = visualItems->value(scanIndex);