Commit 682ea675 authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #4961 from DonLakeFlyer/ResumeMission

Better resume mission generation
parents 26a34be2 a45768c6
......@@ -90,8 +90,9 @@ QGCView {
}
MissionController {
id: flyMissionController
Component.onCompleted: start(false /* editMode */)
id: flyMissionController
Component.onCompleted: start(false /* editMode */)
onResumeMissionReady: guidedActionsController.confirmAction(guidedActionsController.actionResumeMissionReady)
}
MessageDialog {
......
......@@ -87,11 +87,6 @@ Item {
onValueChanged: _setInstrumentWidget()
}
Connections {
target: missionController
onResumeMissionReady: _guidedModeBar.confirmAction(_guidedModeBar.confirmResumeMissionReady)
}
Component.onCompleted: {
_setInstrumentWidget()
}
......
......@@ -51,7 +51,7 @@ Item {
readonly property string emergencyStopMessage: qsTr("WARNING: This still stop all motors. If vehicle is currently in air it will crash.")
readonly property string takeoffMessage: qsTr("Takeoff from ground and hold position.")
readonly property string startMissionMessage: qsTr("Start the mission which is currently displayed above. If the vehicle is on the ground it will takeoff.")
property string resumeMissionMessage: qsTr("Resume the mission which is displayed above. This will re-generate the mission from waypoint %1, takeoff and continue the mission.").arg(_resumeMissionItem)
property string resumeMissionMessage: qsTr("Resume the mission which is displayed above. This will re-generate the mission from waypoint %1, takeoff and continue the mission.").arg(_resumeMissionIndex)
readonly property string resumeMissionReadyMessage: qsTr("Review the modified mission above. Confirm if you want to takeoff and begin mission.")
readonly property string landMessage: qsTr("Land the vehicle at the current position.")
readonly property string rtlMessage: qsTr("Return to the home position of the vehicle.")
......@@ -84,7 +84,7 @@ Item {
property bool showTakeoff: _activeVehicle && _activeVehicle.guidedModeSupported && !_activeVehicle.flying && !_activeVehicle.fixedWing
property bool showLand: _activeVehicle && _activeVehicle.guidedModeSupported && _activeVehicle.armed && !_activeVehicle.fixedWing && !_vehicleInLandMode
property bool showStartMission: _activeVehicle && _missionAvailable && !_missionActive
property bool showResumeMission: _activeVehicle && !_activeVehicle.flying && _missionAvailable && _resumeMissionItem > 1
property bool showResumeMission: _activeVehicle && !_activeVehicle.flying && _missionAvailable && _resumeMissionIndex > 0
property bool showPause: _activeVehicle && _activeVehicle.armed && _activeVehicle.pauseVehicleSupported && _activeVehicle.flying && !_vehiclePaused
property bool showChangeAlt: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed && !_missionActive
property bool showOrbit: !_hideOrbit && _activeVehicle && _activeVehicle.flying && _activeVehicle.orbitModeSupported && _activeVehicle.armed && !_missionActive
......@@ -97,7 +97,7 @@ Item {
property bool _vehiclePaused: _activeVehicle ? _activeVehicle.flightMode === _activeVehicle.pauseFlightMode : false
property bool _vehicleInRTLMode: _activeVehicle ? _activeVehicle.flightMode === _activeVehicle.rtlFlightMode : false
property bool _vehicleInLandMode: _activeVehicle ? _activeVehicle.flightMode === _activeVehicle.landFlightMode : false
property int _resumeMissionItem: missionController.resumeMissionItem
property int _resumeMissionIndex: missionController.resumeMissionIndex
property bool _hideEmergenyStop: !QGroundControl.corePlugin.options.guidedBarShowEmergencyStop
property bool _hideOrbit: !QGroundControl.corePlugin.options.guidedBarShowOrbit
property var _actionData
......@@ -174,6 +174,9 @@ Item {
title = pauseTitle
message = pauseMessage
break;
default:
console.warn("Unknown actionCode", actionCode)
return
}
showConfirmAction(title, message, actionCode, actionData)
}
......@@ -191,7 +194,7 @@ Item {
_activeVehicle.guidedModeTakeoff()
break
case actionResumeMission:
missionController.resumeMission(missionController.resumeMissionItem)
missionController.resumeMission(missionController.resumeMissionIndex)
break
case actionResumeMissionReady:
_activeVehicle.startMission()
......
......@@ -988,7 +988,18 @@
"decimalPlaces": 0
}
},
{ "id": 2001, "rawName": "MAV_CMD_IMAGE_STOP_CAPTURE", "friendlyName": "Stop image capture" },
{
"id": 2001,
"rawName": "MAV_CMD_IMAGE_STOP_CAPTURE",
"friendlyName": "Stop image capture",
"description": "Stop taking photos.",
"category": "Camera",
"param1": {
"label": "Camera id",
"default": 0,
"decimalPlaces": 0
}
},
{ "id": 2003, "rawName": "MAV_CMD_DO_TRIGGER_CONTROL", "friendlyName": "Trigger control" },
{
"id": 2500,
......
......@@ -684,6 +684,11 @@ void MissionController::loadFromFile(const QString& filename)
MissionController::_scanForAdditionalSettings(_visualItems, _activeVehicle);
_initAllVisualItems();
if (!_activeVehicle->isOfflineEditingVehicle()) {
// Needs a sync to vehicle
setDirty(true);
}
}
bool MissionController::loadItemsFromFile(Vehicle* vehicle, const QString& filename, QmlObjectListModel** visualItems)
......@@ -1232,7 +1237,9 @@ void MissionController::_initAllVisualItems(void)
qWarning() << "First item not MissionSettingsItem";
return;
}
_settingsItem->setIsCurrentItem(true);
if (_editMode) {
_settingsItem->setIsCurrentItem(true);
}
if (!_editMode && _activeVehicle) {
_settingsItem->setCoordinate(_activeVehicle->homePosition());
......@@ -1320,8 +1327,8 @@ void MissionController::_activeVehicleBeingRemoved(void)
disconnect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle);
disconnect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged);
disconnect(missionManager, &MissionManager::currentItemChanged, this, &MissionController::_currentMissionItemChanged);
disconnect(missionManager, &MissionManager::lastCurrentItemChanged, this, &MissionController::resumeMissionItemChanged);
disconnect(missionManager, &MissionManager::currentIndexChanged, this, &MissionController::_currentMissionIndexChanged);
disconnect(missionManager, &MissionManager::lastCurrentIndexChanged, this, &MissionController::resumeMissionIndexChanged);
disconnect(missionManager, &MissionManager::resumeMissionReady, this, &MissionController::resumeMissionReady);
disconnect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged);
......@@ -1340,8 +1347,8 @@ void MissionController::_activeVehicleSet(void)
connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle);
connect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged);
connect(missionManager, &MissionManager::currentItemChanged, this, &MissionController::_currentMissionItemChanged);
connect(missionManager, &MissionManager::lastCurrentItemChanged, this, &MissionController::resumeMissionItemChanged);
connect(missionManager, &MissionManager::currentIndexChanged, this, &MissionController::_currentMissionIndexChanged);
connect(missionManager, &MissionManager::lastCurrentIndexChanged, this, &MissionController::resumeMissionIndexChanged);
connect(missionManager, &MissionManager::resumeMissionReady, this, &MissionController::resumeMissionReady);
connect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged);
connect(_activeVehicle, &Vehicle::defaultCruiseSpeedChanged, this, &MissionController::_recalcMissionFlightStatus);
......@@ -1357,7 +1364,7 @@ void MissionController::_activeVehicleSet(void)
_activeVehicleHomePositionChanged(_activeVehicle->homePosition());
emit complexMissionItemNamesChanged();
emit resumeMissionItemChanged();
emit resumeMissionIndexChanged();
}
void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition)
......@@ -1468,27 +1475,26 @@ void MissionController::_addMissionSettings(Vehicle* vehicle, QmlObjectListModel
}
}
int MissionController::resumeMissionItem(void) const
int MissionController::resumeMissionIndex(void) const
{
int resumeIndex = -1;
int resumeIndex = 0;
if (!_editMode) {
int firstTrueItemIndex = _activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() ? 1 : 0;
resumeIndex = _activeVehicle->missionManager()->lastCurrentItem();
if (resumeIndex > firstTrueItemIndex) {
if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
resumeIndex++;
}
resumeIndex = _activeVehicle->missionManager()->lastCurrentIndex() + (_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() ? 0 : 1);
if (resumeIndex > 1) {
// Resume at the item previous to the item we were heading towards
resumeIndex--;
} else {
resumeIndex = 0;
}
}
qDebug() << "resumeIndex" << resumeIndex;
return resumeIndex;
}
void MissionController::_currentMissionItemChanged(int sequenceNumber)
void MissionController::_currentMissionIndexChanged(int sequenceNumber)
{
if (!_editMode) {
if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
......
......@@ -65,7 +65,7 @@ public:
Q_PROPERTY(QStringList complexMissionItemNames READ complexMissionItemNames NOTIFY complexMissionItemNamesChanged)
Q_PROPERTY(QGeoCoordinate plannedHomePosition READ plannedHomePosition NOTIFY plannedHomePositionChanged)
Q_PROPERTY(int resumeMissionItem READ resumeMissionItem NOTIFY resumeMissionItemChanged)
Q_PROPERTY(int resumeMissionIndex READ resumeMissionIndex NOTIFY resumeMissionIndexChanged)
Q_PROPERTY(double missionDistance READ missionDistance NOTIFY missionDistanceChanged)
Q_PROPERTY(double missionTime READ missionTime NOTIFY missionTimeChanged)
......@@ -131,7 +131,7 @@ public:
QGeoCoordinate plannedHomePosition (void) const;
/// Returns the item index two which a mission should be resumed. -1 indicates resume mission not available.
int resumeMissionItem(void) const;
int resumeMissionIndex(void) const;
double missionDistance (void) const { return _missionFlightStatus.totalDistance; }
double missionTime (void) const { return _missionFlightStatus.totalTime; }
......@@ -156,7 +156,7 @@ signals:
void missionCruiseTimeChanged(void);
void missionMaxTelemetryChanged(double missionMaxTelemetry);
void complexMissionItemNamesChanged(void);
void resumeMissionItemChanged(void);
void resumeMissionIndexChanged(void);
void resumeMissionReady(void);
void batteryChangePointChanged(int batteryChangePoint);
void batteriesRequiredChanged(int batteriesRequired);
......@@ -167,7 +167,7 @@ private slots:
void _itemCommandChanged(void);
void _activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition);
void _inProgressChanged(bool inProgress);
void _currentMissionItemChanged(int sequenceNumber);
void _currentMissionIndexChanged(int sequenceNumber);
void _recalcWaypointLines(void);
void _recalcMissionFlightStatus(void);
void _updateContainsItems(void);
......
......@@ -27,8 +27,8 @@ MissionManager::MissionManager(Vehicle* vehicle)
, _readTransactionInProgress(false)
, _writeTransactionInProgress(false)
, _resumeMission(false)
, _currentMissionItem(-1)
, _lastCurrentItem(-1)
, _currentMissionIndex(-1)
, _lastCurrentIndex(-1)
{
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived);
......@@ -60,10 +60,10 @@ void MissionManager::_writeMissionItemsWorker(void)
emit inProgressChanged(true);
_writeMissionCount();
_currentMissionItem = -1;
_lastCurrentItem = -1;
emit currentItemChanged(-1);
emit lastCurrentItemChanged(-1);
_currentMissionIndex = -1;
_lastCurrentIndex = -1;
emit currentIndexChanged(-1);
emit lastCurrentIndexChanged(-1);
}
......@@ -412,8 +412,8 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m
mavlink_msg_mission_item_int_decode(&message, &missionItem);
command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
param2 = missionItem.param2;
param3 = missionItem.param3;
param4 = missionItem.param4;
......@@ -428,8 +428,8 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m
mavlink_msg_mission_item_decode(&message, &missionItem);
command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
param2 = missionItem.param2;
param3 = missionItem.param3;
param4 = missionItem.param4;
......@@ -448,7 +448,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}
qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << seq << command;
qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current" << seq << command << isCurrentItem;
if (_itemIndicesToRead.contains(seq)) {
_itemIndicesToRead.removeOne(seq);
......@@ -787,15 +787,16 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
mavlink_msg_mission_current_decode(&message, &missionCurrent);
if (missionCurrent.seq != _currentMissionItem) {
qCDebug(MissionManagerLog) << "_handleMissionCurrent seq:" << missionCurrent.seq;
_currentMissionItem = missionCurrent.seq;
emit currentItemChanged(_currentMissionItem);
if (missionCurrent.seq != _currentMissionIndex) {
qCDebug(MissionManagerLog) << "_handleMissionCurrent currentIndex:" << missionCurrent.seq;
_currentMissionIndex = missionCurrent.seq;
emit currentIndexChanged(_currentMissionIndex);
}
if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionItem != _lastCurrentItem) {
_lastCurrentItem = _currentMissionItem;
emit lastCurrentItemChanged(_lastCurrentItem);
if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionIndex != _lastCurrentIndex) {
qCDebug(MissionManagerLog) << "_handleMissionCurrent lastCurrentIndex:" << _currentMissionIndex;
_lastCurrentIndex = _currentMissionIndex;
emit lastCurrentIndexChanged(_lastCurrentIndex);
}
}
......@@ -817,6 +818,14 @@ void MissionManager::generateResumeMission(int resumeIndex)
return;
}
for (int i=0; i<_missionItems.count(); i++) {
MissionItem* item = _missionItems[i];
if (item->command() == MAV_CMD_DO_JUMP) {
qgcApp()->showMessage(tr("Unable to generate resume mission due to MAV_CMD_DO_JUMP command."));
return;
}
}
int seqNum = 0;
QList<MissionItem*> resumeMission;
......@@ -839,9 +848,13 @@ void MissionManager::generateResumeMission(int resumeIndex)
bool addHomePosition = _vehicle->firmwarePlugin()->sendHomePositionToVehicle();
int setCurrentIndex = addHomePosition ? 1 : 0;
int resumeCommandCount = 0;
for (int i=0; i<_missionItems.count(); i++) {
MissionItem* oldItem = _missionItems[i];
if ((i == 0 && addHomePosition) || i >= resumeIndex || includedResumeCommands.contains(oldItem->command())) {
if (i < resumeIndex) {
resumeCommandCount++;
}
MissionItem* newItem = new MissionItem(*oldItem, this);
newItem->setIsCurrentItem( i == setCurrentIndex);
newItem->setSequenceNumber(seqNum++);
......@@ -849,7 +862,91 @@ void MissionManager::generateResumeMission(int resumeIndex)
}
}
// Handle DO_JUMP seq num update
// De-dup and remove no-ops from the commands which were added to the front of the mission
bool foundROI = false;
bool foundCamTrigDist = false;
QList<int> imageStartCameraIds;
QList<int> imageStopCameraIds;
QList<int> videoStartCameraIds;
QList<int> videoStopCameraIds;
while (resumeIndex >= 0) {
MissionItem* resumeItem = resumeMission[resumeIndex];
switch (resumeItem->command()) {
case MAV_CMD_DO_SET_ROI:
// Only keep the last one
if (foundROI) {
resumeMission.removeAt(resumeIndex);
}
foundROI = true;
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
// Only keep the last one
if (foundCamTrigDist) {
resumeMission.removeAt(resumeIndex);
}
foundCamTrigDist = true;
break;
case MAV_CMD_IMAGE_START_CAPTURE:
{
// FIXME: Handle single image capture
int cameraId = resumeItem->param6();
if (resumeItem->param1() == 0) {
// This is an individual image capture command, remove it
resumeMission.removeAt(resumeIndex);
break;
}
// If we already found an image stop, then all image start/stop commands are useless
// De-dup repeated image start commands
// Otherwise keep only the last image start
if (imageStopCameraIds.contains(cameraId) || imageStartCameraIds.contains(cameraId)) {
resumeMission.removeAt(resumeIndex);
}
if (!imageStopCameraIds.contains(cameraId)) {
imageStopCameraIds.append(cameraId);
}
}
break;
case MAV_CMD_IMAGE_STOP_CAPTURE:
{
int cameraId = resumeItem->param1();
// Image stop only matters to kill all previous image starts
if (!imageStopCameraIds.contains(cameraId)) {
imageStopCameraIds.append(cameraId);
}
resumeMission.removeAt(resumeIndex);
}
break;
case MAV_CMD_VIDEO_START_CAPTURE:
{
int cameraId = resumeItem->param1();
// If we already found an video stop, then all video start/stop commands are useless
// De-dup repeated video start commands
// Otherwise keep only the last video start
if (videoStopCameraIds.contains(cameraId) || videoStopCameraIds.contains(cameraId)) {
resumeMission.removeAt(resumeIndex);
}
if (!videoStopCameraIds.contains(cameraId)) {
videoStopCameraIds.append(cameraId);
}
}
break;
case MAV_CMD_VIDEO_STOP_CAPTURE:
{
int cameraId = resumeItem->param1();
// Video stop only matters to kill all previous video starts
if (!videoStopCameraIds.contains(cameraId)) {
videoStopCameraIds.append(cameraId);
}
resumeMission.removeAt(resumeIndex);
}
break;
default:
break;
}
resumeIndex--;
}
// Send to vehicle
_clearAndDeleteMissionItems();
......
......@@ -38,10 +38,10 @@ public:
const QList<MissionItem*>& missionItems(void) { return _missionItems; }
/// Current mission item as reported by MISSION_CURRENT
int currentItem(void) const { return _currentMissionItem; }
int currentIndex(void) const { return _currentMissionIndex; }
/// Last current mission item reported while in Mission flight mode
int lastCurrentItem(void) const { return _lastCurrentItem; }
int lastCurrentIndex(void) const { return _lastCurrentIndex; }
void requestMissionItems(void);
......@@ -81,8 +81,8 @@ signals:
void newMissionItemsAvailable(bool removeAllRequested);
void inProgressChanged(bool inProgress);
void error(int errorCode, const QString& errorMsg);
void currentItemChanged(int currentItem);
void lastCurrentItemChanged(int lastCurrentMissionItem);
void currentIndexChanged(int currentIndex);
void lastCurrentIndexChanged(int lastCurrentIndex);
void resumeMissionReady(void);
private slots:
......@@ -134,8 +134,8 @@ private:
QMutex _dataMutex;
QList<MissionItem*> _missionItems;
int _currentMissionItem;
int _lastCurrentItem;
int _currentMissionIndex;
int _lastCurrentIndex;
};
#endif
......@@ -109,6 +109,7 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, const MissionItem& missio
_editorQml = QStringLiteral("qrc:/qml/SimpleItemEditor.qml");
_altitudeRelativeToHomeFact.setRawValue(true);
_isCurrentItem = missionItem.isCurrentItem();
_setupMetaData();
_connectSignals();
......
......@@ -98,7 +98,7 @@ Rectangle {
MenuItem {
text: qsTr("Insert waypoint")
onTriggered: insert()
onTriggered: insertWaypoint()
}
MenuItem {
......@@ -126,17 +126,18 @@ Rectangle {
MenuItem {
text: qsTr("Change command...")
onTriggered: commandPicker.clicked()
visible: !_waypointsOnlyMode
}
MenuSeparator {
visible: missionItem.isSimpleItem
visible: missionItem.isSimpleItem && !_waypointsOnlyMode
}
MenuItem {
text: qsTr("Show all values")
checkable: true
checked: missionItem.isSimpleItem ? missionItem.rawEdit : false
visible: missionItem.isSimpleItem
visible: missionItem.isSimpleItem && !_waypointsOnlyMode
onTriggered: {
if (missionItem.rawEdit) {
......@@ -152,38 +153,6 @@ Rectangle {
}
}
}
Menu {
id: waypointsOnlyMenu
MenuItem {
text: qsTr("Insert waypoint")
onTriggered: insertWaypoint()
}
MenuItem {
text: qsTr("Delete")
onTriggered: remove()
}
Menu {
id: waypointsOnlyPatternMenu
title: qsTr("Insert pattern")
Instantiator {
model: missionController.complexMissionItemNames
onObjectAdded: waypointsOnlyPatternMenu.insertItem(index, object)
onObjectRemoved: waypointsOnlyPatternMenu.removeItem(object)
MenuItem {
text: modelData
onTriggered: insertComplexItem(modelData)
}
}
}
}
}
QGCButton {
......
......@@ -83,13 +83,6 @@ QGCView {
property bool _firstRallyLoadComplete: false
property bool _firstLoadComplete: false
function checkFirstLoadComplete() {
if (!_firstLoadComplete && _firstMissionLoadComplete && _firstRallyLoadComplete && _firstFenceLoadComplete) {
_firstLoadComplete = true
mapFitFunctions.fitMapViewportToAllItems()
}
}
MapFitFunctions {
id: mapFitFunctions
map: editorMap
......@@ -182,8 +175,6 @@ QGCView {
mapFitFunctions.fitMapViewportToMissionItems()
}
setCurrentItem(0)
_firstMissionLoadComplete = true
checkFirstLoadComplete()
}
}
......@@ -212,18 +203,6 @@ QGCView {
function fitViewportToItems() {
mapFitFunctions.fitMapViewportToFenceItems()
}
onLoadComplete: {
_firstFenceLoadComplete = true
switch (_syncDropDownController) {
case geoFenceController:
mapFitFunctions.fitMapViewportToFenceItems()
break
case missionController:
checkFirstLoadComplete()
break
}
}
}
RallyPointController {
......@@ -259,18 +238,6 @@ QGCView {
function fitViewportToItems() {
mapFitFunctions.fitMapViewportToRallyItems()
}
onLoadComplete: {
_firstRallyLoadComplete = true
switch (_syncDropDownController) {
case rallyPointController:
mapFitFunctions.fitMapViewportToRallyItems()
break
case missionController:
checkFirstLoadComplete()
break
}
}
}
QGCPalette { id: qgcPal; colorGroupEnabled: enabled }
......
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