Skip to content

Commit

Permalink
Add support for multiple landing sequences
Browse files Browse the repository at this point in the history 
Added an option to configure multiple landing sequences in a mission.
This feature allows users to define alternative landing sites, which can
be useful for contingency planning. When multiple landing sequences are
present, the first one is used for the primary mission, but in the event
of an RTL, the closest landing sequence will be used, as outlined in the
MAVLink spec:
https://mavlink.io/en/messages/common.html#MAV_CMD_DO_LAND_START.

Key changes:
- Disabled the restriction to only one landing sequence.
- Modified LandingComplexItem to support scanning for multiple complex
  items, and configuring the DO_LAND_START mission items to contain the
  coordinates of the first element in the landing sequence.
- Updated MissionController to handle multiple landing sequences,
  including proper flight path segment calculation and handling of
  discontinuities after landing commands.
- The TerrainProfile displays the entire mission with any possible
  alternative landing sequences, but the mission stats (distance and
  time) represent the primary mission, up to the default landing.
- Added an option in the Plan View settings to control this feature,
  defaulting to match the previous behavior.
  • Loading branch information
@rubenp02
rubenp02 committed Jan 9, 2025
1 parent e2d7ca7 commit 2647246
Show file tree
Hide file tree
Showing 16 changed files with 149 additions and 81 deletions.
4 changes: 2 additions & 2 deletions src/MissionManager/FixedWingLandingComplexItem.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,9 +149,9 @@ bool FixedWingLandingComplexItem::_isValidLandItem(const MissionItem& missionIte
}
}

bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController)
bool FixedWingLandingComplexItem::scanForItems(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController)
{
return _scanForItem(visualItems, flyView, masterController, _isValidLandItem, _createItem);
return _scanForItems(visualItems, flyView, masterController, _isValidLandItem, _createItem);
}

// Never call this method directly. If you want to update the flight segments you emit _updateFlightPathSegmentsSignal()
Expand Down
2 changes: 1 addition & 1 deletion src/MissionManager/FixedWingLandingComplexItem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ class FixedWingLandingComplexItem : public LandingComplexItem
Fact* valueSetIsDistance (void) { return &_valueSetIsDistanceFact; }

/// Scans the loaded items for a landing pattern complex item
static bool scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController);
static bool scanForItems(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController);

// Overrides from ComplexMissionItem
QString patternName (void) const final { return name; }
Expand Down
49 changes: 34 additions & 15 deletions src/MissionManager/LandingComplexItem.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -311,8 +311,12 @@ MissionItem* LandingComplexItem::_createDoLandStartItem(int seqNum, QObject* par
{
return new MissionItem(seqNum, // sequence number
MAV_CMD_DO_LAND_START, // MAV_CMD
MAV_FRAME_MISSION, // MAV_FRAME
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 1-7
_altitudesAreRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
0.0, 0.0, 0.0, 0.0, // param 1-4
// Set to the approach coordinates to make use of the closest landing sequence in the event of an RTL
_finalApproachCoordinate.latitude(),
_finalApproachCoordinate.longitude(),
_finalApproachAltitude()->rawValue().toFloat(),
true, // autoContinue
false, // isCurrentItem
parent);
Expand Down Expand Up @@ -363,14 +367,31 @@ MissionItem* LandingComplexItem::_createFinalApproachItem(int seqNum, QObject* p
}
}

bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc)
bool LandingComplexItem::_scanForItems(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc)
{
qCDebug(LandingComplexItemLog) << "VTOLLandingComplexItem::scanForItem count" << visualItems->count();

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

// Start looking for the commands in reverse order from the end of the list
int startIndex = visualItems->count();
bool foundAny = false;

while (startIndex >= 0) {
if (_scanForItem(visualItems, startIndex, flyView, masterController, isLandItemFunc, createItemFunc)) {
foundAny = true;
} else {
startIndex--;
}
}

return foundAny;
}

bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, int& startIndex, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc)
{
// 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
Expand All @@ -379,9 +400,7 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV
// MAV_CMD_NAV_LOITER_TO_ALT or MAV_CMD_NAV_WAYPOINT
// MAV_CMD_NAV_LAND or MAV_CMD_NAV_VTOL_LAND

// Start looking for the commands in reverse order from the end of the list

int scanIndex = visualItems->count() - 1;
int scanIndex = startIndex - 1;

if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
return false;
Expand Down Expand Up @@ -427,16 +446,16 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV
return false;
}

