Skip to content

Q
qgroundcontrol
Commit 6454b0f0 authored by DonLakeFlyer's avatar DonLakeFlyer
Browse files
Options

Add resume mission support

parent fe004f8b
Hide whitespace changes
Inline Side-by-side
Showing with 299 additions and 155 deletions
src/FlightDisplay/FlightDisplayViewWidgets.qml
View file @ 6454b0f0
......@@ -38,7 +38,7 @@ Item {
// Guided bar properties
property bool _missionAvailable: missionController.containsItems
property bool _missionActive: _activeVehicle ? _activeVehicle.flightMode === _activeVehicle.missionFlightMode : false
property bool _missionInProgress: missionController.missionInProgress
property int _resumeMissionItem: missionController.resumeMissionItem
property bool _showEmergenyStop: QGroundControl.corePlugin.options.guidedBarShowEmergencyStop
property bool _showOrbit: QGroundControl.corePlugin.options.guidedBarShowOrbit
......@@ -92,6 +92,11 @@ Item {
onValueChanged: _setInstrumentWidget()
}
Connections {
target: missionController
onResumeMissionReady: _guidedModeBar.confirmAction(_guidedModeBar.confirmResumeMissionReady)
}
Component.onCompleted: {
_setInstrumentWidget()
}
......@@ -219,19 +224,20 @@ Item {
}
}
readonly property int confirmHome: 1
readonly property int confirmLand: 2
readonly property int confirmTakeoff: 3
readonly property int confirmArm: 4
readonly property int confirmDisarm: 5
readonly property int confirmEmergencyStop: 6
readonly property int confirmChangeAlt: 7
readonly property int confirmGoTo: 8
readonly property int confirmRetask: 9
readonly property int confirmOrbit: 10
readonly property int confirmAbort: 11
readonly property int confirmStartMission: 12
readonly property int confirmResumeMission: 13
readonly property int confirmHome: 1
readonly property int confirmLand: 2
readonly property int confirmTakeoff: 3
readonly property int confirmArm: 4
readonly property int confirmDisarm: 5
readonly property int confirmEmergencyStop: 6
readonly property int confirmChangeAlt: 7
readonly property int confirmGoTo: 8
readonly property int confirmRetask: 9
readonly property int confirmOrbit: 10
readonly property int confirmAbort: 11
readonly property int confirmStartMission: 12
readonly property int confirmResumeMission: 13
readonly property int confirmResumeMissionReady: 14
property int confirmActionCode
property real _showMargin: _margins
......@@ -250,6 +256,11 @@ Item {
_activeVehicle.guidedModeTakeoff()
break;
case confirmResumeMission:
missionController.resumeMission(missionController.resumeMissionItem)
break;
case confirmResumeMissionReady:
_activeVehicle.startMission()
break;
case confirmStartMission:
_activeVehicle.startMission()
break;
......@@ -318,6 +329,9 @@ Item {
case confirmResumeMission:
guidedModeConfirm.confirmText = qsTr("resume mission")
break;
case confirmResumeMissionReady:
guidedModeConfirm.confirmText = qsTr("resume modified mission after review")
break;
case confirmLand:
guidedModeConfirm.confirmText = qsTr("land")
break;
......@@ -400,15 +414,15 @@ Item {
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: qsTr("Start Mission")
text: _resumeMissionItem !== -1 ? qsTr("Restart Mission") : qsTr("Start Mission")
visible: _activeVehicle && !_activeVehicle.flying && _missionAvailable
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmStartMission)
}
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: qsTr("Resume Mission")
visible: _activeVehicle && _activeVehicle.guidedModeSupported && !_activeVehicle.flying && _missionAvailable && _missionInProgress
text: qsTr("Resume Mission (%1)").arg(_resumeMissionItem)
visible: _activeVehicle && !_activeVehicle.flying && _missionAvailable && _resumeMissionItem !== -1
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmResumeMission)
}
......@@ -422,14 +436,14 @@ Item {
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: qsTr("Change Altitude")
visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed
visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed && !_missionActive
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmChangeAlt)
}
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: qsTr("Orbit")
visible: _showOrbit && _activeVehicle && _activeVehicle.flying && _activeVehicle.orbitModeSupported && _activeVehicle.armed
visible: _showOrbit && _activeVehicle && _activeVehicle.flying && _activeVehicle.orbitModeSupported && _activeVehicle.armed && !_missionActive
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmOrbit)
}
......
src/MissionManager/MissionController.cc
View file @ 6454b0f0
......@@ -53,7 +53,6 @@ MissionController::MissionController(QObject *parent)
, _settingsItem(NULL)
, _firstItemsFromVehicle(false)
, _missionItemsRequested(false)
, _queuedSend(false)
, _surveyMissionItemName(tr("Survey"))
, _fwLandingMissionItemName(tr("Fixed Wing Landing"))
, _appSettings(qgcApp()->toolbox()->settingsManager()->appSettings())
......@@ -647,7 +646,6 @@ void MissionController::loadFromFile(const QString& filename)
MissionController::_scanForAdditionalSettings(_visualItems, _activeVehicle);
_initAllVisualItems();
sendToVehicle();
}
bool MissionController::loadItemsFromFile(Vehicle* vehicle, const QString& filename, QmlObjectListModel** visualItems)
......@@ -1280,6 +1278,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::resumeMissionReady, this, &MissionController::resumeMissionReady);
disconnect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged);
// We always remove all items on vehicle change. This leaves a user model hole:
......@@ -1298,6 +1298,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::resumeMissionReady, this, &MissionController::resumeMissionReady);
connect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged);
connect(_activeVehicle, &Vehicle::defaultCruiseSpeedChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(_activeVehicle, &Vehicle::defaultHoverSpeedChanged, this, &MissionController::_recalcMissionFlightStatus);
......@@ -1312,6 +1314,7 @@ void MissionController::_activeVehicleSet(void)
_activeVehicleHomePositionChanged(_activeVehicle->homePosition());
emit complexMissionItemNamesChanged();
emit resumeMissionItemChanged();
}
void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition)
......@@ -1478,11 +1481,29 @@ void MissionController::_addMissionSettings(Vehicle* vehicle, QmlObjectListModel
}
}
void MissionController::_currentMissionItemChanged(int sequenceNumber)
int MissionController::resumeMissionItem(void) const
{
int resumeIndex = -1;
if (!_editMode) {
bool prevMissionInProgress = missionInProgress();
int firstTrueItemIndex = _activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() ? 1 : 0;
resumeIndex = _activeVehicle->missionManager()->lastCurrentItem();
if (resumeIndex > firstTrueItemIndex) {
if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
resumeIndex++;
}
// Resume at the item previous to the item we were heading towards
resumeIndex--;
}
}
return resumeIndex;
}
void MissionController::_currentMissionItemChanged(int sequenceNumber)
{
if (!_editMode) {
if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
sequenceNumber++;
}
......@@ -1491,10 +1512,6 @@ void MissionController::_currentMissionItemChanged(int sequenceNumber)
VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
item->setIsCurrentItem(item->sequenceNumber() == sequenceNumber);
}
if (prevMissionInProgress != missionInProgress()) {
emit missionInProgressChanged();
}
}
}
......@@ -1576,11 +1593,6 @@ QStringList MissionController::complexMissionItemNames(void) const
return complexItems;
}
bool MissionController::missionInProgress(void) const
{
return _visualItems && _visualItems->count() > 1 && (!_visualItems->value<VisualMissionItem*>(0)->isCurrentItem() && !_visualItems->value<VisualMissionItem*>(1)->isCurrentItem());
}
void MissionController::_visualItemsDirtyChanged(bool dirty)
{
if (dirty) {
......@@ -1594,3 +1606,11 @@ void MissionController::_visualItemsDirtyChanged(bool dirty)
emit dirtyChanged(false);
}
}
void MissionController::resumeMission(int resumeIndex)
{
if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
resumeIndex--;
}
_activeVehicle->missionManager()->generateResumeMission(resumeIndex);
}
src/MissionManager/MissionController.h
View file @ 6454b0f0
......@@ -55,7 +55,7 @@ public:
Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines NOTIFY waypointLinesChanged)
Q_PROPERTY(QStringList complexMissionItemNames READ complexMissionItemNames NOTIFY complexMissionItemNamesChanged)
Q_PROPERTY(bool missionInProgress READ missionInProgress NOTIFY missionInProgressChanged) ///< true: Mission sequence is beyond first item
Q_PROPERTY(int resumeMissionItem READ resumeMissionItem NOTIFY resumeMissionItemChanged)
Q_PROPERTY(double missionDistance READ missionDistance NOTIFY missionDistanceChanged)
Q_PROPERTY(double missionTime READ missionTime NOTIFY missionTimeChanged)
......@@ -79,6 +79,8 @@ public:
/// @return Sequence number for new item
Q_INVOKABLE int insertComplexMissionItem(QString itemName, QGeoCoordinate mapCenterCoordinate, int i);
Q_INVOKABLE void resumeMission(int resumeIndex);
/// Loads the mission items from the specified file
/// @param[in] vehicle Vehicle we are loading items for
/// @param[in] filename File to load from
......@@ -110,7 +112,9 @@ public:
QmlObjectListModel* visualItems (void) { return _visualItems; }
QmlObjectListModel* waypointLines (void) { return &_waypointLines; }
QStringList complexMissionItemNames (void) const;
bool missionInProgress (void) const;
/// Returns the item index two which a mission should be resumed. -1 indicates resume mission not available.
int resumeMissionItem(void) const;
double missionDistance (void) const { return _missionFlightStatus.totalDistance; }
double missionTime (void) const { return _missionFlightStatus.totalTime; }
......@@ -132,7 +136,8 @@ signals:
void missionCruiseTimeChanged(void);
void missionMaxTelemetryChanged(double missionMaxTelemetry);
void complexMissionItemNamesChanged(void);
bool missionInProgressChanged(void);
void resumeMissionItemChanged(void);
void resumeMissionReady(void);
private slots:
void _newMissionItemsAvailableFromVehicle(bool removeAllRequested);
......@@ -188,7 +193,6 @@ private:
CoordVectHashTable _linesTable;
bool _firstItemsFromVehicle;
bool _missionItemsRequested;
bool _queuedSend;
MissionFlightStatus_t _missionFlightStatus;
QString _surveyMissionItemName;
QString _fwLandingMissionItemName;
......
src/MissionManager/MissionManager.cc
View file @ 6454b0f0
......@@ -26,7 +26,9 @@ MissionManager::MissionManager(Vehicle* vehicle)
, _expectedAck(AckNone)
, _readTransactionInProgress(false)
, _writeTransactionInProgress(false)
, _resumeMission(false)
, _currentMissionItem(-1)
, _lastCurrentItem(-1)
{
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived);
......@@ -42,6 +44,29 @@ MissionManager::~MissionManager()
}
void MissionManager::_writeMissionItemsWorker(void)
{
emit newMissionItemsAvailable(_missionItems.count() == 0);
qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count();
// Prime write list
for (int i=0; i<_missionItems.count(); i++) {
_itemIndicesToWrite << i;
}
_writeTransactionInProgress = true;
_retryCount = 0;
emit inProgressChanged(true);
_writeMissionCount();
_currentMissionItem = -1;
_lastCurrentItem = -1;
emit currentItemChanged(-1);
emit lastCurrentItemChanged(-1);
}
void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
{
if (_vehicle->isOfflineEditingVehicle()) {
......@@ -55,10 +80,10 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle();
_missionItems.clear();
_clearAndDeleteMissionItems();
int firstIndex = skipFirstItem ? 1 : 0;
for (int i=firstIndex; i<missionItems.count(); i++) {
MissionItem* item = new MissionItem(*missionItems[i]);
_missionItems.append(item);
......@@ -73,19 +98,8 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
}
}
}
emit newMissionItemsAvailable(missionItems.count() == 0);
qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count();
// Prime write list
for (int i=0; i<_missionItems.count(); i++) {
_itemIndicesToWrite << i;
}
_writeTransactionInProgress = true;
_retryCount = 0;
emit inProgressChanged(true);
_writeMissionCount();
_writeMissionItemsWorker();
}
/// This begins the write sequence with the vehicle. This may be called during a retry.
......@@ -398,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;
......@@ -414,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;
......@@ -470,7 +484,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m
void MissionManager::_clearMissionItems(void)
{
_itemIndicesToRead.clear();
_missionItems.clear();
_clearAndDeleteMissionItems();
}
void MissionManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt)
......@@ -572,45 +586,45 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
switch (savedExpectedAck) {
case AckNone:
// State machine is idle. Vehicle is confused.
_sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
break;
case AckMissionCount:
// MISSION_COUNT message expected
_sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
break;
case AckMissionItem:
// MISSION_ITEM expected
_sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
break;
case AckMissionRequest:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
if (_itemIndicesToWrite.count() == 0) {
qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete";
_finishTransaction(true);
} else {
_sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1. Vehicle only has partial list of mission items.").arg(_itemIndicesToWrite.count()));
_finishTransaction(false);
}
} else {
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
}
break;
case AckGuidedItem:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
qCDebug(MissionManagerLog) << "_handleMissionAck guided mode item accepted";
case AckNone:
// State machine is idle. Vehicle is confused.
_sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
break;
case AckMissionCount:
// MISSION_COUNT message expected
_sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
break;
case AckMissionItem:
// MISSION_ITEM expected
_sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
break;
case AckMissionRequest:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
if (_itemIndicesToWrite.count() == 0) {
qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete";
_finishTransaction(true);
} else {
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1. Vehicle only has partial list of mission items.").arg(_itemIndicesToWrite.count()));
_finishTransaction(false);
}
break;
} else {
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
}
break;
case AckGuidedItem:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
qCDebug(MissionManagerLog) << "_handleMissionAck guided mode item accepted";
_finishTransaction(true);
} else {
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
}
break;
}
}
......@@ -662,73 +676,73 @@ void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
QString MissionManager::_ackTypeToString(AckType_t ackType)
{
switch (ackType) {
case AckNone:
return QString("No Ack");
case AckMissionCount:
return QString("MISSION_COUNT");
case AckMissionItem:
return QString("MISSION_ITEM");
case AckMissionRequest:
return QString("MISSION_REQUEST");
case AckGuidedItem:
return QString("Guided Mode Item");
default:
qWarning(MissionManagerLog) << "Fell off end of switch statement";
return QString("QGC Internal Error");
}
case AckNone:
return QString("No Ack");
case AckMissionCount:
return QString("MISSION_COUNT");
case AckMissionItem:
return QString("MISSION_ITEM");
case AckMissionRequest:
return QString("MISSION_REQUEST");
case AckGuidedItem:
return QString("Guided Mode Item");
default:
qWarning(MissionManagerLog) << "Fell off end of switch statement";
return QString("QGC Internal Error");
}
}
QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result)
{
switch (result) {
case MAV_MISSION_ACCEPTED:
return QString("Mission accepted (MAV_MISSION_ACCEPTED)");
break;
case MAV_MISSION_ERROR:
return QString("Unspecified error (MAV_MISSION_ERROR)");
break;
case MAV_MISSION_UNSUPPORTED_FRAME:
return QString("Coordinate frame is not supported (MAV_MISSION_UNSUPPORTED_FRAME)");
break;
case MAV_MISSION_UNSUPPORTED:
return QString("Command is not supported (MAV_MISSION_UNSUPPORTED)");
break;
case MAV_MISSION_NO_SPACE:
return QString("Mission item exceeds storage space (MAV_MISSION_NO_SPACE)");
break;
case MAV_MISSION_INVALID:
return QString("One of the parameters has an invalid value (MAV_MISSION_INVALID)");
break;
case MAV_MISSION_INVALID_PARAM1:
return QString("Param1 has an invalid value (MAV_MISSION_INVALID_PARAM1)");
break;
case MAV_MISSION_INVALID_PARAM2:
return QString("Param2 has an invalid value (MAV_MISSION_INVALID_PARAM2)");
break;
case MAV_MISSION_INVALID_PARAM3:
return QString("param3 has an invalid value (MAV_MISSION_INVALID_PARAM3)");
break;
case MAV_MISSION_INVALID_PARAM4:
return QString("Param4 has an invalid value (MAV_MISSION_INVALID_PARAM4)");
break;
case MAV_MISSION_INVALID_PARAM5_X:
return QString("X/Param5 has an invalid value (MAV_MISSION_INVALID_PARAM5_X)");
break;
case MAV_MISSION_INVALID_PARAM6_Y:
return QString("Y/Param6 has an invalid value (MAV_MISSION_INVALID_PARAM6_Y)");
break;
case MAV_MISSION_INVALID_PARAM7:
return QString("Param7 has an invalid value (MAV_MISSION_INVALID_PARAM7)");
break;
case MAV_MISSION_INVALID_SEQUENCE:
return QString("Received mission item out of sequence (MAV_MISSION_INVALID_SEQUENCE)");
break;
case MAV_MISSION_DENIED:
return QString("Not accepting any mission commands (MAV_MISSION_DENIED)");
break;
default:
qWarning(MissionManagerLog) << "Fell off end of switch statement";
return QString("QGC Internal Error");
case MAV_MISSION_ACCEPTED:
return QString("Mission accepted (MAV_MISSION_ACCEPTED)");
break;
case MAV_MISSION_ERROR:
return QString("Unspecified error (MAV_MISSION_ERROR)");
break;
case MAV_MISSION_UNSUPPORTED_FRAME:
return QString("Coordinate frame is not supported (MAV_MISSION_UNSUPPORTED_FRAME)");
break;
case MAV_MISSION_UNSUPPORTED:
return QString("Command is not supported (MAV_MISSION_UNSUPPORTED)");
break;
case MAV_MISSION_NO_SPACE:
return QString("Mission item exceeds storage space (MAV_MISSION_NO_SPACE)");
break;
case MAV_MISSION_INVALID:
return QString("One of the parameters has an invalid value (MAV_MISSION_INVALID)");
break;
case MAV_MISSION_INVALID_PARAM1:
return QString("Param1 has an invalid value (MAV_MISSION_INVALID_PARAM1)");
break;
case MAV_MISSION_INVALID_PARAM2:
return QString("Param2 has an invalid value (MAV_MISSION_INVALID_PARAM2)");
break;
case MAV_MISSION_INVALID_PARAM3:
return QString("param3 has an invalid value (MAV_MISSION_INVALID_PARAM3)");
break;
case MAV_MISSION_INVALID_PARAM4:
return QString("Param4 has an invalid value (MAV_MISSION_INVALID_PARAM4)");
break;
case MAV_MISSION_INVALID_PARAM5_X:
return QString("X/Param5 has an invalid value (MAV_MISSION_INVALID_PARAM5_X)");
break;
case MAV_MISSION_INVALID_PARAM6_Y:
return QString("Y/Param6 has an invalid value (MAV_MISSION_INVALID_PARAM6_Y)");
break;
case MAV_MISSION_INVALID_PARAM7:
return QString("Param7 has an invalid value (MAV_MISSION_INVALID_PARAM7)");
break;
case MAV_MISSION_INVALID_SEQUENCE:
return QString("Received mission item out of sequence (MAV_MISSION_INVALID_SEQUENCE)");
break;
case MAV_MISSION_DENIED:
return QString("Not accepting any mission commands (MAV_MISSION_DENIED)");
break;
default:
qWarning(MissionManagerLog) << "Fell off end of switch statement";
return QString("QGC Internal Error");
}
}
......@@ -736,7 +750,7 @@ void MissionManager::_finishTransaction(bool success)
{
if (!success && _readTransactionInProgress) {
// Read from vehicle failed, clear partial list
_missionItems.clear();
_clearAndDeleteMissionItems();
emit newMissionItemsAvailable(false);
}
......@@ -748,6 +762,11 @@ void MissionManager::_finishTransaction(bool success)
_writeTransactionInProgress = false;
emit inProgressChanged(false);
}
if (_resumeMission) {
_resumeMission = false;
emit resumeMissionReady();
}
}
bool MissionManager::inProgress(void)
......@@ -766,6 +785,11 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
_currentMissionItem = missionCurrent.seq;
emit currentItemChanged(_currentMissionItem);
}
if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionItem != _lastCurrentItem) {
_lastCurrentItem = _currentMissionItem;
emit lastCurrentItemChanged(_lastCurrentItem);
}
}
void MissionManager::removeAll(void)
......@@ -774,3 +798,70 @@ void MissionManager::removeAll(void)
writeMissionItems(emptyList);
}
void MissionManager::generateResumeMission(int resumeIndex)
{
if (_vehicle->isOfflineEditingVehicle()) {
return;
}
if (inProgress()) {
qCDebug(MissionManagerLog) << "generateResumeMission called while transaction in progress";
return;
}
int seqNum = 0;
QList<MissionItem*> resumeMission;
QList<MAV_CMD> includedResumeCommands;
// If any command in this list occurs before the resumeIndex it will be added to the front of the mission
includedResumeCommands << MAV_CMD_DO_CONTROL_VIDEO
<< MAV_CMD_DO_SET_ROI
<< MAV_CMD_DO_DIGICAM_CONFIGURE
<< MAV_CMD_DO_DIGICAM_CONTROL
<< MAV_CMD_DO_MOUNT_CONFIGURE
<< MAV_CMD_DO_MOUNT_CONTROL
<< MAV_CMD_DO_SET_CAM_TRIGG_DIST
<< MAV_CMD_DO_FENCE_ENABLE
<< MAV_CMD_IMAGE_START_CAPTURE
<< MAV_CMD_IMAGE_STOP_CAPTURE
<< MAV_CMD_VIDEO_START_CAPTURE
<< MAV_CMD_VIDEO_STOP_CAPTURE;
bool addHomePosition = _vehicle->firmwarePlugin()->sendHomePositionToVehicle();
int setCurrentIndex = addHomePosition ? 1 : 0;
for (int i=0; i<_missionItems.count(); i++) {
MissionItem* oldItem = _missionItems[i];
if ((i == 0 && addHomePosition) || i >= resumeIndex || includedResumeCommands.contains(oldItem->command())) {
MissionItem* newItem = new MissionItem(*oldItem, this);
newItem->setIsCurrentItem( i == setCurrentIndex);
newItem->setSequenceNumber(seqNum++);
resumeMission.append(newItem);
}
}
// Handle DO_JUMP seq num update
// Send to vehicle
_clearAndDeleteMissionItems();
for (int i=0; i<resumeMission.count(); i++) {
_missionItems.append(new MissionItem(*resumeMission[i], this));
}
_resumeMission = true;
_writeMissionItemsWorker();
// Clean up no longer needed resume items
for (int i=0; i<resumeMission.count(); i++) {
resumeMission[i]->deleteLater();
}
}
void MissionManager::_clearAndDeleteMissionItems(void)
{
for (int i=0; i<_missionItems.count(); i++) {
_missionItems[i]->deleteLater();
}
_missionItems.clear();
}
src/MissionManager/MissionManager.h
View file @ 6454b0f0
......@@ -36,7 +36,12 @@ public:
bool inProgress(void);
const QList<MissionItem*>& missionItems(void) { return _missionItems; }
int currentItem(void) { return _currentMissionItem; }
/// Current mission item as reported by MISSION_CURRENT
int currentItem(void) const { return _currentMissionItem; }
/// Last current mission item reported while in Mission flight mode
int lastCurrentItem(void) const { return _lastCurrentItem; }
void requestMissionItems(void);
......@@ -52,6 +57,10 @@ public:
/// Removes all mission items from vehicle
void removeAll(void);
/// Generates a new mission which starts from the specified index. It will include all the CMD_DO items
/// from mission start to resumeIndex in the generate mission.
void generateResumeMission(int resumeIndex);
/// Error codes returned in error signal
typedef enum {
InternalError,
......@@ -73,7 +82,9 @@ signals:
void inProgressChanged(bool inProgress);
void error(int errorCode, const QString& errorMsg);
void currentItemChanged(int currentItem);
void lastCurrentItemChanged(int lastCurrentMissionItem);
void resumeMissionReady(void);
private slots:
void _mavlinkMessageReceived(const mavlink_message_t& message);
void _ackTimeout(void);
......@@ -103,6 +114,8 @@ private:
void _finishTransaction(bool success);
void _requestList(void);
void _writeMissionCount(void);
void _writeMissionItemsWorker(void);
void _clearAndDeleteMissionItems(void);
private:
Vehicle* _vehicle;
......@@ -114,6 +127,7 @@ private:
bool _readTransactionInProgress;
bool _writeTransactionInProgress;
bool _resumeMission;
QList<int> _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle
QList<int> _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle
......@@ -121,6 +135,7 @@ private:
QList<MissionItem*> _missionItems;
int _currentMissionItem;
int _lastCurrentItem;
};
#endif
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