From 0fbfd43e952ea846755501a1d3c5313930c9daa0 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 21 Apr 2017 19:11:01 -0700 Subject: [PATCH] Sequential upload/down of individual managers --- src/FirmwarePlugin/APM/APMGeoFenceManager.cc | 2 +- src/MissionManager/GeoFenceController.cc | 4 -- src/MissionManager/GeoFenceManager.cc | 4 +- src/MissionManager/MissionController.cc | 7 ++- src/MissionManager/MissionManager.cc | 5 +- src/MissionManager/PlanMasterController.cc | 55 ++++++++++++++++---- src/MissionManager/PlanMasterController.h | 6 +++ src/MissionManager/RallyPointController.cc | 3 -- src/MissionManager/RallyPointManager.cc | 3 +- src/MissionManager/RallyPointManager.h | 2 +- 10 files changed, 66 insertions(+), 25 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc index 181929cad..a558227e1 100644 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc @@ -132,7 +132,7 @@ void APMGeoFenceManager::loadFromVehicle(void) _requestFencePoint(_currentFencePoint); } -/// Called when a new mavlink message for out vehicle is received +/// Called when a new mavlink message for our vehicle is received void APMGeoFenceManager::_mavlinkMessageReceived(const mavlink_message_t& message) { if (message.msgid == MAVLINK_MSG_ID_FENCE_POINT) { diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index a766690bb..41293a292 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -107,10 +107,6 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &GeoFenceController::_loadComplete); connect(_geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged); - if (!_geoFenceManager->inProgress()) { - _loadComplete(_geoFenceManager->breachReturnPoint(), _geoFenceManager->polygon()); - } - _signalAll(); } diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc index a2e8f14bb..54e1593cd 100644 --- a/src/MissionManager/GeoFenceManager.cc +++ b/src/MissionManager/GeoFenceManager.cc @@ -40,8 +40,8 @@ void GeoFenceManager::loadFromVehicle(void) void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon) { + // No geofence support in unknown vehicle Q_UNUSED(breachReturn); Q_UNUSED(polygon); - - // No geofence support in unknown vehicle + emit loadComplete(QGeoCoordinate(), QList()); } diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index cea35f7cb..5dbb4f475 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -446,8 +446,11 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec // Mission Settings AppSettings* appSettings = qgcApp()->toolbox()->settingsManager()->appSettings(); - appSettings->offlineEditingFirmwareType()->setRawValue(AppSettings::offlineEditingFirmwareTypeFromFirmwareType((MAV_AUTOPILOT)json[_jsonVehicleTypeKey].toInt())); - appSettings->offlineEditingVehicleType()->setRawValue(AppSettings::offlineEditingVehicleTypeFromVehicleType((MAV_TYPE)json[_jsonVehicleTypeKey].toInt())); + if (_masterController->offline()) { + // We only update if offline since if we are online we use the online vehicle settings + appSettings->offlineEditingFirmwareType()->setRawValue(AppSettings::offlineEditingFirmwareTypeFromFirmwareType((MAV_AUTOPILOT)json[_jsonVehicleTypeKey].toInt())); + appSettings->offlineEditingVehicleType()->setRawValue(AppSettings::offlineEditingVehicleTypeFromVehicleType((MAV_TYPE)json[_jsonVehicleTypeKey].toInt())); + } if (json.contains(_jsonCruiseSpeedKey)) { appSettings->offlineEditingCruiseSpeed()->setRawValue(json[_jsonCruiseSpeedKey].toDouble()); } diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index ceca20cf5..f7134f5d4 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -48,7 +48,6 @@ void MissionManager::_writeMissionItemsWorker(void) { _lastMissionRequest = -1; - emit newMissionItemsAvailable(_missionItems.count() == 0); emit progressPct(0); qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count(); @@ -870,6 +869,10 @@ void MissionManager::_finishTransaction(bool success) emit newMissionItemsAvailable(false); } + if (_transactionInProgress == TransactionWrite) { + emit newMissionItemsAvailable(_missionItems.count() == 0); + } + _itemIndicesToRead.clear(); _itemIndicesToWrite.clear(); diff --git a/src/MissionManager/PlanMasterController.cc b/src/MissionManager/PlanMasterController.cc index 8d7e0a3d4..c404805dd 100644 --- a/src/MissionManager/PlanMasterController.cc +++ b/src/MissionManager/PlanMasterController.cc @@ -13,6 +13,7 @@ #include "SettingsManager.h" #include "AppSettings.h" #include "JsonHelper.h" +#include "MissionManager.h" #include #include @@ -33,6 +34,10 @@ PlanMasterController::PlanMasterController(QObject* parent) , _missionController(this) , _geoFenceController(this) , _rallyPointController(this) + , _loadGeoFence(false) + , _loadRallyPoints(false) + , _sendGeoFence(false) + , _sendRallyPoints(false) { connect(&_missionController, &MissionController::dirtyChanged, this, &PlanMasterController::dirtyChanged); connect(&_geoFenceController, &GeoFenceController::dirtyChanged, this, &PlanMasterController::dirtyChanged); @@ -82,9 +87,17 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle) _managerVehicle = _controllerVehicle; newOffline = true; } else { - // FIXME: Check for vehicle compatibility. (edit mode only) - _managerVehicle = activeVehicle; newOffline = false; + _managerVehicle = activeVehicle; + + // Update controllerVehicle to the currently connected vehicle + AppSettings* appSettings = qgcApp()->toolbox()->settingsManager()->appSettings(); + appSettings->offlineEditingFirmwareType()->setRawValue(AppSettings::offlineEditingFirmwareTypeFromFirmwareType(_managerVehicle->firmwareType())); + appSettings->offlineEditingVehicleType()->setRawValue(AppSettings::offlineEditingVehicleTypeFromVehicleType(_managerVehicle->vehicleType())); + + // We use these signals to sequence upload and download to the multiple controller/managers + connect(_managerVehicle->missionManager(), &MissionManager::newMissionItemsAvailable, this, &PlanMasterController::_loadSendMissionComplete); + connect(_managerVehicle->geoFenceManager(), &GeoFenceManager::loadComplete, this, &PlanMasterController::_loadSendGeoFenceCompelte); } if (newOffline != _offline) { _offline = newOffline; @@ -106,23 +119,46 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle) void PlanMasterController::loadFromVehicle(void) { - // FIXME: Hack implementation + _loadGeoFence = true; _missionController.loadFromVehicle(); - _geoFenceController.loadFromVehicle(); - _rallyPointController.loadFromVehicle(); setDirty(false); } + +void PlanMasterController::_loadSendMissionComplete(void) +{ + if (_loadGeoFence) { + _loadGeoFence = false; + _loadRallyPoints = true; + _geoFenceController.loadFromVehicle(); + setDirty(false); + } else if (_sendGeoFence) { + _sendGeoFence = false; + _sendRallyPoints = true; + _geoFenceController.sendToVehicle(); + setDirty(false); + } +} + +void PlanMasterController::_loadSendGeoFenceCompelte(void) +{ + if (_loadRallyPoints) { + _loadRallyPoints = false; + _rallyPointController.loadFromVehicle(); + setDirty(false); + } else if (_sendRallyPoints) { + _sendRallyPoints = false; + _rallyPointController.sendToVehicle(); + } +} + void PlanMasterController::sendToVehicle(void) { - // FIXME: Hack implementation + _sendGeoFence = true; _missionController.sendToVehicle(); - _geoFenceController.sendToVehicle(); - _rallyPointController.sendToVehicle(); setDirty(false); } - void PlanMasterController::loadFromFile(const QString& filename) { QString errorString; @@ -291,4 +327,3 @@ void PlanMasterController::sendPlanToVehicle(Vehicle* vehicle, const QString& fi controller->loadFromFile(filename); delete controller; } - diff --git a/src/MissionManager/PlanMasterController.h b/src/MissionManager/PlanMasterController.h index b0d7133c2..db4f69cba 100644 --- a/src/MissionManager/PlanMasterController.h +++ b/src/MissionManager/PlanMasterController.h @@ -83,6 +83,8 @@ signals: private slots: void _activeVehicleChanged(Vehicle* activeVehicle); + void _loadSendMissionComplete(void); + void _loadSendGeoFenceCompelte(void); private: MultiVehicleManager* _multiVehicleMgr; @@ -93,6 +95,10 @@ private: MissionController _missionController; GeoFenceController _geoFenceController; RallyPointController _rallyPointController; + bool _loadGeoFence; + bool _loadRallyPoints; + bool _sendGeoFence; + bool _sendRallyPoints; static const int _planFileVersion; static const char* _planFileType; diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc index 6cde8b28b..5807109c9 100644 --- a/src/MissionManager/RallyPointController.cc +++ b/src/MissionManager/RallyPointController.cc @@ -70,9 +70,6 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle) connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &RallyPointController::_loadComplete); connect(_rallyPointManager, &RallyPointManager::inProgressChanged, this, &RallyPointController::syncInProgressChanged); - if (!syncInProgress()) { - _loadComplete(_rallyPointManager->points()); - } emit rallyPointsSupportedChanged(rallyPointsSupported()); } diff --git a/src/MissionManager/RallyPointManager.cc b/src/MissionManager/RallyPointManager.cc index ba92da6a1..4b83624e4 100644 --- a/src/MissionManager/RallyPointManager.cc +++ b/src/MissionManager/RallyPointManager.cc @@ -39,6 +39,7 @@ void RallyPointManager::loadFromVehicle(void) void RallyPointManager::sendToVehicle(const QList& rgPoints) { - Q_UNUSED(rgPoints); // No support in generic vehicle + Q_UNUSED(rgPoints); + emit loadComplete(QList()); } diff --git a/src/MissionManager/RallyPointManager.h b/src/MissionManager/RallyPointManager.h index ca391edaa..4b78b9572 100644 --- a/src/MissionManager/RallyPointManager.h +++ b/src/MissionManager/RallyPointManager.h @@ -58,7 +58,7 @@ signals: void loadComplete (const QList rgPoints); void inProgressChanged (bool inProgress); void error (int errorCode, const QString& errorMsg); - + protected: void _sendError(ErrorCode_t errorCode, const QString& errorMsg); -- 2.22.0