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

Merge pull request #5015 from DonLakeFlyer/MissionRemoveAll 

Mission remove all change, plus fixes
QGC4.4
Don Gagne 8 years ago committed by GitHub
parent
261154aa51 ebf3969e9c
commit
44682709f6
4 changed files with 91 additions and 29 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. 5
      src/FlightMap/Widgets/MapFitFunctions.qml
  2. 4
      src/MissionManager/CameraSection.FactMetaData.json
  3. 89
      src/MissionManager/MissionManager.cc
  4. 22
      src/MissionManager/MissionManager.h

5
src/FlightMap/Widgets/MapFitFunctions.qml
Unescape View File

@ -48,7 +48,10 @@ Item { @@ -48,7 +48,10 @@ Item {
function fitMapViewportToAllCoordinates(coordList) {
var mapFitViewport = Qt.rect(0, 0, map.width, map.height)
if (coordList.length == 0) {
map.center = fitHomePosition()
var homeCoord = fitHomePosition()
if (homeCoord.isValid) {
map.center = homeCoord
}
return
}

4
src/MissionManager/CameraSection.FactMetaData.json
Unescape View File

@ -30,8 +30,8 @@ @@ -30,8 +30,8 @@
"shortDescription": "Gimbal pitch rotation.",
"type": "double",
"units": "deg",
"min": 0.0,
"max": 360.0,
"min": -90,
"max": 0,
"decimalPlaces": 0,
"defaultValue": 0
},

89
src/MissionManager/MissionManager.cc
Unescape View File

@ -24,8 +24,7 @@ MissionManager::MissionManager(Vehicle* vehicle) @@ -24,8 +24,7 @@ MissionManager::MissionManager(Vehicle* vehicle)
, _dedicatedLink(NULL)
, _ackTimeoutTimer(NULL)
, _expectedAck(AckNone)
, _readTransactionInProgress(false)
, _writeTransactionInProgress(false)
, _transactionInProgress(TransactionNone)
, _resumeMission(false)
, _lastMissionRequest(-1)
, _currentMissionIndex(-1)
@ -58,7 +57,7 @@ void MissionManager::_writeMissionItemsWorker(void) @@ -58,7 +57,7 @@ void MissionManager::_writeMissionItemsWorker(void)
_itemIndicesToWrite << i;
}
_writeTransactionInProgress = true;
_transactionInProgress = TransactionWrite;
_retryCount = 0;
emit inProgressChanged(true);
_writeMissionCount();
@ -136,7 +135,7 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC @@ -136,7 +135,7 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC
return;
}
_writeTransactionInProgress = true;
_transactionInProgress = TransactionWrite;
mavlink_message_t messageOut;
mavlink_mission_item_t missionItem;
@ -183,7 +182,7 @@ void MissionManager::requestMissionItems(void) @@ -183,7 +182,7 @@ void MissionManager::requestMissionItems(void)
}
_retryCount = 0;
_readTransactionInProgress = true;
_transactionInProgress = TransactionRead;
emit inProgressChanged(true);
_requestList();
}
@ -271,6 +270,17 @@ void MissionManager::_ackTimeout(void) @@ -271,6 +270,17 @@ void MissionManager::_ackTimeout(void)
_finishTransaction(false);
}
break;
case AckMissionClearAll:
// MISSION_ACK expected
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission remove all, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(MissionManagerLog) << "Retrying MISSION_CLEAR_ALL retry Count" << _retryCount;
_removeAllWorker();
}
break;
case AckGuidedItem:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
default:
@ -514,16 +524,17 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message, boo @@ -514,16 +524,17 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message, boo
}
mavlink_msg_mission_request_decode(&message, &missionRequest);
qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber" << missionRequest.seq;
if (missionRequest.seq > _missionItems.count() - 1) {
_sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_missionItems.count()).arg(missionRequest.seq));
_finishTransaction(false);
return;
}
_lastMissionRequest = missionRequest.seq;
if (!_itemIndicesToWrite.contains(missionRequest.seq)) {
if (missionRequest.seq > _missionItems.count()) {
_sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_missionItems.count()).arg(missionRequest.seq));
_finishTransaction(false);
return;
} else {
qCDebug(MissionManagerLog) << "_handleMissionRequest sequence number requested which has already been sent, sending again:" << missionRequest.seq;
}
qCDebug(MissionManagerLog) << "_handleMissionRequest sequence number requested which has already been sent, sending again:" << missionRequest.seq;
} else {
_itemIndicesToWrite.removeOne(missionRequest.seq);
}
@ -635,6 +646,13 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) @@ -635,6 +646,13 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
_finishTransaction(false);
}
break;
case AckMissionClearAll:
// MISSION_ACK expected
if (missionAck.type != MAV_MISSION_ACCEPTED) {
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle remove all failed.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
}
_finishTransaction(missionAck.type == MAV_MISSION_ACCEPTED);
break;
case AckGuidedItem:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
@ -807,7 +825,7 @@ QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result) @@ -807,7 +825,7 @@ QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result)
void MissionManager::_finishTransaction(bool success)
{
if (!success && _readTransactionInProgress) {
if (!success && _transactionInProgress == TransactionRead) {
// Read from vehicle failed, clear partial list
_clearAndDeleteMissionItems();
emit newMissionItemsAvailable(false);
@ -816,9 +834,8 @@ void MissionManager::_finishTransaction(bool success) @@ -816,9 +834,8 @@ void MissionManager::_finishTransaction(bool success)
_itemIndicesToRead.clear();
_itemIndicesToWrite.clear();
if (_readTransactionInProgress || _writeTransactionInProgress) {
_readTransactionInProgress = false;
_writeTransactionInProgress = false;
if (_transactionInProgress != TransactionNone) {
_transactionInProgress = TransactionNone;
emit inProgressChanged(false);
}
@ -830,7 +847,7 @@ void MissionManager::_finishTransaction(bool success) @@ -830,7 +847,7 @@ void MissionManager::_finishTransaction(bool success)
bool MissionManager::inProgress(void)
{
return _readTransactionInProgress || _writeTransactionInProgress;
return _transactionInProgress != TransactionNone;
}
void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
@ -852,11 +869,45 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message) @@ -852,11 +869,45 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
}
}
void MissionManager::_removeAllWorker(void)
{
mavlink_message_t message;
qCDebug(MissionManagerLog) << "_removeAllWorker";
_dedicatedLink = _vehicle->priorityLink();
mavlink_msg_mission_clear_all_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
_vehicle->id(),
MAV_COMP_ID_MISSIONPLANNER,
MAV_MISSION_TYPE_MISSION);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
_startAckTimeout(AckMissionClearAll);
}
void MissionManager::removeAll(void)
{
QList<MissionItem*> emptyList;
if (inProgress()) {
return;
}
qCDebug(MissionManagerLog) << "removeAll";
_clearAndDeleteMissionItems();
_currentMissionIndex = -1;
_lastCurrentIndex = -1;
emit currentIndexChanged(-1);
emit lastCurrentIndexChanged(-1);
emit newMissionItemsAvailable(true /* removeAllRequested */);
_transactionInProgress = TransactionClearAll;
_retryCount = 0;
emit inProgressChanged(true);
writeMissionItems(emptyList);
_removeAllWorker();
}
void MissionManager::generateResumeMission(int resumeIndex)