scanIndex -= CameraSection::stopTakingVideoCommandCount();
startIndex -= CameraSection::stopTakingVideoCommandCount();
bool stopTakingVideo = CameraSection::scanStopTakingVideo(visualItems, scanIndex, false /* removeScannedItems */);
if (!stopTakingVideo) {
scanIndex += CameraSection::stopTakingVideoCommandCount();
startIndex += CameraSection::stopTakingVideoCommandCount();
}

scanIndex -= CameraSection::stopTakingPhotosCommandCount();
startIndex -= CameraSection::stopTakingPhotosCommandCount();
bool stopTakingPhotos = CameraSection::scanStopTakingPhotos(visualItems, scanIndex, false /* removeScannedItems */);
if (!stopTakingPhotos) {
scanIndex += CameraSection::stopTakingPhotosCommandCount();
startIndex += CameraSection::stopTakingPhotosCommandCount();
}

scanIndex--;
Expand Down Expand Up @@ -470,13 +489,12 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV
}
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.param7() != 0) {
missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 ||
missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0) {
return false;
}

// We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission.
// Since we have scanned it we need to remove the items for it fromt the list
int deleteCount = 3;
if (stopTakingPhotos) {
deleteCount += CameraSection::stopTakingPhotosCommandCount();
Expand All @@ -487,7 +505,7 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV
if (useDoChangeSpeed) {
deleteCount++;
}
int firstItem = visualItems->count() - deleteCount;
int firstItem = startIndex - deleteCount;
while (deleteCount--) {
visualItems->removeAt(firstItem)->deleteLater();
}
Expand Down Expand Up @@ -526,7 +544,8 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV
complexItem->_recalcFromCoordinateChange();
complexItem->setDirty(false);

visualItems->append(complexItem);
visualItems->insert(firstItem, complexItem);
startIndex = firstItem;

return true;
}
Expand Down
3 changes: 2 additions & 1 deletion src/MissionManager/LandingComplexItem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,7 +170,8 @@ protected slots:
typedef bool (*IsLandItemFunc)(const MissionItem& missionItem);
typedef LandingComplexItem* (*CreateItemFunc)(PlanMasterController* masterController, bool flyView);

static bool _scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc);
static bool _scanForItems(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc);
static bool _scanForItem(QmlObjectListModel* visualItems, int& startIndex, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc);

int _sequenceNumber = 0;
bool _dirty = false;
Expand Down
89 changes: 59 additions & 30 deletions src/MissionManager/MissionController.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,9 +53,11 @@ MissionController::MissionController(PlanMasterController* masterController, QOb

_updateTimer.setSingleShot(true);

connect(&_updateTimer, &QTimer::timeout, this, &MissionController::_updateTimeout);
connect(_planViewSettings->takeoffItemNotRequired(), &Fact::rawValueChanged, this, &MissionController::_takeoffItemNotRequiredChanged);
connect(this, &MissionController::missionDistanceChanged, this, &MissionController::recalcTerrainProfile);
connect(&_updateTimer, &QTimer::timeout, this, &MissionController::_updateTimeout);
connect(_planViewSettings->takeoffItemNotRequired(), &Fact::rawValueChanged, this, &MissionController::_forceRecalcOfAllowedBits);
connect(
_planViewSettings->allowMultipleLandingPatterns(), &Fact::rawValueChanged, this, &MissionController::_forceRecalcOfAllowedBits);
connect(this, &MissionController::missionPlannedDistanceChanged, this, &MissionController::recalcTerrainProfile);

// The follow is used to compress multiple recalc calls in a row to into a single call.
connect(this, &MissionController::_recalcMissionFlightStatusSignal, this, &MissionController::_recalcMissionFlightStatus, Qt::QueuedConnection);
Expand All @@ -72,7 +74,8 @@ MissionController::~MissionController()

void MissionController::_resetMissionFlightStatus(void)
{
_missionFlightStatus.totalDistance = 0.0;
_missionFlightStatus.totalDistance = 0.0;
_missionFlightStatus.plannedDistance = 0.0;
_missionFlightStatus.maxTelemetryDistance = 0.0;
_missionFlightStatus.totalTime = 0.0;
_missionFlightStatus.hoverTime = 0.0;
Expand Down Expand Up @@ -101,7 +104,7 @@ void MissionController::_resetMissionFlightStatus(void)
_missionFlightStatus.ampMinutesAvailable = static_cast<double>(_missionFlightStatus.mAhBattery) / 1000.0 * 60.0 * ((100.0 - batteryPercentRemainingAnnounce) / 100.0);
}

emit missionDistanceChanged(_missionFlightStatus.totalDistance);
emit missionPlannedDistanceChanged(_missionFlightStatus.plannedDistance);
emit missionTimeChanged();
emit missionHoverDistanceChanged(_missionFlightStatus.hoverDistance);
emit missionCruiseDistanceChanged(_missionFlightStatus.cruiseDistance);
Expand Down Expand Up @@ -1328,6 +1331,12 @@ void MissionController::_recalcFlightPathSegments(void)
break;
}

