Skip to content

Commit

Permalink
Add final approach speed to the Landing Pattern
Browse files Browse the repository at this point in the history 
Introduced an option for setting an airspeed for the final approach in
the Landing Pattern editor, almost identical to the one found in
standard waypoints, for the purposes of energy management before the
glide slope.

It works by inserting a DO_CHANGE_SPEED mission element right after the
DO_LAND_START, in such a way that the speed is also changed in the event
of an RTL. The option is unchecked by default.
  • Loading branch information
@rubenp02
rubenp02 committed Jan 2, 2025
1 parent 27d1ad0 commit cda7b95
Show file tree
Hide file tree
Showing 10 changed files with 136 additions and 17 deletions.
15 changes: 15 additions & 0 deletions src/MissionManager/FWLandingPattern.FactMetaData.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,21 @@
"decimalPlaces": 1,
"default": 40.0
},
{
"name": "UseDoChangeSpeed",
"shortDesc": "Command a specific speed for the approach, useful for reducing energy before the glide slope.",
"type": "bool",
"default": false
},
{
"name": "FinalApproachSpeed",
"shortDesc": "Speed to perform the approach at.",
"type": "double",
"units": "m/s",
"min": 0.0,
"decimalPlaces": 1,
"default": 9.0
},
{
"name": "LoiterRadius",
"shortDesc": "Loiter radius.",
Expand Down
2 changes: 2 additions & 0 deletions src/MissionManager/FixedWingLandingComplexItem.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(PlanMasterController* m
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/FWLandingPattern.FactMetaData.json"), this))
, _landingDistanceFact (settingsGroup, _metaDataMap[finalApproachToLandDistanceName])
, _finalApproachAltitudeFact(settingsGroup, _metaDataMap[finalApproachAltitudeName])
, _useDoChangeSpeedFact (settingsGroup, _metaDataMap[useDoChangeSpeedName])
, _finalApproachSpeedFact (settingsGroup, _metaDataMap[finalApproachSpeedName])
, _loiterRadiusFact (settingsGroup, _metaDataMap[loiterRadiusName])
, _loiterClockwiseFact (settingsGroup, _metaDataMap[loiterClockwiseName])
, _landingHeadingFact (settingsGroup, _metaDataMap[landingHeadingName])
Expand Down
4 changes: 4 additions & 0 deletions src/MissionManager/FixedWingLandingComplexItem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,8 @@ private slots:

// Overrides from LandingComplexItem
const Fact* _finalApproachAltitude (void) const final { return &_finalApproachAltitudeFact; }
const Fact* _useDoChangeSpeed (void) const final { return &_useDoChangeSpeedFact; }
const Fact* _finalApproachSpeed (void) const final { return &_finalApproachSpeedFact; }
const Fact* _loiterRadius (void) const final { return &_loiterRadiusFact; }
const Fact* _loiterClockwise (void) const final { return &_loiterClockwiseFact; }
const Fact* _landingAltitude (void) const final { return &_landingAltitudeFact; }
Expand All @@ -80,6 +82,8 @@ private slots:

Fact _landingDistanceFact;
Fact _finalApproachAltitudeFact;
Fact _useDoChangeSpeedFact;
Fact _finalApproachSpeedFact;
Fact _loiterRadiusFact;
Fact _loiterClockwiseFact;
Fact _landingHeadingFact;
Expand Down
57 changes: 57 additions & 0 deletions src/MissionManager/LandingComplexItem.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,8 @@ void LandingComplexItem::_init(void)
connect(useLoiterToAlt(), &Fact::rawValueChanged, this, &LandingComplexItem::_recalcFromCoordinateChange);

connect(finalApproachAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(useDoChangeSpeed(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(finalApproachSpeed(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(landingAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(landingDistance(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(landingHeading(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
Expand Down Expand Up @@ -282,6 +284,10 @@ void LandingComplexItem::appendMissionItems(QList<MissionItem*>& items, QObject*
MissionItem* item = _createDoLandStartItem(seqNum++, missionItemParent);
items.append(item);

if (useDoChangeSpeed()->rawValue().toBool()) {
item = _createDoChangeSpeedItem(SPEED_TYPE_AIRSPEED, finalApproachSpeed()->rawValue().toDouble(), -1, seqNum++, missionItemParent);
items.append(item);
}

if (stopTakingPhotos()->rawValue().toBool()) {
CameraSection::appendStopTakingPhotos(items, seqNum, missionItemParent);
Expand Down Expand Up @@ -312,6 +318,18 @@ MissionItem* LandingComplexItem::_createDoLandStartItem(int seqNum, QObject* par
parent);
}

MissionItem* LandingComplexItem::_createDoChangeSpeedItem(int speedType, int speedValue, int throttlePercentage, int seqNum, QObject* parent)
{
return new MissionItem(seqNum++, // sequence number
MAV_CMD_DO_CHANGE_SPEED, // MAV_CMD
MAV_FRAME_MISSION, // MAV_FRAME
speedType, speedValue, throttlePercentage, // param 1-3
0.0, 0.0, 0.0, 0.0, // param 4-7
true, // autoContinue
false, // isCurrentItem
parent);
}

MissionItem* LandingComplexItem::_createFinalApproachItem(int seqNum, QObject* parent)
{
if (useLoiterToAlt()->rawValue().toBool()) {
Expand Down Expand Up @@ -355,6 +373,7 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV

// A valid landing pattern is comprised of the follow commands in this order at the end of the item list:
// MAV_CMD_DO_LAND_START - required
// MAV_CMD_DO_CHANGE_SPEED - optional
// Stop taking photos sequence - optional
// Stop taking video sequence - optional
// MAV_CMD_NAV_LOITER_TO_ALT or MAV_CMD_NAV_WAYPOINT
Expand Down Expand Up @@ -420,6 +439,27 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV
scanIndex += CameraSection::stopTakingPhotosCommandCount();
}

scanIndex--;
bool useDoChangeSpeed = false;
double finalApproachSpeed = 0;
if (scanIndex >= 0 && scanIndex < visualItems->count()) {
item = visualItems->value<SimpleMissionItem*>(scanIndex);
if (item) {
MissionItem& missionItemChangeSpeed = item->missionItem();
if (missionItemChangeSpeed.command() == MAV_CMD_DO_CHANGE_SPEED &&
missionItemChangeSpeed.param1() == SPEED_TYPE_AIRSPEED &&
missionItemChangeSpeed.param2() >= -2 &&
missionItemChangeSpeed.param3() == -1 &&
missionItemChangeSpeed.param4() == 0) {
useDoChangeSpeed = true;
finalApproachSpeed = missionItemChangeSpeed.param2();
}
}
}
if (!useDoChangeSpeed) {
scanIndex++;
}

scanIndex--;
if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
return false;
Expand All @@ -444,6 +484,9 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV
if (stopTakingVideo) {
deleteCount += CameraSection::stopTakingVideoCommandCount();
}
if (useDoChangeSpeed) {
deleteCount++;
}
int firstItem = visualItems->count() - deleteCount;
while (deleteCount--) {
visualItems->removeAt(firstItem)->deleteLater();
Expand All @@ -458,8 +501,12 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV
complexItem->_altitudesAreRelative = landPointFrame == MAV_FRAME_GLOBAL_RELATIVE_ALT;
complexItem->setFinalApproachCoordinate(QGeoCoordinate(missionItemFinalApproach.param5(), missionItemFinalApproach.param6()));
complexItem->finalApproachAltitude()->setRawValue(missionItemFinalApproach.param7());
complexItem->useDoChangeSpeed()->setRawValue(useDoChangeSpeed);
complexItem->useLoiterToAlt()->setRawValue(useLoiterToAlt);

if (useDoChangeSpeed) {
complexItem->finalApproachSpeed()->setRawValue(finalApproachSpeed);
}
if (useLoiterToAlt) {
complexItem->loiterRadius()->setRawValue(qAbs(missionItemFinalApproach.param2()));
complexItem->loiterClockwise()->setRawValue(missionItemFinalApproach.param2() > 0);
Expand Down Expand Up @@ -562,6 +609,9 @@ QJsonObject LandingComplexItem::_save(void)
JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate);
saveObject[_jsonFinalApproachCoordinateKey] = jsonCoordinate;

saveObject[_jsonUseDoChangeSpeedKey] = useDoChangeSpeed()->rawValue().toBool();
saveObject[_jsonFinalApproachSpeedKey] = finalApproachSpeed()->rawValue().toDouble();

coordinate = _landingCoordinate;
coordinate.setAltitude(landingAltitude()->rawValue().toDouble());
JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate);
Expand All @@ -585,6 +635,8 @@ bool LandingComplexItem::_load(const QJsonObject& complexObject, int sequenceNum
{ ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true },
{ _jsonDeprecatedLoiterCoordinateKey, QJsonValue::Array, false }, // Loiter changed to Final Approach
{ _jsonFinalApproachCoordinateKey, QJsonValue::Array, false },
{ _jsonUseDoChangeSpeedKey, QJsonValue::Bool, false },
{ _jsonFinalApproachSpeedKey, QJsonValue::Double, false },
{ _jsonLoiterRadiusKey, QJsonValue::Double, true },
{ _jsonLoiterClockwiseKey, QJsonValue::Bool, true },
{ _jsonLandingCoordinateKey, QJsonValue::Array, true },
Expand Down Expand Up @@ -654,6 +706,11 @@ bool LandingComplexItem::_load(const QJsonObject& complexObject, int sequenceNum
_finalApproachCoordinate = coordinate;
finalApproachAltitude()->setRawValue(coordinate.altitude());

useDoChangeSpeed()->setRawValue(complexObject[_jsonUseDoChangeSpeedKey].toBool(false));
finalApproachSpeed()->setRawValue(complexObject.contains(_jsonFinalApproachSpeedKey)
? complexObject[_jsonFinalApproachSpeedKey].toDouble()
: finalApproachSpeed()->rawDefaultValue());

if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLandingCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
return false;
}
Expand Down
47 changes: 30 additions & 17 deletions src/MissionManager/LandingComplexItem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,8 @@ class LandingComplexItem : public ComplexMissionItem
LandingComplexItem(PlanMasterController* masterController, bool flyView);

Q_PROPERTY(Fact* finalApproachAltitude READ finalApproachAltitude CONSTANT)
Q_PROPERTY(Fact* useDoChangeSpeed READ useDoChangeSpeed CONSTANT)
Q_PROPERTY(Fact* finalApproachSpeed READ finalApproachSpeed CONSTANT)
Q_PROPERTY(Fact* loiterRadius READ loiterRadius CONSTANT)
Q_PROPERTY(Fact* landingAltitude READ landingAltitude CONSTANT)
Q_PROPERTY(Fact* landingHeading READ landingHeading CONSTANT)
Expand All @@ -48,6 +50,8 @@ class LandingComplexItem : public ComplexMissionItem
Q_INVOKABLE void setLandingHeadingToTakeoffHeading();

const Fact* finalApproachAltitude (void) const { return _finalApproachAltitude(); }
const Fact* useDoChangeSpeed (void) const { return _useDoChangeSpeed(); }
const Fact* finalApproachSpeed (void) const { return _finalApproachSpeed(); }
const Fact* loiterRadius (void) const { return _loiterRadius(); }
const Fact* loiterClockwise (void) const { return _loiterClockwise(); }
const Fact* landingAltitude (void) const { return _landingAltitude(); }
Expand All @@ -58,6 +62,8 @@ class LandingComplexItem : public ComplexMissionItem
const Fact* stopTakingVideo (void) const { return _stopTakingVideo(); }

Fact* finalApproachAltitude (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_finalApproachAltitude()); };
Fact* useDoChangeSpeed (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_useDoChangeSpeed()); };
Fact* finalApproachSpeed (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_finalApproachSpeed()); };
Fact* loiterRadius (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_loiterRadius()); };
Fact* loiterClockwise (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_loiterClockwise()); };
Fact* landingAltitude (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_landingAltitude()); };
Expand Down Expand Up @@ -110,15 +116,17 @@ class LandingComplexItem : public ComplexMissionItem
double minAMSLAltitude (void) const final { return amslExitAlt(); }
double maxAMSLAltitude (void) const final { return amslEntryAlt(); }

static constexpr const char* finalApproachToLandDistanceName = "LandingDistance";
static constexpr const char* landingHeadingName = "LandingHeading";
static constexpr const char* finalApproachAltitudeName = "FinalApproachAltitude";
static constexpr const char* loiterRadiusName = "LoiterRadius";
static constexpr const char* loiterClockwiseName = "LoiterClockwise";
static constexpr const char* landingAltitudeName = "LandingAltitude";
static constexpr const char* useLoiterToAltName = "UseLoiterToAlt";
static constexpr const char* stopTakingPhotosName = "StopTakingPhotos";
static constexpr const char* stopTakingVideoName = "StopTakingVideo";
static constexpr const char* finalApproachToLandDistanceName = "LandingDistance";
static constexpr const char* landingHeadingName = "LandingHeading";
static constexpr const char* finalApproachAltitudeName = "FinalApproachAltitude";
static constexpr const char* useDoChangeSpeedName = "UseDoChangeSpeed";
static constexpr const char* finalApproachSpeedName = "FinalApproachSpeed";
static constexpr const char* loiterRadiusName = "LoiterRadius";
static constexpr const char* loiterClockwiseName = "LoiterClockwise";
static constexpr const char* landingAltitudeName = "LandingAltitude";
static constexpr const char* useLoiterToAltName = "UseLoiterToAlt";
static constexpr const char* stopTakingPhotosName = "StopTakingPhotos";
static constexpr const char* stopTakingVideoName = "StopTakingVideo";

signals:
void finalApproachCoordinateChanged (QGeoCoordinate coordinate);
Expand All @@ -137,6 +145,8 @@ protected slots:

protected:
virtual const Fact* _finalApproachAltitude (void) const = 0;
virtual const Fact* _useDoChangeSpeed (void) const = 0;
virtual const Fact* _finalApproachSpeed (void) const = 0;
virtual const Fact* _loiterRadius (void) const = 0;
virtual const Fact* _loiterClockwise (void) const = 0;
virtual const Fact* _landingAltitude (void) const = 0;
Expand All @@ -151,6 +161,7 @@ protected slots:
void _init (void);
QPointF _rotatePoint (const QPointF& point, const QPointF& origin, double angle);
MissionItem* _createDoLandStartItem (int seqNum, QObject* parent);
MissionItem* _createDoChangeSpeedItem(int speedType, int speedValue, int throttlePercentage, int seqNum, QObject* parent);
MissionItem* _createFinalApproachItem(int seqNum, QObject* parent);
QJsonObject _save (void);
bool _load (const QJsonObject& complexObject, int sequenceNumber, const QString& jsonComplexItemTypeValue, bool useDeprecatedRelAltKeys, QString& errorString);
Expand Down Expand Up @@ -178,14 +189,16 @@ protected slots:
// the new support for using either a loiter or just a waypoint as the approach entry point.
static constexpr const char* _jsonDeprecatedLoiterCoordinateKey = "loiterCoordinate";

static constexpr const char* _jsonFinalApproachCoordinateKey = "landingApproachCoordinate";
static constexpr const char* _jsonLoiterRadiusKey = "loiterRadius";
static constexpr const char* _jsonLoiterClockwiseKey = "loiterClockwise";
static constexpr const char* _jsonLandingCoordinateKey = "landCoordinate";
static constexpr const char* _jsonAltitudesAreRelativeKey = "altitudesAreRelative";
static constexpr const char* _jsonUseLoiterToAltKey = "useLoiterToAlt";
static constexpr const char* _jsonStopTakingPhotosKey = "stopTakingPhotos";
static constexpr const char* _jsonStopTakingVideoKey = "stopVideoPhotos";
static constexpr const char* _jsonFinalApproachCoordinateKey = "landingApproachCoordinate";
static constexpr const char* _jsonUseDoChangeSpeedKey = "useDoChangeSpeed";
static constexpr const char* _jsonFinalApproachSpeedKey = "finalApproachSpeed";
static constexpr const char* _jsonLoiterRadiusKey = "loiterRadius";
static constexpr const char* _jsonLoiterClockwiseKey = "loiterClockwise";
static constexpr const char* _jsonLandingCoordinateKey = "landCoordinate";
static constexpr const char* _jsonAltitudesAreRelativeKey = "altitudesAreRelative";
static constexpr const char* _jsonUseLoiterToAltKey = "useLoiterToAlt";
static constexpr const char* _jsonStopTakingPhotosKey = "stopTakingPhotos";
static constexpr const char* _jsonStopTakingVideoKey = "stopVideoPhotos";

private slots:
void _recalcFromRadiusChange (void);
Expand Down
2 changes: 2 additions & 0 deletions src/MissionManager/VTOLLandingComplexItem.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@ VTOLLandingComplexItem::VTOLLandingComplexItem(PlanMasterController* masterContr
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/VTOLLandingPattern.FactMetaData.json"), this))
, _landingDistanceFact (settingsGroup, _metaDataMap[finalApproachToLandDistanceName])
, _finalApproachAltitudeFact(settingsGroup, _metaDataMap[finalApproachAltitudeName])
, _useDoChangeSpeedFact (settingsGroup, _metaDataMap[useDoChangeSpeedName])
, _finalApproachSpeedFact (settingsGroup, _metaDataMap[finalApproachSpeedName])
, _loiterRadiusFact (settingsGroup, _metaDataMap[loiterRadiusName])
, _loiterClockwiseFact (settingsGroup, _metaDataMap[loiterClockwiseName])
, _landingHeadingFact (settingsGroup, _metaDataMap[landingHeadingName])
Expand Down
4 changes: 4 additions & 0 deletions src/MissionManager/VTOLLandingComplexItem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,8 @@ private slots:

// Overrides from LandingComplexItem
const Fact* _finalApproachAltitude (void) const final { return &_finalApproachAltitudeFact; }
const Fact* _useDoChangeSpeed (void) const final { return &_useDoChangeSpeedFact; }
const Fact* _finalApproachSpeed (void) const final { return &_finalApproachSpeedFact; }
const Fact* _loiterRadius (void) const final { return &_loiterRadiusFact; }
const Fact* _loiterClockwise (void) const final { return &_loiterClockwiseFact; }
const Fact* _landingAltitude (void) const final { return &_landingAltitudeFact; }
Expand All @@ -67,6 +69,8 @@ private slots:

Fact _landingDistanceFact;
Fact _finalApproachAltitudeFact;
Fact _useDoChangeSpeedFact;
Fact _finalApproachSpeedFact;
Fact _loiterRadiusFact;
Fact _loiterClockwiseFact;
Fact _landingHeadingFact;
Expand Down
Loading

0 comments on commit cda7b95

Please sign in to comment.