|
|
|
@ -31,44 +31,26 @@
@@ -31,44 +31,26 @@
|
|
|
|
|
QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog") |
|
|
|
|
|
|
|
|
|
MissionManager::MissionManager(Vehicle* vehicle) |
|
|
|
|
: QThread() |
|
|
|
|
, _vehicle(vehicle) |
|
|
|
|
: _vehicle(vehicle) |
|
|
|
|
, _cMissionItems(0) |
|
|
|
|
, _canEdit(true) |
|
|
|
|
, _ackTimeoutTimer(NULL) |
|
|
|
|
, _retryAck(AckNone) |
|
|
|
|
{ |
|
|
|
|
moveToThread(this); |
|
|
|
|
|
|
|
|
|
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived); |
|
|
|
|
|
|
|
|
|
connect(this, &MissionManager::_writeMissionItemsOnThread, this, &MissionManager::_writeMissionItems); |
|
|
|
|
connect(this, &MissionManager::_requestMissionItemsOnThread, this, &MissionManager::_requestMissionItems); |
|
|
|
|
|
|
|
|
|
start(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
MissionManager::~MissionManager() |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionManager::run(void) |
|
|
|
|
{ |
|
|
|
|
_ackTimeoutTimer = new QTimer(this); |
|
|
|
|
_ackTimeoutTimer->setSingleShot(true); |
|
|
|
|
_ackTimeoutTimer->setInterval(_ackTimeoutMilliseconds); |
|
|
|
|
|
|
|
|
|
connect(_ackTimeoutTimer, &QTimer::timeout, this, &MissionManager::_ackTimeout); |
|
|
|
|
|
|
|
|
|
_requestMissionItems(); |
|
|
|
|
|
|
|
|
|
exec(); |
|
|
|
|
requestMissionItems(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionManager::requestMissionItems(void) |
|
|
|
|
MissionManager::~MissionManager() |
|
|
|
|
{ |
|
|
|
|
emit _requestMissionItemsOnThread(); |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems) |
|
|
|
@ -78,10 +60,28 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
@@ -78,10 +60,28 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
|
|
|
|
|
_missionItems.append(new MissionItem(*qobject_cast<const MissionItem*>(missionItems[i]))); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
emit _writeMissionItemsOnThread(); |
|
|
|
|
qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count(); |
|
|
|
|
|
|
|
|
|
if (inProgress()) { |
|
|
|
|
qCDebug(MissionManagerLog) << "writeMissionItems called while transaction in progress"; |
|
|
|
|
// FIXME: Better error handling
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
mavlink_message_t message; |
|
|
|
|
mavlink_mission_count_t missionCount; |
|
|
|
|
|
|
|
|
|
missionCount.target_system = _vehicle->id(); |
|
|
|
|
missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER; |
|
|
|
|
missionCount.count = _missionItems.count(); |
|
|
|
|
|
|
|
|
|
mavlink_msg_mission_count_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionCount); |
|
|
|
|
|
|
|
|
|
_vehicle->sendMessage(message); |
|
|
|
|
_startAckTimeout(AckMissionRequest, message); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionManager::_requestMissionItems(void) |
|
|
|
|
void MissionManager::requestMissionItems(void) |
|
|
|
|
{ |
|
|
|
|
qCDebug(MissionManagerLog) << "_requestMissionItems"; |
|
|
|
|
|
|
|
|
@ -173,7 +173,7 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message)
@@ -173,7 +173,7 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message)
|
|
|
|
|
qCDebug(MissionManagerLog) << "_handleMissionCount count:" << _cMissionItems; |
|
|
|
|
|
|
|
|
|
if (_cMissionItems == 0) { |
|
|
|
|
_sendTransactionComplete(); |
|
|
|
|
emit newMissionItemsAvailable(); |
|
|
|
|
} else { |
|
|
|
|
_requestNextMissionItem(0); |
|
|
|
|
} |
|
|
|
@ -251,29 +251,6 @@ void MissionManager::_clearMissionItems(void)
@@ -251,29 +251,6 @@ void MissionManager::_clearMissionItems(void)
|
|
|
|
|
_missionItems.clear(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionManager::_writeMissionItems(void) |
|
|
|
|
{ |
|
|
|
|
qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count(); |
|
|
|
|
|
|
|
|
|
if (inProgress()) { |
|
|
|
|
qCDebug(MissionManagerLog) << "writeMissionItems called while transaction in progress"; |
|
|
|
|
// FIXME: Better error handling
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
mavlink_message_t message; |
|
|
|
|
mavlink_mission_count_t missionCount; |
|
|
|
|
|
|
|
|
|
missionCount.target_system = _vehicle->id(); |
|
|
|
|
missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER; |
|
|
|
|
missionCount.count = _missionItems.count(); |
|
|
|
|
|
|
|
|
|
mavlink_msg_mission_count_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionCount); |
|
|
|
|
|
|
|
|
|
_vehicle->sendMessage(message); |
|
|
|
|
_startAckTimeout(AckMissionRequest, message); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionManager::_handleMissionRequest(const mavlink_message_t& message) |
|
|
|
|
{ |
|
|
|
|
mavlink_mission_request_t missionRequest; |
|
|
|
|