Skip to content

Q
qgroundcontrol
Commit 34b4f681 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #4844 from DonLakeFlyer/MissionEndAction 

Plan: Support for Mission End Actions
parents c6e2153a 1e97127c
Hide whitespace changes
Inline Side-by-side
Showing with 187 additions and 20 deletions
src/MissionEditor/MissionSettingsEditor.qml
View file @ 34b4f681
......@@ -217,15 +217,12 @@ Rectangle {
}
} // GridLayout
/*
FIXME: NYI
FactComboBox {
anchors.left: parent.left
anchors.right: parent.right
fact: missionItem.missionEndAction
indexModel: false
}
*/
}
CameraSection {
......
src/MissionManager/MissionController.cc
View file @ 34b4f681
......@@ -166,22 +166,46 @@ void MissionController::sendToVehicle(void)
_visualItems->setDirty(false);
}
/// Converts from visual items to MissionItems
/// @param missionItemParent QObject parent for newly allocated MissionItems
/// @return true: Mission end action was added to end of list
bool MissionController::_convertToMissionItems(QmlObjectListModel* visualMissionItems, QList<MissionItem*>& rgMissionItems, QObject* missionItemParent)
{
bool endActionSet = false;
int lastSeqNum = 0;
for (int i=0; i<visualMissionItems->count(); i++) {
VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(visualMissionItems->get(i));
lastSeqNum = visualItem->lastSequenceNumber();
visualItem->appendMissionItems(rgMissionItems, missionItemParent);
qCDebug(MissionControllerLog) << "_convertToMissionItems seqNum:lastSeqNum:command"
<< visualItem->sequenceNumber()
<< lastSeqNum
<< visualItem->commandName();
}
// Mission settings has a special case for end mission action
MissionSettingsComplexItem* settingsItem = visualMissionItems->value<MissionSettingsComplexItem*>(0);
if (settingsItem) {
endActionSet = settingsItem->addMissionEndAction(rgMissionItems, lastSeqNum + 1, missionItemParent);
}
return endActionSet;
}
void MissionController::sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems)
{
if (vehicle) {
// Convert to MissionItems so we can send to vehicle
QList<MissionItem*> missionItems;
QList<MissionItem*> rgMissionItems;
for (int i=0; i<visualMissionItems->count(); i++) {
VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(visualMissionItems->get(i));
_convertToMissionItems(visualMissionItems, rgMissionItems, vehicle);
visualItem->appendMissionItems(missionItems, NULL);
}
vehicle->missionManager()->writeMissionItems(rgMissionItems);
vehicle->missionManager()->writeMissionItems(missionItems);
for (int i=0; i<missionItems.count(); i++) {
missionItems[i]->deleteLater();
for (int i=0; i<rgMissionItems.count(); i++) {
rgMissionItems[i]->deleteLater();
}
}
}
......@@ -464,9 +488,9 @@ bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObje
QString itemType = itemObject[VisualMissionItem::jsonTypeKey].toString();
if (itemType == VisualMissionItem::jsonTypeSimpleItemValue) {
qCDebug(MissionControllerLog) << "Loading MISSION_ITEM: nextSequenceNumber" << nextSequenceNumber;
SimpleMissionItem* simpleItem = new SimpleMissionItem(vehicle, visualItems);
if (simpleItem->load(itemObject, nextSequenceNumber, errorString)) {
qCDebug(MissionControllerLog) << "Loading simple item: nextSequenceNumber:command" << nextSequenceNumber << simpleItem->command();
nextSequenceNumber = simpleItem->lastSequenceNumber() + 1;
visualItems->append(simpleItem);
} else {
......@@ -709,12 +733,30 @@ void MissionController::saveToFile(const QString& filename)
missionFileObject[_jsonHoverSpeedKey] = _activeVehicle->defaultHoverSpeed();
// Save the visual items
QJsonArray rgMissionItems;
QJsonArray rgJsonMissionItems;
for (int i=0; i<_visualItems->count(); i++) {
VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
visualItem->save(rgMissionItems);
visualItem->save(rgJsonMissionItems);
}
// Mission settings has a special case for end mission action
if (settingsItem) {
QList<MissionItem*> rgMissionItems;
if (_convertToMissionItems(_visualItems, rgMissionItems, this /* missionItemParent */)) {
QJsonObject saveObject;
MissionItem* missionItem = rgMissionItems[rgMissionItems.count() - 1];
missionItem->save(saveObject);
rgJsonMissionItems.append(saveObject);
}
for (int i=0; i<rgMissionItems.count(); i++) {
rgMissionItems[i]->deleteLater();
}
}
missionFileObject[_jsonItemsKey] = rgMissionItems;
missionFileObject[_jsonItemsKey] = rgJsonMissionItems;
QJsonDocument saveDoc(missionFileObject);
file.write(saveDoc.toJson());
......
src/MissionManager/MissionController.h
View file @ 34b4f681
......@@ -21,6 +21,7 @@
class CoordinateVector;
class VisualMissionItem;
class MissionItem;
Q_DECLARE_LOGGING_CATEGORY(MissionControllerLog)
......@@ -174,6 +175,7 @@ private:
void _setMissionCruiseTime(double missionCruiseTime);
void _setMissionMaxTelemetry(double missionMaxTelemetry);
static void _scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle);
static bool _convertToMissionItems(QmlObjectListModel* visualMissionItems, QList<MissionItem*>& rgMissionItems, QObject* missionItemParent);
// Overrides from PlanElementController
void _activeVehicleBeingRemoved(void) final;
......
src/MissionManager/MissionManager.cc
View file @ 34b4f681
......@@ -496,7 +496,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message, boo
}
MissionItem* item = _missionItems[missionRequest.seq];
qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:" << missionRequest.seq << item->command();
qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:command" << missionRequest.seq << item->command();
mavlink_message_t messageOut;
if (missionItemInt) {
......
src/MissionManager/MissionSettingsComplexItem.cc
View file @ 34b4f681
......@@ -15,6 +15,7 @@
#include "SimpleMissionItem.h"
#include "SettingsManager.h"
#include "AppSettings.h"
#include "MissionCommandUIInfo.h"
#include <QPolygonF>
......@@ -180,7 +181,7 @@ void MissionSettingsComplexItem::appendMissionItems(QList<MissionItem*>& items,
items.append(item);
if (_specifyMissionFlightSpeed) {
qDebug() << _missionFlightSpeedFact.rawValue().toDouble();
qCDebug(MissionSettingsComplexItemLog) << "Appending MAV_CMD_DO_CHANGE_SPEED";
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_DO_CHANGE_SPEED,
MAV_FRAME_MISSION,
......@@ -198,6 +199,85 @@ void MissionSettingsComplexItem::appendMissionItems(QList<MissionItem*>& items,
_cameraSection.appendMissionItems(items, missionItemParent, seqNum);
}
bool MissionSettingsComplexItem::addMissionEndAction(QList<MissionItem*>& items, int seqNum, QObject* missionItemParent)
{
MissionItem* item = NULL;
// IMPORTANT NOTE: If anything changes here you must also change MissionSettingsComplexItem::scanForMissionSettings
// Find last waypoint coordinate information so we have a lat/lon/alt to use
QGeoCoordinate lastWaypointCoord;
MAV_FRAME lastWaypointFrame;
bool found = false;
for (int i=items.count()-1; i>0; i--) {
MissionItem* missionItem = items[i];
const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, (MAV_CMD)missionItem->command());
if (uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate()) {
lastWaypointCoord = missionItem->coordinate();
lastWaypointFrame = missionItem->frame();
found = true;
break;
}
}
if (!found) {
return false;
}
switch(_missionEndActionFact.rawValue().toInt()) {
case MissionEndLoiter:
qCDebug(MissionSettingsComplexItemLog) << "Appending end action Loiter seqNum" << seqNum;
item = new MissionItem(seqNum,
MAV_CMD_NAV_LOITER_UNLIM,
lastWaypointFrame,
0, 0, // param 1-2 unused
0, // use default loiter radius
0, // param 4 not used
lastWaypointCoord.latitude(),
lastWaypointCoord.longitude(),
lastWaypointCoord.altitude(),
true, // autoContinue
false, // isCurrentItem
missionItemParent);
break;
case MissionEndLand:
qCDebug(MissionSettingsComplexItemLog) << "Appending end action Land seqNum" << seqNum;
item = new MissionItem(seqNum,
MAV_CMD_NAV_LAND,
MAV_FRAME_GLOBAL_RELATIVE_ALT,
0, // abort Altitude
0, 0, // not used
0, // yaw
lastWaypointCoord.latitude(),
lastWaypointCoord.longitude(),
0, // altitude
true, // autoContinue
false, // isCurrentItem
missionItemParent);
break;
case MissionEndRTL:
qCDebug(MissionSettingsComplexItemLog) << "Appending end action RTL seqNum" << seqNum;
item = new MissionItem(seqNum,
MAV_CMD_NAV_RETURN_TO_LAUNCH,
MAV_FRAME_MISSION,
0, 0, 0, 0, 0, 0, 0, // param 1-7 not used
true, // autoContinue
false, // isCurrentItem
missionItemParent);
break;
default:
break;
}
if (item) {
items.append(item);
return true;
} else {
return false;
}
}
bool MissionSettingsComplexItem::scanForMissionSettings(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle)
{
bool foundSpeed = false;
......@@ -245,6 +325,7 @@ bool MissionSettingsComplexItem::scanForMissionSettings(QmlObjectListModel* visu
settingsItem->setSpecifyMissionFlightSpeed(true);
settingsItem->missionFlightSpeed()->setRawValue(missionItem.param2());
visualItems->removeAt(scanIndex)->deleteLater();
qCDebug(MissionSettingsComplexItemLog) << "Scan: Found MAV_CMD_DO_CHANGE_SPEED";
continue;
}
stopLooking = true;
......@@ -254,6 +335,7 @@ bool MissionSettingsComplexItem::scanForMissionSettings(QmlObjectListModel* visu
if (!foundCameraSection) {
if (settingsItem->_cameraSection.scanForCameraSection(visualItems, scanIndex)) {
foundCameraSection = true;
qCDebug(MissionSettingsComplexItemLog) << "Scan: Found Camera Section";
continue;
}
}
......@@ -262,6 +344,43 @@ bool MissionSettingsComplexItem::scanForMissionSettings(QmlObjectListModel* visu
}
}
// Look at the end of the mission for end actions
int lastIndex = visualItems->count() - 1;
SimpleMissionItem* item = visualItems->value<SimpleMissionItem*>(lastIndex);
if (item) {
MissionItem& missionItem = item->missionItem();
switch ((MAV_CMD)item->command()) {
case MAV_CMD_NAV_LOITER_UNLIM:
if (missionItem.param1() == 0 && missionItem.param2() == 0 && missionItem.param3() == 0 && missionItem.param4() == 0) {
qCDebug(MissionSettingsComplexItemLog) << "Scan: Found end action Loiter";
settingsItem->missionEndAction()->setRawValue(MissionEndLoiter);
visualItems->removeAt(lastIndex)->deleteLater();
}
break;
case MAV_CMD_NAV_LAND:
if (missionItem.param1() == 0 && missionItem.param2() == 0 && missionItem.param3() == 0 && missionItem.param4() == 0 && missionItem.param7() == 0) {
qCDebug(MissionSettingsComplexItemLog) << "Scan: Found end action Land";
settingsItem->missionEndAction()->setRawValue(MissionEndLand);
visualItems->removeAt(lastIndex)->deleteLater();
}
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (missionItem.param1() == 0 && missionItem.param2() == 0 && missionItem.param3() == 0 && missionItem.param4() == 0 && missionItem.param5() == 0 && missionItem.param6() == 0 && missionItem.param7() == 0) {
qCDebug(MissionSettingsComplexItemLog) << "Scan: Found end action RTL";
settingsItem->missionEndAction()->setRawValue(MissionEndRTL);
visualItems->removeAt(lastIndex)->deleteLater();
}
break;
default:
break;
}
}
return foundSpeed || foundCameraSection;
}
......
src/MissionManager/MissionSettingsComplexItem.h
View file @ 34b4f681
......@@ -52,7 +52,14 @@ public:
QObject* cameraSection(void) { return &_cameraSection; }
/// Scans the loaded items for settings items
static bool scanForMissionSettings(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicl);
static bool scanForMissionSettings(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle);
/// Adds the optional mission end action to the list
/// @param items Mission items list to append to
/// @param seqNum Sequence number for new item
/// @param missionItemParent Parent for newly allocated MissionItems
/// @return true: Mission end action was added
bool addMissionEndAction(QList<MissionItem*>& items, int seqNum, QObject* missionItemParent);
// Overrides from ComplexMissionItem
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment