zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Add resume mission support

QGC4.4
DonLakeFlyer 8 years ago
parent
fe004f8b1e
commit
6454b0f085
5 changed files with 299 additions and 155 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 52
      src/FlightDisplay/FlightDisplayViewWidgets.qml
  2. 46
      src/MissionManager/MissionController.cc
  3. 12
      src/MissionManager/MissionController.h
  4. 325
      src/MissionManager/MissionManager.cc
  5. 19
      src/MissionManager/MissionManager.h

52
src/FlightDisplay/FlightDisplayViewWidgets.qml
Unescape View File

@ -38,7 +38,7 @@ Item { @@ -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 { @@ -92,6 +92,11 @@ Item {
onValueChanged: _setInstrumentWidget()
}
Connections {
target: missionController
onResumeMissionReady: _guidedModeBar.confirmAction(_guidedModeBar.confirmResumeMissionReady)
}
Component.onCompleted: {
_setInstrumentWidget()
}
@ -219,19 +224,20 @@ Item { @@ -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 { @@ -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 { @@ -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 { @@ -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 { @@ -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)
}

46
src/MissionManager/MissionController.cc
Unescape View File

@ -53,7 +53,6 @@ MissionController::MissionController(QObject *parent) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 @@ -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) @@ -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 @@ -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) @@ -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);
}

12
src/MissionManager/MissionController.h
Unescape View File

@ -55,7 +55,7 @@ public: @@ -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: @@ -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: @@ -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: @@ -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: @@ -188,7 +193,6 @@ private:
CoordVectHashTable _linesTable;
bool _firstItemsFromVehicle;
bool _missionItemsRequested;
bool _queuedSend;
MissionFlightStatus_t _missionFlightStatus;
QString _surveyMissionItemName;
QString _fwLandingMissionItemName;

325
src/MissionManager/MissionManager.cc
Unescape View File

@ -26,7 +26,9 @@ MissionManager::MissionManager(Vehicle* vehicle) @@ -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() @@ -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) @@ -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) @@ -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 @@ -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 @@ -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 @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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();
}

19
src/MissionManager/MissionManager.h
Unescape View File

@ -36,7 +36,12 @@ public: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -121,6 +135,7 @@ private:
QList<MissionItem*> _missionItems;
int _currentMissionItem;
int _lastCurrentItem;
};
#endif

Write Preview
Loading…
Cancel
Save