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 Dec 30, 2024
1 parent 6628eed commit 3a3a036
Show file tree
Hide file tree
Showing 10 changed files with 144 additions and 25 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
73 changes: 65 additions & 8 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 @@ -398,9 +417,9 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV
}
} else if (missionItemFinalApproach.command() == MAV_CMD_NAV_WAYPOINT) {
if (missionItemFinalApproach.frame() != landPointFrame ||
missionItemFinalApproach.param1() != 0 || missionItemFinalApproach.param2() != 0 || missionItemFinalApproach.param3() != 0 ||
(!masterController->managerVehicle()->apmFirmware() && !qIsNaN(missionItemFinalApproach.param4())) ||
qIsNaN(missionItemFinalApproach.param5()) || qIsNaN(missionItemFinalApproach.param6()) || qIsNaN(missionItemFinalApproach.param6())) {
missionItemFinalApproach.param1() != 0 || missionItemFinalApproach.param2() != 0 || missionItemFinalApproach.param3() != 0 ||
(!masterController->managerVehicle()->apmFirmware() && !qIsNaN(missionItemFinalApproach.param4())) ||
qIsNaN(missionItemFinalApproach.param5()) || qIsNaN(missionItemFinalApproach.param6()) || qIsNaN(missionItemFinalApproach.param6())) {
return false;
}
useLoiterToAlt = false;
Expand All @@ -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() >= 0 && missionItemChangeSpeed.param1() <= 3 &&
missionItemChangeSpeed.param2() >= -2 &&
missionItemChangeSpeed.param3() >= -2 && missionItemChangeSpeed.param3() <= 100 &&
missionItemChangeSpeed.param4() == 0) {
useDoChangeSpeed = true;
finalApproachSpeed = missionItemChangeSpeed.param2();
}
}
}
if (!useDoChangeSpeed) {
scanIndex++;
}

scanIndex--;
if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
return false;
Expand All @@ -430,11 +470,11 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV
}
MissionItem& missionItemDoLandStart = item->missionItem();
if (missionItemDoLandStart.command() != MAV_CMD_DO_LAND_START ||
(masterController->managerVehicle()->apmFirmware() ?
missionItemDoLandStart.frame() != MAV_FRAME_GLOBAL :
missionItemDoLandStart.frame() != MAV_FRAME_MISSION) ||
missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0 ||
missionItemDoLandStart.param5() != 0 || missionItemDoLandStart.param6() != 0 || missionItemDoLandStart.param7() != 0) {
(masterController->managerVehicle()->apmFirmware() ?
missionItemDoLandStart.frame() != MAV_FRAME_GLOBAL :
missionItemDoLandStart.frame() != MAV_FRAME_MISSION) ||
missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0 ||
missionItemDoLandStart.param5() != 0 || missionItemDoLandStart.param6() != 0 || missionItemDoLandStart.param7() != 0) {
return false;
}

Expand All @@ -447,6 +487,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 @@ -461,8 +504,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 @@ -565,6 +612,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 @@ -588,6 +638,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 @@ -657,6 +709,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
Loading

0 comments on commit 3a3a036

Please sign in to comment.