22
src/MissionManager/MissionManager.h
Unescape View File

@ -95,9 +95,17 @@ private: @@ -95,9 +95,17 @@ private:
AckMissionCount, ///< MISSION_COUNT message expected
AckMissionItem, ///< MISSION_ITEM expected
AckMissionRequest, ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
AckMissionClearAll, ///< MISSION_CLEAR_ALL sent, MISSION_ACK is expected
AckGuidedItem, ///< MISSION_ACK expected in response to ArduPilot guided mode single item send
} AckType_t;
typedef enum {
TransactionNone,
TransactionRead,
TransactionWrite,
TransactionClearAll
} TransactionType_t;
void _startAckTimeout(AckType_t ack);
bool _checkForExpectedAck(AckType_t receivedAck);
void _readTransactionComplete(void);
@ -117,6 +125,7 @@ private: @@ -117,6 +125,7 @@ private:
void _writeMissionItemsWorker(void);
void _clearAndDeleteMissionItems(void);
QString _lastMissionReqestString(MAV_MISSION_RESULT result);
void _removeAllWorker(void);
private:
Vehicle* _vehicle;
@ -126,12 +135,11 @@ private: @@ -126,12 +135,11 @@ private:
AckType_t _expectedAck;
int _retryCount;
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
int _lastMissionRequest; ///< Index of item last requested by MISSION_REQUEST
TransactionType_t _transactionInProgress;
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
int _lastMissionRequest; ///< Index of item last requested by MISSION_REQUEST
QMutex _dataMutex;

Write Preview
Loading…
Cancel
Save