// Don't draw segments immediately after a landing item
if (lastFlyThroughVI->isLandCommand()) {
lastFlyThroughVI = visualItem;
continue;
}

if (visualItem->specifiesCoordinate() && !visualItem->isStandaloneCoordinate()) {
// Incomplete items are complex items which are waiting for the user to complete setup before there visuals can become valid.
// They may not yet have valid entry/exit coordinates associated with them while in the incomplete state.
Expand Down Expand Up @@ -1448,7 +1457,7 @@ void MissionController::_addHoverTime(double hoverTime, double hoverDistance, in
_missionFlightStatus.totalTime += hoverTime;
_missionFlightStatus.hoverTime += hoverTime;
_missionFlightStatus.hoverDistance += hoverDistance;
_missionFlightStatus.totalDistance += hoverDistance;
_missionFlightStatus.plannedDistance += hoverDistance;
_updateBatteryInfo(waypointIndex);
}

Expand All @@ -1457,7 +1466,7 @@ void MissionController::_addCruiseTime(double cruiseTime, double cruiseDistance,
_missionFlightStatus.totalTime += cruiseTime;
_missionFlightStatus.cruiseTime += cruiseTime;
_missionFlightStatus.cruiseDistance += cruiseDistance;
_missionFlightStatus.totalDistance += cruiseDistance;
_missionFlightStatus.plannedDistance += cruiseDistance;
_updateBatteryInfo(waypointIndex);
}

Expand Down Expand Up @@ -1517,6 +1526,7 @@ void MissionController::_recalcMissionFlightStatus()

bool linkStartToHome = false;
bool foundRTL = false;
bool pastLandCommand = false;
double totalHorizontalDistance = 0;

for (int i=0; i<_visualItems->count(); i++) {
Expand Down Expand Up @@ -1582,7 +1592,8 @@ void MissionController::_recalcMissionFlightStatus()
}
}

_addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, 0, 0, item->additionalTimeDelay(), 0, -1);
if (!pastLandCommand)
_addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, 0, 0, item->additionalTimeDelay(), 0, -1);

if (item->specifiesCoordinate()) {

Expand Down Expand Up @@ -1621,28 +1632,37 @@ void MissionController::_recalcMissionFlightStatus()
double azimuth, distance, altDifference;

_calcPrevWaypointValues(item, lastFlyThroughVI, &azimuth, &distance, &altDifference);
totalHorizontalDistance += distance;

// If the last waypoint was a land command, there's a discontinuity at this point
if (!lastFlyThroughVI->isLandCommand()) {
totalHorizontalDistance += distance;
item->setDistance(distance);

if (!pastLandCommand) {
// Calculate time/distance
double hoverTime = distance / _missionFlightStatus.hoverSpeed;
double cruiseTime = distance / _missionFlightStatus.cruiseSpeed;
_addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, 0, distance, item->sequenceNumber());
}
}

item->setAltDifference(altDifference);
item->setAzimuth(azimuth);
item->setDistance(distance);
item->setDistanceFromStart(totalHorizontalDistance);

_missionFlightStatus.maxTelemetryDistance = qMax(_missionFlightStatus.maxTelemetryDistance, _calcDistanceToHome(item, _settingsItem));

// Calculate time/distance
double hoverTime = distance / _missionFlightStatus.hoverSpeed;
double cruiseTime = distance / _missionFlightStatus.cruiseSpeed;
_addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, 0, distance, item->sequenceNumber());
}

if (complexItem) {
// Add in distance/time inside complex items as well
double distance = complexItem->complexDistance();
_missionFlightStatus.maxTelemetryDistance = qMax(_missionFlightStatus.maxTelemetryDistance, complexItem->greatestDistanceTo(complexItem->exitCoordinate()));

double hoverTime = distance / _missionFlightStatus.hoverSpeed;
double cruiseTime = distance / _missionFlightStatus.cruiseSpeed;
_addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, 0, distance, item->sequenceNumber());
if (!pastLandCommand) {
double hoverTime = distance / _missionFlightStatus.hoverSpeed;
double cruiseTime = distance / _missionFlightStatus.cruiseSpeed;
_addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, 0, distance, item->sequenceNumber());
}

totalHorizontalDistance += distance;
}
Expand Down Expand Up @@ -1696,6 +1716,10 @@ void MissionController::_recalcMissionFlightStatus()
break;
}
}

if (item->isLandCommand()) {
pastLandCommand = true;
}
}
lastFlyThroughVI->setMissionVehicleYaw(_missionFlightStatus.vehicleYaw);

Expand All @@ -1704,13 +1728,17 @@ void MissionController::_recalcMissionFlightStatus()
double azimuth, distance, altDifference;
_calcPrevWaypointValues(lastFlyThroughVI, _settingsItem, &azimuth, &distance, &altDifference);

// Calculate time/distance
double hoverTime = distance / _missionFlightStatus.hoverSpeed;
double cruiseTime = distance / _missionFlightStatus.cruiseSpeed;
double landTime = qAbs(altDifference) / _appSettings->offlineEditingDescentSpeed()->rawValue().toDouble();
_addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, distance, landTime, -1);
if (!pastLandCommand) {
// Calculate time/distance
double hoverTime = distance / _missionFlightStatus.hoverSpeed;
double cruiseTime = distance / _missionFlightStatus.cruiseSpeed;
double landTime = qAbs(altDifference) / _appSettings->offlineEditingDescentSpeed()->rawValue().toDouble();
_addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, distance, landTime, -1);
}
}

_missionFlightStatus.totalDistance = totalHorizontalDistance;

if (_missionFlightStatus.mAhBattery != 0 && _missionFlightStatus.batteryChangePoint == -1) {
_missionFlightStatus.batteryChangePoint = 0;
}
Expand All @@ -1722,7 +1750,8 @@ void MissionController::_recalcMissionFlightStatus()
}

emit missionMaxTelemetryChanged (_missionFlightStatus.maxTelemetryDistance);
emit missionDistanceChanged (_missionFlightStatus.totalDistance);
emit missionTotalDistanceChanged (_missionFlightStatus.totalDistance);
emit missionPlannedDistanceChanged (_missionFlightStatus.plannedDistance);
emit missionHoverDistanceChanged (_missionFlightStatus.hoverDistance);
emit missionCruiseDistanceChanged (_missionFlightStatus.cruiseDistance);
emit missionTimeChanged ();
Expand Down Expand Up @@ -2165,9 +2194,9 @@ void MissionController::setDirty(bool dirty)

void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualItems, PlanMasterController* masterController)
{
// First we look for a Landing Patterns which are at the end
if (!FixedWingLandingComplexItem::scanForItem(visualItems, _flyView, masterController)) {
VTOLLandingComplexItem::scanForItem(visualItems, _flyView, masterController);
// First we look for Landing Patterns which are at the end
if (!FixedWingLandingComplexItem::scanForItems(visualItems, _flyView, masterController)) {
VTOLLandingComplexItem::scanForItems(visualItems, _flyView, masterController);
}

int scanIndex = 0;
Expand Down Expand Up @@ -2473,8 +2502,8 @@ void MissionController::setCurrentPlanViewSeqNum(int sequenceNumber, bool force)
}
}

if (foundLand) {
// Can't have more than one land sequence
if (foundLand && !_planViewSettings->allowMultipleLandingPatterns()->rawValue().toBool()) {
// Can't have more than one land sequence unless allowed in settings
_isInsertLandValid = false;
if (sequenceNumber >= landSeqNum) {
// Can't have fly through commands after a land item
Expand Down Expand Up @@ -2594,7 +2623,7 @@ bool MissionController::isEmpty(void) const
return _visualItems->count() <= 1;
}

void MissionController::_takeoffItemNotRequiredChanged(void)
void MissionController::_forceRecalcOfAllowedBits(void)
{
// Force a recalc of allowed bits
setCurrentPlanViewSeqNum(_currentPlanViewSeqNum, true /* force */);
Expand Down
Loading

0 comments on commit 2647246

Please sign in to comment.