diff --git a/QGCApplication.pro b/QGCApplication.pro index 7892edcf73192ece672a618722d151b41d7be008..5e606a094a50ed5cab9e760f3c62456eb93f1092 100644 --- a/QGCApplication.pro +++ b/QGCApplication.pro @@ -260,7 +260,6 @@ HEADERS += \ src/QmlControls/ParameterEditorController.h \ src/QmlControls/ScreenToolsController.h \ src/SerialPortIds.h \ - src/uas/QGCMAVLinkUASFactory.h \ src/uas/FileManager.h \ src/uas/UAS.h \ src/uas/UASInterface.h \ @@ -394,7 +393,6 @@ SOURCES += \ src/QmlControls/MavManager.cc \ src/QmlControls/ParameterEditorController.cc \ src/QmlControls/ScreenToolsController.cc \ - src/uas/QGCMAVLinkUASFactory.cc \ src/uas/FileManager.cc \ src/uas/UAS.cc \ src/uas/UASManager.cc \ @@ -559,15 +557,12 @@ SOURCES += \ # INCLUDEPATH += \ + src/AutoPilotPlugins/PX4 \ src/FirmwarePlugin \ + src/Vehicle \ src/VehicleSetup \ - src/AutoPilotPlugins/PX4 \ HEADERS+= \ - src/FirmwarePlugin/FirmwarePluginManager.h \ - src/FirmwarePlugin/FirmwarePlugin.h \ - src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h \ - src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \ src/AutoPilotPlugins/AutoPilotPlugin.h \ src/AutoPilotPlugins/AutoPilotPluginManager.h \ src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h \ @@ -587,6 +582,12 @@ HEADERS+= \ src/AutoPilotPlugins/PX4/SafetyComponent.h \ src/AutoPilotPlugins/PX4/SensorsComponent.h \ src/AutoPilotPlugins/PX4/SensorsComponentController.h \ + src/FirmwarePlugin/FirmwarePluginManager.h \ + src/FirmwarePlugin/FirmwarePlugin.h \ + src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h \ + src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \ + src/Vehicle/MultiVehicleManager.h \ + src/Vehicle/Vehicle.h \ src/VehicleSetup/SetupView.h \ src/VehicleSetup/VehicleComponent.h \ @@ -600,9 +601,6 @@ HEADERS += \ } SOURCES += \ - src/FirmwarePlugin/FirmwarePluginManager.cc \ - src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc \ - src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc \ src/AutoPilotPlugins/AutoPilotPlugin.cc \ src/AutoPilotPlugins/AutoPilotPluginManager.cc \ src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc \ @@ -622,6 +620,11 @@ SOURCES += \ src/AutoPilotPlugins/PX4/SafetyComponent.cc \ src/AutoPilotPlugins/PX4/SensorsComponent.cc \ src/AutoPilotPlugins/PX4/SensorsComponentController.cc \ + src/FirmwarePlugin/FirmwarePluginManager.cc \ + src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc \ + src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc \ + src/Vehicle/MultiVehicleManager.cc \ + src/Vehicle/Vehicle.cc \ src/VehicleSetup/SetupView.cc \ src/VehicleSetup/VehicleComponent.cc \ diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.cc b/src/AutoPilotPlugins/AutoPilotPlugin.cc index f010b1be3831c6275cbe0a9385e0281c5e3e0071..22e5fadb7480269ede5820e93c3a040427a6ae4b 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc @@ -30,16 +30,16 @@ #include "MainWindow.h" #include "ParameterLoader.h" -AutoPilotPlugin::AutoPilotPlugin(UASInterface* uas, QObject* parent) : +AutoPilotPlugin::AutoPilotPlugin(Vehicle* vehicle, QObject* parent) : QObject(parent), - _uas(uas), + _vehicle(vehicle), _pluginReady(false), _setupComplete(false) { - Q_ASSERT(_uas); + Q_ASSERT(vehicle); - connect(_uas, &UASInterface::disconnected, this, &AutoPilotPlugin::_uasDisconnected); - connect(_uas, &UASInterface::armingChanged, this, &AutoPilotPlugin::armedChanged); + connect(_vehicle->uas(), &UASInterface::disconnected, this, &AutoPilotPlugin::_uasDisconnected); + connect(_vehicle->uas(), &UASInterface::armingChanged, this, &AutoPilotPlugin::armedChanged); connect(this, &AutoPilotPlugin::pluginReadyChanged, this, &AutoPilotPlugin::_pluginReadyChanged); } @@ -102,8 +102,8 @@ void AutoPilotPlugin::resetAllParametersToDefaults(void) mavlink_message_t msg; MAVLinkProtocol* mavlink = MAVLinkProtocol::instance(); - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _uas->getUASID(), 0, MAV_CMD_PREFLIGHT_STORAGE, 0, 2, -1, 0, 0, 0, 0, 0); - _uas->sendMessage(msg); + mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->uas()->getUASID(), 0, MAV_CMD_PREFLIGHT_STORAGE, 0, 2, -1, 0, 0, 0, 0, 0); + _vehicle->sendMessage(msg); } void AutoPilotPlugin::refreshAllParameters(void) @@ -169,7 +169,7 @@ const QMap >& AutoPilotPlugin::getGroupMap(void) void AutoPilotPlugin::writeParametersToStream(QTextStream &stream) { - _getParameterLoader()->writeParametersToStream(stream, _uas->getUASName()); + _getParameterLoader()->writeParametersToStream(stream, _vehicle->uas()->getUASName()); } QString AutoPilotPlugin::readParametersFromStream(QTextStream &stream) @@ -179,5 +179,5 @@ QString AutoPilotPlugin::readParametersFromStream(QTextStream &stream) bool AutoPilotPlugin::armed(void) { - return _uas->isArmed(); + return _vehicle->uas()->isArmed(); } diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.h b/src/AutoPilotPlugins/AutoPilotPlugin.h index a27b2dd663877f15885a638578b1b14e13a39ca1..6a8b66172ef0a9971b7a5470f6098d6f0eacd9d1 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/AutoPilotPlugin.h @@ -32,11 +32,12 @@ #include #include -#include "UASInterface.h" #include "VehicleComponent.h" #include "FactSystem.h" +#include "Vehicle.h" class ParameterLoader; +class Vehicle; /// This is the base class for AutoPilot plugins /// @@ -50,7 +51,7 @@ class AutoPilotPlugin : public QObject Q_OBJECT public: - AutoPilotPlugin(UASInterface* uas, QObject* parent); + AutoPilotPlugin(Vehicle* vehicle, QObject* parent); ~AutoPilotPlugin(); /// true: plugin is ready for use, plugin should no longer be used @@ -119,7 +120,7 @@ public: bool setupComplete(void); bool armed(void); - UASInterface* uas(void) { return _uas; } + Vehicle* vehicle(void) { return _vehicle; } signals: void pluginReadyChanged(bool pluginReady); @@ -134,9 +135,9 @@ protected: /// Returns the ParameterLoader virtual ParameterLoader* _getParameterLoader(void) = 0; - UASInterface* _uas; - bool _pluginReady; - bool _setupComplete; + Vehicle* _vehicle; + bool _pluginReady; + bool _setupComplete; private slots: void _uasDisconnected(void); diff --git a/src/AutoPilotPlugins/AutoPilotPluginManager.cc b/src/AutoPilotPlugins/AutoPilotPluginManager.cc index 3edaa33fed6f6a54cf807639dce0b92116986d5e..245d0641b8d1ea25eba4e71830ebcd4009378b5a 100644 --- a/src/AutoPilotPlugins/AutoPilotPluginManager.cc +++ b/src/AutoPilotPlugins/AutoPilotPluginManager.cc @@ -27,98 +27,26 @@ #include "AutoPilotPluginManager.h" #include "PX4/PX4AutoPilotPlugin.h" #include "Generic/GenericAutoPilotPlugin.h" -#include "QGCApplication.h" -#include "QGCMessageBox.h" -#include "UASManager.h" IMPLEMENT_QGC_SINGLETON(AutoPilotPluginManager, AutoPilotPluginManager) AutoPilotPluginManager::AutoPilotPluginManager(QObject* parent) : QGCSingleton(parent) { - UASManagerInterface* uasMgr = UASManager::instance(); - Q_ASSERT(uasMgr); - - // We need to track uas coming and going so that we can instantiate plugins for each uas - connect(uasMgr, &UASManagerInterface::UASCreated, this, &AutoPilotPluginManager::_uasCreated); - connect(uasMgr, SIGNAL(UASDeleted(UASInterface*)), this, SLOT(_uasDeleted(UASInterface*))); + } AutoPilotPluginManager::~AutoPilotPluginManager() { -#ifdef QT_DEBUG - foreach(MAV_AUTOPILOT mavType, _pluginMap.keys()) { - Q_ASSERT_X(_pluginMap[mavType].count() == 0, "AutoPilotPluginManager", "LinkManager::_shutdown should have already closed all uas"); - } -#endif - _pluginMap.clear(); - PX4AutoPilotPlugin::clearStaticData(); GenericAutoPilotPlugin::clearStaticData(); } -/// Create the plugin for this uas -void AutoPilotPluginManager::_uasCreated(UASInterface* uas) -{ - Q_ASSERT(uas); - - MAV_AUTOPILOT autopilotType = static_cast(uas->getAutopilotType()); - int uasId = uas->getUASID(); - Q_ASSERT(uasId != 0); - - if (_pluginMap.contains(autopilotType)) { - Q_ASSERT_X(!_pluginMap[autopilotType].contains(uasId), "AutoPilotPluginManager", "Either we have duplicate UAS ids, or a UAS was not removed correctly."); - } - - AutoPilotPlugin* plugin; - switch (autopilotType) { - case MAV_AUTOPILOT_PX4: - plugin = new PX4AutoPilotPlugin(uas, this); - Q_CHECK_PTR(plugin); - _pluginMap[MAV_AUTOPILOT_PX4][uasId] = QSharedPointer(plugin); - break; - case MAV_AUTOPILOT_GENERIC: - default: - plugin = new GenericAutoPilotPlugin(uas, this); - Q_CHECK_PTR(plugin); - _pluginMap[MAV_AUTOPILOT_GENERIC][uasId] = QSharedPointer(plugin); - QGCMessageBox::warning("Partial Support AutoPilot", - "Warning: You have connected QGroundControl to a firmware flight stack which is only partially supported. " - "If you are using the APM Flight Stack it is currently recommended to use Mission Planner, APM Planner or Tower as your ground control station."); - } -} - -/// Destroy the plugin associated with this uas -void AutoPilotPluginManager::_uasDeleted(UASInterface* uas) +AutoPilotPlugin* AutoPilotPluginManager::newAutopilotPluginForVehicle(Vehicle* vehicle) { - Q_ASSERT(uas); - - MAV_AUTOPILOT autopilotType = _installedAutopilotType(static_cast(uas->getAutopilotType())); - int uasId = uas->getUASID(); - Q_ASSERT(uasId != 0); - - if (_pluginMap.contains(autopilotType) && _pluginMap[autopilotType].contains(uasId)) { - _pluginMap[autopilotType][uasId].clear(); - _pluginMap[autopilotType].remove(uasId); + if (vehicle->firmwareType() == MAV_AUTOPILOT_PX4) { + return new PX4AutoPilotPlugin(vehicle, vehicle); + } else { + return new GenericAutoPilotPlugin(vehicle, vehicle); } } - -QSharedPointer AutoPilotPluginManager::getInstanceForAutoPilotPlugin(UASInterface* uas) -{ - Q_ASSERT(uas); - - MAV_AUTOPILOT autopilotType = _installedAutopilotType(static_cast(uas->getAutopilotType())); - int uasId = uas->getUASID(); - Q_ASSERT(uasId != 0); - - Q_ASSERT(_pluginMap.contains(autopilotType)); - Q_ASSERT(_pluginMap[autopilotType].contains(uasId)); - - return _pluginMap[autopilotType][uasId]; -} - -/// If autopilot is not an installed plugin, returns MAV_AUTOPILOT_GENERIC -MAV_AUTOPILOT AutoPilotPluginManager::_installedAutopilotType(MAV_AUTOPILOT autopilot) -{ - return _pluginMap.contains(autopilot) ? autopilot : MAV_AUTOPILOT_GENERIC; -} diff --git a/src/AutoPilotPlugins/AutoPilotPluginManager.h b/src/AutoPilotPlugins/AutoPilotPluginManager.h index a26b43b292943bb14d501657682712e8e1166337..b8f17e5d79d7968b032d9a49715bf815130edeaa 100644 --- a/src/AutoPilotPlugins/AutoPilotPluginManager.h +++ b/src/AutoPilotPlugins/AutoPilotPluginManager.h @@ -31,12 +31,9 @@ #include #include -#include "UASInterface.h" -#include "VehicleComponent.h" #include "AutoPilotPlugin.h" #include "QGCSingleton.h" -#include "QGCMAVLink.h" - +#include "Vehicle.h" /// AutoPilotPlugin manager is a singleton which maintains the list of AutoPilotPlugin objects. @@ -47,23 +44,12 @@ class AutoPilotPluginManager : public QGCSingleton DECLARE_QGC_SINGLETON(AutoPilotPluginManager, AutoPilotPluginManager) public: - /// Returns the singleton AutoPilotPlugin instance for the specified uas. Returned as QSharedPointer - /// to prevent shutdown ordering problems with Qml destruction happening after Facts are destroyed. - /// @param uas Uas to get plugin for - QSharedPointer getInstanceForAutoPilotPlugin(UASInterface* uas); - -private slots: - void _uasCreated(UASInterface* uas); - void _uasDeleted(UASInterface* uas); - + AutoPilotPlugin* newAutopilotPluginForVehicle(Vehicle* vehicle); + private: /// All access to singleton is through AutoPilotPluginManager::instance AutoPilotPluginManager(QObject* parent = NULL); ~AutoPilotPluginManager(); - - MAV_AUTOPILOT _installedAutopilotType(MAV_AUTOPILOT autopilot); - - QMap > > _pluginMap; ///< Map of AutoPilot plugins _pluginMap[MAV_TYPE][UASid] }; #endif diff --git a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc index 034868612065bde916cc155ecca57d078dc073d1..ece104b09124e8bcd9b47af2eec747682cd6291e 100644 --- a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc @@ -26,12 +26,12 @@ #include "GenericAutoPilotPlugin.h" -GenericAutoPilotPlugin::GenericAutoPilotPlugin(UASInterface* uas, QObject* parent) : - AutoPilotPlugin(uas, parent) +GenericAutoPilotPlugin::GenericAutoPilotPlugin(Vehicle* vehicle, QObject* parent) : + AutoPilotPlugin(vehicle, parent) { - Q_ASSERT(uas); + Q_ASSERT(vehicle); - _parameterFacts = new GenericParameterFacts(this, uas, this); + _parameterFacts = new GenericParameterFacts(this, vehicle, this); Q_CHECK_PTR(_parameterFacts); connect(_parameterFacts, &GenericParameterFacts::parametersReady, this, &GenericAutoPilotPlugin::_parametersReady); diff --git a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h index 7bdb2125819d8a835c829713d8622b11c928d60b..29410c7276a4721454811f627e25e1143fa3d146 100644 --- a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h +++ b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h @@ -25,7 +25,6 @@ #define GENERICAUTOPILOT_H #include "AutoPilotPlugin.h" -#include "AutoPilotPluginManager.h" #include "GenericParameterFacts.h" /// @file @@ -38,7 +37,7 @@ class GenericAutoPilotPlugin : public AutoPilotPlugin Q_OBJECT public: - GenericAutoPilotPlugin(UASInterface* uas, QObject* parent = NULL); + GenericAutoPilotPlugin(Vehicle* vehicle, QObject* parent = NULL); // Overrides from AutoPilotPlugin virtual const QVariantList& vehicleComponents(void); diff --git a/src/AutoPilotPlugins/Generic/GenericParameterFacts.cc b/src/AutoPilotPlugins/Generic/GenericParameterFacts.cc index fb1e1d18a0b633b41f18ade6c58515dc2e0bc440..24831fcaac358e148ce2071ed1ac013efb1eff18 100644 --- a/src/AutoPilotPlugins/Generic/GenericParameterFacts.cc +++ b/src/AutoPilotPlugins/Generic/GenericParameterFacts.cc @@ -28,8 +28,8 @@ #include -GenericParameterFacts::GenericParameterFacts(AutoPilotPlugin* autopilot, UASInterface* uas, QObject* parent) : - ParameterLoader(autopilot, uas, parent) +GenericParameterFacts::GenericParameterFacts(AutoPilotPlugin* autopilot, Vehicle* vehicle, QObject* parent) : + ParameterLoader(autopilot, vehicle, parent) { - Q_ASSERT(uas); + Q_ASSERT(vehicle); } diff --git a/src/AutoPilotPlugins/Generic/GenericParameterFacts.h b/src/AutoPilotPlugins/Generic/GenericParameterFacts.h index dd5d10ed8b86468936a6d4dde180c0f9e95b373d..c7a90c60c90db47de6f7882efeed739ba960c807 100644 --- a/src/AutoPilotPlugins/Generic/GenericParameterFacts.h +++ b/src/AutoPilotPlugins/Generic/GenericParameterFacts.h @@ -41,7 +41,7 @@ class GenericParameterFacts : public ParameterLoader public: /// @param uas Uas which this set of facts is associated with - GenericParameterFacts(AutoPilotPlugin* autopilot, UASInterface* uas, QObject* parent = NULL); + GenericParameterFacts(AutoPilotPlugin* autopilot, Vehicle* vehicle, QObject* parent = NULL); /// Override from ParameterLoader virtual QString getDefaultComponentIdParam(void) const { return QString(); } diff --git a/src/AutoPilotPlugins/PX4/AirframeComponentController.cc b/src/AutoPilotPlugins/PX4/AirframeComponentController.cc index cfef735dd4db17745f5abcfd503e8e3e6a30db42..a7ce9a4182421c4571bb3d95e5b6c8dee7a897f1 100644 --- a/src/AutoPilotPlugins/PX4/AirframeComponentController.cc +++ b/src/AutoPilotPlugins/PX4/AirframeComponentController.cc @@ -27,7 +27,7 @@ #include "AirframeComponentController.h" #include "AirframeComponentAirframes.h" #include "QGCMAVLink.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" #include "AutoPilotPluginManager.h" #include "QGCApplication.h" #include "QGCMessageBox.h" @@ -98,7 +98,7 @@ AirframeComponentController::~AirframeComponentController() void AirframeComponentController::changeAutostart(void) { - if (UASManager::instance()->getUASList().count() > 1) { + if (MultiVehicleManager::instance()->vehicles().count() > 1) { QGCMessageBox::warning("Airframe Config", "You cannot change airframe configuration while connected to multiple vehicles."); return; } diff --git a/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc b/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc index 4755c9bc8aeae92b37433e351fcc1242556253c6..32de22c33de24868c14644615692b57af2422997 100644 --- a/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc +++ b/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc @@ -26,7 +26,6 @@ #include "FlightModesComponentController.h" #include "QGCMAVLink.h" -#include "UASManager.h" #include "AutoPilotPluginManager.h" #include diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc index 3eed8bc88decf44bcb7b75a63ece2f4a58f1b59f..6ea6c537721b0c3eb55f5677d594ad260fccdcaa 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc @@ -23,7 +23,6 @@ #include "PX4AutoPilotPlugin.h" #include "AutoPilotPluginManager.h" -#include "UASManager.h" #include "PX4ParameterLoader.h" #include "PX4AirframeLoader.h" #include "FlightModesComponentController.h" @@ -64,8 +63,8 @@ union px4_custom_mode { float data_float; }; -PX4AutoPilotPlugin::PX4AutoPilotPlugin(UASInterface* uas, QObject* parent) : - AutoPilotPlugin(uas, parent), +PX4AutoPilotPlugin::PX4AutoPilotPlugin(Vehicle* vehicle, QObject* parent) : + AutoPilotPlugin(vehicle, parent), _parameterFacts(NULL), _airframeComponent(NULL), _radioComponent(NULL), @@ -75,15 +74,15 @@ PX4AutoPilotPlugin::PX4AutoPilotPlugin(UASInterface* uas, QObject* parent) : _powerComponent(NULL), _incorrectParameterVersion(false) { - Q_ASSERT(uas); + Q_ASSERT(vehicle); - _parameterFacts = new PX4ParameterLoader(this, uas, this); + _parameterFacts = new PX4ParameterLoader(this, vehicle, this); Q_CHECK_PTR(_parameterFacts); connect(_parameterFacts, &PX4ParameterLoader::parametersReady, this, &PX4AutoPilotPlugin::_pluginReadyPreChecks); connect(_parameterFacts, &PX4ParameterLoader::parameterListProgress, this, &PX4AutoPilotPlugin::parameterListProgress); - _airframeFacts = new PX4AirframeLoader(this, uas, this); + _airframeFacts = new PX4AirframeLoader(this, _vehicle->uas(), this); Q_CHECK_PTR(_airframeFacts); PX4ParameterLoader::loadParameterFactMetaData(); @@ -105,7 +104,7 @@ void PX4AutoPilotPlugin::clearStaticData(void) const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void) { if (_components.count() == 0 && !_incorrectParameterVersion) { - Q_ASSERT(_uas); + Q_ASSERT(_vehicle); if (pluginReady()) { bool noRCTransmitter = false; @@ -114,34 +113,34 @@ const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void) noRCTransmitter = rcFact->value().toInt() == 1; } - _airframeComponent = new AirframeComponent(_uas, this); + _airframeComponent = new AirframeComponent(_vehicle->uas(), this); Q_CHECK_PTR(_airframeComponent); _airframeComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent)); if (!noRCTransmitter) { - _radioComponent = new RadioComponent(_uas, this); + _radioComponent = new RadioComponent(_vehicle->uas(), this); Q_CHECK_PTR(_radioComponent); _radioComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_radioComponent)); - _flightModesComponent = new FlightModesComponent(_uas, this); + _flightModesComponent = new FlightModesComponent(_vehicle->uas(), this); Q_CHECK_PTR(_flightModesComponent); _flightModesComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_flightModesComponent)); } - _sensorsComponent = new SensorsComponent(_uas, this); + _sensorsComponent = new SensorsComponent(_vehicle->uas(), this); Q_CHECK_PTR(_sensorsComponent); _sensorsComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_sensorsComponent)); - _powerComponent = new PowerComponent(_uas, this); + _powerComponent = new PowerComponent(_vehicle->uas(), this); Q_CHECK_PTR(_powerComponent); _powerComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_powerComponent)); - _safetyComponent = new SafetyComponent(_uas, this); + _safetyComponent = new SafetyComponent(_vehicle->uas(), this); Q_CHECK_PTR(_safetyComponent); _safetyComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_safetyComponent)); diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h index a8fe0108068518f8c86137f3be4650d807cc691b..7582c3f515112a1972fd89a1de79084ea2ad7d8e 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h @@ -25,8 +25,6 @@ #define PX4AUTOPILOT_H #include "AutoPilotPlugin.h" -#include "AutoPilotPluginManager.h" -#include "UASInterface.h" #include "PX4ParameterLoader.h" #include "PX4AirframeLoader.h" #include "AirframeComponent.h" @@ -35,6 +33,7 @@ #include "SensorsComponent.h" #include "SafetyComponent.h" #include "PowerComponent.h" +#include "Vehicle.h" #include @@ -47,7 +46,7 @@ class PX4AutoPilotPlugin : public AutoPilotPlugin Q_OBJECT public: - PX4AutoPilotPlugin(UASInterface* uas, QObject* parent); + PX4AutoPilotPlugin(Vehicle* vehicle, QObject* parent); ~PX4AutoPilotPlugin(); // Overrides from AutoPilotPlugin diff --git a/src/AutoPilotPlugins/PX4/PX4ParameterLoader.cc b/src/AutoPilotPlugins/PX4/PX4ParameterLoader.cc index 27f4d333d70b013d320734e393578bd14e0d3a8c..aea72c11f4e4ddd93139dd04a864c7fc6c662b59 100644 --- a/src/AutoPilotPlugins/PX4/PX4ParameterLoader.cc +++ b/src/AutoPilotPlugins/PX4/PX4ParameterLoader.cc @@ -38,10 +38,10 @@ QGC_LOGGING_CATEGORY(PX4ParameterLoaderLog, "PX4ParameterLoaderLog") bool PX4ParameterLoader::_parameterMetaDataLoaded = false; QMap PX4ParameterLoader::_mapParameterName2FactMetaData; -PX4ParameterLoader::PX4ParameterLoader(AutoPilotPlugin* autopilot, UASInterface* uas, QObject* parent) : - ParameterLoader(autopilot, uas, parent) +PX4ParameterLoader::PX4ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, QObject* parent) : + ParameterLoader(autopilot, vehicle, parent) { - Q_ASSERT(uas); + Q_ASSERT(vehicle); } /// Converts a string to a typed QVariant diff --git a/src/AutoPilotPlugins/PX4/PX4ParameterLoader.h b/src/AutoPilotPlugins/PX4/PX4ParameterLoader.h index c90631c0ec2a4286fa347cddd4fd3ea341eba0bf..8d4b8b91d30f01a33e149fc39f2cdef903f2d84c 100644 --- a/src/AutoPilotPlugins/PX4/PX4ParameterLoader.h +++ b/src/AutoPilotPlugins/PX4/PX4ParameterLoader.h @@ -31,8 +31,8 @@ #include "ParameterLoader.h" #include "FactSystem.h" -#include "UASInterface.h" #include "AutoPilotPlugin.h" +#include "Vehicle.h" /// @file /// @author Don Gagne @@ -47,7 +47,7 @@ class PX4ParameterLoader : public ParameterLoader public: /// @param uas Uas which this set of facts is associated with - PX4ParameterLoader(AutoPilotPlugin* autpilot,UASInterface* uas, QObject* parent = NULL); + PX4ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, QObject* parent = NULL); /// Override from ParameterLoader virtual QString getDefaultComponentIdParam(void) const { return QString("SYS_AUTOSTART"); } diff --git a/src/AutoPilotPlugins/PX4/PowerComponentController.cc b/src/AutoPilotPlugins/PX4/PowerComponentController.cc index 6892985e94f6de42bb08f2ea3907d72802ab93c0..bcd085a517d1c0ac5b918ba63983866ccd5bf8cb 100644 --- a/src/AutoPilotPlugins/PX4/PowerComponentController.cc +++ b/src/AutoPilotPlugins/PX4/PowerComponentController.cc @@ -26,7 +26,6 @@ #include "PowerComponentController.h" #include "QGCMAVLink.h" -#include "UASManager.h" #include "QGCMessageBox.h" #include @@ -71,7 +70,7 @@ void PowerComponentController::_handleUASTextMessage(int uasId, int compId, int Q_UNUSED(compId); Q_UNUSED(severity); - UASInterface* uas = _autopilot->uas(); + UASInterface* uas = _autopilot->vehicle()->uas(); Q_ASSERT(uas); if (uasId != uas->getUASID()) { return; diff --git a/src/AutoPilotPlugins/PX4/RadioComponentController.cc b/src/AutoPilotPlugins/PX4/RadioComponentController.cc index 1117d4460b63ebb33c10be6dc52c65e42e3466ba..f18f2055a4148a1fa88ffa9a8e1ce986f1f57437 100644 --- a/src/AutoPilotPlugins/PX4/RadioComponentController.cc +++ b/src/AutoPilotPlugins/PX4/RadioComponentController.cc @@ -26,7 +26,6 @@ /// @author Don Gagne @@ -208,7 +207,7 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in Q_UNUSED(compId); Q_UNUSED(severity); - UASInterface* uas = _autopilot->uas(); + UASInterface* uas = _autopilot->vehicle()->uas(); Q_ASSERT(uas); if (uasId != uas->getUASID()) { return; @@ -427,7 +426,7 @@ void SensorsComponentController::_refreshParams(void) bool SensorsComponentController::fixedWing(void) { - UASInterface* uas = _autopilot->uas(); + UASInterface* uas = _autopilot->vehicle()->uas(); Q_ASSERT(uas); return uas->getSystemType() == MAV_TYPE_FIXED_WING || uas->getSystemType() == MAV_TYPE_VTOL_DUOROTOR || diff --git a/src/FactSystem/FactControls/FactPanelController.cc b/src/FactSystem/FactControls/FactPanelController.cc index c6bfaa309732a3d985064dbe0b111d3d9db8afae..44aa7a9501a4ee01c331e1b00dcfd04ee6734f99 100644 --- a/src/FactSystem/FactControls/FactPanelController.cc +++ b/src/FactSystem/FactControls/FactPanelController.cc @@ -22,8 +22,7 @@ ======================================================================*/ #include "FactPanelController.h" -#include "UASManager.h" -#include "AutoPilotPluginManager.h" +#include "MultiVehicleManager.h" #include "QGCMessageBox.h" #include @@ -34,15 +33,15 @@ QGC_LOGGING_CATEGORY(FactPanelControllerLog, "FactPanelControllerLog") FactPanelController::FactPanelController(void) : - _autopilot(NULL), _factPanel(NULL) { - // FIXME: Get rid of these asserts + _vehicle = MultiVehicleManager::instance()->activeVehicle(); + Q_ASSERT(_vehicle); - _uas = UASManager::instance()->getActiveUAS(); + _uas = _vehicle->uas(); Q_ASSERT(_uas); - _autopilot = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_uas); + _autopilot = _vehicle->autopilotPlugin(); Q_ASSERT(_autopilot); Q_ASSERT(_autopilot->pluginReady()); diff --git a/src/FactSystem/FactControls/FactPanelController.h b/src/FactSystem/FactControls/FactPanelController.h index e2da76fd12cadd136ef6de87384a10a2b630d134..69dc7ee17400986550ee01f8fd067fe21755745b 100644 --- a/src/FactSystem/FactControls/FactPanelController.h +++ b/src/FactSystem/FactControls/FactPanelController.h @@ -32,7 +32,6 @@ #include "UASInterface.h" #include "AutoPilotPlugin.h" -#include "UASManagerInterface.h" #include "QGCLoggingCategory.h" Q_DECLARE_LOGGING_CATEGORY(FactPanelControllerLog) @@ -62,8 +61,9 @@ protected: /// Report a missing parameter to the FactPanel Qml element void _reportMissingParameter(int componentId, const QString& name); - UASInterface* _uas; - QSharedPointer _autopilot; + Vehicle* _vehicle; + UASInterface* _uas; + AutoPilotPlugin* _autopilot; private slots: void _checkForMissingFactPanel(void); diff --git a/src/FactSystem/FactSystem.cc b/src/FactSystem/FactSystem.cc index 32409fabfbb2b76e746df65fbb2fee3d059f3d0d..ae10d1ceefe4d188b61f5677f2039aa2f5d21fd4 100644 --- a/src/FactSystem/FactSystem.cc +++ b/src/FactSystem/FactSystem.cc @@ -25,8 +25,6 @@ /// @author Don Gagne #include "FactSystem.h" -#include "UASManager.h" -#include "QGCApplication.h" #include "FactPanelController.h" #include diff --git a/src/FactSystem/FactSystemTestBase.cc b/src/FactSystem/FactSystemTestBase.cc index 5650f708f4f16b0ac6dc169e652d05ff25c312b9..01168f0a9594554d57c44b6a39c8d2b8535b2ebf 100644 --- a/src/FactSystem/FactSystemTestBase.cc +++ b/src/FactSystem/FactSystemTestBase.cc @@ -27,8 +27,7 @@ #include "FactSystemTestBase.h" #include "LinkManager.h" #include "MockLink.h" -#include "AutoPilotPluginManager.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" #include "QGCApplication.h" #include "QGCMessageBox.h" #include "QGCQuickWidget.h" @@ -45,45 +44,20 @@ void FactSystemTestBase::_init(MAV_AUTOPILOT autopilot) { UnitTest::init(); - LinkManager* _linkMgr = LinkManager::instance(); - MockLink* link = new MockLink(); link->setAutopilotType(autopilot); - _linkMgr->_addLink(link); - - if (autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA) { - // Connect will pop a warning dialog - setExpectedMessageBox(QGCMessageBox::Ok); - } - _linkMgr->connectLink(link); - - // Wait for the uas to work it's way through the various threads - - QSignalSpy spyUas(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*))); - QCOMPARE(spyUas.wait(5000), true); + LinkManager::instance()->_addLink(link); - if (autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA) { - checkExpectedMessageBox(); - } + LinkManager::instance()->connectLink(link); - _uas = UASManager::instance()->getActiveUAS(); - Q_ASSERT(_uas); + // Wait for the Vehicle to get created + QSignalSpy spyVehicle(MultiVehicleManager::instance(), SIGNAL(parameterReadyVehicleAvailableChanged(bool))); + QCOMPARE(spyVehicle.wait(5000), true); + QVERIFY(MultiVehicleManager::instance()->parameterReadyVehicleAvailable()); + QVERIFY(MultiVehicleManager::instance()->activeVehicle()); - // Get the plugin for the uas - - AutoPilotPluginManager* pluginMgr = AutoPilotPluginManager::instance(); - Q_ASSERT(pluginMgr); - - _plugin = pluginMgr->getInstanceForAutoPilotPlugin(_uas).data(); + _plugin = MultiVehicleManager::instance()->activeVehicle()->autopilotPlugin(); Q_ASSERT(_plugin); - - // Wait for the plugin to be ready - - QSignalSpy spyPlugin(_plugin, SIGNAL(pluginReadyChanged(bool))); - if (!_plugin->pluginReady()) { - QCOMPARE(spyPlugin.wait(60000), true); - } - Q_ASSERT(_plugin->pluginReady()); } void FactSystemTestBase::_cleanup(void) diff --git a/src/FactSystem/FactSystemTestBase.h b/src/FactSystem/FactSystemTestBase.h index 0a6cf311939d81143bf7f9a238523ae9f67d9bdb..86af306a0c75b3d7e0f856df2297b2b4b2186853 100644 --- a/src/FactSystem/FactSystemTestBase.h +++ b/src/FactSystem/FactSystemTestBase.h @@ -48,9 +48,7 @@ protected: void _qml_test(void); void _qmlUpdate_test(void); - UASInterface* _uas; AutoPilotPlugin* _plugin; - LinkManager* _linkMgr; }; #endif diff --git a/src/FactSystem/FactSystemTestGeneric.cc b/src/FactSystem/FactSystemTestGeneric.cc index dc86e1ff6a40706fc81a4635a38aad2f0bcecd99..80536cc3d39369f8088fa947c27070c5595b6aa6 100644 --- a/src/FactSystem/FactSystemTestGeneric.cc +++ b/src/FactSystem/FactSystemTestGeneric.cc @@ -25,14 +25,7 @@ /// @author Don Gagne #include "FactSystemTestGeneric.h" -#include "LinkManager.h" -#include "MockLink.h" -#include "AutoPilotPluginManager.h" -#include "UASManager.h" -#include "QGCApplication.h" -#include "QGCQuickWidget.h" - -#include +#include "QGCMAVLink.h" UT_REGISTER_TEST(FactSystemTestGeneric) diff --git a/src/FactSystem/FactSystemTestPX4.cc b/src/FactSystem/FactSystemTestPX4.cc index 2e30f72a0c27b35e735a677fd3405e0f3b8c69e5..8a53049403fe0325b76925eda290acf539d10611 100644 --- a/src/FactSystem/FactSystemTestPX4.cc +++ b/src/FactSystem/FactSystemTestPX4.cc @@ -25,14 +25,7 @@ /// @author Don Gagne #include "FactSystemTestPX4.h" -#include "LinkManager.h" -#include "MockLink.h" -#include "AutoPilotPluginManager.h" -#include "UASManager.h" -#include "QGCApplication.h" -#include "QGCQuickWidget.h" - -#include +#include "QGCMAVLink.h" UT_REGISTER_TEST(FactSystemTestPX4) diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index 30e39784ed59b985088f7bb4a8a56974ef7dd6d1..0c152ccbccf69c5573fa3f508b9218ae5b83fbca 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -38,10 +38,10 @@ QGC_LOGGING_CATEGORY(ParameterLoaderLog, "ParameterLoaderLog") Fact ParameterLoader::_defaultFact; -ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, UASInterface* uas, QObject* parent) : +ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, QObject* parent) : QObject(parent), _autopilot(autopilot), - _uas(uas), + _vehicle(vehicle), _mavlink(MAVLinkProtocol::instance()), _parametersReady(false), _initialLoadComplete(false), @@ -49,7 +49,7 @@ ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, UASInterface* uas, _totalParamCount(0) { Q_ASSERT(_autopilot); - Q_ASSERT(_uas); + Q_ASSERT(_vehicle); Q_ASSERT(_mavlink); // We signal this to ouselves in order to start timer on our thread @@ -60,7 +60,7 @@ ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, UASInterface* uas, connect(&_waitingParamTimeoutTimer, &QTimer::timeout, this, &ParameterLoader::_waitingParamTimeout); // FIXME: Why not direct connect? - connect(_uas, SIGNAL(parameterUpdate(int, int, QString, int, int, int, QVariant)), this, SLOT(_parameterUpdate(int, int, QString, int, int, int, QVariant))); + connect(_vehicle->uas(), SIGNAL(parameterUpdate(int, int, QString, int, int, int, QVariant)), this, SLOT(_parameterUpdate(int, int, QString, int, int, int, QVariant))); // Request full param list refreshAllParameters(); @@ -77,7 +77,7 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param bool setMetaData = false; // Is this for our uas? - if (uasId != _uas->getUASID()) { + if (uasId != _vehicle->id()) { return; } @@ -297,8 +297,8 @@ void ParameterLoader::refreshAllParameters(void) Q_ASSERT(mavlink); mavlink_message_t msg; - mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _uas->getUASID(), MAV_COMP_ID_ALL); - _uas->sendMessage(msg); + mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), MAV_COMP_ID_ALL); + _vehicle->sendMessage(msg); qCDebug(ParameterLoaderLog) << "Request to refresh all parameters"; } @@ -490,16 +490,16 @@ void ParameterLoader::_readParameterRaw(int componentId, const QString& paramNam mavlink_msg_param_request_read_pack(_mavlink->getSystemId(), // Our system id _mavlink->getComponentId(), // Our component id &msg, // Pack into this mavlink_message_t - _uas->getUASID(), // Target system id + _vehicle->id(), // Target system id componentId, // Target component id fixedParamName, // Named parameter being requested paramIndex); // Parameter index being requested, -1 for named - _uas->sendMessage(msg); + _vehicle->sendMessage(msg); } void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramName, const QVariant& value) { - bool floatHack = _uas->getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA; + bool floatHack = _vehicle->firmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA; mavlink_param_set_t p; mavlink_param_union_t union_value; @@ -566,21 +566,21 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa } p.param_value = union_value.param_float; - p.target_system = (uint8_t)_uas->getUASID(); + p.target_system = (uint8_t)_vehicle->id(); p.target_component = (uint8_t)componentId; strncpy(p.param_id, paramName.toStdString().c_str(), sizeof(p.param_id)); mavlink_message_t msg; mavlink_msg_param_set_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &p); - _uas->sendMessage(msg); + _vehicle->sendMessage(msg); } void ParameterLoader::_saveToEEPROM(void) { mavlink_message_t msg; - mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _uas->getUASID(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0); - _uas->sendMessage(msg); + mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _vehicle->id(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0); + _vehicle->sendMessage(msg); qCDebug(ParameterLoaderLog) << "_saveToEEPROM"; } @@ -595,11 +595,11 @@ QString ParameterLoader::readParametersFromStream(QTextStream& stream) QStringList wpParams = line.split("\t"); int lineMavId = wpParams.at(0).toInt(); if (wpParams.size() == 5) { - if (!userWarned && (_uas->getUASID() != lineMavId)) { + if (!userWarned && (_vehicle->id() != lineMavId)) { userWarned = true; QString msg("The parameters in the stream have been saved from System Id %1, but the current vehicle has the System Id %2."); QGCMessageBox::StandardButton button = QGCMessageBox::warning("Parameter Load", - msg.arg(lineMavId).arg(_uas->getUASID()), + msg.arg(lineMavId).arg(_vehicle->id()), QGCMessageBox::Ok | QGCMessageBox::Cancel, QGCMessageBox::Cancel); if (button == QGCMessageBox::Cancel) { @@ -649,7 +649,7 @@ void ParameterLoader::writeParametersToStream(QTextStream &stream, const QString Fact* fact = _mapParameterName2Variant[componentId][paramName].value(); Q_ASSERT(fact); - stream << _uas->getUASID() << "\t" << componentId << "\t" << paramName << "\t" << fact->valueString() << "\t" << QString("%1").arg(_factTypeToMavType(fact->type())) << "\n"; + stream << _vehicle->id() << "\t" << componentId << "\t" << paramName << "\t" << fact->valueString() << "\t" << QString("%1").arg(_factTypeToMavType(fact->type())) << "\n"; } } diff --git a/src/FactSystem/ParameterLoader.h b/src/FactSystem/ParameterLoader.h index 1906a9685f8a2a5af9fd31fdddc0a5b769e67f61..9a4cb99f620bf27c91cfdde0a457eb4ec998cf92 100644 --- a/src/FactSystem/ParameterLoader.h +++ b/src/FactSystem/ParameterLoader.h @@ -31,10 +31,10 @@ #include #include "FactSystem.h" -#include "UASInterface.h" #include "MAVLinkProtocol.h" #include "AutoPilotPlugin.h" #include "QGCMAVLink.h" +#include "Vehicle.h" /// @file /// @author Don Gagne @@ -48,7 +48,7 @@ class ParameterLoader : public QObject public: /// @param uas Uas which this set of facts is associated with - ParameterLoader(AutoPilotPlugin* autopilot, UASInterface* uas, QObject* parent = NULL); + ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, QObject* parent = NULL); ~ParameterLoader(); @@ -123,7 +123,7 @@ private: void _checkInitialLoadComplete(void); AutoPilotPlugin* _autopilot; - UASInterface* _uas; + Vehicle* _vehicle; MAVLinkProtocol* _mavlink; /// First mapping is by component id diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index e9d6b1d3c936d730060ab97ed33646d4303bd3f8..f4f42243d87936da74e774ce181d4aebb81dce18 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -86,8 +86,10 @@ G_END_DECLS #include "VehicleComponent.h" #include "MavManager.h" #include "FirmwarePluginManager.h" +#include "MultiVehicleManager.h" #include "Generic/GenericFirmwarePlugin.h" #include "PX4/PX4FirmwarePlugin.h" +#include "Vehicle.h" #ifdef QGC_RTLAB_ENABLED #include "OpalLink.h" @@ -339,6 +341,7 @@ void QGCApplication::_initCommon(void) qmlRegisterUncreatableType("QGroundControl.AutoPilotPlugin", 1, 0, "AutoPilotPlugin", "Can only reference, cannot create"); qmlRegisterUncreatableType("QGroundControl.AutoPilotPlugin", 1, 0, "VehicleComponent", "Can only reference, cannot create"); + qmlRegisterUncreatableType("QGroundControl.Vehicle", 1, 0, "Vehicle", "Can only reference, cannot create"); qmlRegisterType("QGroundControl.Controllers", 1, 0, "ViewWidgetController"); qmlRegisterType("QGroundControl.Controllers", 1, 0, "ParameterEditorController"); @@ -379,9 +382,6 @@ void QGCApplication::_initCommon(void) "Your saved settings have been reset to defaults.")); } - _styleIsDark = settings.value(_styleKey, _styleIsDark).toBool(); - _loadCurrentStyle(); - // Load saved files location and validate QString savedFilesLocation; @@ -421,6 +421,9 @@ bool QGCApplication::_initForNormalAppBoot(void) _createSingletons(); + _styleIsDark = settings.value(_styleKey, _styleIsDark).toBool(); + _loadCurrentStyle(); + // Show splash screen QPixmap splashImage(":/res/SplashScreen"); QSplashScreen* splashScreen = new QSplashScreen(splashImage); @@ -583,6 +586,11 @@ void QGCApplication::_createSingletons(void) Q_UNUSED(firmwarePluginManager); Q_ASSERT(firmwarePluginManager); + // No dependencies + MultiVehicleManager* multiVehicleManager = MultiVehicleManager::_createSingleton(); + Q_UNUSED(multiVehicleManager); + Q_ASSERT(multiVehicleManager); + // No dependencies GAudioOutput* audio = GAudioOutput::_createSingleton(); Q_UNUSED(audio); @@ -631,11 +639,6 @@ void QGCApplication::_destroySingletons(void) LinkManager::instance()->_shutdown(); } - if (UASManager::instance(true /* nullOk */)) { - // This will delete all uas from the system - UASManager::instance()->_shutdown(); - } - // Let the signals flow through the main thread processEvents(QEventLoop::ExcludeUserInputEvents); @@ -648,6 +651,7 @@ void QGCApplication::_destroySingletons(void) UASManager::_deleteSingleton(); LinkManager::_deleteSingleton(); GAudioOutput::_deleteSingleton(); + MultiVehicleManager::_deleteSingleton(); FirmwarePluginManager::_deleteSingleton(); GenericFirmwarePlugin::_deleteSingleton(); PX4FirmwarePlugin::_deleteSingleton(); diff --git a/src/QGCQuickWidget.cc b/src/QGCQuickWidget.cc index 14899edca663067c00271c3a927e2b2aaf6b263f..4b21422fd3032413fb32bf9d24d6acddfe28b343 100644 --- a/src/QGCQuickWidget.cc +++ b/src/QGCQuickWidget.cc @@ -24,6 +24,7 @@ #include "QGCQuickWidget.h" #include "AutoPilotPluginManager.h" #include "QGCMessageBox.h" +#include "MultiVehicleManager.h" #include #include @@ -38,6 +39,7 @@ QGCQuickWidget::QGCQuickWidget(QWidget* parent) : QQuickWidget(parent) { rootContext()->engine()->addImportPath("qrc:/qml"); + rootContext()->setContextProperty("multiVehicleManager", MultiVehicleManager::instance()); } void QGCQuickWidget::setAutoPilot(AutoPilotPlugin* autoPilot) diff --git a/src/QmlControls/MavManager.cc b/src/QmlControls/MavManager.cc index 7c042cafa34772119ea4fc2803abdc5cdecd4b72..abd0d08ed4a8d469aca5b26c727fc9dcf2192c08 100644 --- a/src/QmlControls/MavManager.cc +++ b/src/QmlControls/MavManager.cc @@ -29,7 +29,7 @@ This file is part of the QGROUNDCONTROL project #include "UAS.h" #include "MainWindow.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" #include "Waypoint.h" #include "MavManager.h" #include "UASMessageHandler.h" @@ -76,9 +76,9 @@ MavManager::MavManager(QObject *parent) , _updateCount(0) { // Connect with UAS signal - _setActiveUAS(UASManager::instance()->getActiveUAS()); - connect(UASManager::instance(), SIGNAL(UASDeleted(UASInterface*)), this, SLOT(_forgetUAS(UASInterface*))); - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(_setActiveUAS(UASInterface*))); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &MavManager::_activeVehicleChanged); + // Refresh timer connect(_refreshTimer, SIGNAL(timeout()), this, SLOT(_checkUpdate())); _refreshTimer->setInterval(UPDATE_TIMER); @@ -103,9 +103,10 @@ QString MavManager::loadSetting(const QString &name, const QString& defaultValue return settings.value(name, defaultValue).toString(); } -void MavManager::_forgetUAS(UASInterface* uas) +void MavManager::_activeVehicleChanged(Vehicle* vehicle) { - if (_mav != NULL && _mav == uas) { + // Disconnect the previous one (if any) + if (_mav) { // Stop listening for system messages disconnect(UASMessageHandler::instance(), &UASMessageHandler::textMessageCountChanged, this, &MavManager::_handleTextMessage); // Disconnect any previously connected active MAV @@ -139,25 +140,17 @@ void MavManager::_forgetUAS(UASInterface* uas) emit mavPresentChanged(); emit satelliteCountChanged(); } -} - -void MavManager::_setActiveUAS(UASInterface* uas) -{ - if (uas == _mav) - return; - // Disconnect the previous one (if any) - if(_mav) { - _forgetUAS(_mav); - } - if (uas) { + + _mav = NULL; + + if (vehicle) { + _mav = vehicle->uas(); // Reset satellite count (no GPS) _satelliteCount = -1; emit satelliteCountChanged(); // Reset connection lost (if any) _currentHeartbeatTimeout = 0; emit heartbeatTimeoutChanged(); - // Set new UAS - _mav = uas; // Listen for system messages connect(UASMessageHandler::instance(), &UASMessageHandler::textMessageCountChanged, this, &MavManager::_handleTextMessage); // Now connect the new UAS diff --git a/src/QmlControls/MavManager.h b/src/QmlControls/MavManager.h index 87e6e1b26a16e6f0e5af4f3713ccfd3d6a41565f..171b79f29e7c6fdaf06ea7d7e0326f8cd26efa06 100644 --- a/src/QmlControls/MavManager.h +++ b/src/QmlControls/MavManager.h @@ -33,7 +33,9 @@ This file is part of the QGROUNDCONTROL project #include #include #include + #include "Waypoint.h" +#include "Vehicle.h" class UASInterface; class UASWaypointManager; @@ -172,6 +174,7 @@ signals: void waypointsChanged (); private slots: + void _activeVehicleChanged(Vehicle* vehicle); void _handleTextMessage (int newCount); /** @brief Attitude from main autopilot / system state */ void _updateAttitude (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp); @@ -183,8 +186,6 @@ private slots: void _updateAltitude (UASInterface* uas, double _altitudeAMSL, double _altitudeWGS84, double _altitudeRelative, double _climbRate, quint64 timestamp); void _updateNavigationControllerErrors (UASInterface* uas, double altitudeError, double speedError, double xtrackError); void _updateNavigationControllerData (UASInterface *uas, float navRoll, float navPitch, float navBearing, float targetBearing, float targetDistance); - void _forgetUAS (UASInterface* uas); - void _setActiveUAS (UASInterface* uas); void _checkUpdate (); void _updateBatteryRemaining (UASInterface*, double voltage, double, double percent, int); void _updateBatteryConsumedChanged (UASInterface*, double current_consumed); diff --git a/src/QmlControls/ParameterEditorController.cc b/src/QmlControls/ParameterEditorController.cc index e254593f6108144d1194a916ae73cff5e7790c6a..9762a21715c0415a8a3a09765b6fb2f453176e23 100644 --- a/src/QmlControls/ParameterEditorController.cc +++ b/src/QmlControls/ParameterEditorController.cc @@ -25,7 +25,6 @@ /// @author Don Gagne #include "ParameterEditorController.h" -#include "UASManager.h" #include "AutoPilotPluginManager.h" #include "QGCFileDialog.h" #include "QGCMessageBox.h" diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc new file mode 100644 index 0000000000000000000000000000000000000000..6dd92e73c1ed3ab4dd335b3ae9a37728643af957 --- /dev/null +++ b/src/Vehicle/MultiVehicleManager.cc @@ -0,0 +1,231 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +/// @file +/// @author Don Gagne + +#include "MultiVehicleManager.h" +#include "AutoPilotPlugin.h" + +IMPLEMENT_QGC_SINGLETON(MultiVehicleManager, MultiVehicleManager) + +MultiVehicleManager::MultiVehicleManager(QObject* parent) : + QGCSingleton(parent) + , _activeVehicleAvailable(false) + , _parameterReadyVehicleAvailable(false) + , _activeVehicle(NULL) + , _offlineWaypointManager(NULL) +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + qmlRegisterUncreatableType("QGroundControl.MultiVehicleManager", 1, 0, "MultiVehicleManager", "Reference only"); +} + +MultiVehicleManager::~MultiVehicleManager() +{ + +} + +bool MultiVehicleManager::notifyHeartbeatInfo(LinkInterface* link, int vehicleId, mavlink_heartbeat_t& heartbeat) +{ + if (!_vehicleMap.contains(vehicleId) && !_ignoreVehicleIds.contains(vehicleId)) { + if (vehicleId == MAVLinkProtocol::instance()->getSystemId()) { + qgcApp()->showToolBarMessage(QString("Warning: A vehicle is using the same system id as QGroundControl: %1").arg(vehicleId)); + } + + QSettings settings; + bool mavlinkVersionCheck = settings.value("VERSION_CHECK_ENABLED", true).toBool(); + if (mavlinkVersionCheck && heartbeat.mavlink_version != MAVLINK_VERSION) { + _ignoreVehicleIds += vehicleId; + qgcApp()->showToolBarMessage(QString("The MAVLink protocol version on vehicle #%1 and QGroundControl differ! " + "It is unsafe to use different MAVLink versions. " + "QGroundControl therefore refuses to connect to vehicle #%1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(vehicleId).arg(heartbeat.mavlink_version).arg(MAVLINK_VERSION)); + return false; + } + + Vehicle* vehicle = new Vehicle(link, vehicleId, (MAV_AUTOPILOT)heartbeat.autopilot); + + if (!vehicle) { + qWarning() << "New Vehicle allocation failed"; + return false; + } + + connect(vehicle, &Vehicle::allLinksDisconnected, this, &MultiVehicleManager::_deleteVehiclePhase1); + connect(vehicle->autopilotPlugin(), &AutoPilotPlugin::pluginReadyChanged, this, &MultiVehicleManager::_autopilotPluginReadyChanged); + + _vehicleMap[vehicleId] = vehicle; + + emit vehicleAdded(vehicle); + + setActiveVehicle(vehicle); + } + + return true; +} + +/// This slot is connected to the Vehicle::allLinksDestroyed signal such that the Vehicle is deleted +/// and all other right things happen when the Vehicle goes away. +void MultiVehicleManager::_deleteVehiclePhase1(void) +{ + Vehicle* vehicle = dynamic_cast(sender()); + if (!vehicle) { + qWarning() << "Dynamic cast failed!"; + return; + } + + _vehicleBeingDeleted = vehicle; + + // Remove from map + bool found; + foreach(int id, _vehicleMap.keys()) { + if (_vehicleMap[id] == _vehicleBeingDeleted) { + _vehicleMap.remove(id); + found = true; + break; + } + } + if (!found) { + qWarning() << "Vehicle not found in map!"; + } + + // First we must signal that a vehicle is no longer available. + _activeVehicleAvailable = false; + _parameterReadyVehicleAvailable = false; + emit activeVehicleAvailableChanged(false); + emit parameterReadyVehicleAvailableChanged(false); + emit vehicleRemoved(vehicle); + + // We must let the above signals flow through the system as well as get back to the main loop event queue + // before we can actually delete the Vehicle. The reason is that Qml may be holding on the references to it. + // Even though the above signals should unload any Qml which has references, that Qml will not be destroyed + // until we get back to the main loop. So we set a short timer which will then fire after Qt has finished + // doing all of its internal nastiness to clean up the Qml. This works for both the normal running case + // as well as the unit testing case whichof course has a different signal flow! + QTimer::singleShot(20, this, &MultiVehicleManager::_deleteVehiclePhase2); +} + +void MultiVehicleManager::_deleteVehiclePhase2 (void) +{ + /// Qml has been notified of vehicle about to go away and should be disconnected from it by now. + /// This means we can now clear the active vehicle property and delete the Vehicle for real. + + Vehicle* newActiveVehicle = NULL; + if (_vehicleMap.count()) { + newActiveVehicle = _vehicleMap.first(); + } + + _activeVehicle = newActiveVehicle; + emit activeVehicleChanged(newActiveVehicle); + + if (_activeVehicle) { + emit activeVehicleAvailableChanged(true); + if (_activeVehicle->autopilotPlugin()->pluginReady()) { + emit parameterReadyVehicleAvailableChanged(true); + } + } + + _vehicleBeingDeleted->deleteLater(); +} + +void MultiVehicleManager::setActiveVehicle(Vehicle* vehicle) +{ + if (vehicle != _activeVehicle) { + if (_activeVehicle) { + // The sequence of signals is very important in order to not leave Qml elements connected + // to a non-existent vehicle. + + // First we must signal that there is no active vehicle available. This will disconnect + // any existing ui from the currently active vehicle. + _activeVehicleAvailable = false; + _parameterReadyVehicleAvailable = false; + emit activeVehicleAvailableChanged(false); + emit parameterReadyVehicleAvailableChanged(false); + } + + // See explanation in _deleteVehiclePhase1 + _vehicleBeingSetActive = vehicle; + QTimer::singleShot(20, this, &MultiVehicleManager::_setActiveVehiclePhase2); + } +} + +void MultiVehicleManager::_setActiveVehiclePhase2(void) +{ + // Now we signal the new active vehicle + _activeVehicle = _vehicleBeingSetActive; + emit activeVehicleChanged(_activeVehicle); + + // And finally vehicle availability + if (_activeVehicle) { + _activeVehicleAvailable = true; + emit activeVehicleAvailableChanged(true); + + if (_activeVehicle->autopilotPlugin()->pluginReady()) { + _parameterReadyVehicleAvailable = true; + emit parameterReadyVehicleAvailableChanged(true); + } + } +} + +void MultiVehicleManager::_autopilotPluginReadyChanged(bool pluginReady) +{ + AutoPilotPlugin* autopilot = dynamic_cast(sender()); + + if (!autopilot) { + qWarning() << "Dynamic cast failed!"; + return; + } + + if (autopilot->vehicle() == _activeVehicle) { + _parameterReadyVehicleAvailable = pluginReady; + emit parameterReadyVehicleAvailableChanged(pluginReady); + } +} + +void MultiVehicleManager::setHomePositionForAllVehicles(double lat, double lon, double alt) +{ + foreach (Vehicle* vehicle, _vehicleMap) { + vehicle->uas()->setHomePosition(lat, lon, alt); + } +} + +UASWaypointManager* MultiVehicleManager::activeWaypointManager(void) +{ + if (_activeVehicle) { + return _activeVehicle->uas()->getWaypointManager(); + } + + if (!_offlineWaypointManager) { + _offlineWaypointManager = new UASWaypointManager(NULL, NULL); + } + return _offlineWaypointManager; +} + +QList MultiVehicleManager::vehicles(void) +{ + QList list; + + foreach (Vehicle* vehicle, _vehicleMap) { + list += vehicle; + } + + return list; +} diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h new file mode 100644 index 0000000000000000000000000000000000000000..e1bd8e5d156d31365a202dd02f51cc7b56cc0d0f --- /dev/null +++ b/src/Vehicle/MultiVehicleManager.h @@ -0,0 +1,106 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +/// @file +/// @author Don Gagne + +#ifndef MultiVehicleManager_H +#define MultiVehicleManager_H + +#include "QGCSingleton.h" +#include "Vehicle.h" +#include "QGCMAVLink.h" +#include "UASWaypointManager.h" + +class MultiVehicleManager : public QGCSingleton +{ + Q_OBJECT + + DECLARE_QGC_SINGLETON(MultiVehicleManager, MultiVehicleManager) + +public: + Q_PROPERTY(bool activeVehicleAvailable READ activeVehicleAvailable NOTIFY activeVehicleAvailableChanged) + Q_PROPERTY(bool parameterReadyVehicleAvailable READ parameterReadyVehicleAvailable NOTIFY parameterReadyVehicleAvailableChanged) + Q_PROPERTY(Vehicle* activeVehicle READ activeVehicle WRITE setActiveVehicle NOTIFY activeVehicleChanged) + + // Property accessors + bool activeVehicleAvailable(void) { return _activeVehicleAvailable; } + bool parameterReadyVehicleAvailable(void) { return _parameterReadyVehicleAvailable; } + Vehicle* activeVehicle(void) { return _activeVehicle; } + void setActiveVehicle(Vehicle* vehicle); + + /// Called to notify that a heartbeat was received with the specified information. MultiVehicleManager + /// will create/update Vehicles as necessary. + /// @param link Heartbeat came through on this link + /// @param vehicleId Mavlink system id for vehicle + /// @param heartbeat Mavlink heartbeat message + /// @return true: continue further processing of this message, false: disregard this message + bool notifyHeartbeatInfo(LinkInterface* link, int vehicleId, mavlink_heartbeat_t& heartbeat); + + Vehicle* getVehicleById(int vehicleId) { return _vehicleMap[vehicleId]; } + + void setHomePositionForAllVehicles(double lat, double lon, double alt); + + UAS* activeUas(void) { return _activeVehicle ? _activeVehicle->uas() : NULL; } + + QList vehicles(void); + + UASWaypointManager* activeWaypointManager(void); + +signals: + void vehicleAdded(Vehicle* vehicle); + void vehicleRemoved(Vehicle* vehicle); + void activeVehicleAvailableChanged(bool activeVehicleAvailable); + void parameterReadyVehicleAvailableChanged(bool parameterReadyVehicleAvailable); + void activeVehicleChanged(Vehicle* activeVehicle); + + void _deleteVehiclePhase2Signal(void); + +private slots: + void _deleteVehiclePhase1(void); + void _deleteVehiclePhase2(void); + void _setActiveVehiclePhase2(void); + void _autopilotPluginReadyChanged(bool pluginReady); + +private: + /// All access to singleton is through MultiVehicleManager::instance + MultiVehicleManager(QObject* parent = NULL); + ~MultiVehicleManager(); + + bool _vehicleExists(int vehicleId); + + bool _activeVehicleAvailable; ///< true: An active vehicle is available + bool _parameterReadyVehicleAvailable; ///< true: An active vehicle with ready parameters is available + Vehicle* _activeVehicle; ///< Currently active vehicle from a ui perspective + + Vehicle* _vehicleBeingDeleted; ///< Vehicle being deleted in queued phases + Vehicle* _vehicleBeingSetActive; ///< Vehicle being set active in queued phases + + QList _ignoreVehicleIds; ///< List of vehicle id for which we ignore further communication + + QMap _vehicleMap; ///< Map of vehicles keyed by id + + UASWaypointManager* _offlineWaypointManager; +}; + +#endif diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc new file mode 100644 index 0000000000000000000000000000000000000000..4888833614c577797b7917bb637634a8ca61810f --- /dev/null +++ b/src/Vehicle/Vehicle.cc @@ -0,0 +1,141 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +/// @file +/// @author Don Gagne + +#include "Vehicle.h" +#include "MAVLinkProtocol.h" +#include "FirmwarePluginManager.h" +#include "LinkManager.h" +#include "FirmwarePlugin.h" +#include "AutoPilotPluginManager.h" + +QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") + +Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) + : _id(vehicleId) + , _firmwareType(firmwareType) + , _uas(NULL) +{ + _addLink(link); + + connect(MAVLinkProtocol::instance(), &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived); + connect(this, &Vehicle::_sendMessageOnThread, this, &Vehicle::_sendMessage, Qt::QueuedConnection); + + _uas = new UAS(MAVLinkProtocol::instance(), this); + + _firmwarePlugin = FirmwarePluginManager::instance()->firmwarePluginForAutopilot(firmwareType); + _autopilotPlugin = AutoPilotPluginManager::instance()->newAutopilotPluginForVehicle(this); +} + +void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) +{ + if (message.sysid != _id) { + return; + } + + if (!_containsLink(link)) { + _addLink(link); + } + + _uas->receiveMessage(message); +} + +bool Vehicle::_containsLink(LinkInterface* link) +{ + foreach (SharedLinkInterface sharedLink, _links) { + if (sharedLink.data() == link) { + return true; + } + } + + return false; +} + +void Vehicle::_addLink(LinkInterface* link) +{ + if (!_containsLink(link)) { + _links += LinkManager::instance()->sharedPointerForLink(link); + qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16); + connect(LinkManager::instance(), &LinkManager::linkDisconnected, this, &Vehicle::_linkDisconnected); + } +} + +void Vehicle::_linkDisconnected(LinkInterface* link) +{ + qCDebug(VehicleLog) << "_linkDisconnected:" << link->getName(); + qCDebug(VehicleLog) << "link count:" << _links.count(); + + for (int i=0; i<_links.count(); i++) { + if (_links[i].data() == link) { + _links.removeAt(i); + break; + } + } + + if (_links.count() == 0) { + emit allLinksDisconnected(); + } +} + +void Vehicle::sendMessage(mavlink_message_t message) +{ + emit _sendMessageOnThread(message); +} + +void Vehicle::_sendMessage(mavlink_message_t message) +{ + // Emit message on all links that are currently connected + foreach (SharedLinkInterface sharedLink, _links) { + LinkInterface* link = sharedLink.data(); + Q_ASSERT(link); + + if (link->isConnected()) { + MAVLinkProtocol* mavlink = MAVLinkProtocol::instance(); + + // Write message into buffer, prepending start sign + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + int len = mavlink_msg_to_send_buffer(buffer, &message); + static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS; + mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getMavlinkChannel(), message.len, messageKeys[message.msgid]); + + if (link->isConnected()) { + link->writeBytes((const char*)buffer, len); + } else { + qWarning() << "Link not connected"; + } + } + } +} + +QList Vehicle::links(void) +{ + QList list; + + foreach (SharedLinkInterface sharedLink, _links) { + list += sharedLink.data(); + } + + return list; +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h new file mode 100644 index 0000000000000000000000000000000000000000..389974f2502c580ab85d177b9fc0e2cc16d619f3 --- /dev/null +++ b/src/Vehicle/Vehicle.h @@ -0,0 +1,95 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +/// @file +/// @author Don Gagne + +#ifndef Vehicle_H +#define Vehicle_H + +#include + +#include "LinkInterface.h" +#include "QGCMAVLink.h" +#include "UAS.h" + +class FirmwarePlugin; +class AutoPilotPlugin; + +Q_DECLARE_LOGGING_CATEGORY(VehicleLog) + +class Vehicle : public QObject +{ + Q_OBJECT + +public: + Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType); + + Q_PROPERTY(int id READ id CONSTANT) + Q_PROPERTY(AutoPilotPlugin* autopilot MEMBER _autopilotPlugin CONSTANT) + + // Property accesors + int id(void) { return _id; } + MAV_AUTOPILOT firmwareType(void) { return _firmwareType; } + + /// Sends this specified message to all links accociated with this vehicle + void sendMessage(mavlink_message_t message); + + /// Provides access to uas from vehicle. Temporary workaround until UAS is fully phased out. + UAS* uas(void) { return _uas; } + + /// Provides access to uas from vehicle. Temporary workaround until AutoPilotPlugin is fully phased out. + AutoPilotPlugin* autopilotPlugin(void) { return _autopilotPlugin; } + + QList links(void); + +signals: + void allLinksDisconnected(void); + + /// Used internally to move sendMessage call to main thread + void _sendMessageOnThread(mavlink_message_t message); + +private slots: + void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); + void _linkDisconnected(LinkInterface* link); + void _sendMessage(mavlink_message_t message); + +private: + bool _containsLink(LinkInterface* link); + void _addLink(LinkInterface* link); + + int _id; ///< Mavlink system id + MAV_AUTOPILOT _firmwareType; + + FirmwarePlugin* _firmwarePlugin; + AutoPilotPlugin* _autopilotPlugin; + + /// List of all links associated with this vehicle. We keep SharedLinkInterface objects + /// which are QSharedPointer's in order to maintain reference counts across threads. + /// This way Link deletion works correctly. + QList _links; + + UAS* _uas; +}; + +#endif diff --git a/src/VehicleSetup/SetupView.cc b/src/VehicleSetup/SetupView.cc index 7fd8a060a2ff607f35925766eee966d975aead1b..7abd562a0dd8331e199716aa093c89e2cfdfbb47 100644 --- a/src/VehicleSetup/SetupView.cc +++ b/src/VehicleSetup/SetupView.cc @@ -42,23 +42,9 @@ #include SetupView::SetupView(QWidget* parent) : - QGCQmlWidgetHolder(parent), - _uasCurrent(NULL), - _initComplete(false), - _readyAutopilot(NULL) + QGCQmlWidgetHolder(parent) { -#ifdef __mobile__ - _showFirmware = false; -#else - _showFirmware = true; -#endif - - connect(UASManager::instance(), &UASManager::activeUASSet, this, &SetupView::_setActiveUAS); - - getRootContext()->setContextProperty("controller", this); setSource(QUrl::fromUserInput("qrc:/qml/SetupView.qml")); - - _setActiveUAS(UASManager::instance()->getActiveUAS()); } SetupView::~SetupView() @@ -66,37 +52,6 @@ SetupView::~SetupView() } -void SetupView::_setActiveUAS(UASInterface* uas) -{ - if (_uasCurrent) { - disconnect(_autopilot.data(), &AutoPilotPlugin::pluginReadyChanged, this, &SetupView::_pluginReadyChanged); - } - - _pluginReadyChanged(false); - - _uasCurrent = uas; - - if (_uasCurrent) { - _autopilot = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_uasCurrent); - if (_autopilot.data()->pluginReady()) { - _pluginReadyChanged(_autopilot.data()->pluginReady()); - } - connect(_autopilot.data(), &AutoPilotPlugin::pluginReadyChanged, this, &SetupView::_pluginReadyChanged); - } -} - -void SetupView::_pluginReadyChanged(bool pluginReady) -{ - if (pluginReady) { - _readyAutopilot = _autopilot.data(); - emit autopilotChanged(_readyAutopilot); - } else { - _readyAutopilot = NULL; - emit autopilotChanged(NULL); - _autopilot.clear(); - } -} - #ifdef UNITTEST_BUILD void SetupView::showFirmware(void) { @@ -141,8 +96,3 @@ void SetupView::showVehicleComponentSetup(VehicleComponent* vehicleComponent) Q_UNUSED(success); } #endif - -AutoPilotPlugin* SetupView::autopilot(void) -{ - return _readyAutopilot; -} diff --git a/src/VehicleSetup/SetupView.h b/src/VehicleSetup/SetupView.h index c8566251c48e269f0cb35ccff9b687662cbd7a44..85b06c12e2c1615bc70e32340242cac8e17003cb 100644 --- a/src/VehicleSetup/SetupView.h +++ b/src/VehicleSetup/SetupView.h @@ -43,31 +43,12 @@ public: explicit SetupView(QWidget* parent = 0); ~SetupView(); - Q_PROPERTY(AutoPilotPlugin* autopilot READ autopilot NOTIFY autopilotChanged) - Q_PROPERTY (bool showFirmware MEMBER _showFirmware CONSTANT) - #ifdef UNITTEST_BUILD void showFirmware(void); void showParameters(void); void showSummary(void); void showVehicleComponentSetup(VehicleComponent* vehicleComponent); #endif - - AutoPilotPlugin* autopilot(void); - -signals: - void autopilotChanged(AutoPilotPlugin* autopilot); - -private slots: - void _setActiveUAS(UASInterface* uas); - void _pluginReadyChanged(bool pluginReady); - -private: - UASInterface* _uasCurrent; - bool _initComplete; ///< true: parameters are ready and ui has been setup - QSharedPointer _autopilot; // Shared pointer to prevent shutdown ordering problems - AutoPilotPlugin* _readyAutopilot; // NULL if autopilot is not yet read - bool _showFirmware; }; #endif diff --git a/src/VehicleSetup/SetupView.qml b/src/VehicleSetup/SetupView.qml index d0d3b852fcdad7d818b99cec6a4b9df46505e375..59ffb31723855e8b6dc61edc2cbe8cad3934ac5b 100644 --- a/src/VehicleSetup/SetupView.qml +++ b/src/VehicleSetup/SetupView.qml @@ -25,13 +25,14 @@ along with QGROUNDCONTROL. If not, see . /// @brief Setup View /// @author Don Gagne -import QtQuick 2.3 +import QtQuick 2.3 import QtQuick.Controls 1.2 -import QGroundControl.AutoPilotPlugin 1.0 -import QGroundControl.Palette 1.0 -import QGroundControl.Controls 1.0 -import QGroundControl.ScreenTools 1.0 +import QGroundControl.AutoPilotPlugin 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.MultiVehicleManager 1.0 Rectangle { id: topLevel @@ -52,7 +53,7 @@ Rectangle { function showSummaryPanel() { - if (controller.autopilot) { + if (multiVehicleManager.parameterReadyVehicleAvailable) { panelLoader.source = "VehicleSummary.qml"; } else { panelLoader.sourceComponent = disconnectedVehicleSummaryComponent @@ -61,8 +62,8 @@ Rectangle { function showFirmwarePanel() { - if (controller.showFirmware) { - if (controller.autopilot && controller.autopilot.armed) { + if (!ScreenTools.isMobile) { + if (multiVehicleManager.activeVehicleAvailable && multiVehicleManager.activeVehicle.autopilot.armed) { messagePanelText = armedVehicleText panelLoader.sourceComponent = messagePanelComponent } else { @@ -78,7 +79,7 @@ Rectangle { function showVehicleComponentPanel(vehicleComponent) { - if (controller.autopilot.armed) { + if (multiVehicleManager.activeVehicle.autopilot.armed) { messagePanelText = armedVehicleText panelLoader.sourceComponent = messagePanelComponent } else { @@ -91,17 +92,17 @@ Rectangle { } } + Component.onCompleted: showSummaryPanel() + Connections { - target: controller + target: multiVehicleManager - onAutopilotChanged: { + onParameterReadyVehicleAvailableChanged: { summaryButton.checked = true showSummaryPanel() } } - Component.onCompleted: showSummaryPanel() - Component { id: disconnectedVehicleSummaryComponent @@ -163,14 +164,14 @@ Rectangle { imageResource: "/qmlimages/FirmwareUpgradeIcon.png" setupIndicator: false exclusiveGroup: setupButtonGroup - visible: controller.showFirmware + visible: !ScreenTools.isMobile text: "FIRMWARE" onClicked: showFirmwarePanel() } Repeater { - model: controller.autopilot ? controller.autopilot.vehicleComponents : 0 + model: multiVehicleManager.parameterReadyVehicleAvailable ? multiVehicleManager.activeVehicle.autopilot.vehicleComponents : 0 SubMenuButton { width: buttonWidth @@ -187,7 +188,7 @@ Rectangle { width: buttonWidth setupIndicator: false exclusiveGroup: setupButtonGroup - visible: controller.autopilot + visible: multiVehicleManager.parameterReadyVehicleAvailable text: "PARAMETERS" onClicked: showParametersPanel() @@ -202,7 +203,5 @@ Rectangle { anchors.right: parent.right anchors.top: parent.top anchors.bottom: parent.bottom - - property var autopilot: controller.autopilot } } diff --git a/src/VehicleSetup/SetupViewTest.cc b/src/VehicleSetup/SetupViewTest.cc index 424e8c9758d7742f0e9eeaabe57562c5826082c2..c7f05e60f4557e043329d364df299b5ad30c1e55 100644 --- a/src/VehicleSetup/SetupViewTest.cc +++ b/src/VehicleSetup/SetupViewTest.cc @@ -29,7 +29,7 @@ #include "QGCMessageBox.h" #include "SetupView.h" #include "UASManager.h" -#include "AutoPilotPluginManager.h" +#include "MultiVehicleManager.h" UT_REGISTER_TEST(SetupViewTest) @@ -69,17 +69,16 @@ void SetupViewTest::_clickThrough_test(void) link->setAutopilotType(MAV_AUTOPILOT_PX4); LinkManager::instance()->_addLink(link); linkMgr->connectLink(link); - QTest::qWait(5000); // Give enough time for UI to settle and heartbeats to go through + + // Wait for the Vehicle to get created + QSignalSpy spyVehicle(MultiVehicleManager::instance(), SIGNAL(parameterReadyVehicleAvailableChanged(bool))); + QCOMPARE(spyVehicle.wait(5000), true); + QVERIFY(MultiVehicleManager::instance()->parameterReadyVehicleAvailable()); + QVERIFY(MultiVehicleManager::instance()->activeVehicle()); - AutoPilotPlugin* autopilot = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(UASManager::instance()->getActiveUAS()).data(); + AutoPilotPlugin* autopilot = MultiVehicleManager::instance()->activeVehicle()->autopilotPlugin(); Q_ASSERT(autopilot); - QSignalSpy spyPlugin(autopilot, SIGNAL(pluginReadyChanged(bool))); - if (!autopilot->pluginReady()) { - QCOMPARE(spyPlugin.wait(60000), true); - } - Q_ASSERT(autopilot->pluginReady()); - // Switch to the Setup view _mainToolBar->onSetupView(); QTest::qWait(1000); diff --git a/src/VehicleSetup/VehicleSummary.qml b/src/VehicleSetup/VehicleSummary.qml index 89824977ee4f8b41e5f653145b339f7310422ee2..3c711cf67a03fe5582481a22b228cafd7105ced1 100644 --- a/src/VehicleSetup/VehicleSummary.qml +++ b/src/VehicleSetup/VehicleSummary.qml @@ -21,14 +21,15 @@ ======================================================================*/ -import QtQuick 2.2 -import QtQuick.Controls 1.2 -import QtQuick.Controls.Styles 1.2 +import QtQuick 2.2 +import QtQuick.Controls 1.2 +import QtQuick.Controls.Styles 1.2 -import QGroundControl.FactSystem 1.0 -import QGroundControl.Palette 1.0 -import QGroundControl.Controls 1.0 -import QGroundControl.ScreenTools 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.MultiVehicleManager 1.0 Rectangle { width: 600 @@ -64,7 +65,7 @@ Rectangle { "Below you will find a summary of the settings for your vehicle. To the left are the setup menus for each component." : "WARNING: Your vehicle requires setup prior to flight. Please resolve the items marked in red using the menu on the left." - property bool setupComplete: autopilot && autopilot.setupComplete + property bool setupComplete: multiVehicleManager.activeVehicle.autopilot.setupComplete } Item { @@ -78,7 +79,8 @@ Rectangle { spacing: 10 Repeater { - model: autopilot ? autopilot.vehicleComponents : 0 + model: multiVehicleManager.activeVehicle.autopilot.vehicleComponents + // Outer summary item rectangle Rectangle { diff --git a/src/ViewWidgets/CustomCommandWidgetController.cc b/src/ViewWidgets/CustomCommandWidgetController.cc index a4282a286dfb2e3593b1ec1135d08194b785794d..538709d3713a1cca98f3c74b04f3cb849b44de5e 100644 --- a/src/ViewWidgets/CustomCommandWidgetController.cc +++ b/src/ViewWidgets/CustomCommandWidgetController.cc @@ -22,7 +22,7 @@ ======================================================================*/ #include "CustomCommandWidgetController.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" #include "QGCMAVLink.h" #include "QGCFileDialog.h" @@ -34,7 +34,7 @@ const char* CustomCommandWidgetController::_settingsKey = "CustomCommand.QmlFile CustomCommandWidgetController::CustomCommandWidgetController(void) : _uas(NULL) { - _uas = UASManager::instance()->getActiveUAS(); + _uas = MultiVehicleManager::instance()->activeVehicle()->uas(); Q_ASSERT(_uas); QSettings settings; diff --git a/src/ViewWidgets/ViewWidgetController.cc b/src/ViewWidgets/ViewWidgetController.cc index c8434f7f5c2b04a279f15982b4be2b34ba923d95..fb5a30799b62c798c6dd827e40f5192ac1d50249 100644 --- a/src/ViewWidgets/ViewWidgetController.cc +++ b/src/ViewWidgets/ViewWidgetController.cc @@ -23,53 +23,33 @@ #include "ViewWidgetController.h" #include "UASManager.h" -#include "AutoPilotPluginManager.h" +#include "MultiVehicleManager.h" ViewWidgetController::ViewWidgetController(void) : _autopilot(NULL), _uas(NULL) { - _uasManager = UASManager::instance(); - Q_ASSERT(_uasManager); - - connect(_uasManager, &UASManagerInterface::activeUASSet, this, &ViewWidgetController::_activeUasChanged); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &ViewWidgetController::_vehicleAvailable); } -void ViewWidgetController::_activeUasChanged(UASInterface* currentUas) +void ViewWidgetController::_vehicleAvailable(bool available) { - if (currentUas != _uas) { - if (_uas) { - disconnect(_autopilot, &AutoPilotPlugin::pluginReadyChanged, this, &ViewWidgetController::_pluginReadyChanged); - _uas = NULL; - _autopilot = NULL; - emit pluginDisconnected(); - } - - if (currentUas) { - _uas = currentUas; - _autopilot = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(currentUas).data(); - Q_ASSERT(_autopilot); - - connect(_autopilot, &AutoPilotPlugin::pluginReadyChanged, this, &ViewWidgetController::_pluginReadyChanged); - if (_autopilot->pluginReady()) { - _pluginReadyChanged(true); - } - } - } + if (_uas) { + _uas = NULL; + _autopilot = NULL; + emit pluginDisconnected(); + } + + if (available) { + Vehicle* vehicle = MultiVehicleManager::instance()->activeVehicle(); + + _uas = vehicle->uas(); + _autopilot = vehicle->autopilotPlugin(); + Q_ASSERT(_autopilot); + emit pluginConnected(QVariant::fromValue(_autopilot)); + } } - -void ViewWidgetController::_pluginReadyChanged(bool pluginReady) -{ - Q_ASSERT(_autopilot); - - if (pluginReady) { - emit pluginConnected(QVariant::fromValue(_autopilot)); - } else { - _activeUasChanged(NULL); - } -} - Q_INVOKABLE void ViewWidgetController::checkForVehicle(void) { - _activeUasChanged(_uasManager->getActiveUAS()); + _vehicleAvailable(MultiVehicleManager::instance()->activeVehicle()); } diff --git a/src/ViewWidgets/ViewWidgetController.h b/src/ViewWidgets/ViewWidgetController.h index 64ba1cdcb373ab7d03200cad192dc38a59ddcf0b..70f9c36d887de9456f47c76dc3359c2ad80373e2 100644 --- a/src/ViewWidgets/ViewWidgetController.h +++ b/src/ViewWidgets/ViewWidgetController.h @@ -29,6 +29,7 @@ #include "UASInterface.h" #include "AutoPilotPlugin.h" #include "UASManagerInterface.h" +#include "Vehicle.h" class ViewWidgetController : public QObject { @@ -44,12 +45,10 @@ signals: void pluginDisconnected(void); private slots: - void _activeUasChanged(UASInterface* UAS); - void _pluginReadyChanged(bool pluginReady); + void _vehicleAvailable(bool available); private: AutoPilotPlugin* _autopilot; - UASManagerInterface* _uasManager; UASInterface* _uas; }; diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 57ad20b4b219ee449f877801672c0f10b9f44b08..379a2cde65d40cb935133ac96579ac47c9983539 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -20,16 +20,15 @@ #include "MAVLinkProtocol.h" #include "UASInterface.h" -#include "UASManager.h" #include "UASInterface.h" #include "UAS.h" #include "configuration.h" #include "LinkManager.h" #include "QGCMAVLink.h" -#include "QGCMAVLinkUASFactory.h" #include "QGC.h" #include "QGCApplication.h" #include "QGCLoggingCategory.h" +#include "MultiVehicleManager.h" Q_DECLARE_METATYPE(mavlink_message_t) IMPLEMENT_QGC_SINGLETON(MAVLinkProtocol, MAVLinkProtocol) @@ -342,57 +341,13 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) } } - // ORDER MATTERS HERE! - // If the matching UAS object does not yet exist, it has to be created - // before emitting the packetReceived signal - - UASInterface* uas = UASManager::instance()->getUASForId(message.sysid); - - // Check and (if necessary) create UAS object - if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT) - { - // ORDER MATTERS HERE! - // The UAS object has first to be created and connected, - // only then the rest of the application can be made aware - // of its existence, as it only then can send and receive - // it's first messages. - - // Check if the UAS has the same id like this system - if (message.sysid == getSystemId()) - { - emit protocolStatusMessage(tr("MAVLink Protocol"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId())); - } - - // Create a new UAS based on the heartbeat received - // Todo dynamically load plugin at run-time for MAV - // WIKISEARCH:AUTOPILOT_TYPE_INSTANTIATION - - // First create new UAS object - // Decode heartbeat message + if (message.msgid == MAVLINK_MSG_ID_HEARTBEAT) { + // Notify the vehicle manager of the heartbeat. This will create/update vehicles as needed. mavlink_heartbeat_t heartbeat; - // Reset version field to 0 - heartbeat.mavlink_version = 0; mavlink_msg_heartbeat_decode(&message, &heartbeat); - - // Check if the UAS has a different protocol version - if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION)) - { - // Bring up dialog to inform user - if (!versionMismatchIgnore) - { - emit protocolStatusMessage(tr("MAVLink Protocol"), tr("The MAVLink protocol version on the MAV and QGroundControl mismatch! " - "It is unsafe to use different MAVLink versions. " - "QGroundControl therefore refuses to connect to system %1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(message.sysid).arg(heartbeat.mavlink_version).arg(MAVLINK_VERSION)); - versionMismatchIgnore = true; - } - - // Ignore this message and continue gracefully + if (!MultiVehicleManager::instance()->notifyHeartbeatInfo(link, message.sysid, heartbeat)) { continue; } - - // Create a new UAS object - uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, (MAV_AUTOPILOT)heartbeat.autopilot); - } // Increase receive counter diff --git a/src/comm/MockLink.params b/src/comm/MockLink.params index 6261bbaae67c75c516f7a96a1ec7f26c9e521f3b..d2b20b4d4f94137834145d3ff35ec3a04225b843 100644 --- a/src/comm/MockLink.params +++ b/src/comm/MockLink.params @@ -237,7 +237,7 @@ 1 50 LNDMC_Z_VEL_MAX 0.3 9 1 50 MAV_COMP_ID 50 6 1 50 MAV_FWDEXTSP 1 6 -1 50 MAV_SYS_ID 1 6 +1 50 MAV_SYS_ID 76 6 1 50 MAV_TYPE 2 6 1 50 MAV_USEHILGPS 0 6 1 50 MC_ACRO_P_MAX 90 9 diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc index cd60e0c5606cfc7d827d6f1d8f6d24491db0b16e..ad950d4297bb813028aedda0211a26241a596c51 100644 --- a/src/input/JoystickInput.cc +++ b/src/input/JoystickInput.cc @@ -16,7 +16,7 @@ #include #include #include "UAS.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" #include "QGC.h" #include #include @@ -49,7 +49,7 @@ JoystickInput::JoystickInput() : { // Make sure we initialize with the correct UAS. - setActiveUAS(UASManager::instance()->getActiveUAS()); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); // Start this thread. This allows the Joystick Settings window to work correctly even w/o any UASes connected. start(); @@ -304,25 +304,14 @@ void JoystickInput::storeJoystickSettings() const settings.endGroup(); } -void JoystickInput::setActiveUAS(UASInterface* uas) +void JoystickInput::_activeVehicleChanged(Vehicle* vehicle) { - // Do nothing if the UAS hasn't changed. - if (uas == this->uas) - { - return; - } - - // Only connect / disconnect is the UAS is of a controllable UAS class - UAS* tmp = 0; if (this->uas) { - tmp = dynamic_cast(this->uas); - if(tmp) - { - disconnect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16,quint8)), tmp, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,quint8))); - disconnect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int))); - } + disconnect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16,quint8)), uas, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,quint8))); + disconnect(this, SIGNAL(actionTriggered(int)), uas, SLOT(triggerAction(int))); uasCanReverse = false; + uas = NULL; } // Save any settings for the last UAS @@ -331,23 +320,22 @@ void JoystickInput::setActiveUAS(UASInterface* uas) storeJoystickSettings(); } - this->uas = uas; - - if (this->uas && (tmp = dynamic_cast(this->uas))) + if (vehicle) { - connect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16,quint8)), tmp, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,quint8))); + uas = vehicle->uas(); + connect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16,quint8)), uas, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,quint8))); qDebug() << "connected joystick"; - connect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int))); - uasCanReverse = tmp->systemCanReverse(); + connect(this, SIGNAL(actionTriggered(int)), uas, SLOT(triggerAction(int))); + uasCanReverse = uas->systemCanReverse(); // Update the joystick settings for a new UAS. autopilotType = uas->getAutopilotType(); systemType = uas->getSystemType(); } - // Make sure any UI elements know we've updated the UAS. The UASManager signal is re-emitted here so that UI elements know to + // Make sure any UI elements know we've updated the UAS. The signal is re-emitted here so that UI elements know to // update their UAS-specific UI. - emit activeUASSet(uas); + emit activeVehicleChanged(vehicle); // Load any joystick-specific settings now that the UAS has changed. if (joystickID > -1) @@ -412,8 +400,8 @@ void JoystickInput::init() setActiveJoystick(activeJoystick); // Now make sure we know what the current UAS is and track changes to it. - setActiveUAS(UASManager::instance()->getActiveUAS()); - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &JoystickInput::_activeVehicleChanged); } void JoystickInput::shutdown() diff --git a/src/input/JoystickInput.h b/src/input/JoystickInput.h index 4b89fe7330caef779051194623872a22b39ae6d9..c3ebc7c7a1c059a008f569128ef12e98d6996806 100644 --- a/src/input/JoystickInput.h +++ b/src/input/JoystickInput.h @@ -52,7 +52,7 @@ This file is part of the PIXHAWK project #include #endif -#include "UASInterface.h" +#include "UAS.h" struct JoystickSettings { QMap axesInverted; ///< Whether each axis should be used inverted from what was reported. @@ -202,7 +202,7 @@ protected: bool done; SDL_Joystick* joystick; - UASInterface* uas; ///< Track the current UAS. + UAS* uas; ///< Track the current UAS. int autopilotType; ///< Cache the autopilotType int systemType; ///< Cache the systemType bool uasCanReverse; ///< Track whether the connect UAS can drive a reverse speed. @@ -303,7 +303,7 @@ signals: /** @brief Signal that the UAS has been updated for this JoystickInput * Note that any UI updates should NOT query this object for joystick details. That should be done in response to the joystickSettingsChanged signal. */ - void activeUASSet(UASInterface*); + void activeVehicleChanged(Vehicle* vehicle); /** @brief Signals that new joystick-specific settings were changed. Useful for triggering updates that at dependent on the current joystick. */ void joystickSettingsChanged(); @@ -314,8 +314,6 @@ signals: public slots: /** @brief Enable or disable emitting the high-level control signals from the joystick. */ void setEnabled(bool enable); - /** @brief Specify the UAS that this input should forward joystickChanged signals and buttonPresses to. */ - void setActiveUAS(UASInterface* uas); /** @brief Switch to a new joystick by ID number. Both buttons and axes are updated with the proper signals emitted. */ void setActiveJoystick(int id); /** @brief Switch calibration mode active */ @@ -372,6 +370,9 @@ public slots: { mode = (JOYSTICK_MODE)newMode; } + +private slots: + void _activeVehicleChanged(Vehicle* vehicle); }; #endif // _JOYSTICKINPUT_H_ diff --git a/src/input/Mouse6dofInput.cpp b/src/input/Mouse6dofInput.cpp index 7ae8e3e655c2e1d5318eb8e637d0f479b3d93f7c..f092616c2995d845906dc4298127d0329873eeeb 100644 --- a/src/input/Mouse6dofInput.cpp +++ b/src/input/Mouse6dofInput.cpp @@ -8,7 +8,7 @@ #include "Mouse6dofInput.h" #include "UAS.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" #include "QGCMessageBox.h" #ifdef QGC_MOUSE_ENABLED_LINUX @@ -38,7 +38,8 @@ Mouse6dofInput::Mouse6dofInput(Mouse3DInput* mouseInput) : bValue(0.0), cValue(0.0) { - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &Mouse6dofInput::_activeVehicleChanged); + // Connect 3DxWare SDK MotionEvent connect(mouseInput, SIGNAL(Move3d(std::vector&)), this, SLOT(motion3DMouse(std::vector&))); connect(mouseInput, SIGNAL(On3dmouseKeyDown(int)), this, SLOT(button3DMouseDown(int))); @@ -62,7 +63,7 @@ Mouse6dofInput::Mouse6dofInput(QWidget* parent) : bValue(0.0), cValue(0.0) { - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &Mouse6dofInput::_activeVehicleChanged); if (!mouseActive) { @@ -111,27 +112,22 @@ Mouse6dofInput::~Mouse6dofInput() done = true; } -void Mouse6dofInput::setActiveUAS(UASInterface* uas) +void Mouse6dofInput::_activeVehicleChanged(Vehicle* vehicle) { - // Only connect / disconnect is the UAS is of a controllable UAS class - UAS* tmp = 0; if (this->uas) { - tmp = dynamic_cast(this->uas); - if(tmp) - { - disconnect(this, SIGNAL(mouse6dofChanged(double,double,double,double,double,double)), tmp, SLOT(setManual6DOFControlCommands(double,double,double,double,double,double))); - // Todo: disconnect button mapping - } + disconnect(this, SIGNAL(mouse6dofChanged(double,double,double,double,double,double)), uas, SLOT(setManual6DOFControlCommands(double,double,double,double,double,double))); + // Todo: disconnect button mapping + uas = NULL; } - this->uas = uas; - - tmp = dynamic_cast(this->uas); - if(tmp) { - connect(this, SIGNAL(mouse6dofChanged(double,double,double,double,double,double)), tmp, SLOT(setManual6DOFControlCommands(double,double,double,double,double,double))); - // Todo: connect button mapping + if (vehicle) { + uas = vehicle->uas(); + + connect(this, SIGNAL(mouse6dofChanged(double,double,double,double,double,double)), uas, SLOT(setManual6DOFControlCommands(double,double,double,double,double,double))); + // Todo: connect button mapping } + if (!isRunning()) { start(); @@ -141,7 +137,7 @@ void Mouse6dofInput::setActiveUAS(UASInterface* uas) void Mouse6dofInput::init() { // Make sure active UAS is set - setActiveUAS(UASManager::instance()->getActiveUAS()); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); } void Mouse6dofInput::run() diff --git a/src/input/Mouse6dofInput.h b/src/input/Mouse6dofInput.h index adb63761df1f4160242f7e73a8ad34ba9db4524a..9fef42f1dc92e7aac3fb14da2cef0c8858639d66 100644 --- a/src/input/Mouse6dofInput.h +++ b/src/input/Mouse6dofInput.h @@ -11,23 +11,24 @@ #include -#ifdef QGC_MOUSE_ENABLED_WIN +#ifdef QGC_MOUSE_ENABLED_WIN #include "Mouse3DInput.h" -#endif //QGC_MOUSE_ENABLED_WIN +#endif //QGC_MOUSE_ENABLED_WIN -#include "UASInterface.h" +#include "UAS.h" +#include "Vehicle.h" class Mouse6dofInput : public QThread { Q_OBJECT public: -#ifdef QGC_MOUSE_ENABLED_WIN +#ifdef QGC_MOUSE_ENABLED_WIN Mouse6dofInput(Mouse3DInput* mouseInput); -#endif //QGC_MOUSE_ENABLED_WIN -#ifdef QGC_MOUSE_ENABLED_LINUX +#endif //QGC_MOUSE_ENABLED_WIN +#ifdef QGC_MOUSE_ENABLED_LINUX Mouse6dofInput(QWidget* parent); -#endif //QGC_MOUSE_ENABLED_LINUX +#endif //QGC_MOUSE_ENABLED_LINUX ~Mouse6dofInput(); void run(); @@ -37,7 +38,7 @@ public: protected: void init(); - UASInterface* uas; + UAS* uas; bool done; bool mouseActive; bool translationActive; @@ -77,18 +78,19 @@ signals: void mouseRotationActiveChanged(bool rotationEnable); public slots: - void setActiveUAS(UASInterface* uas); -#ifdef QGC_MOUSE_ENABLED_WIN +#ifdef QGC_MOUSE_ENABLED_WIN /** @brief Get a motion input from 3DMouse */ void motion3DMouse(std::vector &motionData); /** @brief Get a button input from 3DMouse */ void button3DMouseDown(int button); -#endif //QGC_MOUSE_ENABLED_WIN -#ifdef QGC_MOUSE_ENABLED_LINUX +#endif //QGC_MOUSE_ENABLED_WIN +#ifdef QGC_MOUSE_ENABLED_LINUX /** @brief Get an XEvent to check it for an 3DMouse event (motion or button) */ void handleX11Event(XEvent* event); -#endif //QGC_MOUSE_ENABLED_LINUX +#endif //QGC_MOUSE_ENABLED_LINUX +private slots: + void _activeVehicleChanged(Vehicle* vehicle); }; #endif // MOUSE6DOFINPUT_H diff --git a/src/qgcunittest/FileManagerTest.cc b/src/qgcunittest/FileManagerTest.cc index 0487114393df45b2d3302af5d610e75170a136d1..5bce2a431874568531fe410ad6e7c4bbed2ccb29 100644 --- a/src/qgcunittest/FileManagerTest.cc +++ b/src/qgcunittest/FileManagerTest.cc @@ -25,7 +25,7 @@ /// @author Don Gagne #include "FileManagerTest.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" //UT_REGISTER_TEST(FileManagerTest) @@ -51,13 +51,13 @@ void FileManagerTest::init(void) _fileServer = _mockLink->getFileServer(); QVERIFY(_fileServer != NULL); - // Wait or the UAS to show up - UASManagerInterface* uasManager = UASManager::instance(); - QSignalSpy spyUasCreate(uasManager, SIGNAL(UASCreated(UASInterface*))); - if (!uasManager->getActiveUAS()) { - QCOMPARE(spyUasCreate.wait(10000), true); + // Wait or the Vehicle to show up + MultiVehicleManager* vehicleManager = MultiVehicleManager::instance(); + QSignalSpy spyVehicleCreate(vehicleManager, SIGNAL(activeVehicleChanged(Vehicle*))); + if (!vehicleManager->activeVehicle()) { + QCOMPARE(spyVehicleCreate.wait(10000), true); } - UASInterface* uas = uasManager->getActiveUAS(); + UASInterface* uas = vehicleManager->activeVehicle()->uas(); QVERIFY(uas != NULL); _fileManager = uas->getFileManager(); diff --git a/src/qgcunittest/MainWindowTest.cc b/src/qgcunittest/MainWindowTest.cc index 113e5860de68aaa92993aa6f308ef0aa5ff1b1c4..c0c233e431229c2b11757420583ae04c08d9d17c 100644 --- a/src/qgcunittest/MainWindowTest.cc +++ b/src/qgcunittest/MainWindowTest.cc @@ -29,6 +29,7 @@ #include "MainWindowTest.h" #include "MockLink.h" #include "QGCMessageBox.h" +#include "MultiVehicleManager.h" UT_REGISTER_TEST(MainWindowTest) @@ -68,20 +69,12 @@ void MainWindowTest::_connectWindowClose_test(MAV_AUTOPILOT autopilot) link->setAutopilotType(autopilot); LinkManager::instance()->_addLink(link); - if (autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA) { - // Connect will pop a warning dialog - setExpectedMessageBox(QGCMessageBox::Ok); - } linkMgr->connectLink(link); - // Wait for the uas to work it's way through the various threads + // Wait for the Vehicle to work it's way through the various threads - QSignalSpy spyUas(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*))); - QCOMPARE(spyUas.wait(5000), true); - - if (autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA) { - checkExpectedMessageBox(); - } + QSignalSpy spyVehicle(MultiVehicleManager::instance(), SIGNAL(activeVehicleChanged(Vehicle*))); + QCOMPARE(spyVehicle.wait(5000), true); // Cycle through all the top level views diff --git a/src/qgcunittest/MavlinkLogTest.cc b/src/qgcunittest/MavlinkLogTest.cc index 16482d02fc5f2fa6f825f1cd33e3dfd1dde4da41..e50f8abb2d790467a927429d9cb1e9857e33f5ff 100644 --- a/src/qgcunittest/MavlinkLogTest.cc +++ b/src/qgcunittest/MavlinkLogTest.cc @@ -32,6 +32,7 @@ #include "QGCTemporaryFile.h" #include "QGCApplication.h" #include "UAS.h" +#include "MultiVehicleManager.h" UT_REGISTER_TEST(MavlinkLogTest) @@ -146,12 +147,10 @@ void MavlinkLogTest::_connectLogWorker(bool arm) // Wait for the uas to work it's way through the various threads - QSignalSpy spyUas(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*))); - QCOMPARE(spyUas.wait(5000), true); + QSignalSpy spyVehicle(MultiVehicleManager::instance(), SIGNAL(activeVehicleChanged(Vehicle*))); + QCOMPARE(spyVehicle.wait(5000), true); - UASInterface* uasInterface = UASManager::instance()->getActiveUAS(); - QVERIFY(uasInterface); - UAS* uas = dynamic_cast(uasInterface); + UAS* uas = MultiVehicleManager::instance()->activeUas(); QVERIFY(uas); QDir logSaveDir; diff --git a/src/qgcunittest/PX4RCCalibrationTest.cc b/src/qgcunittest/PX4RCCalibrationTest.cc index 86b12b50e02b2ef6591c8a472002a69931f43477..e5f5160297d31ddff6051084e3affab6cf55a274 100644 --- a/src/qgcunittest/PX4RCCalibrationTest.cc +++ b/src/qgcunittest/PX4RCCalibrationTest.cc @@ -23,8 +23,7 @@ #include "PX4RCCalibrationTest.h" #include "RadioComponentController.h" -#include "UASManager.h" -#include "AutoPilotPluginManager.h" +#include "MultiVehicleManager.h" /// @file /// @brief QRadioComponentController Widget unit test @@ -154,16 +153,15 @@ void RadioConfigTest::init(void) Q_CHECK_PTR(_mockLink); LinkManager::instance()->_addLink(_mockLink); LinkManager::instance()->connectLink(_mockLink); - QTest::qWait(5000); // Give enough time for UI to settle and heartbeats to go through - _autopilot = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(UASManager::instance()->getActiveUAS()).data(); - Q_ASSERT(_autopilot); + // Wait for the Vehicle to get created + QSignalSpy spyVehicle(MultiVehicleManager::instance(), SIGNAL(parameterReadyVehicleAvailableChanged(bool))); + QCOMPARE(spyVehicle.wait(5000), true); + QVERIFY(MultiVehicleManager::instance()->parameterReadyVehicleAvailable()); + QVERIFY(MultiVehicleManager::instance()->activeVehicle()); - QSignalSpy spyPlugin(_autopilot, SIGNAL(pluginReadyChanged(bool))); - if (!_autopilot->pluginReady()) { - QCOMPARE(spyPlugin.wait(60000), true); - } - Q_ASSERT(_autopilot->pluginReady()); + _autopilot = MultiVehicleManager::instance()->activeVehicle()->autopilotPlugin(); + Q_ASSERT(_autopilot); // This will instatiate the widget with an active uas with ready parameters _calWidget = new QGCQmlWidgetHolder(); diff --git a/src/qgcunittest/UASUnitTest.cc b/src/qgcunittest/UASUnitTest.cc deleted file mode 100644 index d5cf5cd273582e71c7fda36dc903fb75403f963c..0000000000000000000000000000000000000000 --- a/src/qgcunittest/UASUnitTest.cc +++ /dev/null @@ -1,299 +0,0 @@ -#include "UASUnitTest.h" -#include -#include - -UT_REGISTER_TEST(UASUnitTest) - -UASUnitTest::UASUnitTest() -{ -} -//This function is called after every test -void UASUnitTest::init() -{ - UnitTest::init(); - - _mavlink = new MAVLinkProtocol(); - _uas = new UAS(_mavlink, UASID); - _uas->deleteSettings(); -} -//this function is called after every test -void UASUnitTest::cleanup() -{ - delete _uas; - _uas = NULL; - - delete _mavlink; - _mavlink = NULL; - - UnitTest::cleanup(); -} - -void UASUnitTest::getUASID_test() -{ - // Test a default ID of zero is assigned - UAS* uas2 = new UAS(_mavlink); - QCOMPARE(uas2->getUASID(), 0); - delete uas2; - - // Test that the chosen ID was assigned at construction - QCOMPARE(_uas->getUASID(), UASID); - - // Make sure that no other ID was set - QEXPECT_FAIL("", "When you set an ID it does not use the default ID of 0", Continue); - QCOMPARE(_uas->getUASID(), 0); - - // Make sure that ID >= 0 - QCOMPARE(_uas->getUASID(), 100); - -} - -void UASUnitTest::getUASName_test() -{ - // Test that the name is build as MAV + ID - QCOMPARE(_uas->getUASName(), "MAV " + QString::number(UASID)); - -} - -void UASUnitTest::getUpTime_test() -{ - UAS* uas2 = new UAS(_mavlink); - // Test that the uptime starts at zero to a - // precision of seconds - QCOMPARE(floor(uas2->getUptime()/1000.0), 0.0); - - // Sleep for three seconds - QTest::qSleep(3000); - - // Test that the up time is computed correctly to a - // precision of seconds - QCOMPARE(floor(uas2->getUptime()/1000.0), 3.0); - - delete uas2; -} - -void UASUnitTest::filterVoltage_test() -{ - float verificar=_uas->filterVoltage(0.4f); - - // We allow the voltage returned to be within a small delta - const float allowedDelta = 0.05f; - const float desiredVoltage = 7.36f; - QVERIFY(verificar > (desiredVoltage - allowedDelta) && verificar < (desiredVoltage + allowedDelta)); -} - -void UASUnitTest:: getAutopilotType_test() -{ - int type = _uas->getAutopilotType(); - // Verify that upon construction the autopilot is set to -1 - QCOMPARE(type, -1); -} - -void UASUnitTest::setAutopilotType_test() -{ - _uas->setAutopilotType(2); - // Verify that the autopilot is set - QCOMPARE(_uas->getAutopilotType(), 2); -} - -//verify that the correct status is returned if a certain statue is given to _uas -void UASUnitTest::getStatusForCode_test() -{ - QString state, desc; - state = ""; - desc = ""; - - _uas->getStatusForCode(MAV_STATE_UNINIT, state, desc); - QVERIFY(state == "UNINIT"); - - _uas->getStatusForCode(MAV_STATE_UNINIT, state, desc); - QVERIFY(state == "UNINIT"); - - _uas->getStatusForCode(MAV_STATE_BOOT, state, desc); - QVERIFY(state == "BOOT"); - - _uas->getStatusForCode(MAV_STATE_CALIBRATING, state, desc); - QVERIFY(state == "CALIBRATING"); - - _uas->getStatusForCode(MAV_STATE_ACTIVE, state, desc); - QVERIFY(state == "ACTIVE"); - - _uas->getStatusForCode(MAV_STATE_STANDBY, state, desc); - QVERIFY(state == "STANDBY"); - - _uas->getStatusForCode(MAV_STATE_CRITICAL, state, desc); - QVERIFY(state == "CRITICAL"); - - _uas->getStatusForCode(MAV_STATE_EMERGENCY, state, desc); - QVERIFY(state == "EMERGENCY"); - - _uas->getStatusForCode(MAV_STATE_POWEROFF, state, desc); - QVERIFY(state == "SHUTDOWN"); - - _uas->getStatusForCode(5325, state, desc); - QVERIFY(state == "UNKNOWN"); -} - -void UASUnitTest::getLocalX_test() -{ - QCOMPARE(_uas->getLocalX(), 0.0); -} -void UASUnitTest::getLocalY_test() -{ - QCOMPARE(_uas->getLocalY(), 0.0); -} -void UASUnitTest::getLocalZ_test() -{ - QCOMPARE(_uas->getLocalZ(), 0.0); -} -void UASUnitTest::getLatitude_test() -{ - QCOMPARE(_uas->getLatitude(), 0.0); -} -void UASUnitTest::getLongitude_test() -{ - QCOMPARE(_uas->getLongitude(), 0.0); -} -void UASUnitTest::getAltitudeAMSL_test() -{ - QCOMPARE(_uas->getAltitudeAMSL(), 0.0); -} -void UASUnitTest::getAltitudeRelative_test() -{ - QCOMPARE(_uas->getAltitudeRelative(), 0.0); -} -void UASUnitTest::getRoll_test() -{ - QCOMPARE(_uas->getRoll(), 0.0); -} -void UASUnitTest::getPitch_test() -{ - QCOMPARE(_uas->getPitch(), 0.0); -} -void UASUnitTest::getYaw_test() -{ - QCOMPARE(_uas->getYaw(), 0.0); -} - -void UASUnitTest::getSelected_test() -{ - QCOMPARE(_uas->getSelected(), false); -} - -void UASUnitTest::getSystemType_test() -{ //check that system type is set to MAV_TYPE_GENERIC when initialized - QCOMPARE(_uas->getSystemType(), 0); - _uas->setSystemType(13); - QCOMPARE(_uas->getSystemType(), 13); -} - -void UASUnitTest::getAirframe_test() -{ - //when _uas is constructed, airframe is set to QGC_AIRFRAME_GENERIC - QVERIFY(_uas->getAirframe() == UASInterface::QGC_AIRFRAME_GENERIC); -} - -void UASUnitTest::setAirframe_test() -{ - //check at construction, that airframe=0 (GENERIC) - QVERIFY(_uas->getAirframe() == UASInterface::QGC_AIRFRAME_GENERIC); - - //check that set airframe works - _uas->setAirframe(UASInterface::QGC_AIRFRAME_HEXCOPTER); - QVERIFY(_uas->getAirframe() == UASInterface::QGC_AIRFRAME_HEXCOPTER); - - //check that setAirframe will not assign a number to airframe, that is - //not defined in the enum - _uas->setAirframe(UASInterface::QGC_AIRFRAME_END_OF_ENUM); - QVERIFY(_uas->getAirframe() == UASInterface::QGC_AIRFRAME_HEXCOPTER); -} - -void UASUnitTest::getWaypointList_test() -{ - QList kk = _uas->getWaypointManager()->getWaypointEditableList(); - QCOMPARE(kk.count(), 0); - - Waypoint* wp = new Waypoint(0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,false, false, MAV_FRAME_GLOBAL, MAV_CMD_MISSION_START, "blah"); - _uas->getWaypointManager()->addWaypointEditable(wp, true); - - kk = _uas->getWaypointManager()->getWaypointEditableList(); - QCOMPARE(kk.count(), 1); - - wp = new Waypoint(); - _uas->getWaypointManager()->addWaypointEditable(wp, false); - - kk = _uas->getWaypointManager()->getWaypointEditableList(); - QCOMPARE(kk.count(), 2); - - _uas->getWaypointManager()->removeWaypoint(1); - kk = _uas->getWaypointManager()->getWaypointEditableList(); - QCOMPARE(kk.count(), 1); - - _uas->getWaypointManager()->removeWaypoint(0); - kk = _uas->getWaypointManager()->getWaypointEditableList(); - QCOMPARE(kk.count(), 0); - - qDebug()<<"disconnect SIGNAL waypointListChanged"; - -} - -void UASUnitTest::getWaypoint_test() -{ - Waypoint* wp = new Waypoint(0,5.6,2.0,3.0,0.0,0.0,0.0,0.0,false, false, MAV_FRAME_GLOBAL, MAV_CMD_MISSION_START, "blah"); - - _uas->getWaypointManager()->addWaypointEditable(wp, true); - - QList wpList = _uas->getWaypointManager()->getWaypointEditableList(); - - QCOMPARE(wpList.count(), 1); - QCOMPARE(static_cast(0), static_cast(wpList.at(0))->getId()); - - Waypoint* wp3 = new Waypoint(1, 5.6, 2.0, 3.0); - _uas->getWaypointManager()->addWaypointEditable(wp3, true); - wpList = _uas->getWaypointManager()->getWaypointEditableList(); - Waypoint* wp2 = static_cast(wpList.at(0)); - - QCOMPARE(wpList.count(), 2); - QCOMPARE(wp3->getX(), wp2->getX()); - QCOMPARE(wp3->getY(), wp2->getY()); - QCOMPARE(wp3->getZ(), wp2->getZ()); - QCOMPARE(wpList.at(1)->getId(), static_cast(1)); - QCOMPARE(wp3->getFrame(), MAV_FRAME_GLOBAL); - QCOMPARE(wp3->getFrame(), wp2->getFrame()); - - delete wp3; - delete wp; -} - -void UASUnitTest::signalWayPoint_test() -{ - QSignalSpy spy(_uas->getWaypointManager(), SIGNAL(waypointEditableListChanged())); - - Waypoint* wp = new Waypoint(0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,false, false, MAV_FRAME_GLOBAL, MAV_CMD_MISSION_START, "blah"); - _uas->getWaypointManager()->addWaypointEditable(wp, true); - - QCOMPARE(spy.count(), 1); // 1 listChanged for add wayPoint - _uas->getWaypointManager()->removeWaypoint(0); - QCOMPARE(spy.count(), 2); // 2 listChanged for remove wayPoint - - QSignalSpy spyDestroyed(_uas->getWaypointManager(), SIGNAL(destroyed())); - QVERIFY(spyDestroyed.isValid()); - QCOMPARE( spyDestroyed.count(), 0 ); - - delete _uas;// delete(destroyed) _uas for validating - _uas = NULL; - QCOMPARE(spyDestroyed.count(), 1);// count destroyed _uas should are 1 - _uas = new UAS(_mavlink, UASID); - QSignalSpy spy2(_uas->getWaypointManager(), SIGNAL(waypointEditableListChanged())); - QCOMPARE(spy2.count(), 0); - Waypoint* wp2 = new Waypoint(0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,false, false, MAV_FRAME_GLOBAL, MAV_CMD_MISSION_START, "blah"); - - _uas->getWaypointManager()->addWaypointEditable(wp2, true); - QCOMPARE(spy2.count(), 1); - - _uas->getWaypointManager()->clearWaypointList(); - QList wpList = _uas->getWaypointManager()->getWaypointEditableList(); - QCOMPARE(wpList.count(), 1); - delete _uas; - _uas = NULL; - delete wp2; -} diff --git a/src/qgcunittest/UASUnitTest.h b/src/qgcunittest/UASUnitTest.h deleted file mode 100644 index ab6b8d008f74cbdc5ed8cad6c839aa1c5c1a5374..0000000000000000000000000000000000000000 --- a/src/qgcunittest/UASUnitTest.h +++ /dev/null @@ -1,57 +0,0 @@ -#ifndef UASUNITTEST_H -#define UASUNITTEST_H - -#include "UAS.h" -#include "MAVLinkProtocol.h" -#include "SerialLink.h" -#include "UASInterface.h" -#include "UnitTest.h" -#include "LinkManager.h" -#include "UASWaypointManager.h" -#include "SerialLink.h" -#include "LinkInterface.h" - -#define UASID 100 - -class UASUnitTest : public UnitTest -{ - Q_OBJECT - -public: - UASUnitTest(void); - -private slots: - void init(void); - void cleanup(void); - - void getUASID_test(void); - void getUASName_test(void); - void getUpTime_test(void); - void filterVoltage_test(void); - void getAutopilotType_test(void); - void setAutopilotType_test(void); - void getStatusForCode_test(void); - void getLocalX_test(void); - void getLocalY_test(void); - void getLocalZ_test(void); - void getLatitude_test(void); - void getLongitude_test(void); - void getAltitudeAMSL_test(void); - void getAltitudeRelative_test(void); - void getRoll_test(void); - void getPitch_test(void); - void getYaw_test(void); - void getSelected_test(void); - void getSystemType_test(void); - void getAirframe_test(void); - void setAirframe_test(void); - void getWaypointList_test(void); - void signalWayPoint_test(void); - void getWaypoint_test(void); - -private: - MAVLinkProtocol* _mavlink; - UAS* _uas; -}; - -#endif diff --git a/src/uas/FileManager.cc b/src/uas/FileManager.cc index 02922b6119fe168193ac12922ebabe0e9d814bab..dc3fe1a56eae4beb2dd4baa71044267f7efe8555 100644 --- a/src/uas/FileManager.cc +++ b/src/uas/FileManager.cc @@ -25,6 +25,7 @@ #include "QGC.h" #include "MAVLinkProtocol.h" #include "MainWindow.h" +#include "Vehicle.h" #include #include @@ -32,17 +33,17 @@ QGC_LOGGING_CATEGORY(FileManagerLog, "FileManagerLog") -FileManager::FileManager(QObject* parent, UASInterface* uas) : +FileManager::FileManager(QObject* parent, Vehicle* vehicle) : QObject(parent), _currentOperation(kCOIdle), - _mav(uas), + _vehicle(vehicle), _lastOutgoingSeqNumber(0), _activeSession(0), _systemIdQGC(0) { connect(&_ackTimer, &QTimer::timeout, this, &FileManager::_ackTimeout); - _systemIdServer = _mav->getUASID(); + _systemIdServer = _vehicle->id(); // Make sure we don't have bad structure packing Q_ASSERT(sizeof(RequestHeader) == 12); @@ -720,7 +721,7 @@ void FileManager::_sendRequest(Request* request) _systemIdQGC = MAVLinkProtocol::instance()->getSystemId(); } - Q_ASSERT(_mav); + Q_ASSERT(_vehicle); mavlink_msg_file_transfer_protocol_pack(_systemIdQGC, // QGC System ID 0, // QGC Component ID &message, // Mavlink Message to pack into @@ -729,5 +730,5 @@ void FileManager::_sendRequest(Request* request) 0, // Target component (uint8_t*)request); // Payload - _mav->sendMessage(message); + _vehicle->sendMessage(message); } diff --git a/src/uas/FileManager.h b/src/uas/FileManager.h index 6f7caf109f3ae747df6d5301de227409b28cd172..3369b650a50854b19ebc097f2d6875a0432cdc6b 100644 --- a/src/uas/FileManager.h +++ b/src/uas/FileManager.h @@ -32,12 +32,14 @@ Q_DECLARE_LOGGING_CATEGORY(FileManagerLog) +class Vehicle; + class FileManager : public QObject { Q_OBJECT public: - FileManager(QObject* parent, UASInterface* uas); + FileManager(QObject* parent, Vehicle* vehicle); /// These methods are only used for testing purposes. bool _sendCmdTestAck(void) { return _sendOpcodeOnlyCmd(kCmdNone, kCOAck); }; @@ -200,7 +202,7 @@ private: OperationState _currentOperation; ///< Current operation of state machine QTimer _ackTimer; ///< Used to signal a timeout waiting for an ack - UASInterface* _mav; + Vehicle* _vehicle; uint16_t _lastOutgoingSeqNumber; ///< Sequence number sent in last outgoing packet diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc deleted file mode 100644 index 42020a548d64774c0b9415973566ed1c62f321b6..0000000000000000000000000000000000000000 --- a/src/uas/QGCMAVLinkUASFactory.cc +++ /dev/null @@ -1,30 +0,0 @@ -#include "QGCMAVLinkUASFactory.h" -#include "UASManager.h" - -QGCMAVLinkUASFactory::QGCMAVLinkUASFactory(QObject *parent) : - QObject(parent) -{ -} - -UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInterface* link, int sysid, MAV_AUTOPILOT autopilotType) -{ - UASInterface* uasInterface; - - UAS* uasObject = new UAS(mavlink, sysid, autopilotType); - Q_CHECK_PTR(uasObject); - uasInterface = uasObject; - - // Connect this robot to the UAS object - // It is IMPORTANT here to use the right object type, - // else the slot of the parent object is called (and thus the special - // packets never reach their goal) - connect(mavlink, &MAVLinkProtocol::messageReceived, uasObject, &UAS::receiveMessage); - - // Make UAS aware that this link can be used to communicate with the actual robot - uasInterface->addLink(link); - - // Now add UAS to "official" list, which makes the whole application aware of it - UASManager::instance()->addUAS(uasInterface); - - return uasInterface; -} diff --git a/src/uas/QGCMAVLinkUASFactory.h b/src/uas/QGCMAVLinkUASFactory.h deleted file mode 100644 index a0820f4af9682c83a3b20ad2bc4265fcfca9a172..0000000000000000000000000000000000000000 --- a/src/uas/QGCMAVLinkUASFactory.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef QGCMAVLINKUASFACTORY_H -#define QGCMAVLINKUASFACTORY_H - -#include - -#include "QGCMAVLink.h" -#include "MAVLinkProtocol.h" -#include "UASInterface.h" -#include "LinkInterface.h" - -// INCLUDE ALL MAV/UAS CLASSES USING MAVLINK -#include "UAS.h" - -class QGCMAVLinkUASFactory : public QObject -{ - Q_OBJECT -public: - explicit QGCMAVLinkUASFactory(QObject *parent = 0); - - /** @brief Create a new UAS object using MAVLink as protocol */ - static UASInterface* createUAS(MAVLinkProtocol* mavlink, LinkInterface* link, int sysid, MAV_AUTOPILOT autopilotType); - -signals: - -public slots: - -}; - -#endif // QGCMAVLINKUASFACTORY_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index db4799e3d6a2c77db7428b055751b5ec04a37d83..505efb0b2a0bceb525defc42b9105f60ac801b54 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -36,6 +36,7 @@ #include "FirmwarePluginManager.h" #include "QGCMessageBox.h" #include "QGCLoggingCategory.h" +#include "Vehicle.h" QGC_LOGGING_CATEGORY(UASLog, "UASLog") @@ -48,10 +49,10 @@ QGC_LOGGING_CATEGORY(UASLog, "UASLog") * creating the UAS. */ -UAS::UAS(MAVLinkProtocol* protocol, int id, MAV_AUTOPILOT autopilotType) : UASInterface(), +UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle) : UASInterface(), lipoFull(4.2f), lipoEmpty(3.5f), - uasId(id), + uasId(vehicle->id()), unknownPackets(), mavlink(protocol), receiveDropRate(0), @@ -60,7 +61,7 @@ UAS::UAS(MAVLinkProtocol* protocol, int id, MAV_AUTOPILOT autopilotType) : UASIn name(""), type(MAV_TYPE_GENERIC), airframe(QGC_AIRFRAME_GENERIC), - autopilot(autopilotType), + autopilot(vehicle->firmwareType()), systemIsArmed(false), base_mode(0), custom_mode(0), @@ -130,8 +131,8 @@ UAS::UAS(MAVLinkProtocol* protocol, int id, MAV_AUTOPILOT autopilotType) : UASIn airSpeed(std::numeric_limits::quiet_NaN()), groundSpeed(std::numeric_limits::quiet_NaN()), - waypointManager(this), - fileManager(this, this), + waypointManager(vehicle, this), + fileManager(this, vehicle), attitudeKnown(false), attitudeStamped(false), @@ -176,7 +177,8 @@ UAS::UAS(MAVLinkProtocol* protocol, int id, MAV_AUTOPILOT autopilotType) : UASIn sensorHil(false), lastSendTimeGPS(0), lastSendTimeSensors(0), - lastSendTimeOpticalFlow(0) + lastSendTimeOpticalFlow(0), + _vehicle(vehicle) { for (unsigned int i = 0; i<255;++i) @@ -185,8 +187,6 @@ UAS::UAS(MAVLinkProtocol* protocol, int id, MAV_AUTOPILOT autopilotType) : UASIn componentMulti[i] = false; } - connect(this, &UAS::_sendMessageOnThread, this, &UAS::_sendMessage, Qt::QueuedConnection); - connect(this, &UAS::_sendMessageOnThreadLink, this, &UAS::_sendMessageLink, Qt::QueuedConnection); connect(mavlink, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), &fileManager, SLOT(receiveMessage(LinkInterface*,mavlink_message_t))); // Store a list of available actions for this UAS. @@ -298,6 +298,7 @@ void UAS::readSettings() QSettings settings; settings.beginGroup(QString("MAV%1").arg(uasId)); this->name = settings.value("NAME", this->name).toString(); + qDebug() << "Name" << name; this->airframe = settings.value("AIRFRAME", this->airframe).toInt(); if (settings.contains("BATTERY_SPECS")) { @@ -375,34 +376,8 @@ void UAS::updateState() } } -/** -* If the acitve UAS (the UAS that was selected) is not the one that is currently -* active, then change the active UAS to the one that was selected. -*/ -void UAS::setSelected() +void UAS::receiveMessage(mavlink_message_t message) { - if (UASManager::instance()->getActiveUAS() != this) - { - UASManager::instance()->setActiveUAS(this); - emit systemSelected(true); - } -} - -/** -* @return if the active UAS is the current UAS -**/ -bool UAS::getSelected() const -{ - return (UASManager::instance()->getActiveUAS() == this); -} - -void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) -{ - if (!_containsLink(link)) { - addLink(link); - // qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName(); - } - if (!components.contains(message.compid)) { QString componentName; @@ -1414,7 +1389,7 @@ void UAS::setHomePosition(double lat, double lon, double alt) mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_DO_SET_HOME, 1, 0, 0, 0, 0, lat, lon, alt); // Send message twice to increase chance that it reaches its goal - sendMessage(msg); + _vehicle->sendMessage(msg); // Send new home position to UAS mavlink_set_gps_global_origin_t home; @@ -1424,7 +1399,7 @@ void UAS::setHomePosition(double lat, double lon, double alt) home.altitude = alt*1000; qDebug() << "lat:" << home.latitude << " lon:" << home.longitude; mavlink_msg_set_gps_global_origin_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &home); - sendMessage(msg); + _vehicle->sendMessage(msg); } else { blockHomePositionChanges = true; } @@ -1444,7 +1419,7 @@ void UAS::setLocalOriginAtCurrentGPSPosition() mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_DO_SET_HOME, 1, 1, 0, 0, 0, 0, 0, 0); // Send message twice to increase chance that it reaches its goal - sendMessage(msg); + _vehicle->sendMessage(msg); } } @@ -1531,7 +1506,7 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType) accelCal, // accel cal airspeedCal, // airspeed cal escCal); // esc cal - sendMessage(msg); + _vehicle->sendMessage(msg); } void UAS::stopCalibration(void) @@ -1551,7 +1526,7 @@ void UAS::stopCalibration(void) 0, // accel cal 0, // airspeed cal 0); // unused - sendMessage(msg); + _vehicle->sendMessage(msg); } void UAS::startBusConfig(UASInterface::StartBusConfigType calType) @@ -1579,7 +1554,7 @@ void UAS::startBusConfig(UASInterface::StartBusConfigType calType) 0, 0, 0); - sendMessage(msg); + _vehicle->sendMessage(msg); } void UAS::stopBusConfig(void) @@ -1599,21 +1574,21 @@ void UAS::stopBusConfig(void) 0, 0, 0); - sendMessage(msg); + _vehicle->sendMessage(msg); } void UAS::startDataRecording() { mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 2, 0, 0, 0); - sendMessage(msg); + _vehicle->sendMessage(msg); } void UAS::stopDataRecording() { mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 0, 0, 0, 0); - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -1783,7 +1758,7 @@ void UAS::setModeArm(uint8_t newBaseMode, uint32_t newCustomMode) mavlink_message_t msg; mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newBaseMode, newCustomMode); qDebug() << "mavlink_msg_set_mode_pack 1"; - sendMessage(msg); + _vehicle->sendMessage(msg); qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", MODE " << newBaseMode << " " << newCustomMode; } else @@ -1792,79 +1767,6 @@ void UAS::setModeArm(uint8_t newBaseMode, uint32_t newCustomMode) } } -/** -* Send a message to every link that is connected. -* @param message that is to be sent -*/ -void UAS::sendMessage(mavlink_message_t message) -{ - emit _sendMessageOnThread(message); -} - -/** -* Send a message to every link that is connected. -* @param message that is to be sent -*/ -void UAS::_sendMessage(mavlink_message_t message) -{ - if (!LinkManager::instance()) - { - qDebug() << "LINKMANAGER NOT AVAILABLE!"; - return; - } - - if (_links.count() < 1) { - qDebug() << "NO LINK AVAILABLE TO SEND!"; - } - - // Emit message on all links that are currently connected - foreach (SharedLinkInterface sharedLink, _links) { - LinkInterface* link = sharedLink.data(); - Q_ASSERT(link); - - if (link->isConnected()) { - sendMessage(link, message); - } - } -} - -/** -* Send a message to the link that is connected. -* @param link that the message will be sent to -* @message that is to be sent -*/ -void UAS::sendMessage(LinkInterface* link, mavlink_message_t message) -{ - emit _sendMessageOnThreadLink(link, message); -} - -/** -* Send a message to the link that is connected. -* @param link that the message will be sent to -* @message that is to be sent -*/ -void UAS::_sendMessageLink(LinkInterface* link, mavlink_message_t message) -{ - if(!link) return; - // Create buffer - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - // Write message into buffer, prepending start sign - int len = mavlink_msg_to_send_buffer(buffer, &message); - static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS; - mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getMavlinkChannel(), message.len, messageKeys[message.msgid]); - - // If link is connected - if (link->isConnected()) - { - // Send the portion of the buffer now occupied by the message - link->writeBytes((const char*)buffer, len); - } - else - { - qDebug() << "LINK NOT CONNECTED, NOT SENDING!"; - } -} - /** * @param value battery voltage */ @@ -1988,7 +1890,7 @@ void UAS::requestImage() { mavlink_message_t msg; mavlink_msg_data_transmission_handshake_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, MAVLINK_DATA_STREAM_IMG_JPEG, 0, 0, 0, 0, 0, 50); - sendMessage(msg); + _vehicle->sendMessage(msg); } } @@ -2063,7 +1965,7 @@ void UAS::enableAllDataTransmission(int rate) // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2087,7 +1989,7 @@ void UAS::enableRawSensorDataTransmission(int rate) // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2111,7 +2013,7 @@ void UAS::enableExtendedSystemStatusTransmission(int rate) // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2122,7 +2024,7 @@ void UAS::enableRCChannelDataTransmission(int rate) #if defined(MAVLINK_ENABLED_UALBERTA_MESSAGES) mavlink_message_t msg; mavlink_msg_request_rc_channels_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, enabled); - sendMessage(msg); + _vehicle->sendMessage(msg); #else mavlink_message_t msg; mavlink_request_data_stream_t stream; @@ -2139,7 +2041,7 @@ void UAS::enableRCChannelDataTransmission(int rate) // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception - sendMessage(msg); + _vehicle->sendMessage(msg); #endif } @@ -2164,7 +2066,7 @@ void UAS::enableRawControllerDataTransmission(int rate) // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception - sendMessage(msg); + _vehicle->sendMessage(msg); } //void UAS::enableRawSensorFusionTransmission(int rate) @@ -2185,8 +2087,8 @@ void UAS::enableRawControllerDataTransmission(int rate) // // Encode and send the message // mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // // Send message twice to increase chance of reception -// sendMessage(msg); -// sendMessage(msg); +// _vehicle->sendMessage(msg); +// _vehicle->sendMessage(msg); //} /** @@ -2210,7 +2112,7 @@ void UAS::enablePositionTransmission(int rate) // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2234,8 +2136,8 @@ void UAS::enableExtra1Transmission(int rate) // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); + _vehicle->sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2259,8 +2161,8 @@ void UAS::enableExtra2Transmission(int rate) // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); + _vehicle->sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2284,8 +2186,8 @@ void UAS::enableExtra3Transmission(int rate) // Encode and send the message mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); + _vehicle->sendMessage(msg); + _vehicle->sendMessage(msg); } //TODO update this to use the parameter manager / param data model instead @@ -2416,7 +2318,7 @@ void UAS::executeCommand(MAV_CMD command) cmd.target_system = uasId; cmd.target_component = 0; mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - sendMessage(msg); + _vehicle->sendMessage(msg); } void UAS::executeCommandAck(int num, bool success) { @@ -2425,7 +2327,7 @@ void UAS::executeCommandAck(int num, bool success) ack.command = num; ack.result = (success ? 1 : 0); mavlink_msg_command_ack_encode(mavlink->getSystemId(),mavlink->getComponentId(),&msg,&ack); - sendMessage(msg); + _vehicle->sendMessage(msg); } void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) @@ -2444,7 +2346,7 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float cmd.target_system = uasId; cmd.target_component = component; mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2455,7 +2357,7 @@ void UAS::launch() { mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_NAV_TAKEOFF, 1, 0, 0, 0, 0, 0, 0, 0); - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2677,7 +2579,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons); } - sendMessage(message); + _vehicle->sendMessage(message); // Emit an update in control values to other UI elements, like the HSI display emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); } @@ -2702,13 +2604,13 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll mavlink_msg_set_attitude_target_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, QGC::groundTimeMilliseconds(), this->uasId, 0, mask, q, 0, 0, 0, 0); - sendMessage(message); + _vehicle->sendMessage(message); quint16 position_mask = (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7) | (1 << 8); mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, QGC::groundTimeMilliseconds(), this->uasId, 0, MAV_FRAME_LOCAL_NED, position_mask, x, y, z, 0, 0, 0, 0, 0, 0, yaw, yawrate); - sendMessage(message); + _vehicle->sendMessage(message); qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGES: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw; //emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); @@ -2750,7 +2652,7 @@ void UAS::halt() { mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_HOLD, MAV_GOTO_HOLD_AT_CURRENT_POSITION, 0, 0, 0, 0, 0); - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2760,7 +2662,7 @@ void UAS::go() { mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, 0, 0, 0, 0, 0); - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2776,7 +2678,7 @@ void UAS::home() int frame = UASManager::instance()->getHomeFrame(); mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, frame, 0, latitude, longitude, altitude); - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2787,7 +2689,7 @@ void UAS::land() mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_LAND, 1, 0, 0, 0, 0, 0, 0, 0); - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2798,7 +2700,7 @@ void UAS::pairRX(int rxType, int rxSubType) mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_START_RX_PAIR, 0, rxType, rxSubType, 0, 0, 0, 0, 0); - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -2841,8 +2743,8 @@ bool UAS::emergencyKILL() // // TODO Replace MG System ID with static function call and allow to change ID in GUI // mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL); // // Send message twice to increase chance of reception - // sendMessage(msg); - // sendMessage(msg); + // _vehicle->sendMessage(msg); + // _vehicle->sendMessage(msg); // result = true; // } // return result; @@ -3050,7 +2952,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa mavlink_msg_hil_state_quaternion_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, q, rollspeed, pitchspeed, yawspeed, lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, ind_airspeed*100, true_airspeed*100, xacc*1000/9.81, yacc*1000/9.81, zacc*1000/9.81); - sendMessage(msg); + _vehicle->sendMessage(msg); } else { @@ -3126,7 +3028,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl time_us, xacc_corrupt, yacc_corrupt, zacc_corrupt, rollspeed_corrupt, pitchspeed_corrupt, yawspeed_corrupt, xmag_corrupt, ymag_corrupt, zmag_corrupt, abs_pressure_corrupt, diff_pressure_corrupt, pressure_alt_corrupt, temperature_corrupt, fields_changed); - sendMessage(msg); + _vehicle->sendMessage(msg); lastSendTimeSensors = QGC::groundTimeMilliseconds(); } else @@ -3159,7 +3061,7 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa mavlink_msg_hil_optical_flow_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, 0, 0 /* hack */, flow_x, flow_y, 0.0f /* hack */, 0.0f /* hack */, 0.0f /* hack */, 0 /* hack */, quality, ground_distance); - sendMessage(msg); + _vehicle->sendMessage(msg); lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds(); #endif } @@ -3193,7 +3095,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi mavlink_msg_hil_gps_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, vn*1e2, ve*1e2, vd*1e2, course*1e2, satellites); lastSendTimeGPS = QGC::groundTimeMilliseconds(); - sendMessage(msg); + _vehicle->sendMessage(msg); } else { @@ -3247,7 +3149,7 @@ void UAS::shutdown() // If the active UAS is set, execute command mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 0, 2, 0, 0, 0, 0, 0); - sendMessage(msg); + _vehicle->sendMessage(msg); } } @@ -3261,7 +3163,7 @@ void UAS::setTargetPosition(float x, float y, float z, float yaw) { mavlink_message_t msg; mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 1, 1, 0, yaw, x, y, z); - sendMessage(msg); + _vehicle->sendMessage(msg); } /** @@ -3294,51 +3196,6 @@ const QString& UAS::getShortMode() const return shortModeText; } -/** -* Add the link and connect a signal to it which will be set off when it is destroyed. -*/ -void UAS::addLink(LinkInterface* link) -{ - if (!_containsLink(link)) - { - _links.append(LinkManager::instance()->sharedPointerForLink(link)); - qCDebug(UASLog) << "addLink:" << QString("%1").arg((ulong)link, 0, 16); - connect(LinkManager::instance(), &LinkManager::linkDisconnected, this, &UAS::_linkDisconnected); - } -} - -void UAS::_linkDisconnected(LinkInterface* link) -{ - qCDebug(UASLog) << "_linkDisconnected:" << link->getName(); - qCDebug(UASLog) << "link count:" << _links.count(); - - for (int i=0; i<_links.count(); i++) { - if (_links[i].data() == link) { - _links.removeAt(i); - break; - } - } - - if (_links.count() == 0) { - // Remove the UAS when all links to it close - UASManager::instance()->removeUAS(this); - } -} - -/** -* @return the list of links -*/ -QList UAS::getLinks() -{ - QList list; - - foreach (SharedLinkInterface sharedLink, _links) { - list << sharedLink.data(); - } - - return list; -} - /** * @rerturn the map of the components */ @@ -3436,7 +3293,7 @@ void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 p scale, valueMin, valueMax); - sendMessage(message); + _vehicle->sendMessage(message); qDebug() << "Mavlink message sent"; } @@ -3458,28 +3315,6 @@ void UAS::unsetRCToParameterMap() 0.0f, 0.0f, 0.0f); - sendMessage(message); - } -} - -bool UAS::_containsLink(LinkInterface* link) -{ - foreach (SharedLinkInterface sharedLink, _links) { - if (sharedLink.data() == link) { - return true; - } - } - - return false; -} - -bool UAS::isLogReplay(void) -{ - QList links = getLinks(); - - if (links.count() == 1) { - return links[0]->isLogReplay(); - } else { - return false; + _vehicle->sendMessage(message); } } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index fd7cb7b28a4b520a656c380870800fd4cc2f3018..29c78b508ee3cefcaa323db8125943488148e0d7 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -47,6 +47,8 @@ This file is part of the QGROUNDCONTROL project Q_DECLARE_LOGGING_CATEGORY(UASLog) +class Vehicle; + /** * @brief A generic MAVLINK-connected MAV/UAV * @@ -59,7 +61,7 @@ class UAS : public UASInterface { Q_OBJECT public: - UAS(MAVLinkProtocol* protocol, int id, MAV_AUTOPILOT autopilotType); + UAS(MAVLinkProtocol* protocol, Vehicle* vehicle); ~UAS(); float lipoFull; ///< 100% charged voltage @@ -87,9 +89,6 @@ public: quint64 getUptime() const; /** @brief Add one measurement and get low-passed voltage */ float filterVoltage(float value) const; - /** @brief Get the links associated with this robot */ - QList getLinks(); - bool isLogReplay(void); Q_PROPERTY(double localX READ getLocalX WRITE setLocalX NOTIFY localXChanged) Q_PROPERTY(double localY READ getLocalY WRITE setLocalY NOTIFY localYChanged) @@ -317,7 +316,6 @@ public: return yaw; } - bool getSelected() const; QVector3D getNedPosGlobalOffset() const { return nedPosGlobalOffset; @@ -393,10 +391,6 @@ protected: //COMMENTS FOR TEST UNIT int uasId; ///< Unique system ID QMap components;///< IDs and names of all detected onboard components - /// List of all links associated with this UAS. We keep SharedLinkInterface objects which are QSharedPointer's in order to - /// maintain reference counts across threads. This way Link deletion works correctly. - QList _links; - QList unknownPackets; ///< Packet IDs which are unknown and have been received MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance float receiveDropRate; ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs) @@ -861,25 +855,14 @@ public slots: void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw); #endif - /** @brief Add a link associated with this robot */ - void addLink(LinkInterface* link); - /** @brief Receive a message from one of the communication links. */ - virtual void receiveMessage(LinkInterface* link, mavlink_message_t message); + virtual void receiveMessage(mavlink_message_t message); #ifdef QGC_PROTOBUF_ENABLED /** @brief Receive a message from one of the communication links. */ virtual void receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr message); #endif - /** @brief Send a message over this link (to this or to all UAS on this link) */ - void sendMessage(LinkInterface* link, mavlink_message_t message); - /** @brief Send a message over all links this UAS can be reached with (!= all links) */ - void sendMessage(mavlink_message_t message); - - /** @brief Set this UAS as the system currently in focus, e.g. in the main display widgets */ - void setSelected(); - /** @brief Set current mode of operation, e.g. auto or manual, always uses the current arming status for safety reason */ void setMode(uint8_t newBaseMode, uint32_t newCustomMode); @@ -964,8 +947,6 @@ signals: void groundSpeedChanged(double val, QString name); void airSpeedChanged(double val, QString name); void bearingToWaypointChanged(double val,QString name); - void _sendMessageOnThread(mavlink_message_t message); - void _sendMessageOnThreadLink(LinkInterface* link, mavlink_message_t message); protected: /** @brief Get the UNIX timestamp in milliseconds, enter microseconds */ quint64 getUnixTime(quint64 time=0); @@ -996,16 +977,9 @@ protected slots: void writeSettings(); /** @brief Read settings from disk */ void readSettings(); - /** @brief Send a message over this link (to this or to all UAS on this link) */ - void _sendMessageLink(LinkInterface* link, mavlink_message_t message); - /** @brief Send a message over all links this UAS can be reached with (!= all links) */ - void _sendMessage(mavlink_message_t message); - -private slots: - void _linkDisconnected(LinkInterface* link); private: - bool _containsLink(LinkInterface* link); + Vehicle* _vehicle; }; diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 01d3f9b38cb8d32e3ea90bfc479592f9fcab0910..18ebcdfe6e1731391ce0f43f008098896dd7b697 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -120,8 +120,6 @@ public: virtual double getPitch() const = 0; virtual double getYaw() const = 0; - virtual bool getSelected() const = 0; - virtual bool isArmed() const = 0; /** @brief Set the airframe of this MAV */ @@ -132,11 +130,6 @@ public: virtual FileManager* getFileManager() = 0; - /** @brief Send a message over this link (to this or to all UAS on this link) */ - virtual void sendMessage(LinkInterface* link, mavlink_message_t message) = 0; - /** @brief Send a message over all links this UAS can be reached with (!= all links) */ - virtual void sendMessage(mavlink_message_t message) = 0; - enum Airframe { QGC_AIRFRAME_GENERIC = 0, QGC_AIRFRAME_EASYSTAR, @@ -156,18 +149,6 @@ public: QGC_AIRFRAME_END_OF_ENUM }; - /** - * @brief Get the links associated with this robot - * - * @return List with all links this robot is associated with. Association is usually - * based on the fact that a message for this robot has been received through that - * interface. The LinkInterface can support multiple protocols. - **/ - virtual QList getLinks() = 0; - - /// @returns true: UAS is connected to log replay link - virtual bool isLogReplay(void) = 0; - /** * @brief Get the color for this UAS * @@ -325,22 +306,6 @@ public slots: /** @brief Set world frame origin / home position at this GPS position */ virtual void setHomePosition(double lat, double lon, double alt) = 0; - /** - * @brief Add a link to the list of current links - * - * Adding the link to the robot's internal link list will allow him so send its own messages - * over all registered links. Usually a link is added once a message for this particular robot - * has been received over a link, but adding could also be done manually. - * @warning Not all links should be added to all robots by default, because else all robots will - * attempt to send over all links, typically choking wireless serial links. - */ - virtual void addLink(LinkInterface* link) = 0; - - /** - * @brief Set the current robot as focused in the user interface - */ - virtual void setSelected() = 0; - virtual void enableAllDataTransmission(int rate) = 0; virtual void enableRawSensorDataTransmission(int rate) = 0; virtual void enableExtendedSystemStatusTransmission(int rate) = 0; @@ -418,11 +383,6 @@ signals: void poiFound(UASInterface* uas, int type, int colorIndex, QString message, float x, float y, float z); void poiConnectionFound(UASInterface* uas, int type, int colorIndex, QString message, float x1, float y1, float z1, float x2, float y2, float z2); - /** - * @brief A misconfiguration has been detected by the UAS - */ - void misconfigurationDetected(UASInterface* uas); - /** @brief A text message from the system has been received */ void textMessageReceived(int uasid, int componentid, int severity, QString text); @@ -601,8 +561,6 @@ signals: void heartbeatTimeout(bool timeout, unsigned int ms); /** @brief Name of system changed */ void nameChanged(QString newName); - /** @brief System has been selected as focused system */ - void systemSelected(bool selected); /** @brief Core specifications have changed */ void systemSpecsChanged(int uasId); diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index 23ccb3a64d95dce30a630ebfd73c1f37db138657..b187b46dfa88fcfd0a47cf6f801dcb3749e4a352 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -19,6 +19,7 @@ #include "QGC.h" #include "QGCMessageBox.h" #include "QGCApplication.h" +#include "MultiVehicleManager.h" #define PI 3.1415926535897932384626433832795 #define MEAN_EARTH_DIAMETER 12756274.0 @@ -28,7 +29,6 @@ IMPLEMENT_QGC_SINGLETON(UASManager, UASManagerInterface) UASManager::UASManager(QObject* parent) : UASManagerInterface(parent), - activeUAS(NULL), offlineUASWaypointManager(NULL), homeLat(47.3769), homeLon(8.549444), @@ -42,20 +42,6 @@ UASManager::UASManager(QObject* parent) : UASManager::~UASManager() { storeSettings(); - Q_ASSERT_X(systems.count() == 0, "~UASManager", "_shutdown should have already removed all uas"); -} - -void UASManager::_shutdown(void) -{ - QList uasList; - - foreach(UASInterface* uas, systems) { - uasList.append(uas); - } - - foreach(UASInterface* uas, uasList) { - removeUAS(uas); - } } void UASManager::storeSettings() @@ -121,13 +107,8 @@ bool UASManager::setHomePositionAndNotify(double lat, double lon, double alt) // and checking for borders bool changed = setHomePosition(lat, lon, alt); - if (changed) - { - // Update all UAVs - foreach (UASInterface* mav, systems) - { - mav->setHomePosition(homeLat, homeLon, homeAlt); - } + if (changed) { + MultiVehicleManager::instance()->setHomePositionForAllVehicles(homeLat, homeLon, homeAlt); } return changed; @@ -240,7 +221,7 @@ void UASManager::nedToWgs84(const double& x, const double& y, const double& z, d void UASManager::uavChangedHomePosition(int uav, double lat, double lon, double alt) { // Accept home position changes from the active UAS - if (uav == activeUAS->getUASID()) + if (uav == MultiVehicleManager::instance()->activeVehicle()->id()) { if (setHomePosition(lat, lon, alt)) { @@ -258,202 +239,3 @@ void UASManager::uavChangedHomePosition(int uav, double lat, double lon, double } } } - -void UASManager::addUAS(UASInterface* uas) -{ - // WARNING: The active uas is set here - // and then announced below. This is necessary - // to make sure the getActiveUAS() function - // returns the UAS once the UASCreated() signal - // is emitted. The code is thus NOT redundant. - bool firstUAS = false; - if (activeUAS == NULL) - { - firstUAS = true; - activeUAS = uas; - } - - // Only execute if there is no UAS at this index - if (!systems.contains(uas)) - { - qDebug() << "Add new UAS: " << uas->getUASID(); - systems.append(uas); - // Set home position on UAV if set in UI - // - this is done on a per-UAV basis - // Set home position in UI if UAV chooses a new one (caution! if multiple UAVs are connected, take care!) - connect(uas, SIGNAL(homePositionChanged(int,double,double,double)), this, SLOT(uavChangedHomePosition(int,double,double,double))); - emit UASCreated(uas); - } - - // If there is no active UAS yet, set the first one as the active UAS - if (firstUAS) - { - setActiveUAS(uas); - // Call getActiveUASWaypointManager instead of referencing variable to make sure of creation - if (getActiveUASWaypointManager()->getWaypointEditableList().size() > 0) - { - if (QGCMessageBox::question(tr("Question"), tr("Do you want to append the offline waypoints to the ones currently on the UAV?"), QMessageBox::Yes, QMessageBox::No) == QMessageBox::Yes) - { - //Need to transfer all waypoints from the offline mode WPManager to the online mode. - for (int i=0;igetWaypointEditableList().size();i++) - { - Waypoint *wp = uas->getWaypointManager()->createWaypoint(); - wp->setLatitude(offlineUASWaypointManager->getWaypointEditableList()[i]->getLatitude()); - wp->setLongitude(offlineUASWaypointManager->getWaypointEditableList()[i]->getLongitude()); - wp->setAltitude(offlineUASWaypointManager->getWaypointEditableList()[i]->getAltitude()); - } - } - offlineUASWaypointManager->deleteLater(); - offlineUASWaypointManager = 0; - } - } -} - -/** - * @brief The function that should be used when removing UASes from QGC. emits UASDeletect(UASInterface*) when triggered - * so that UI elements can update accordingly. - * @param uas The UAS to remove - */ -void UASManager::removeUAS(UASInterface* uas) -{ - if (uas) - { - int listindex = systems.indexOf(uas); - - // Remove this system from local data store. - systems.removeAt(listindex); - - // If this is the active UAS, select a new one if one exists otherwise - // indicate that there are no active UASes. - if (uas == activeUAS) - { - if (systems.count()) - { - setActiveUAS(systems.first()); - } - else - { - setActiveUAS(NULL); - } - } - - // Notify other UI elements that a UAS is being deleted before finally deleting it. - qDebug() << "Deleting UAS object: " << uas->getUASName(); - emit UASDeleted(uas); - emit UASDeleted(uas->getUASID()); - uas->deleteLater(); - } -} - -QList UASManager::getUASList() -{ - return systems; -} - -UASInterface* UASManager::getActiveUAS() -{ - return activeUAS; ///< Return zero pointer if no UAS has been loaded -} - -UASInterface* UASManager::silentGetActiveUAS() -{ - return activeUAS; ///< Return zero pointer if no UAS has been loaded -} -UASWaypointManager *UASManager::getActiveUASWaypointManager() -{ - if (activeUAS) - { - return activeUAS->getWaypointManager(); - } - if (!offlineUASWaypointManager) - { - offlineUASWaypointManager = new UASWaypointManager(NULL); - } - return offlineUASWaypointManager; - - -} - -bool UASManager::launchActiveUAS() -{ - // If the active UAS is set, execute command - if (getActiveUAS()) activeUAS->launch(); - return (activeUAS); ///< Returns true if the UAS exists, false else -} - -bool UASManager::haltActiveUAS() -{ - // If the active UAS is set, execute command - if (getActiveUAS()) activeUAS->halt(); - return (activeUAS); ///< Returns true if the UAS exists, false else -} - -bool UASManager::continueActiveUAS() -{ - // If the active UAS is set, execute command - if (getActiveUAS()) activeUAS->go(); - return (activeUAS); ///< Returns true if the UAS exists, false else -} - -bool UASManager::returnActiveUAS() -{ - // If the active UAS is set, execute command - if (getActiveUAS()) activeUAS->home(); - return (activeUAS); ///< Returns true if the UAS exists, false else -} - -bool UASManager::stopActiveUAS() -{ - // If the active UAS is set, execute command - if (getActiveUAS()) activeUAS->emergencySTOP(); - return (activeUAS); ///< Returns true if the UAS exists, false else -} - -bool UASManager::killActiveUAS() -{ - if (getActiveUAS()) activeUAS->emergencyKILL(); - return (activeUAS); -} - -bool UASManager::shutdownActiveUAS() -{ - if (getActiveUAS()) activeUAS->shutdown(); - return (activeUAS); -} - -UASInterface* UASManager::getUASForId(int id) -{ - UASInterface* system = NULL; - - foreach(UASInterface* sys, systems) { - if (sys->getUASID() == id) { - system = sys; - } - } - - // Return NULL if not found - return system; -} - -void UASManager::setActiveUAS(UASInterface* uas) -{ - // Signal components that the last UAS is no longer active. - activeUASMutex.lock(); - if (activeUAS != NULL) { - emit activeUASStatusChanged(activeUAS, false); - emit activeUASStatusChanged(activeUAS->getUASID(), false); - } - activeUAS = uas; - activeUASMutex.unlock(); - - // And signal that a new UAS is. - emit activeUASSet(activeUAS); - if (activeUAS) - { - activeUAS->setSelected(); - emit activeUASSetListIndex(systems.indexOf(activeUAS)); - emit activeUASStatusChanged(activeUAS, true); - emit activeUASStatusChanged(activeUAS->getUASID(), true); - } -} - diff --git a/src/uas/UASManager.h b/src/uas/UASManager.h index 540022b752c263bd67f9e626148b78942a739e80..0f47129ebc40f53ce36c62103784935da095db92 100644 --- a/src/uas/UASManager.h +++ b/src/uas/UASManager.h @@ -55,31 +55,6 @@ class UASManager : public UASManagerInterface DECLARE_QGC_SINGLETON(UASManager, UASManagerInterface) public: - /** - * @brief Get the currently selected UAS - * - * @return NULL pointer if no UAS exists, active UAS else - **/ - UASInterface* getActiveUAS(); - /** - * @brief getActiveUASWaypointManager - * @return uas->getUASWaypointManager(), or if not connected, a singleton instance of a UASWaypointManager. - */ - UASWaypointManager *getActiveUASWaypointManager(); - - UASInterface* silentGetActiveUAS(); - /** - * @brief Get the UAS with this id - * - * Although not enforced by this implementation, the IDs are constrained to be - * in the range of 1 - 127 by the MAVLINK protocol. - * - * @param id unique system / aircraft id - * @return UAS with the given ID, NULL pointer else - **/ - UASInterface* getUASForId(int id); - - QList getUASList(); /** @brief Get home position latitude */ double getHomeLatitude() const { return homeLat; @@ -145,88 +120,6 @@ public: public slots: - - /** - * @brief Add a new UAS to the list - * - * This command will only be executed if this UAS does not yet exist. - * @param UAS unmanned air system to add - **/ - void addUAS(UASInterface* UAS); - - /** @brief Remove a system from the list. If this is the active UAS, it switches to another one calling setActiveUAS. Also triggers the UAS to kill itself. */ - void removeUAS(UASInterface* uas); - - - /** - * @brief Set a UAS as currently selected. NULL is a valid value for when no other valid UAS's are available. - * - * @param UAS Unmanned Air System to set - **/ - void setActiveUAS(UASInterface* UAS); - - - /** - * @brief Launch the active UAS - * - * The groundstation has always one Unmanned Air System selected. - * All commands are executed on the UAS in focus. This command starts - * the launch sequence. - * - * @return True if the UAS could be launched, false else - */ - bool launchActiveUAS(); - - bool haltActiveUAS(); - - bool continueActiveUAS(); - - /** - * @brief Land the active UAS - * - * The groundstation has always one Unmanned Air System selected. - * All commands are executed on the UAS in focus. This command starts - * the land sequence. Depending on the onboard control, this could mean - * returning to the landing spot as well as descending on the current - * position. - * - * @return True if the UAS could be landed, false else - */ - bool returnActiveUAS(); - - - /** - * @brief EMERGENCY: Stop active UAS - * - * The groundstation has always one Unmanned Air System selected. - * All commands are executed on the UAS in focus. This command - * starts an emergency landing. Depending on the onboard control, - * this usually means descending rapidly on the current position. - * - * @warning This command can severely damage the UAS! - * - * @return True if the UAS could be landed, false else - */ - bool stopActiveUAS(); - - /** - * @brief EMERGENCY: Kill active UAS - * - * The groundstation has always one Unmanned Air System selected. - * All commands are executed on the UAS in focus. This command - * shuts off all onboard motors immediately. This leads to a - * system crash, but might prevent external damage, e.g. to people. - * This command is secured by an additional popup message window. - * - * @warning THIS COMMAND RESULTS IN THE LOSS OF THE SYSTEM! - * - * @return True if the UAS could be landed, false else - */ - bool killActiveUAS(); - - /** @brief Shut down the onboard operating system down */ - bool shutdownActiveUAS(); - /** @brief Set the current home position, but do not change it on the UAVs */ bool setHomePosition(double lat, double lon, double alt); @@ -243,15 +136,10 @@ public slots: void loadSettings(); /** @brief Store settings */ void storeSettings(); - - void _shutdown(void); protected: - QList systems; - UASInterface* activeUAS; UASWaypointManager *offlineUASWaypointManager; - QMutex activeUASMutex; double homeLat; double homeLon; double homeAlt; diff --git a/src/uas/UASManagerInterface.h b/src/uas/UASManagerInterface.h index 3943590d22762f604d1636dc9f79a5765bf80d36..dd93988cf54af94585a7a38507e4d953b3c55a7a 100644 --- a/src/uas/UASManagerInterface.h +++ b/src/uas/UASManagerInterface.h @@ -57,11 +57,6 @@ class UASManagerInterface : public QGCSingleton Q_OBJECT public: - virtual UASInterface* getActiveUAS() = 0; - virtual UASWaypointManager *getActiveUASWaypointManager() = 0; - virtual UASInterface* silentGetActiveUAS() = 0; - virtual UASInterface* getUASForId(int id) = 0; - virtual QList getUASList() = 0; virtual double getHomeLatitude() const = 0; virtual double getHomeLongitude() const = 0; virtual double getHomeAltitude() const = 0; @@ -75,39 +70,14 @@ public: virtual bool isInLocalNEDSafetyLimits(double x, double y, double z) = 0; public slots: - virtual void addUAS(UASInterface* UAS) = 0; - virtual void removeUAS(UASInterface* uas) = 0; - virtual void setActiveUAS(UASInterface* UAS) = 0; - virtual bool launchActiveUAS() = 0; - virtual bool haltActiveUAS() = 0; - virtual bool continueActiveUAS() = 0; - virtual bool returnActiveUAS() = 0; - virtual bool stopActiveUAS() = 0; - virtual bool killActiveUAS() = 0; - virtual bool shutdownActiveUAS() = 0; virtual bool setHomePosition(double lat, double lon, double alt) = 0; virtual bool setHomePositionAndNotify(double lat, double lon, double alt) = 0; virtual void setLocalNEDSafetyBorders(double x1, double y1, double z1, double x2, double y2, double z2) = 0; virtual void uavChangedHomePosition(int uav, double lat, double lon, double alt) = 0; virtual void loadSettings() = 0; virtual void storeSettings() = 0; - virtual void _shutdown(void) = 0; signals: - /** A new system was created */ - void UASCreated(UASInterface* UAS); - /** A system was deleted */ - void UASDeleted(UASInterface* UAS); - /** A system was deleted */ - void UASDeleted(int systemId); - /** @brief The UAS currently under main operator control changed */ - void activeUASSet(UASInterface* UAS); - /** @brief The UAS currently under main operator control changed */ - void activeUASSetListIndex(int listIndex); - /** @brief The UAS currently under main operator control changed */ - void activeUASStatusChanged(UASInterface* UAS, bool active); - /** @brief The UAS currently under main operator control changed */ - void activeUASStatusChanged(int systemId, bool active); /** @brief Current home position changed */ void homePositionChanged(double lat, double lon, double alt); diff --git a/src/uas/UASMessageHandler.cc b/src/uas/UASMessageHandler.cc index c069cfcda44fbeb777b1da0d9ecb7d90551b25bb..3ad901b1e1cfa04dfda8bd726b31d6309e0cd37e 100644 --- a/src/uas/UASMessageHandler.cc +++ b/src/uas/UASMessageHandler.cc @@ -29,7 +29,7 @@ This file is part of the QGROUNDCONTROL project #include "QGCApplication.h" #include "UASMessageHandler.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" UASMessage::UASMessage(int componentid, int severity, QString text) { @@ -62,7 +62,7 @@ UASMessageHandler::UASMessageHandler(QObject *parent) , _normalCount(0) , _showErrorsInToolbar(false) { - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &UASMessageHandler::_activeVehicleChanged); emit textMessageReceived(NULL); emit textMessageCountChanged(0); } @@ -86,10 +86,10 @@ void UASMessageHandler::clearMessages() emit textMessageCountChanged(0); } -void UASMessageHandler::setActiveUAS(UASInterface* uas) +void UASMessageHandler::_activeVehicleChanged(Vehicle* vehicle) { // If we were already attached to an autopilot, disconnect it. - if (_activeUAS && _activeUAS != uas) + if (_activeUAS) { disconnect(_activeUAS, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(handleTextMessage(int,int,int,QString))); _activeUAS = NULL; @@ -97,8 +97,10 @@ void UASMessageHandler::setActiveUAS(UASInterface* uas) emit textMessageReceived(NULL); } // And now if there's an autopilot to follow, set up the UI. - if (uas) + if (vehicle) { + UAS* uas = vehicle->uas(); + // Connect to the new UAS. clearMessages(); _activeUAS = uas; diff --git a/src/uas/UASMessageHandler.h b/src/uas/UASMessageHandler.h index 8a68c314c9e01be72de6b6304022541108dfac0b..cf05311cec29ac0188457a0890460fd2ac1108f9 100644 --- a/src/uas/UASMessageHandler.h +++ b/src/uas/UASMessageHandler.h @@ -35,6 +35,7 @@ This file is part of the QGROUNDCONTROL project #include #include "QGCSingleton.h" +#include "Vehicle.h" class UASInterface; class UASMessageHandler; @@ -126,11 +127,6 @@ public: void showErrorsInToolbar(void) { _showErrorsInToolbar = true; } public slots: - /** - * @brief Set currently active UAS - * @param uas The current active UAS - */ - void setActiveUAS(UASInterface* uas); /** * @brief Handle text message from current active UAS * @param uasid UAS Id @@ -152,6 +148,9 @@ signals: */ void textMessageCountChanged(int count); +private slots: + void _activeVehicleChanged(Vehicle* vehicle); + private: // Stores the UAS that we're currently receiving messages from. UASInterface* _activeUAS; diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 13740bd0196bd1a9bda9633a0068b065b01a66d4..82e657058b29d04cda55626e92b6114afa1b696e 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -34,13 +34,14 @@ This file is part of the QGROUNDCONTROL project #include "mavlink_types.h" #include "UASManager.h" #include "QGCMessageBox.h" +#include "Vehicle.h" #define PROTOCOL_TIMEOUT_MS 2000 ///< maximum time to wait for pending messages until timeout #define PROTOCOL_DELAY_MS 20 ///< minimum delay between sent messages #define PROTOCOL_MAX_RETRIES 5 ///< maximum number of send retries (after timeout) const float UASWaypointManager::defaultAltitudeHomeOffset = 30.0f; -UASWaypointManager::UASWaypointManager(UAS* _uas) - : uas(_uas), +UASWaypointManager::UASWaypointManager(Vehicle* vehicle, UAS* uas) + : _vehicle(vehicle), current_retries(0), current_wp_id(0), current_count(0), @@ -53,9 +54,9 @@ UASWaypointManager::UASWaypointManager(UAS* _uas) { _offlineEditingModeMessage = tr("You are in offline editing mode. Make sure to save your mission to a file before connecting to a system - you will need to load the file into the system, the offline list will be cleared on connect."); - if (uas) + if (_vehicle) { - uasid = uas->getUASID(); + uasid = _vehicle->id(); connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout())); connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleLocalPositionChanged(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(handleGlobalPositionChanged(UASInterface*,double,double,double,double,quint64))); @@ -366,7 +367,7 @@ void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, m void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_mission_item_reached_t *wpr) { Q_UNUSED(compId); - if (!uas) return; + if (!_vehicle) return; if (systemId == uasid) { emit updateStatusString(tr("Reached waypoint %1").arg(wpr->seq)); } @@ -375,7 +376,7 @@ void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, m void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_mission_current_t *wpc) { Q_UNUSED(compId); - if (!uas) return; + if (!_vehicle) return; if (systemId == uasid) { // FIXME Petri if (current_state == WP_SETCURRENT) { @@ -485,7 +486,7 @@ void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActi if (wp) { // Check if this is the first waypoint in an offline list - if (waypointsEditable.count() == 0 && uas == NULL) { + if (waypointsEditable.count() == 0 && _vehicle == NULL) { QGCMessageBox::critical(tr("Waypoint Manager"), _offlineEditingModeMessage); } @@ -509,7 +510,7 @@ void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActi Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive) { // Check if this is the first waypoint in an offline list - if (waypointsEditable.count() == 0 && uas == NULL) { + if (waypointsEditable.count() == 0 && _vehicle == NULL) { QGCMessageBox::critical(tr("Waypoint Manager"), _offlineEditingModeMessage); } @@ -934,13 +935,13 @@ void UASWaypointManager::readWaypoints(bool readToEdit) } bool UASWaypointManager::guidedModeSupported() { - return (uas->getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA); + return (_vehicle->firmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA); } void UASWaypointManager::goToWaypoint(Waypoint *wp) { //Don't try to send a guided mode message to an AP that does not support it. - if (uas->getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) + if (_vehicle->firmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { mavlink_mission_item_t mission; memset(&mission, 0, sizeof(mavlink_mission_item_t)); //initialize with zeros @@ -961,8 +962,8 @@ void UASWaypointManager::goToWaypoint(Waypoint *wp) mavlink_message_t message; mission.target_system = uasid; mission.target_component = MAV_COMP_ID_MISSIONPLANNER; - mavlink_msg_mission_item_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &mission); - uas->sendMessage(message); + mavlink_msg_mission_item_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &mission); + _vehicle->sendMessage(message); QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); } } @@ -1037,13 +1038,13 @@ void UASWaypointManager::writeWaypoints() void UASWaypointManager::sendWaypointClearAll() { - if (!uas) return; + if (!_vehicle) return; // Send the message. mavlink_message_t message; mavlink_mission_clear_all_t wpca = {(quint8)uasid, MAV_COMP_ID_MISSIONPLANNER}; - mavlink_msg_mission_clear_all_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpca); - uas->sendMessage(message); + mavlink_msg_mission_clear_all_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &wpca); + _vehicle->sendMessage(message); // And update the UI. emit updateStatusString(tr("Clearing waypoint list...")); @@ -1053,13 +1054,13 @@ void UASWaypointManager::sendWaypointClearAll() void UASWaypointManager::sendWaypointSetCurrent(quint16 seq) { - if (!uas) return; + if (!_vehicle) return; // Send the message. mavlink_message_t message; mavlink_mission_set_current_t wpsc = {seq, (quint8)uasid, MAV_COMP_ID_MISSIONPLANNER}; - mavlink_msg_mission_set_current_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpsc); - uas->sendMessage(message); + mavlink_msg_mission_set_current_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &wpsc); + _vehicle->sendMessage(message); // And update the UI. emit updateStatusString(tr("Updating target waypoint...")); @@ -1069,14 +1070,14 @@ void UASWaypointManager::sendWaypointSetCurrent(quint16 seq) void UASWaypointManager::sendWaypointCount() { - if (!uas) return; + if (!_vehicle) return; // Tell the UAS how many missions we'll sending. mavlink_message_t message; mavlink_mission_count_t wpc = {current_count, (quint8)uasid, MAV_COMP_ID_MISSIONPLANNER}; - mavlink_msg_mission_count_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpc); - uas->sendMessage(message); + mavlink_msg_mission_count_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &wpc); + _vehicle->sendMessage(message); // And update the UI. emit updateStatusString(tr("Starting to transmit waypoints...")); @@ -1086,13 +1087,13 @@ void UASWaypointManager::sendWaypointCount() void UASWaypointManager::sendWaypointRequestList() { - if (!uas) return; + if (!_vehicle) return; // Send a MISSION_REQUEST message to the uas for this mission manager, using the MISSIONPLANNER component. mavlink_message_t message; mavlink_mission_request_list_t wprl = {(quint8)uasid, MAV_COMP_ID_MISSIONPLANNER}; - mavlink_msg_mission_request_list_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wprl); - uas->sendMessage(message); + mavlink_msg_mission_request_list_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &wprl); + _vehicle->sendMessage(message); // And update the UI. QString statusMsg(tr("Requesting waypoint list...")); @@ -1104,13 +1105,13 @@ void UASWaypointManager::sendWaypointRequestList() void UASWaypointManager::sendWaypointRequest(quint16 seq) { - if (!uas) return; + if (!_vehicle) return; // Send a MISSION_REQUEST message to the UAS's MISSIONPLANNER component. mavlink_message_t message; mavlink_mission_request_t wpr = {seq, (quint8)uasid, MAV_COMP_ID_MISSIONPLANNER}; - mavlink_msg_mission_request_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpr); - uas->sendMessage(message); + mavlink_msg_mission_request_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &wpr); + _vehicle->sendMessage(message); // And update the UI. emit updateStatusString(tr("Retrieving waypoint ID %1 of %2").arg(wpr.seq).arg(current_count)); @@ -1120,7 +1121,7 @@ void UASWaypointManager::sendWaypointRequest(quint16 seq) void UASWaypointManager::sendWaypoint(quint16 seq) { - if (!uas) return; + if (!_vehicle) return; mavlink_message_t message; if (seq < waypoint_buffer.count()) { @@ -1131,8 +1132,8 @@ void UASWaypointManager::sendWaypoint(quint16 seq) wp->target_component = MAV_COMP_ID_MISSIONPLANNER; // Transmit the new mission - mavlink_msg_mission_item_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, wp); - uas->sendMessage(message); + mavlink_msg_mission_item_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, wp); + _vehicle->sendMessage(message); // And update the UI. emit updateStatusString(tr("Sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count)); @@ -1143,19 +1144,19 @@ void UASWaypointManager::sendWaypoint(quint16 seq) void UASWaypointManager::sendWaypointAck(quint8 type) { - if (!uas) return; + if (!_vehicle) return; // Send the message. mavlink_message_t message; mavlink_mission_ack_t wpa = {(quint8)uasid, MAV_COMP_ID_MISSIONPLANNER, type}; - mavlink_msg_mission_ack_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpa); - uas->sendMessage(message); + mavlink_msg_mission_ack_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &wpa); + _vehicle->sendMessage(message); QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); } UAS* UASWaypointManager::getUAS() { - return this->uas; ///< Returns the owning UAS + return _vehicle ? _vehicle->uas() : NULL; ///< Returns the owning UAS } float UASWaypointManager::getAltitudeRecommendation() @@ -1185,11 +1186,11 @@ float UASWaypointManager::getAcceptanceRadiusRecommendation() else { // Default to rotary wing waypoint radius for offline editing - if (!uas || uas->isRotaryWing()) + if (!_vehicle || _vehicle->uas()->isRotaryWing()) { return UASInterface::WAYPOINT_RADIUS_DEFAULT_ROTARY_WING; } - else if (uas->isFixedWing()) + else if (_vehicle->uas()->isFixedWing()) { return UASInterface::WAYPOINT_RADIUS_DEFAULT_FIXED_WING; } diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index a9555e7beb322046f66a42370de2ce2cffcbd10c..5a81186f3a4a977286052e69ba02990b53c8a336 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -38,8 +38,10 @@ This file is part of the QGROUNDCONTROL project #include #include "Waypoint.h" #include "QGCMAVLink.h" + class UAS; class UASInterface; +class Vehicle; /** * @brief Implementation of the MAVLINK waypoint protocol @@ -65,7 +67,7 @@ private: }; ///< The possible states for the waypoint protocol public: - UASWaypointManager(UAS* uas=NULL); ///< Standard constructor + UASWaypointManager(Vehicle* vehicle, UAS* uas); ///< Standard constructor ~UASWaypointManager(); bool guidedModeSupported(); @@ -171,7 +173,7 @@ private slots: void _updateWPonTimer(void); ///< Starts requesting WP on timer timeout private: - UAS* uas; ///< Reference to the corresponding UAS + Vehicle* _vehicle; quint32 current_retries; ///< The current number of retries left quint16 current_wp_id; ///< The last used waypoint ID in the current protocol transaction quint16 current_count; ///< The number of waypoints in the current protocol transaction diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index 6a513b62bafb29c13345090c03252b8619d7ec52..f732d1edfcc079d5e5567d774f497fa0d046b6db 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -19,13 +19,13 @@ #include #include #include -#include "UASManager.h" #include "HDDisplay.h" #include "ui_HDDisplay.h" #include "MG.h" #include "QGC.h" #include "QGCApplication.h" #include +#include "MultiVehicleManager.h" HDDisplay::HDDisplay(const QStringList &plotList, QString title, QWidget *parent) : QGraphicsView(parent), @@ -126,8 +126,8 @@ HDDisplay::HDDisplay(const QStringList &plotList, QString title, QWidget *parent if (font.family() != fontFamilyName) qDebug() << "ERROR! Font not loaded: " << fontFamilyName; // Connect with UAS - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)), Qt::UniqueConnection); - setActiveUAS(UASManager::instance()->getActiveUAS()); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &HDDisplay::_activeVehicleChanged); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); } HDDisplay::~HDDisplay() @@ -471,7 +471,7 @@ void HDDisplay::renderOverlay() * * @param uas the UAS/MAV to monitor/display with the HUD */ -void HDDisplay::setActiveUAS(UASInterface* uas) +void HDDisplay::_activeVehicleChanged(Vehicle* vehicle) { // Disconnect any previously connected active UAS if (this->uas != NULL) { @@ -479,10 +479,10 @@ void HDDisplay::setActiveUAS(UASInterface* uas) this->uas = NULL; } - if (uas) { + if (vehicle) { + this->uas = vehicle->uas(); // Now connect the new UAS addSource(uas); - this->uas = uas; } } diff --git a/src/ui/HDDisplay.h b/src/ui/HDDisplay.h index 29aa075ded3b69a7b83a28493273237bb7b3bacd..8f646250d39560826c8aec675ec1fd9a9e2ed873 100644 --- a/src/ui/HDDisplay.h +++ b/src/ui/HDDisplay.h @@ -42,6 +42,7 @@ This file is part of the QGROUNDCONTROL project #include #include "UASInterface.h" +#include "Vehicle.h" namespace Ui { @@ -67,8 +68,6 @@ public slots: /** @brief Update the HDD with new data */ void updateValue(const int uasId, const QString& name, const QString& unit, const QVariant &value, const quint64 msec); - virtual void setActiveUAS(UASInterface* uas); - /** @brief Connects a source to the updateValue() signals */ void addSource(QObject* obj); /** @brief Disconnects a source to the updateValue() signals */ @@ -197,6 +196,9 @@ protected: QAction* setTitleAction; ///< Action setting the title QAction* setColumnsAction; ///< Action setting the number of columns bool valuesChanged; + +private slots: + void _activeVehicleChanged(Vehicle* vehicle); private: Ui::HDDisplay *m_ui; diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 8b233963ee0707741b23dacaf48c35548a077b09..9c9d3567b81a28e75c8aaf82a61da70cb641a328 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -37,6 +37,7 @@ This file is part of the QGROUNDCONTROL project #include #include +#include "MultiVehicleManager.h" #include "UASManager.h" #include "HSIDisplay.h" #include "QGC.h" @@ -178,16 +179,9 @@ HSIDisplay::HSIDisplay(QWidget *parent) : // XXX this looks a potential recursive issue //connect(&statusClearTimer, SIGNAL(timeout()), this, SLOT(clearStatusMessage())); - if (UASManager::instance()->getActiveUAS()) - { - setActiveUAS(UASManager::instance()->getActiveUAS()); - } - - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), - this, SLOT(setActiveUAS(UASInterface*))); - - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), - this, SLOT(setActiveUAS(UASInterface*))); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &HSIDisplay::_activeVehicleChanged); + + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); setFocusPolicy(Qt::StrongFocus); } @@ -918,7 +912,7 @@ void HSIDisplay::setMetricWidth(double width) * * @param uas the UAS/MAV to monitor/display with the HUD */ -void HSIDisplay::setActiveUAS(UASInterface* uas) +void HSIDisplay::_activeVehicleChanged(Vehicle* vehicle) { if (this->uas != NULL) { disconnect(this->uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool))); @@ -953,9 +947,12 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) disconnect(this->uas, &UASInterface::navigationControllerErrorsChanged, this, &HSIDisplay::UpdateNavErrors); } + + this->uas = NULL; - if (uas) + if (vehicle) { + this->uas = vehicle->uas(); connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool))); connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), @@ -1019,8 +1016,6 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) statusClearTimer.stop(); } - this->uas = uas; - resetMAVState(); } diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index a99eab7850a00ded1817d0c5e6a7d5f99824410a..9080f16451b0b11bab83ed63dfe49ac8a7342013 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -42,6 +42,7 @@ This file is part of the QGROUNDCONTROL project #include "HDDisplay.h" #include "MG.h" +#include "Vehicle.h" class HSIDisplay : public HDDisplay { @@ -51,7 +52,6 @@ public: ~HSIDisplay(); public slots: - void setActiveUAS(UASInterface* uas); /** @brief Set the width in meters this widget shows from top */ void setMetricWidth(double width); void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used); @@ -422,7 +422,8 @@ protected: bool userZSetPointSet; ///< User set the Z position already bool userYawSetPointSet; ///< User set the YAW position already -private: +private slots: + void _activeVehicleChanged(Vehicle* vehicle); }; #endif // HSIDISPLAY_H diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 58c457e7f7a7fd29328ee083ba52c8cc2e0a72a4..3eaefdc7da8bfb445a6cc2924f3012df3051f8b9 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -40,12 +40,12 @@ This file is part of the QGROUNDCONTROL project #include #include -#include "UASManager.h" #include "UAS.h" #include "HUD.h" #include "QGC.h" #include "QGCApplication.h" #include "QGCFileDialog.h" +#include "MultiVehicleManager.h" /** * @warning The HUD widget will not start painting its content automatically @@ -159,11 +159,11 @@ HUD::HUD(int width, int height, QWidget* parent) connect(qgcApp(), &QGCApplication::styleChanged, this, &HUD::styleChanged); // Connect with UAS - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &HUD::_activeVehicleChanged); createActions(); - - if (UASManager::instance()->getActiveUAS() != NULL) setActiveUAS(UASManager::instance()->getActiveUAS()); + + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); } HUD::~HUD() @@ -265,7 +265,7 @@ void HUD::createActions() * * @param uas the UAS/MAV to monitor/display with the HUD */ -void HUD::setActiveUAS(UASInterface* uas) +void HUD::_activeVehicleChanged(Vehicle* vehicle) { if (this->uas != NULL) { // Disconnect any previously connected active MAV @@ -288,7 +288,10 @@ void HUD::setActiveUAS(UASInterface* uas) } } - if (uas) { + this->uas = NULL; + + if (vehicle) { + this->uas = vehicle->uas(); // Now connect the new UAS // Setup communication connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64))); @@ -310,8 +313,6 @@ void HUD::setActiveUAS(UASInterface* uas) } } - // Set new UAS - this->uas = uas; } //void HUD::updateAttitudeThrustSetPoint(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 msec) diff --git a/src/ui/HUD.h b/src/ui/HUD.h index a8c037a32e39ffd2664f5e5a1de149f5115a74ee..aba2861b96330e6c640c9849673ebfa09b7413cf 100644 --- a/src/ui/HUD.h +++ b/src/ui/HUD.h @@ -39,8 +39,10 @@ This file is part of the QGROUNDCONTROL project #include #include #include + #include "UASInterface.h" #include "MainWindow.h" +#include "Vehicle.h" /** * @brief Displays a Head Up Display (HUD) @@ -62,9 +64,6 @@ public: public slots: void styleChanged(bool styleIsDark); - /** @brief Set the currently monitored UAS */ - virtual void setActiveUAS(UASInterface* uas); - /** @brief Attitude from main autopilot / system state */ void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp); /** @brief Attitude from one specific component / redundant autopilot */ @@ -98,6 +97,7 @@ public slots: protected slots: + void _activeVehicleChanged(Vehicle* vehicle); void paintRollPitchStrips(); void paintPitchLines(float pitch, QPainter* painter); /** @brief Paint text on top of the image and OpenGL drawings */ diff --git a/src/ui/JoystickAxis.cc b/src/ui/JoystickAxis.cc index 5effd6971b6c8cb4481bbebf8d97fb93752a15ed..cfe741b21c65cee3cb9e8c0ec56443b878feb0de 100644 --- a/src/ui/JoystickAxis.cc +++ b/src/ui/JoystickAxis.cc @@ -1,7 +1,7 @@ #include "JoystickAxis.h" #include "JoystickInput.h" #include "ui_JoystickAxis.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" #include JoystickAxis::JoystickAxis(int id, QWidget *parent) : @@ -38,7 +38,7 @@ void JoystickAxis::setMapping(JoystickInput::JOYSTICK_INPUT_MAPPING newMapping) { ui->rangeCheckBox->hide(); } - this->setActiveUAS(UASManager::instance()->getActiveUAS()); + this->activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); } void JoystickAxis::setInverted(bool newValue) @@ -55,7 +55,7 @@ void JoystickAxis::mappingComboBoxChanged(int newMapping) { JoystickInput::JOYSTICK_INPUT_MAPPING mapping = (JoystickInput::JOYSTICK_INPUT_MAPPING)newMapping; emit mappingChanged(id, mapping); - updateUIBasedOnUAS(UASManager::instance()->getActiveUAS(), mapping); + updateUIBasedOnUAS(MultiVehicleManager::instance()->activeVehicle(), mapping); } void JoystickAxis::inversionCheckBoxChanged(bool inverted) @@ -68,13 +68,19 @@ void JoystickAxis::rangeCheckBoxChanged(bool limited) emit rangeChanged(id, limited); } -void JoystickAxis::setActiveUAS(UASInterface* uas) +void JoystickAxis::activeVehicleChanged(Vehicle* vehicle) { - updateUIBasedOnUAS(uas, (JoystickInput::JOYSTICK_INPUT_MAPPING)ui->comboBox->currentIndex()); + updateUIBasedOnUAS(vehicle, (JoystickInput::JOYSTICK_INPUT_MAPPING)ui->comboBox->currentIndex()); } -void JoystickAxis::updateUIBasedOnUAS(UASInterface* uas, JoystickInput::JOYSTICK_INPUT_MAPPING axisMapping) +void JoystickAxis::updateUIBasedOnUAS(Vehicle* vehicle, JoystickInput::JOYSTICK_INPUT_MAPPING axisMapping) { + UAS* uas = NULL; + + if (vehicle) { + uas = vehicle->uas(); + } + // Set the throttle display to only positive if: // * This is the throttle axis AND // * The current UAS can't reverse OR there is no current UAS diff --git a/src/ui/JoystickAxis.h b/src/ui/JoystickAxis.h index 7d766232f882f06f6691a7ed5e586aa34d44019f..9693ee994c069947591f2b75a1ad903a694157f7 100644 --- a/src/ui/JoystickAxis.h +++ b/src/ui/JoystickAxis.h @@ -30,6 +30,7 @@ This file is part of the QGROUNDCONTROL project #include #include "JoystickInput.h" +#include "Vehicle.h" namespace Ui { class JoystickAxis; @@ -62,7 +63,7 @@ public slots: */ void setValue(float value); /** @brief Specify the UAS that this axis should track for displaying throttle properly. */ - void setActiveUAS(UASInterface* uas); + void activeVehicleChanged(Vehicle* vehicle); private: int id; ///< The ID for this axis. Corresponds to the IDs used by JoystickInput. @@ -72,7 +73,7 @@ private: * @param uas The currently-active UAS. * @param axisMapping The new mapping for this axis. */ - void updateUIBasedOnUAS(UASInterface* uas, JoystickInput::JOYSTICK_INPUT_MAPPING axisMapping); + void updateUIBasedOnUAS(Vehicle* vehicle, JoystickInput::JOYSTICK_INPUT_MAPPING axisMapping); private slots: /** @brief Handle changes to the mapping dropdown bar. */ diff --git a/src/ui/JoystickButton.cc b/src/ui/JoystickButton.cc index 97941557ba0678bd8bea4c5aee891c491ee3a1c1..5f094ff51f09935f799514cb25195fb05cb99364 100644 --- a/src/ui/JoystickButton.cc +++ b/src/ui/JoystickButton.cc @@ -1,6 +1,6 @@ #include "JoystickButton.h" #include "ui_JoystickButton.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" JoystickButton::JoystickButton(int id, QWidget *parent) : QWidget(parent), @@ -9,7 +9,7 @@ JoystickButton::JoystickButton(int id, QWidget *parent) : { m_ui->setupUi(this); m_ui->joystickButtonLabel->setText(QString::number(id)); - this->setActiveUAS(UASManager::instance()->getActiveUAS()); + this->activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); connect(m_ui->joystickAction, SIGNAL(currentIndexChanged(int)), this, SLOT(actionComboBoxChanged(int))); } @@ -18,12 +18,14 @@ JoystickButton::~JoystickButton() delete m_ui; } -void JoystickButton::setActiveUAS(UASInterface* uas) +void JoystickButton::activeVehicleChanged(Vehicle* vehicle) { // Disable signals so that changes to joystickAction don't trigger JoystickInput updates. disconnect(m_ui->joystickAction, SIGNAL(currentIndexChanged(int)), this, SLOT(actionComboBoxChanged(int))); - if (uas) + if (vehicle) { + UAS* uas = vehicle->uas(); + m_ui->joystickAction->setEnabled(true); m_ui->joystickAction->clear(); m_ui->joystickAction->addItem("--"); diff --git a/src/ui/JoystickButton.h b/src/ui/JoystickButton.h index 559f49201891615b996bdad4af0fa8939735b680..4e76cde2133e280bd50e59862c9e4a1078384f9e 100644 --- a/src/ui/JoystickButton.h +++ b/src/ui/JoystickButton.h @@ -30,7 +30,7 @@ This file is part of the QGROUNDCONTROL project #include -#include "UASInterface.h" +#include "Vehicle.h" namespace Ui { @@ -47,7 +47,7 @@ public: public slots: /** @brief Specify the UAS that this axis should track for displaying throttle properly. */ - void setActiveUAS(UASInterface* uas); + void activeVehicleChanged(Vehicle* vehicle); /** @brieft Specify which action this button should correspond to. * Values 0 and higher indicate a specific action, while -1 indicates no action. */ void setAction(int index); diff --git a/src/ui/JoystickWidget.cc b/src/ui/JoystickWidget.cc index dec912b53604dbf4fc56432400bb589d9218dd9f..5926322befaa0f5b2cf2bcbaacdb2cbd96e2675b 100644 --- a/src/ui/JoystickWidget.cc +++ b/src/ui/JoystickWidget.cc @@ -187,7 +187,7 @@ void JoystickWidget::createUIForJoystick() JoystickButton* button = new JoystickButton(i, m_ui->buttonBox); button->setAction(joystick->getActionForButton(i)); connect(button, SIGNAL(actionChanged(int,int)), this->joystick, SLOT(setButtonAction(int,int))); - connect(this->joystick, SIGNAL(activeUASSet(UASInterface*)), button, SLOT(setActiveUAS(UASInterface*))); + connect(this->joystick, &JoystickInput::activeVehicleChanged, button, &JoystickButton::activeVehicleChanged); m_ui->buttonLayout->addWidget(button); buttons.append(button); } @@ -230,7 +230,7 @@ void JoystickWidget::createUIForJoystick() connect(axis, SIGNAL(mappingChanged(int,JoystickInput::JOYSTICK_INPUT_MAPPING)), this->joystick, SLOT(setAxisMapping(int,JoystickInput::JOYSTICK_INPUT_MAPPING))); connect(axis, SIGNAL(inversionChanged(int,bool)), this->joystick, SLOT(setAxisInversion(int,bool))); connect(axis, SIGNAL(rangeChanged(int,bool)), this->joystick, SLOT(setAxisRangeLimit(int,bool))); - connect(this->joystick, SIGNAL(activeUASSet(UASInterface*)), axis, SLOT(setActiveUAS(UASInterface*))); + connect(this->joystick, &JoystickInput::activeVehicleChanged, axis, &JoystickAxis::activeVehicleChanged); m_ui->axesLayout->addWidget(axis); axes.append(axis); } diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index f92f9526d2f9894ca9d0bc3104c69491b2fb8bb5..1632c2cd119eee005b8e95c201d0c0e900f749e0 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -71,6 +71,7 @@ This file is part of the QGROUNDCONTROL project #include "QGCFileDialog.h" #include "QGCMessageBox.h" #include "QGCDockWidget.h" +#include "MultiVehicleManager.h" #include "CustomCommandWidget.h" #ifdef UNITTEST_BUILD @@ -569,13 +570,13 @@ void MainWindow::_createInnerDockWidget(const QString& widgetName) #ifndef __mobile__ void MainWindow::_showHILConfigurationWidgets(void) { - UASInterface* uas = UASManager::instance()->getActiveUAS(); + Vehicle* vehicle = MultiVehicleManager::instance()->activeVehicle(); - if (!uas) { + if (!vehicle) { return; } - UAS* mav = dynamic_cast(uas); + UAS* mav = vehicle->uas(); Q_ASSERT(mav); int uasId = mav->getUASID(); @@ -757,26 +758,12 @@ void MainWindow::connectCommonActions() _ui.actionSetup->activate(QAction::Trigger); } - // The UAS actions are not enabled without connection to system - _ui.actionLiftoff->setEnabled(false); - _ui.actionLand->setEnabled(false); - _ui.actionEmergency_Kill->setEnabled(false); - _ui.actionEmergency_Land->setEnabled(false); - _ui.actionShutdownMAV->setEnabled(false); - // Connect actions from ui connect(_ui.actionAdd_Link, SIGNAL(triggered()), this, SLOT(manageLinks())); // Connect internal actions - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(UASCreated(UASInterface*))); - connect(UASManager::instance(), SIGNAL(UASDeleted(int)), this, SLOT(UASDeleted(int))); - - // Unmanned System controls - connect(_ui.actionLiftoff, SIGNAL(triggered()), UASManager::instance(), SLOT(launchActiveUAS())); - connect(_ui.actionLand, SIGNAL(triggered()), UASManager::instance(), SLOT(returnActiveUAS())); - connect(_ui.actionEmergency_Land, SIGNAL(triggered()), UASManager::instance(), SLOT(stopActiveUAS())); - connect(_ui.actionEmergency_Kill, SIGNAL(triggered()), UASManager::instance(), SLOT(killActiveUAS())); - connect(_ui.actionShutdownMAV, SIGNAL(triggered()), UASManager::instance(), SLOT(shutdownActiveUAS())); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleAdded, this, &MainWindow::_vehicleAdded); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleRemoved, this, &MainWindow::_vehicleRemoved); // Views actions connect(_ui.actionFlight, SIGNAL(triggered()), this, SLOT(loadFlightView())); @@ -853,17 +840,9 @@ void MainWindow::commsWidgetDestroyed(QObject *obj) } } -void MainWindow::UASCreated(UASInterface* uas) +void MainWindow::_vehicleAdded(Vehicle* vehicle) { - // The UAS actions are not enabled without connection to system - _ui.actionLiftoff->setEnabled(true); - _ui.actionLand->setEnabled(true); - _ui.actionEmergency_Kill->setEnabled(true); - _ui.actionEmergency_Land->setEnabled(true); - _ui.actionShutdownMAV->setEnabled(true); - - connect(uas, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)), this, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64))); - connect(uas, SIGNAL(misconfigurationDetected(UASInterface*)), this, SLOT(handleMisconfiguration(UASInterface*))); + connect(vehicle->uas(), SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)), this, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64))); // HIL #ifndef __mobile__ @@ -883,11 +862,13 @@ void MainWindow::UASCreated(UASInterface* uas) } } -void MainWindow::UASDeleted(int uasId) +void MainWindow::_vehicleRemoved(Vehicle* vehicle) { - if (_mapUasId2HilDockWidget.contains(uasId)) { - _mapUasId2HilDockWidget[uasId]->deleteLater(); - _mapUasId2HilDockWidget.remove(uasId); + int vehicleId = vehicle->id(); + + if (_mapUasId2HilDockWidget.contains(vehicleId)) { + _mapUasId2HilDockWidget[vehicleId]->deleteLater(); + _mapUasId2HilDockWidget.remove(vehicleId); } } @@ -1029,33 +1010,6 @@ void MainWindow::_showDockWidgetAction(bool show) } -void MainWindow::handleMisconfiguration(UASInterface* uas) -{ - static QTime lastTime; - // We have to debounce this signal - if (!lastTime.isValid()) { - lastTime.start(); - } else { - if (lastTime.elapsed() < 10000) { - lastTime.start(); - return; - } - } - // Ask user if they want to handle this now - QMessageBox::StandardButton button = - QGCMessageBox::question( - tr("Missing or Invalid Onboard Configuration"), - tr("The onboard system configuration is missing or incomplete. Do you want to resolve this now?"), - QMessageBox::Ok | QMessageBox::Cancel, - QMessageBox::Ok); - if (button == QMessageBox::Ok) { - // They want to handle it, make sure this system is selected - UASManager::instance()->setActiveUAS(uas); - // Flick to config view - loadSetupView(); - } -} - void MainWindow::loadAnalyzeView() { if (_currentView != VIEW_ANALYZE) diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 021cd0e824ce90f4f9f6497a6e544ef5df457eb6..6db461f3725fbd7e33aae5eff63825636ca28460 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -65,6 +65,7 @@ This file is part of the QGROUNDCONTROL project #include "QGCMAVLinkLogPlayer.h" #include "MAVLinkDecoder.h" #include "QGCUASFileViewMulti.h" +#include "Vehicle.h" class QGCMapTool; class QGCMAVLinkMessageSender; @@ -134,13 +135,6 @@ public slots: /** @brief Show the application settings */ void showSettings(); - /** @brief Add a new UAS */ - void UASCreated(UASInterface* uas); - - /** @brief Remove an old UAS */ - void UASDeleted(int uasID); - - void handleMisconfiguration(UASInterface* uas); /** @brief Load configuration views */ void loadSetupView(); /** @brief Load view for pilot */ @@ -288,6 +282,10 @@ private slots: void _showQmlTestWidget(void); #endif void _closeWindow(void) { close(); } + +private slots: + void _vehicleAdded(Vehicle* vehicle); + void _vehicleRemoved(Vehicle* vehicle); private: /// Constructor is private since all creation should be through MainWindow::_create diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index 5cd863f9cd276f050a91e915c85528934abf3bbf..1a74850e0a2816eb0a39faabb959ca6bd21ca899 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -117,52 +117,6 @@ Ctrl+Q - - - true - - - - :/res/Launch - :/res/Launch:/res/Launch - - - Liftoff - - - - - - :/res/Land:/res/Land - - - Land - - - - - - :/res/Kill:/res/Kill - - - Emergency Land - - - Ctrl+L - - - - - - :/res/Kill:/res/Kill - - - Kill UAS - - - Ctrl+K - - Manage Communication Links @@ -215,21 +169,6 @@ Mute Audio Output - - - - :/res/Shutdown:/res/Shutdown - - - Shutdown MAV - - - Shutdown the onboard computer - works not during flight - - - Shutdown the onboard computer - works not during flight - - Settings diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc deleted file mode 100644 index df97f886bfab93ca2c6c18c2828ca0e225c390d3..0000000000000000000000000000000000000000 --- a/src/ui/MapWidget.cc +++ /dev/null @@ -1,1113 +0,0 @@ -/*================================================================== -======================================================================*/ - -/** - * @file - * @brief Implementation of MapWidget - * - * @author Lorenz Meier - * @author Mariano Lizarraga - * - */ - -#include -#include -#include - -#include "QGC.h" -#include "MapWidget.h" -#include "ui_MapWidget.h" -#include "UASInterface.h" -#include "UASManager.h" -#include "MAV2DIcon.h" -#include "Waypoint2DIcon.h" -#include "UASWaypointManager.h" - -#include "MG.h" - - -MapWidget::MapWidget(QWidget *parent) : - QWidget(parent), - mc(NULL), - zoomLevel(0), - uasIcons(), - uasTrails(), - mav(NULL), - lastUpdate(0), - initialized(false), - m_ui(new Ui::MapWidget) -{ - m_ui->setupUi(this); - init(); -} - -void MapWidget::init() -{ - if (!initialized) { - mc = new qmapcontrol::MapControl(this->size()); - // display the MapControl in the application - QGridLayout* layout = new QGridLayout(this); - layout->setMargin(0); - layout->setSpacing(0); - layout->addWidget(mc, 0, 0); - setLayout(layout); - - // VISUAL MAP STYLE - QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)} QAbstractButton:checked { border: 2px solid #379AC3; }"); - mc->setPen(QGC::colorCyan.darker(400)); - - waypointIsDrag = false; - - // Accept focus by clicking or keyboard - this->setFocusPolicy(Qt::StrongFocus); - - // create MapControl - - mc->showScale(true); - mc->showCoord(true); - mc->enablePersistentCache(); - mc->setMouseTracking(true); // required to update the mouse position for diplay and capture - - // create MapAdapter to get maps from - //TileMapAdapter* osmAdapter = new TileMapAdapter("tile.openstreetmap.org", "/%1/%2/%3.png", 256, 0, 17); - - qmapcontrol::MapAdapter* mapadapter_overlay = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=2.2&t=h&s=256&x=%2&y=%3&z=%1"); - - // MAP BACKGROUND - mapadapter = new qmapcontrol::GoogleSatMapAdapter(); - l = new qmapcontrol::MapLayer("Google Satellite", mapadapter); - mc->addLayer(l); - - // STREET OVERLAY - overlay = new qmapcontrol::MapLayer("Overlay", mapadapter_overlay); - overlay->setVisible(false); - mc->addLayer(overlay); - - // MAV FLIGHT TRACKS - tracks = new qmapcontrol::MapLayer("Tracking", mapadapter); - mc->addLayer(tracks); - - // WAYPOINT LAYER - // create a layer with the mapadapter and type GeometryLayer (for waypoints) - geomLayer = new qmapcontrol::GeometryLayer("Waypoints", mapadapter); - mc->addLayer(geomLayer); - - - - // - // Layer* gsatLayer = new Layer("Google Satellite", gsat, Layer::MapLayer); - // mc->addLayer(gsatLayer); - - - // Zurich, ETH - - int lastZoom = 16; - double lastLat = 47.376889; - double lastLon = 8.548056; - - QSettings settings; - settings.beginGroup("QGC_MAPWIDGET"); - lastLat = settings.value("LAST_LATITUDE", lastLat).toDouble(); - lastLon = settings.value("LAST_LONGITUDE", lastLon).toDouble(); - lastZoom = settings.value("LAST_ZOOM", lastZoom).toInt(); - settings.endGroup(); - - // SET INITIAL POSITION AND ZOOM - // Set default zoom level - mc->setZoom(lastZoom); - mc->setView(QPointF(lastLon, lastLat)); - - // Veracruz Mexico - //mc->setView(QPointF(-96.105208,19.138955)); - - // Add controls to select map provider - ///////////////////////////////////////////////// - QActionGroup* mapproviderGroup = new QActionGroup(this); - osmAction = new QAction(QIcon(":/res/openstreetmap.png"), tr("OpenStreetMap"), mapproviderGroup); - yahooActionMap = new QAction(QIcon(":/res/yahoo.png"), tr("Yahoo: Map"), mapproviderGroup); - yahooActionSatellite = new QAction(QIcon(":/res/yahoo.png"), tr("Yahoo: Satellite"), mapproviderGroup); - googleActionMap = new QAction(QIcon(":/res/google.png"), tr("Google: Map"), mapproviderGroup); - googleSatAction = new QAction(QIcon(":/res/google.png"), tr("Google: Sat"), mapproviderGroup); - osmAction->setCheckable(true); - yahooActionMap->setCheckable(true); - yahooActionSatellite->setCheckable(true); - googleActionMap->setCheckable(true); - googleSatAction->setCheckable(true); - googleSatAction->setChecked(true); - connect(mapproviderGroup, SIGNAL(triggered(QAction*)), - this, SLOT(mapproviderSelected(QAction*))); - - // Overlay seems currently broken - // yahooActionOverlay = new QAction(tr("Yahoo: street overlay"), this); - // yahooActionOverlay->setCheckable(true); - // yahooActionOverlay->setChecked(overlay->isVisible()); - // connect(yahooActionOverlay, SIGNAL(toggled(bool)), - // overlay, SLOT(setVisible(bool))); - - // mapproviderGroup->addAction(googleSatAction); - // mapproviderGroup->addAction(osmAction); - // mapproviderGroup->addAction(yahooActionOverlay); - // mapproviderGroup->addAction(googleActionMap); - // mapproviderGroup->addAction(yahooActionMap); - // mapproviderGroup->addAction(yahooActionSatellite); - - // Create map provider selection menu - mapMenu = new QMenu(this); - mapMenu->addActions(mapproviderGroup->actions()); - mapMenu->addSeparator(); - // mapMenu->addAction(yahooActionOverlay); - - mapButton = new QPushButton(this); - mapButton->setText("Map Source"); - mapButton->setMenu(mapMenu); - mapButton->setStyleSheet(buttonStyle); - - // create buttons to control the map (zoom, GPS tracking and WP capture) - QPushButton* zoomin = new QPushButton(QIcon(":/res/PlusSign"), "", this); - zoomin->setStyleSheet(buttonStyle); - QPushButton* zoomout = new QPushButton(QIcon(":/res/MinusSign"), "", this); - zoomout->setStyleSheet(buttonStyle); - createPath = new QPushButton(QIcon(":/res/BottomArrow"), "", this); - createPath->setStyleSheet(buttonStyle); - createPath->setToolTip(tr("Start / end waypoint add mode")); - createPath->setStatusTip(tr("Start / end waypoint add mode")); - // clearTracking = new QPushButton(QIcon(""), "", this); - // clearTracking->setStyleSheet(buttonStyle); - followgps = new QPushButton(QIcon(":/res/SystemLockScreen"), "", this); - followgps->setStyleSheet(buttonStyle); - followgps->setToolTip(tr("Follow the position of the current MAV with the map center")); - followgps->setStatusTip(tr("Follow the position of the current MAV with the map center")); - QPushButton* goToButton = new QPushButton(QIcon(""), "T", this); - goToButton->setStyleSheet(buttonStyle); - goToButton->setToolTip(tr("Enter a latitude/longitude position to move the map to")); - goToButton->setStatusTip(tr("Enter a latitude/longitude position to move the map to")); - - zoomin->setMaximumWidth(30); - zoomout->setMaximumWidth(30); - createPath->setMaximumWidth(30); - // clearTracking->setMaximumWidth(30); - followgps->setMaximumWidth(30); - goToButton->setMaximumWidth(30); - - // Set checkable buttons - // TODO: Currently checked buttons are are very difficult to distinguish when checked. - // create a style and the slots to change the background so it is easier to distinguish - followgps->setCheckable(true); - createPath->setCheckable(true); - - // add buttons to control the map (zoom, GPS tracking and WP capture) - QGridLayout* innerlayout = new QGridLayout(mc); - innerlayout->setMargin(3); - innerlayout->setSpacing(3); - innerlayout->addWidget(zoomin, 0, 0); - innerlayout->addWidget(zoomout, 1, 0); - innerlayout->addWidget(followgps, 2, 0); - innerlayout->addWidget(createPath, 3, 0); - //innerlayout->addWidget(clearTracking, 4, 0); - // Add spacers to compress buttons on the top left - innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 5, 0); - innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 0, 1, 0, 7); - innerlayout->addWidget(mapButton, 0, 6); - innerlayout->addWidget(goToButton, 0, 7); - innerlayout->setRowStretch(0, 1); - innerlayout->setRowStretch(1, 100); - mc->setLayout(innerlayout); - - // Configure the WP Path's pen - pointPen = new QPen(QColor(0, 255,0)); - pointPen->setWidth(3); - waypointPath = new qmapcontrol::LineString (wps, "Waypoint path", pointPen); - mc->layer("Waypoints")->addGeometry(waypointPath); - - //Camera Control - // CAMERA INDICATOR LAYER - // create a layer with the mapadapter and type GeometryLayer (for camera indicator) - camLayer = new qmapcontrol::GeometryLayer("Camera", mapadapter); - mc->addLayer(camLayer); - - //camLine = new qmapcontrol::LineString(camPoints,"Camera Eje", camBorderPen); - - drawCamBorder = false; - radioCamera = 10; - - // Done set state - initialized = true; - - - // Connect the required signals-slots - connect(zoomin, SIGNAL(clicked(bool)), - mc, SLOT(zoomIn())); - - connect(zoomout, SIGNAL(clicked(bool)), - mc, SLOT(zoomOut())); - - connect(goToButton, SIGNAL(clicked()), this, SLOT(goTo())); - - QList systems = UASManager::instance()->getUASList(); - foreach(UASInterface* system, systems) { - addUAS(system); - } - - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), - this, SLOT(addUAS(UASInterface*))); - - activeUASSet(UASManager::instance()->getActiveUAS()); - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUASSet(UASInterface*))); - - connect(mc, SIGNAL(mouseEventCoordinate(const QMouseEvent*, const QPointF)), - this, SLOT(captureMapClick(const QMouseEvent*, const QPointF))); - - connect(createPath, SIGNAL(clicked(bool)), - this, SLOT(createPathButtonClicked(bool))); - - - connect(geomLayer, SIGNAL(geometryClicked(Geometry*,QPoint)), - this, SLOT(captureGeometryClick(Geometry*, QPoint))); - - connect(geomLayer, SIGNAL(geometryDragged(Geometry*, QPointF)), - this, SLOT(captureGeometryDrag(Geometry*, QPointF))); - - connect(geomLayer, SIGNAL(geometryEndDrag(Geometry*, QPointF)), - this, SLOT(captureGeometryEndDrag(Geometry*, QPointF))); - - qDebug() << "CHECK END"; - } -} - -void MapWidget::goTo() -{ - bool ok; - QString text = QInputDialog::getText(this, tr("Please enter coordinates"), - tr("Coordinates (Lat,Lon):"), QLineEdit::Normal, - QString("%1,%2").arg(mc->currentCoordinate().y()).arg(mc->currentCoordinate().x()), &ok); - if (ok && !text.isEmpty()) { - QStringList split = text.split(","); - if (split.length() == 2) { - bool convert; - double latitude = split.first().toDouble(&convert); - ok &= convert; - double longitude = split.last().toDouble(&convert); - ok &= convert; - - if (ok) { - mc->setView(QPointF(longitude, latitude)); - } - } - } -} - - -void MapWidget::mapproviderSelected(QAction* action) -{ - if (mc) { - //delete mapadapter; - mapButton->setText(action->text()); - if (action == osmAction) { - int zoom = mapadapter->adaptedZoom(); - mc->setZoom(0); - - mapadapter = new qmapcontrol::OSMMapAdapter(); - l->setMapAdapter(mapadapter); - geomLayer->setMapAdapter(mapadapter); - - if (isVisible()) mc->updateRequestNew(); - mc->setZoom(zoom); - // yahooActionOverlay->setEnabled(false); - overlay->setVisible(false); - // yahooActionOverlay->setChecked(false); - - } else if (action == yahooActionMap) { - int zoom = mapadapter->adaptedZoom(); - mc->setZoom(0); - - mapadapter = new qmapcontrol::YahooMapAdapter(); - l->setMapAdapter(mapadapter); - geomLayer->setMapAdapter(mapadapter); - - if (isVisible()) mc->updateRequestNew(); - mc->setZoom(zoom); - // yahooActionOverlay->setEnabled(false); - overlay->setVisible(false); - // yahooActionOverlay->setChecked(false); - } else if (action == yahooActionSatellite) { - int zoom = mapadapter->adaptedZoom(); - QPointF a = mc->currentCoordinate(); - mc->setZoom(0); - - mapadapter = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=1.7&t=a&s=256&x=%2&y=%3&z=%1"); - l->setMapAdapter(mapadapter); - geomLayer->setMapAdapter(mapadapter); - - if (isVisible()) mc->updateRequestNew(); - mc->setZoom(zoom); - overlay->setVisible(false); - // yahooActionOverlay->setEnabled(true); - } else if (action == googleActionMap) { - int zoom = mapadapter->adaptedZoom(); - mc->setZoom(0); - mapadapter = new qmapcontrol::GoogleMapAdapter(); - l->setMapAdapter(mapadapter); - geomLayer->setMapAdapter(mapadapter); - - if (isVisible()) mc->updateRequestNew(); - mc->setZoom(zoom); - // yahooActionOverlay->setEnabled(false); - overlay->setVisible(false); - // yahooActionOverlay->setChecked(false); - } else if (action == googleSatAction) { - int zoom = mapadapter->adaptedZoom(); - mc->setZoom(0); - mapadapter = new qmapcontrol::GoogleSatMapAdapter(); - l->setMapAdapter(mapadapter); - geomLayer->setMapAdapter(mapadapter); - - if (isVisible()) mc->updateRequestNew(); - mc->setZoom(zoom); - // yahooActionOverlay->setEnabled(false); - overlay->setVisible(false); - // yahooActionOverlay->setChecked(false); - } else { - mapButton->setText("Select.."); - } - } -} - - -void MapWidget::createPathButtonClicked(bool checked) -{ - if (mc) { - Q_UNUSED(checked); - - if (createPath->isChecked()) { - // change the cursor shape - this->setCursor(Qt::PointingHandCursor); - mc->setMouseMode(qmapcontrol::MapControl::None); - - - // emit signal start to create a Waypoint global - //emit createGlobalWP(true, mc->currentCoordinate()); - - // // Clear the previous WP track - // // TODO: Move this to an actual clear track button and add a warning dialog - // mc->layer("Waypoints")->clearGeometries(); - // wps.clear(); - // path->setPoints(wps); - // mc->layer("Waypoints")->addGeometry(path); - // wpIndex.clear(); - } else { - - this->setCursor(Qt::ArrowCursor); - mc->setMouseMode(qmapcontrol::MapControl::Panning); - } - } -} - -/** - * Captures a click on the map and if in create WP path mode, it adds the WP on MouseButtonRelease - * - * @param event The mouse event - * @param coordinate The coordinate in which it occured the mouse event - * @note This slot is connected to the mouseEventCoordinate of the QMapControl object - */ - -void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordinate) -{ - if (QEvent::MouseButtonRelease == event->type() && createPath->isChecked()) { - // Create waypoint name - QString str; - - // create the WP and set everything in the LineString to display the path - Waypoint2DIcon* tempCirclePoint; - - if (mav) { - double altitude = 0.0; - double yaw = 0.0; - int wpListCount = mav->getWaypointManager()->getWaypointList().count(); - if (wpListCount > 0) { - altitude = mav->getWaypointManager()->getWaypointList().at(wpListCount-1)->getAltitude(); - yaw = mav->getWaypointManager()->getWaypointList().at(wpListCount-1)->getYaw(); - } - mav->getWaypointManager()->addWaypoint(new Waypoint(wpListCount, coordinate.y(), coordinate.x(), altitude, yaw, true)); - } else { - str = QString("%1").arg(waypointPath->numberOfPoints()); - tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle); - wpIcons.append(tempCirclePoint); - - mc->layer("Waypoints")->addGeometry(tempCirclePoint); - - qmapcontrol::Point* tempPoint = new qmapcontrol::Point(coordinate.x(), coordinate.y(),str); - wps.append(tempPoint); - waypointPath->addPoint(tempPoint); - - // Refresh the screen - if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect()); - } - - // emit signal mouse was clicked - //emit captureMapCoordinateClick(coordinate); - } -} - -void MapWidget::updateWaypoint(int uas, Waypoint* wp) -{ - // Update waypoint list and redraw map (last parameter) - updateWaypoint(uas, wp, true); -} - -/** - * This function is called if a a single waypoint is updated and - * also if the whole list changes. - */ -void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView) -{ - if (mc) { - // Make sure this is the right UAS - if (uas == this->mav->getUASID()) { - // Only accept waypoints in global coordinate frame - if (wp->getFrame() == MAV_FRAME_GLOBAL && wp->isNavigationType()) { - // We're good, this is a global waypoint - - // Get the index of this waypoint - // note the call to getGlobalFrameAndNavTypeIndexOf() - // as we're only handling global waypoints - int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameAndNavTypeIndexOf(wp); - // If not found, return (this should never happen, but helps safety) - if (wpindex == -1) return; - - // Check if wp exists yet in map - if (!(wpIcons.count() > wpindex)) { - // Waypoint is new, a new icon is created - QPointF coordinate; - coordinate.setX(wp->getLongitude()); - coordinate.setY(wp->getLatitude()); - createWaypointGraphAtMap(wpindex, coordinate); - } else { - // Waypoint exists, update it if we're not - // currently dragging it with the mouse - if(!waypointIsDrag) { - QPointF coordinate; - coordinate.setX(wp->getLongitude()); - coordinate.setY(wp->getLatitude()); - - Point* waypoint; - waypoint = wps.at(wpindex); - if (waypoint) { - // First set waypoint coordinate - waypoint->setCoordinate(coordinate); - // Now update icon position - wpIcons.at(wpindex)->setCoordinate(coordinate); - // Update pen - wpIcons.at(wpindex)->setPen(mavPens.value(uas)); - // Then waypoint line coordinate - Point* linesegment = NULL; - // If the line segment already exists, just update it - // else create a new one - if (waypointPath->points().size() > wpindex) { - linesegment = waypointPath->points().at(wpindex); - if (linesegment) linesegment->setCoordinate(coordinate); - } else { - waypointPath->addPoint(waypoint); - } - - // Update view - if (updateView) if (isVisible()) mc->updateRequest(waypoint->boundingBox().toRect()); - } - } - } - } else { - // Check if the index of this waypoint is larger than the global - // waypoint list. This implies that the coordinate frame of this - // waypoint was changed and the list containing only global - // waypoints was shortened. Thus update the whole list - if (waypointPath->points().count() > UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameAndNavTypeCount()) { - updateWaypointList(uas); - } - } - } - } -} - -void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate) -{ - //if (!wpExists(coordinate)) - { - // Create waypoint name - QString str; - - // create the WP and set everything in the LineString to display the path - //CirclePoint* tempCirclePoint = new CirclePoint(coordinate.x(), coordinate.y(), 10, str); - Waypoint2DIcon* tempCirclePoint; - - if (mav) { - int uas = mav->getUASID(); - str = QString("%1").arg(id); - qDebug() << "Waypoint list count:" << str; - tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, mavPens.value(uas)); - } else { - str = QString("%1").arg(id); - tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle); - } - - - mc->layer("Waypoints")->addGeometry(tempCirclePoint); - wpIcons.append(tempCirclePoint); - - Point* tempPoint = new Point(coordinate.x(), coordinate.y(),str); - wps.append(tempPoint); - waypointPath->addPoint(tempPoint); - - //wpIndex.insert(str,tempPoint); - qDebug()<<"Funcion createWaypointGraphAtMap WP= "< x= "<latitude()<<" y= "<longitude(); - - // Refresh the screen - if (isVisible()) if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect()); - } - - //// // emit signal mouse was clicked - // emit captureMapCoordinateClick(coordinate); -} - -int MapWidget::wpExists(const QPointF coordinate) -{ - if (mc) { - for (int i = 0; i < wps.size(); i++) { - if (wps.at(i)->latitude() == coordinate.y() && - wps.at(i)->longitude()== coordinate.x()) { - return 1; - } - } - } - return 0; -} - - -void MapWidget::captureGeometryClick(Geometry* geom, QPoint point) -{ - Q_UNUSED(geom); - Q_UNUSED(point); - - if (mc) mc->setMouseMode(qmapcontrol::MapControl::None); -} - -void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate) -{ - waypointIsDrag = true; - - // Refresh the screen - if (isVisible()) mc->updateRequest(geom->boundingBox().toRect()); - - int temp = 0; - - // Get waypoint index in list - bool wpIndexOk; - int index = geom->name().toInt(&wpIndexOk); - - Waypoint2DIcon* point2Find = dynamic_cast (geom); - - if (wpIndexOk && point2Find && wps.count() > index) { - // Update visual - point2Find->setCoordinate(coordinate); - waypointPath->points().at(index)->setCoordinate(coordinate); - if (isVisible()) mc->updateRequest(waypointPath->boundingBox().toRect()); - - // Update waypoint data storage - if (mav) { - QVector wps = mav->getWaypointManager()->getGlobalFrameAndNavTypeWaypointList(); - - if (wps.size() > index) { - Waypoint* wp = wps.at(index); - wp->setLatitude(coordinate.y()); - wp->setLongitude(coordinate.x()); - mav->getWaypointManager()->notifyOfChange(wp); - } - } - - // qDebug() << geom->name(); - temp = geom->get_myIndex(); - //qDebug() << temp; - emit sendGeometryEndDrag(coordinate,temp); - } - - waypointIsDrag = false; -} - -void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate) -{ - Q_UNUSED(geom); - Q_UNUSED(coordinate); - // TODO: Investigate why when creating the waypoint path this slot is being called - - // Only change the mouse mode back to panning when not creating a WP path - if (!createPath->isChecked()) { - waypointIsDrag = false; - mc->setMouseMode(qmapcontrol::MapControl::Panning); - } - -} - -MapWidget::~MapWidget() -{ - delete mc; - delete m_ui; -} -/** - * - * @param uas the UAS/MAV to monitor/display with the HUD - */ -void MapWidget::addUAS(UASInterface* uas) -{ - connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int))); -} - -/** - * Update the whole list of waypoints. This is e.g. necessary if the list order changed. - * The UAS manager will emit the appropriate signal whenever updating the list - * is necessary. - */ -void MapWidget::updateWaypointList(int uas) -{ - if (mc) { - // Get already existing waypoints - UASInterface* uasInstance = UASManager::instance()->getUASForId(uas); - if (uasInstance) { - // Get update rect of old content, this is what will be redrawn - // in the last step - QRect updateRect = waypointPath->boundingBox().toRect(); - - // Get all waypoints, including non-global waypoints - QVector wpList = uasInstance->getWaypointManager()->getWaypointList(); - - // Clear if necessary - if (wpList.count() == 0) { - clearWaypoints(uas); - return; - } - - // Trim internal list to number of global waypoints in the waypoint manager list - int overSize = waypointPath->points().count() - uasInstance->getWaypointManager()->getGlobalFrameAndNavTypeCount(); - if (overSize > 0) { - // Remove n waypoints at the end of the list - // the remaining waypoints will be updated - // in the next step - for (int i = 0; i < overSize; ++i) { - wps.removeLast(); - mc->layer("Waypoints")->removeGeometry(wpIcons.last()); - wpIcons.removeLast(); - waypointPath->points().removeLast(); - } - } - - // Load all existing waypoints into map view - foreach (Waypoint* wp, wpList) { - // Block map draw updates, since we update everything in the next step - // but update internal data structures. - // Please note that updateWaypoint() ignores non-global waypoints - updateWaypoint(mav->getUASID(), wp, false); - } - - // Update view - if (isVisible()) mc->updateRequest(updateRect); - } - } -} - -void MapWidget::redoWaypoints(int uas) -{ - // QObject* sender = QObject::sender(); - // UASWaypointManager* manager = dynamic_cast(sender); - // if (sender) - // { - // Get waypoint list for this MAV - - // Clear all waypoints - //clearWaypoints(); - // Re-add the updated waypoints - - // } - - updateWaypointList(uas); -} - -void MapWidget::activeUASSet(UASInterface* uas) -{ - // Disconnect old MAV - if (mav) { - // Disconnect the waypoint manager / data storage from the UI - disconnect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int))); - disconnect(mav->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); - disconnect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*))); - } - - if (uas && mc) { - mav = uas; - QColor color = mav->getColor(); - color.setAlphaF(0.9); - QPen* pen = new QPen(color); - pen->setWidth(3.0); - mavPens.insert(mav->getUASID(), pen); - // FIXME Remove after refactoring - waypointPath->setPen(pen); - - // Delete all waypoints and add waypoint from new system - //redoWaypoints(); - updateWaypointList(uas->getUASID()); - - // Connect the waypoint manager / data storage to the UI - connect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int))); - connect(mav->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); - connect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*))); - - updateSystemSpecs(mav->getUASID()); - updateSelectedSystem(mav->getUASID()); - mc->updateRequest(waypointPath->boundingBox().toRect()); - } -} - -void MapWidget::updateSystemSpecs(int uas) -{ - if (mc) { - foreach (qmapcontrol::Point* p, uasIcons.values()) { - MAV2DIcon* icon = dynamic_cast(p); - if (icon && icon->getUASId() == uas) { - // Set new airframe - icon->setAirframe(UASManager::instance()->getUASForId(uas)->getAirframe()); - icon->drawIcon(); - } - } - } -} - -void MapWidget::updateSelectedSystem(int uas) -{ - if (mc) { - foreach (qmapcontrol::Point* p, uasIcons.values()) { - MAV2DIcon* icon = dynamic_cast(p); - if (icon) { - // Set as selected if ids match - icon->setSelectedUAS((icon->getUASId() == uas)); - } - } - } -} - -void MapWidget::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec) -{ - Q_UNUSED(roll); - Q_UNUSED(pitch); - Q_UNUSED(usec); - if (mc) { - - if (uas) { - MAV2DIcon* icon = dynamic_cast(uasIcons.value(uas->getUASID(), NULL)); - if (icon) { - icon->setYaw(yaw); - } - } - } -} - -/** - * Updates the global position of one MAV and append the last movement to the trail - * - * @param uas The unmanned air system - * @param lat Latitude in WGS84 ellipsoid - * @param lon Longitutde in WGS84 ellipsoid - * @param alt Altitude over mean sea level - * @param usec Timestamp of the position message in milliseconds FIXME will move to microseconds - */ -void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec) -{ - Q_UNUSED(usec); - Q_UNUSED(alt); // FIXME Use altitude - if (mc) { - // create a LineString - //QList points; - // Points with a circle - // A QPen can be used to customize the - //pointpen->setWidth(3); - //points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen)); - - qmapcontrol::Point* p; - QPointF coordinate; - coordinate.setX(lon); - coordinate.setY(lat); - - if (!uasIcons.contains(uas->getUASID())) { - // Get the UAS color - QColor uasColor = uas->getColor(); - - // Icon - //QPen* pointpen = new QPen(uasColor); - qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__; - p = new MAV2DIcon(uas, 68, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle); - uasIcons.insert(uas->getUASID(), p); - mc->layer("Waypoints")->addGeometry(p); - - // Line - // A QPen also can use transparency - - // QList points; - // points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y())); - // QPen* linepen = new QPen(uasColor.darker()); - // linepen->setWidth(2); - - // // Create tracking line string - // qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen); - // uasTrails.insert(uas->getUASID(), ls); - - // // Add the LineString to the layer - // mc->layer("Waypoints")->addGeometry(ls); - } else { - // p = dynamic_cast(uasIcons.value(uas->getUASID())); - // if (p) - // { - p = uasIcons.value(uas->getUASID()); - p->setCoordinate(QPointF(lon, lat)); - //p->setYaw(uas->getYaw()); - // } - // Extend trail - // uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y())); - } - - if (isVisible()) mc->updateRequest(p->boundingBox().toRect()); - - //if (isVisible()) mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect()); - - if (this->mav && uas->getUASID() == this->mav->getUASID()) { - // Limit the position update rate - quint64 currTime = MG::TIME::getGroundTimeNow(); - if (currTime - lastUpdate > 120) { - lastUpdate = currTime; - // Sets the view to the interesting area - if (followgps->isChecked()) { - updatePosition(0, lon, lat); - } else { - // Refresh the screen - //if (isVisible()) mc->updateRequestNew(); - } - } - } - } -} - -/** - * Center the view on this position - */ -void MapWidget::updatePosition(float time, double lat, double lon) -{ - Q_UNUSED(time); - //gpsposition->setText(QString::number(time) + " / " + QString::number(lat) + " / " + QString::number(lon)); - if (followgps->isChecked() && isVisible() && mc) { - if (mc) mc->setView(QPointF(lat, lon)); - } -} - -void MapWidget::wheelEvent(QWheelEvent *event) -{ - if (mc) { - int numDegrees = event->delta() / 8; - int numSteps = numDegrees / 15; - // Calculate new zoom level - int newZoom = mc->currentZoom()+numSteps; - // Set new zoom level, level is bounded by map control - mc->setZoom(newZoom); - // Detail zoom level is the number of steps zoomed in further - // after the bounding has taken effect - detailZoom = qAbs(qMin(0, mc->currentZoom()-newZoom)); - - // visual field of camera - updateCameraPosition(20*newZoom,0,"no"); - } -} - -void MapWidget::keyPressEvent(QKeyEvent *event) -{ - if (mc) { - switch (event->key()) { - case Qt::Key_Plus: - mc->zoomIn(); - break; - case Qt::Key_Minus: - mc->zoomOut(); - break; - case Qt::Key_Left: - mc->scrollLeft(this->width()/scrollStep); - break; - case Qt::Key_Right: - mc->scrollRight(this->width()/scrollStep); - break; - case Qt::Key_Down: - mc->scrollDown(this->width()/scrollStep); - break; - case Qt::Key_Up: - mc->scrollUp(this->width()/scrollStep); - break; - default: - QWidget::keyPressEvent(event); - } - } -} - -void MapWidget::resizeEvent(QResizeEvent* event ) -{ - Q_UNUSED(event); - if (!initialized) { - init(); - } - if (mc) mc->resize(this->size()); -} - -void MapWidget::showEvent(QShowEvent* event) -{ - Q_UNUSED(event); -// if (isVisible()) -// { -// if (!initialized) -// { -// init(); -// } -// } -} - -void MapWidget::hideEvent(QHideEvent* event) -{ - Q_UNUSED(event); - if (mc) { - QSettings settings; - settings.beginGroup("QGC_MAPWIDGET"); - QPointF currentPos = mc->currentCoordinate(); - settings.setValue("LAST_LATITUDE", currentPos.y()); - settings.setValue("LAST_LONGITUDE", currentPos.x()); - settings.setValue("LAST_ZOOM", mc->currentZoom()); - settings.endGroup(); - settings.sync(); - } -} - - -void MapWidget::changeEvent(QEvent *e) -{ - QWidget::changeEvent(e); - switch (e->type()) { - case QEvent::LanguageChange: - m_ui->retranslateUi(this); - break; - default: - break; - } -} - -void MapWidget::clearWaypoints(int uas) -{ - if (mc) { - Q_UNUSED(uas); - // Clear the previous WP track - - //mc->layer("Waypoints")->clearGeometries(); - wps.clear(); - foreach (Point* p, wpIcons) { - mc->layer("Waypoints")->removeGeometry(p); - } - wpIcons.clear(); - - // Get bounding box of this object BEFORE deleting the content - QRect box = waypointPath->boundingBox().toRect(); - - // Delete the content - waypointPath->points().clear(); - - //delete waypointPath; - //waypointPath = new - //mc->layer("Waypoints")->addGeometry(waypointPath); - //wpIndex.clear(); - if (isVisible()) mc->updateRequest(box);//(waypointPath->boundingBox().toRect()); - - if(createPath->isChecked()) { - createPath->click(); - } - } -} - -void MapWidget::clearPath(int uas) -{ - Q_UNUSED(uas); - if (mc) { - mc->layer("Tracking")->clearGeometries(); - foreach (qmapcontrol::LineString* ls, uasTrails) { - QPen* linepen = ls->pen(); - delete ls; - qmapcontrol::LineString* lsNew = new qmapcontrol::LineString(QList(), "", linepen); - mc->layer("Tracking")->addGeometry(lsNew); - } - // FIXME update this with update request only for bounding box of trails - if (isVisible()) mc->updateRequestNew();//(QRect(0, 0, width(), height())); - } -} - -void MapWidget::updateCameraPosition(double radio, double bearing, QString dir) -{ - Q_UNUSED(dir); - Q_UNUSED(bearing); - if (mc) { - // FIXME Mariano - //camPoints.clear(); - QPointF currentPos = mc->currentCoordinate(); - // QPointF actualPos = getPointxBearing_Range(currentPos.y(),currentPos.x(),bearing,distance); - - // qmapcontrol::Point* tempPoint1 = new qmapcontrol::Point(currentPos.x(), currentPos.y(),"inicial",qmapcontrol::Point::Middle); - // qmapcontrol::Point* tempPoint2 = new qmapcontrol::Point(actualPos.x(), actualPos.y(),"final",qmapcontrol::Point::Middle); - - // camPoints.append(tempPoint1); - // camPoints.append(tempPoint2); - - // camLine->setPoints(camPoints); - - QPen* camBorderPen = new QPen(QColor(255,0,0)); - camBorderPen->setWidth(2); - - //radio = mc->currentZoom() - - if(drawCamBorder) { - //clear camera borders - mc->layer("Camera")->clearGeometries(); - - //create a camera borders - qmapcontrol::CirclePoint* camBorder = new qmapcontrol::CirclePoint(currentPos.x(), currentPos.y(), radio, "camBorder", qmapcontrol::Point::Middle, camBorderPen); - - //camBorder->setCoordinate(currentPos); - - mc->layer("Camera")->addGeometry(camBorder); - // mc->layer("Camera")->addGeometry(camLine); - if (isVisible()) mc->updateRequestNew(); - - } else { - //clear camera borders - mc->layer("Camera")->clearGeometries(); - if (isVisible()) mc->updateRequestNew(); - - } - } -} - -void MapWidget::drawBorderCamAtMap(bool status) -{ - drawCamBorder = status; - updateCameraPosition(20,0,"no"); - -} - -QPointF MapWidget::getPointxBearing_Range(double lat1, double lon1, double bearing, double distance) -{ - QPointF temp; - - double rad = M_PI/180; - - bearing = bearing*rad; - temp.setX((lon1 + ((distance/60) * (sin(bearing))))); - temp.setY((lat1 + ((distance/60) * (cos(bearing))))); - - return temp; -} - diff --git a/src/ui/MapWidget.h b/src/ui/MapWidget.h deleted file mode 100644 index d2b98a3ddf7bad6abdd65d675faf54c56b55a3c2..0000000000000000000000000000000000000000 --- a/src/ui/MapWidget.h +++ /dev/null @@ -1,195 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Definition of MapWidget - * - * @author Lorenz Meier - * @author Mariano Lizarraga - * - */ - -#ifndef MAPWIDGET_H -#define MAPWIDGET_H - -#include -#include -#include "qmapcontrol.h" -#include "UASInterface.h" -#include "QPointF" - -#include - - - -class QMenu; -class Waypoint; -class Waypoint2DIcon; - -namespace Ui -{ -class MapWidget; -} - -using namespace qmapcontrol; - -/** - * @brief 2D Moving map - * - * This map displays street maps, aerial images and the MAV position, - * waypoints and trails on top. - */ -class MapWidget : public QWidget -{ - Q_OBJECT -public: - MapWidget(QWidget *parent = 0); - ~MapWidget(); - -public slots: - void addUAS(UASInterface* uas); - void activeUASSet(UASInterface* uas); - /** @brief Update the system specs of one system */ - void updateSystemSpecs(int uas); - /** @brief Update the selected system */ - void updateSelectedSystem(int uas); - /** @brief Update the attitude */ - void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec); - void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec); - void updatePosition(float time, double lat, double lon); - void updateCameraPosition(double distance, double bearing, QString dir); - QPointF getPointxBearing_Range(double lat1, double lon1, double bearing, double distance); - - /** @brief Clear the waypoints overlay layer */ - void clearWaypoints(int uas=0); - /** @brief Clear the UAV tracks on the map */ - void clearPath(int uas=0); - - /** @brief Update waypoint list */ - void updateWaypointList(int uas); - /** @brief Clear all waypoints and re-add them from updated list */ - void redoWaypoints(int uas=0); - - /** @brief Update waypoint */ - void updateWaypoint(int uas, Waypoint* wp); - void updateWaypoint(int uas, Waypoint* wp, bool updateView); - - void drawBorderCamAtMap(bool status); - /** @brief Bring up dialog to go to a specific location */ - void goTo(); - -protected: - void changeEvent(QEvent* e); - void wheelEvent(QWheelEvent *event); - void keyPressEvent(QKeyEvent *event); - void resizeEvent(QResizeEvent* event); - /** @brief Start widget updating */ - void showEvent(QShowEvent* event); - /** @brief Stop widget updating */ - void hideEvent(QHideEvent* event); - - QAction* osmAction; - QAction* yahooActionMap; - QAction* yahooActionSatellite; - QAction* yahooActionOverlay; - QAction* googleActionMap; - QAction* googleSatAction; - - - QPushButton* followgps; - QPushButton* createPath; - QPushButton* clearTracking; - QLabel* gpsposition; - QMenu* mapMenu; - QPushButton* mapButton; - - qmapcontrol::MapControl* mc; ///< QMapControl widget - qmapcontrol::MapAdapter* mapadapter; ///< Adapter to load the map data - qmapcontrol::Layer* l; ///< Current map layer (background) - qmapcontrol::Layer* overlay; ///< Street overlay (foreground) - qmapcontrol::Layer* tracks; ///< Layer for UAV tracks - qmapcontrol::GeometryLayer* geomLayer; ///< Layer for waypoints - - //only for experiment - qmapcontrol::GeometryLayer* camLayer; ///< Layer for camera indicator - - int zoomLevel; - int detailZoom; ///< Steps zoomed in further than qMapControl allows - static const int scrollStep = 40; ///< Scroll n pixels per keypress - static const int maxZoom = 50; ///< Maximum zoom level - - //Layer* gSatLayer; - - QMap uasIcons; - QMap uasTrails; - QMap mavPens; - //QMap > mavWps; - //QMap waypointPaths; - UASInterface* mav; - quint64 lastUpdate; - bool initialized; - - /** @brief Initialize view */ - void init(); - -protected slots: - void captureMapClick (const QMouseEvent* event, const QPointF coordinate); - void captureGeometryClick(Geometry*, QPoint); - void captureGeometryDrag(Geometry* geom, QPointF coordinate); - void captureGeometryEndDrag(Geometry* geom, QPointF coordinate); - - void createPathButtonClicked(bool checked); - - /** @brief Create the graphic representation of the waypoint */ - void createWaypointGraphAtMap(int id, const QPointF coordinate); - void mapproviderSelected(QAction* action); - -signals: - //void movePoint(QPointF newCoord); - //void captureMapCoordinateClick(const QPointF coordinate); //ROCA - //void createGlobalWP(bool value, QPointF centerCoordinate); - void waypointCreated(Waypoint* wp); - void sendGeometryEndDrag(const QPointF coordinate, const int index); - - -private: - Ui::MapWidget *m_ui; - QList wps; - QListwpIcons; - qmapcontrol::LineString* waypointPath; - //QHash wpIndex; - QPen* pointPen; - int wpExists(const QPointF coordinate); - bool waypointIsDrag; - - - qmapcontrol::LineString* camLine; - QList camPoints; - QPointF lastCamBorderPos; - bool drawCamBorder; - int radioCamera; - -}; - -#endif // MAPWIDGET_H diff --git a/src/ui/MapWidget.ui b/src/ui/MapWidget.ui deleted file mode 100644 index e9521cb4a3369b5bf76a6125b26162ba7ea51d55..0000000000000000000000000000000000000000 --- a/src/ui/MapWidget.ui +++ /dev/null @@ -1,25 +0,0 @@ - - - MapWidget - - - - 0 - 0 - 400 - 300 - - - - ArrowCursor - - - true - - - Form - - - - - diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc index fe3c6bb49e98e7eea1ce241b69311a747da0fe66..bd0f2a0e3abf946864852626a3b47814bd97e302 100644 --- a/src/ui/QGCMAVLinkInspector.cc +++ b/src/ui/QGCMAVLinkInspector.cc @@ -2,7 +2,7 @@ #include "QGCMAVLink.h" #include "QGCMAVLinkInspector.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" #include "ui_QGCMAVLinkInspector.h" #include @@ -50,7 +50,7 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(MAVLinkProtocol* protocol, QWidget *par connect(ui->clearButton, SIGNAL(clicked()), this, SLOT(clearView())); // Connect external connections - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addSystem(UASInterface*))); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleAdded, this, &QGCMAVLinkInspector::_vehicleAdded); connect(protocol, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), this, SLOT(receiveMessage(LinkInterface*,mavlink_message_t))); // Attach the UI's refresh rate to a timer. @@ -58,12 +58,12 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(MAVLinkProtocol* protocol, QWidget *par updateTimer.start(updateInterval); } -void QGCMAVLinkInspector::addSystem(UASInterface* uas) +void QGCMAVLinkInspector::_vehicleAdded(Vehicle* vehicle) { - ui->systemComboBox->addItem(uas->getUASName(), uas->getUASID()); + ui->systemComboBox->addItem(vehicle->uas()->getUASName(), vehicle->id()); // Add a tree for a new UAS - addUAStoTree(uas->getUASID()); + addUAStoTree(vehicle->id()); } void QGCMAVLinkInspector::selectDropDownMenuSystem(int dropdownid) @@ -97,9 +97,10 @@ void QGCMAVLinkInspector::rebuildComponentList() ui->componentComboBox->addItem(tr("All"), 0); // Fill - UASInterface* uas = UASManager::instance()->getUASForId(selectedSystemID); - if (uas) + Vehicle* vehicle = MultiVehicleManager::instance()->getVehicleById(selectedSystemID); + if (vehicle) { + UASInterface* uas = vehicle->uas(); QMap components = uas->getComponents(); foreach (int id, components.keys()) { @@ -329,10 +330,10 @@ void QGCMAVLinkInspector::addUAStoTree(int sysId) if(!uasTreeWidgetItems.contains(sysId)) { // Add the UAS to the main tree after it has been created - UASInterface* uas = UASManager::instance()->getUASForId(sysId); - - if (uas) + Vehicle* vehicle = MultiVehicleManager::instance()->getVehicleById(sysId); + if (vehicle) { + UASInterface* uas = vehicle->uas(); QStringList idstring; if (uas->getUASName() == "") { diff --git a/src/ui/QGCMAVLinkInspector.h b/src/ui/QGCMAVLinkInspector.h index 2c86e0730cb10a5b60ad258c6c706d1107943a92..3c5a154b1785470d5b690c5891dff905aa0809bb 100644 --- a/src/ui/QGCMAVLinkInspector.h +++ b/src/ui/QGCMAVLinkInspector.h @@ -6,6 +6,7 @@ #include #include "MAVLinkProtocol.h" +#include "Vehicle.h" namespace Ui { class QGCMAVLinkInspector; @@ -28,8 +29,6 @@ public slots: void clearView(); /** @brief Update view */ void refreshView(); - /** @brief Add system to the list */ - void addSystem(UASInterface* uas); /** @brief Add component to the list */ void addComponent(int uas, int component, const QString& name); /** @Brief Select a system through the drop down menu */ @@ -71,6 +70,9 @@ protected: static const unsigned int updateInterval; ///< The update interval of the refresh function static const float updateHzLowpass; ///< The low-pass filter value for the frequency of each message + +private slots: + void _vehicleAdded(Vehicle* vehicle); private: Ui::QGCMAVLinkInspector *ui; diff --git a/src/ui/QGCMapRCToParamDialog.cpp b/src/ui/QGCMapRCToParamDialog.cpp index 760c17d3a217c53a6c8502bd30e15006b25f9c40..19c79201101d8e97c58d99f82a4efd369d915f95 100644 --- a/src/ui/QGCMapRCToParamDialog.cpp +++ b/src/ui/QGCMapRCToParamDialog.cpp @@ -26,7 +26,7 @@ #include "QGCMapRCToParamDialog.h" #include "ui_QGCMapRCToParamDialog.h" -#include "AutoPilotPluginManager.h" +#include "MultiVehicleManager.h" #include #include @@ -49,7 +49,7 @@ QGCMapRCToParamDialog::QGCMapRCToParamDialog(QString param_id, UASInterface *mav ui->paramIdLabel->setText(param_id); // refresh the parameter from onboard to make sure the current value is used - AutoPilotPlugin* autopilot = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(mav).data(); + AutoPilotPlugin* autopilot = MultiVehicleManager::instance()->getVehicleById(mav->getUASID())->autopilotPlugin(); Q_ASSERT(autopilot); connect(autopilot->getParameterFact(FactSystem::defaultComponentId, param_id), &Fact::valueChanged, this, &QGCMapRCToParamDialog::_parameterUpdated); autopilot->refreshParameter(FactSystem::defaultComponentId, param_id); diff --git a/src/ui/QGCRGBDView.cc b/src/ui/QGCRGBDView.cc index bea5d1725fda94a4b7a980e2e06962df0153ed00..7f224d1b17e7771ffc141dc210daf1e664ff63c9 100644 --- a/src/ui/QGCRGBDView.cc +++ b/src/ui/QGCRGBDView.cc @@ -3,7 +3,7 @@ #include #include "QGCRGBDView.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" QGCRGBDView::QGCRGBDView(int width, int height, QWidget *parent) : HUD(width, height, parent), @@ -22,7 +22,8 @@ QGCRGBDView::QGCRGBDView(int width, int height, QWidget *parent) : enableDepthAction->setChecked(depthEnabled); connect(enableDepthAction, SIGNAL(triggered(bool)), this, SLOT(enableDepth(bool))); - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &QGCRGBDView::_activeVehicleChanged); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); clearData(); loadSettings(); @@ -52,7 +53,7 @@ void QGCRGBDView::loadSettings() settings.endGroup(); } -void QGCRGBDView::setActiveUAS(UASInterface* uas) +void QGCRGBDView::_activeVehicleChanged(Vehicle* vehicle) { if (this->uas != NULL) { @@ -62,14 +63,12 @@ void QGCRGBDView::setActiveUAS(UASInterface* uas) clearData(); } - if (uas) + if (vehicle) { // Now connect the new UAS // Setup communication - connect(uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*))); + connect(vehicle->uas(), SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*))); } - - HUD::setActiveUAS(uas); } void QGCRGBDView::clearData(void) diff --git a/src/ui/QGCRGBDView.h b/src/ui/QGCRGBDView.h index 58b1503dbc9ba1b3c13ce4edd1350fce18232737..67c6f46dceb72465e61b6e3cb48f2494f417cee8 100644 --- a/src/ui/QGCRGBDView.h +++ b/src/ui/QGCRGBDView.h @@ -2,6 +2,7 @@ #define QGCRGBDVIEW_H #include "HUD.h" +#include "Vehicle.h" class QGCRGBDView : public HUD { @@ -13,8 +14,6 @@ public: signals: public slots: - void setActiveUAS(UASInterface* uas); - void clearData(void); void enableRGB(bool enabled); void enableDepth(bool enabled); @@ -31,6 +30,9 @@ protected: void storeSettings(); /** @brief Load configuration of widget */ void loadSettings(); + +private slots: + void _activeVehicleChanged(Vehicle* vehicle); }; #endif // QGCRGBDVIEW_H diff --git a/src/ui/QGCUASFileViewMulti.cc b/src/ui/QGCUASFileViewMulti.cc index e2751625dc954e094f461f561e53d0272745ad25..a8b715e625152ee12d4595f7051e5f54bd368b77 100644 --- a/src/ui/QGCUASFileViewMulti.cc +++ b/src/ui/QGCUASFileViewMulti.cc @@ -1,7 +1,7 @@ #include "QGCUASFileViewMulti.h" #include "ui_QGCUASFileViewMulti.h" #include "UASInterface.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" #include "QGCUASFileView.h" QGCUASFileViewMulti::QGCUASFileViewMulti(QWidget *parent) : @@ -10,48 +10,47 @@ QGCUASFileViewMulti::QGCUASFileViewMulti(QWidget *parent) : { ui->setupUi(this); setMinimumSize(600, 80); - connect(UASManager::instance(), &UASManager::UASCreated, this, &QGCUASFileViewMulti::systemCreated); - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(systemSetActive(UASInterface*))); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &QGCUASFileViewMulti::_activeVehicleChanged); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleAdded, this, &QGCUASFileViewMulti::_vehicleAdded); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleRemoved, this, &QGCUASFileViewMulti::_vehicleRemoved); - if (UASManager::instance()->getActiveUAS()) { - systemCreated(UASManager::instance()->getActiveUAS()); - systemSetActive(UASManager::instance()->getActiveUAS()); + if (MultiVehicleManager::instance()->activeVehicle()) { + _vehicleAdded(MultiVehicleManager::instance()->activeVehicle()); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); } - } -void QGCUASFileViewMulti::systemDeleted(QObject* uas) +void QGCUASFileViewMulti::_vehicleRemoved(Vehicle* vehicle) { + UAS* uas = vehicle->uas(); Q_ASSERT(uas); - // Do not dynamic cast or de-reference QObject, since object is either in destructor or may have already - // been destroyed. - - UASInterface* mav = static_cast(uas); - QGCUASFileView* list = lists.value(mav, NULL); + QGCUASFileView* list = lists.value(uas, NULL); if (list) { delete list; - lists.remove(mav); + lists.remove(uas); } } -void QGCUASFileViewMulti::systemCreated(UASInterface* uas) +void QGCUASFileViewMulti::_vehicleAdded(Vehicle* vehicle) { - Q_ASSERT(uas); - - QGCUASFileView* list = new QGCUASFileView(ui->stackedWidget, uas->getFileManager()); - lists.insert(uas, list); - ui->stackedWidget->addWidget(list); - // Ensure widget is deleted when system is deleted - connect(uas, SIGNAL(destroyed(QObject*)), this, SLOT(systemDeleted(QObject*))); + UAS* uas = vehicle->uas(); + + if (!lists.contains(uas)) { + QGCUASFileView* list = new QGCUASFileView(ui->stackedWidget, uas->getFileManager()); + lists.insert(uas, list); + ui->stackedWidget->addWidget(list); + } } -void QGCUASFileViewMulti::systemSetActive(UASInterface* uas) +void QGCUASFileViewMulti::_activeVehicleChanged(Vehicle* vehicle) { - QGCUASFileView* list = lists.value(uas, NULL); - if (list) { - ui->stackedWidget->setCurrentWidget(list); + if (vehicle) { + QGCUASFileView* list = lists.value(vehicle->uas(), NULL); + if (list) { + ui->stackedWidget->setCurrentWidget(list); + } } } diff --git a/src/ui/QGCUASFileViewMulti.h b/src/ui/QGCUASFileViewMulti.h index f9ed4f7b04ec494b708da833557da9ac180aa410..5cb89da833412c4926bba4f9a3c1422e257bf3cc 100644 --- a/src/ui/QGCUASFileViewMulti.h +++ b/src/ui/QGCUASFileViewMulti.h @@ -5,7 +5,7 @@ #include #include "QGCUASFileView.h" -#include "UASInterface.h" +#include "UAS.h" namespace Ui { @@ -20,14 +20,14 @@ public: explicit QGCUASFileViewMulti(QWidget *parent = 0); ~QGCUASFileViewMulti(); -public slots: - void systemDeleted(QObject* uas); - void systemCreated(UASInterface* uas); - void systemSetActive(UASInterface* uas); - protected: void changeEvent(QEvent *e); - QMap lists; + QMap lists; + +private slots: + void _vehicleAdded(Vehicle* vehicle); + void _vehicleRemoved(Vehicle* vehicle); + void _activeVehicleChanged(Vehicle* vehicle); private: Ui::QGCUASFileViewMulti *ui; diff --git a/src/ui/QGCWaypointListMulti.cc b/src/ui/QGCWaypointListMulti.cc index c24eb38aab277854d7b8e15252d95d803875d932..d272868cd33a756baa6400e3e70a1463d34a0f61 100644 --- a/src/ui/QGCWaypointListMulti.cc +++ b/src/ui/QGCWaypointListMulti.cc @@ -1,6 +1,6 @@ #include "QGCWaypointListMulti.h" #include "ui_QGCWaypointListMulti.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" void* QGCWaypointListMulti::_offlineUAS = NULL; @@ -11,18 +11,18 @@ QGCWaypointListMulti::QGCWaypointListMulti(QWidget *parent) : _ui->setupUi(this); setMinimumSize(600, 80); - connect(UASManager::instance(), &UASManager::UASCreated, this, &QGCWaypointListMulti::_systemCreated); - connect(UASManager::instance(), &UASManager::activeUASSet, this, &QGCWaypointListMulti::_systemSetActive); - - WaypointList* list = new WaypointList(_ui->stackedWidget, UASManager::instance()->getActiveUASWaypointManager()); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleAdded, this, &QGCWaypointListMulti::_vehicleAdded); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleRemoved, this, &QGCWaypointListMulti::_vehicleRemoved); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &QGCWaypointListMulti::_activeVehicleChanged); + + WaypointList* list = new WaypointList(_ui->stackedWidget, MultiVehicleManager::instance()->activeWaypointManager()); _lists.insert(_offlineUAS, list); _ui->stackedWidget->addWidget(list); - if (UASManager::instance()->getActiveUAS()) { - _systemCreated(UASManager::instance()->getActiveUAS()); - _systemSetActive(UASManager::instance()->getActiveUAS()); + if (MultiVehicleManager::instance()->activeVehicle()) { + _vehicleAdded(MultiVehicleManager::instance()->activeVehicle()); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); } - } QGCWaypointListMulti::~QGCWaypointListMulti() @@ -30,12 +30,14 @@ QGCWaypointListMulti::~QGCWaypointListMulti() delete _ui; } -void QGCWaypointListMulti::_systemDeleted(QObject* uas) +void QGCWaypointListMulti::_vehicleRemoved(Vehicle* vehicle) { // Do not dynamic cast or de-reference QObject, since object is either in destructor or may have already // been destroyed. - if (uas) { + if (vehicle) { + UAS* uas = vehicle->uas(); + WaypointList* list = _lists.value(uas, NULL); if (list) { delete list; @@ -44,20 +46,24 @@ void QGCWaypointListMulti::_systemDeleted(QObject* uas) } } -void QGCWaypointListMulti::_systemCreated(UASInterface* uas) +void QGCWaypointListMulti::_vehicleAdded(Vehicle* vehicle) { + UAS* uas = vehicle->uas(); + WaypointList* list = new WaypointList(_ui->stackedWidget, uas->getWaypointManager()); _lists.insert(uas, list); _ui->stackedWidget->addWidget(list); - // Ensure widget is deleted when system is deleted - connect(uas, &QObject::destroyed, this, &QGCWaypointListMulti::_systemDeleted); } -void QGCWaypointListMulti::_systemSetActive(UASInterface* uas) +void QGCWaypointListMulti::_activeVehicleChanged(Vehicle* vehicle) { - WaypointList* list = _lists.value(uas, NULL); - if (list) { - _ui->stackedWidget->setCurrentWidget(list); + if (vehicle) { + UAS* uas = vehicle->uas(); + + WaypointList* list = _lists.value(uas, NULL); + if (list) { + _ui->stackedWidget->setCurrentWidget(list); + } } } diff --git a/src/ui/QGCWaypointListMulti.h b/src/ui/QGCWaypointListMulti.h index 6c201fef932f271910190d4e9dfdfce844217dd8..aad940bffcefb61a6c27777af0b5a54db0709ce4 100644 --- a/src/ui/QGCWaypointListMulti.h +++ b/src/ui/QGCWaypointListMulti.h @@ -5,7 +5,7 @@ #include #include "WaypointList.h" -#include "UASInterface.h" +#include "MultiVehicleManager.h" namespace Ui { @@ -25,9 +25,9 @@ protected: virtual void changeEvent(QEvent *e); private slots: - void _systemDeleted(QObject* uas); - void _systemCreated(UASInterface* uas); - void _systemSetActive(UASInterface* uas); + void _vehicleRemoved(Vehicle* vehicle); + void _vehicleAdded(Vehicle* vehicle); + void _activeVehicleChanged(Vehicle* vehicle); private: diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index f2f39b3a890ab0425d1080e19fbae9288a3f48b4..6f27aed458b0f8923b0e0c5a0a03f581c143ec94 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -85,8 +85,6 @@ WaypointList::WaypointList(QWidget *parent, UASWaypointManager* wpm) : connect(m_ui->saveButton, SIGNAL(clicked()), this, SLOT(saveWaypoints())); connect(m_ui->loadButton, SIGNAL(clicked()), this, SLOT(loadWaypoints())); - //connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); - //VIEW TAB viewOnlyListLayout = new QVBoxLayout(m_ui->viewOnlyListWidget); diff --git a/src/ui/WaypointList.cc.orig b/src/ui/WaypointList.cc.orig deleted file mode 100644 index d55c5aca173543411cf62b3a1087d75fe00964c2..0000000000000000000000000000000000000000 --- a/src/ui/WaypointList.cc.orig +++ /dev/null @@ -1,642 +0,0 @@ -/*===================================================================== - -PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - -(c) 2009, 2010 PIXHAWK PROJECT - -This file is part of the PIXHAWK project - - PIXHAWK is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - PIXHAWK is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with PIXHAWK. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Waypoint list widget - * - * @author Lorenz Meier - * @author Benjamin Knecht - * @author Petri Tanskanen - * - */ - -#include "WaypointList.h" -#include "ui_WaypointList.h" -#include -#include -#include -#include -#include -#include - -WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : - QWidget(parent), - uas(NULL), - mavX(0.0), - mavY(0.0), - mavZ(0.0), - mavYaw(0.0), - showOfflineWarning(false), - m_ui(new Ui::WaypointList) -{ - - m_ui->setupUi(this); - - //EDIT TAB - - editableListLayout = new QVBoxLayout(m_ui->editableListWidget); - editableListLayout->setSpacing(0); - editableListLayout->setMargin(0); - editableListLayout->setAlignment(Qt::AlignTop); - m_ui->editableListWidget->setLayout(editableListLayout); - - // ADD WAYPOINT - // Connect add action, set right button icon and connect action to this class - connect(m_ui->addButton, SIGNAL(clicked()), m_ui->actionAddWaypoint, SIGNAL(triggered())); - connect(m_ui->actionAddWaypoint, SIGNAL(triggered()), this, SLOT(addEditable())); - - // ADD WAYPOINT AT CURRENT POSITION - connect(m_ui->positionAddButton, SIGNAL(clicked()), this, SLOT(addCurrentPositionWaypoint())); - - // SEND WAYPOINTS - connect(m_ui->transmitButton, SIGNAL(clicked()), this, SLOT(transmit())); - - // DELETE ALL WAYPOINTS - connect(m_ui->clearWPListButton, SIGNAL(clicked()), this, SLOT(clearWPWidget())); - - // REQUEST WAYPOINTS - connect(m_ui->readButton, SIGNAL(clicked()), this, SLOT(read())); - - // SAVE/LOAD WAYPOINTS - connect(m_ui->saveButton, SIGNAL(clicked()), this, SLOT(saveWaypoints())); - connect(m_ui->loadButton, SIGNAL(clicked()), this, SLOT(loadWaypoints())); - - //connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); - - //VIEW TAB - - viewOnlyListLayout = new QVBoxLayout(m_ui->viewOnlyListWidget); - viewOnlyListLayout->setSpacing(0); - viewOnlyListLayout->setMargin(0); - viewOnlyListLayout->setAlignment(Qt::AlignTop); - m_ui->viewOnlyListWidget->setLayout(viewOnlyListLayout); - - // REFRESH VIEW TAB - - connect(m_ui->refreshButton, SIGNAL(clicked()), this, SLOT(refresh())); - - - // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED - if (uas) - { - WPM = uas->getWaypointManager(); - //setUAS(uas); - } - else - { - // Hide buttons, which don't make sense without valid UAS - m_ui->positionAddButton->hide(); - m_ui->transmitButton->hide(); - m_ui->readButton->hide(); - m_ui->refreshButton->hide(); - //FIXME: The whole "Onboard Waypoints"-tab should be hidden, instead of "refresh" button - UnconnectedUASInfoWidget* inf = new UnconnectedUASInfoWidget(this); - viewOnlyListLayout->insertWidget(0, inf); //insert a "NO UAV" info into the Onboard Tab - showOfflineWarning = true; - WPM = new UASWaypointManager(NULL); - } - - setUAS(uas); - - // STATUS LABEL - updateStatusLabel(""); - - this->setVisible(false); - loadFileGlobalWP = false; - readGlobalWP = false; - centerMapCoordinate.setX(0.0); - centerMapCoordinate.setY(0.0); - -} - -WaypointList::~WaypointList() -{ - delete m_ui; -} - -void WaypointList::updatePosition(UASInterface* uas, double x, double y, double z, quint64 usec) -{ - Q_UNUSED(uas); - Q_UNUSED(usec); - mavX = x; - mavY = y; - mavZ = z; -} - -void WaypointList::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec) -{ - Q_UNUSED(uas); - Q_UNUSED(usec); - Q_UNUSED(roll); - Q_UNUSED(pitch); - mavYaw = yaw; -} - -void WaypointList::setUAS(UASInterface* uas) -{ - //if (this->uas == NULL && uas != NULL) - if (this->uas == NULL) - { - this->uas = uas; -<<<<<<< HEAD -======= - if(uas != NULL) - { - connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updatePosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); - } - ->>>>>>> 25e35803a3bef8d831e710769b2c1eb3ee2f967e - connect(WPM, SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &))); - connect(WPM, SIGNAL(waypointEditableListChanged(void)), this, SLOT(waypointEditableListChanged(void))); - connect(WPM, SIGNAL(waypointEditableChanged(int,Waypoint*)), this, SLOT(updateWaypointEditable(int,Waypoint*))); - connect(WPM, SIGNAL(waypointViewOnlyListChanged(void)), this, SLOT(waypointViewOnlyListChanged(void))); - connect(WPM, SIGNAL(waypointViewOnlyChanged(int,Waypoint*)), this, SLOT(updateWaypointViewOnly(int,Waypoint*))); - connect(WPM, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointViewOnlyChanged(quint16))); - if (uas != NULL) - { - connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updatePosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); - } - //connect(WPM,SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP())); - //connect(WPM,SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool))); - } -} - -void WaypointList::saveWaypoints() -{ - - QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)")); - WPM->saveWaypoints(fileName); - -} - -void WaypointList::loadWaypoints() -{ - //create a popup notifying the user about the limitations of offline editing - if (showOfflineWarning == true) - { - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Warning); - msgBox.setText("Offline editor!"); - msgBox.setInformativeText("You are using the offline mission editor. Please don't forget to save your mission plan before connecting the UAV, otherwise it will be lost."); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - int ret = msgBox.exec(); - showOfflineWarning = false; - } - QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); - WPM->loadWaypoints(fileName); -} - -void WaypointList::transmit() -{ - if (uas) - { - WPM->writeWaypoints(); - } -} - -void WaypointList::read() -{ - if (uas) - { - WPM->readWaypoints(true); - } -} - -void WaypointList::refresh() -{ - if (uas) - { - WPM->readWaypoints(false); - } -} - -void WaypointList::addEditable() -{ - - const QVector &waypoints = WPM->getWaypointEditableList(); - Waypoint *wp; - if (waypoints.size() > 0) - { - // Create waypoint with last frame - Waypoint *last = waypoints.at(waypoints.size()-1); - wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getParam1(), last->getParam2(), last->getParam3(), last->getParam4(), - last->getAutoContinue(), false, last->getFrame(), last->getAction()); - WPM->addWaypointEditable(wp); - } - else - { - if (uas) - { - // Create first waypoint at current MAV position - addCurrentPositionWaypoint(); - } - else - { - //Since no UAV available, create first default waypoint. - updateStatusLabel(tr("No UAV. Added default LOCAL (NED) waypoint")); - wp = new Waypoint(0, 0, 0, -0.50, 0, 0.20, 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); - WPM->addWaypointEditable(wp); - //create a popup notifying the user about the limitations of offline editing - if (showOfflineWarning == true) - { - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Warning); - msgBox.setText("Offline editor!"); - msgBox.setInformativeText("You are using the offline mission editor. Please don't forget to save your mission plan before connecting the UAV, otherwise it will be lost."); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - int ret = msgBox.exec(); - showOfflineWarning = false; - } - } - } - -} - - -void WaypointList::addCurrentPositionWaypoint() -{ - if (uas) - { - const QVector &waypoints = WPM->getWaypointEditableList(); - Waypoint *wp; - Waypoint *last = 0; - if (waypoints.size() > 0) - { - last = waypoints.at(waypoints.size()-1); - } - - if (uas->globalPositionKnown()) - { - float acceptanceRadiusGlobal = 10.0f; - float holdTime = 0.0f; - float yawGlobal = 0.0f; - if (last) - { - acceptanceRadiusGlobal = last->getAcceptanceRadius(); - holdTime = last->getHoldTime(); - yawGlobal = last->getYaw(); - } - // Create global frame waypoint per default - wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT); - WPM->addWaypointEditable(wp); - updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint")); - } - else if (uas->localPositionKnown()) - { - float acceptanceRadiusLocal = 0.2f; - float holdTime = 0.5f; - if (last) - { - acceptanceRadiusLocal = last->getAcceptanceRadius(); - holdTime = last->getHoldTime(); - } - // Create local frame waypoint as second option - wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, false, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); - WPM->addWaypointEditable(wp); - updateStatusLabel(tr("Added LOCAL (NED) waypoint")); - } - else - { - // Do nothing - updateStatusLabel(tr("Not adding waypoint, no position of MAV known yet.")); - } - } -} - -void WaypointList::updateStatusLabel(const QString &string) -{ - // Status label in write widget - m_ui->statusLabel->setText(string); - // Status label in read only widget - m_ui->viewStatusLabel->setText(string); -} - -// Request UASWaypointManager to send the SET_CURRENT message to UAV -void WaypointList::changeCurrentWaypoint(quint16 seq) -{ - if (uas) - { - WPM->setCurrentWaypoint(seq); - } -} - -// Request UASWaypointManager to set the new "current" and make sure all other waypoints are not "current" -void WaypointList::currentWaypointEditableChanged(quint16 seq) -{ - WPM->setCurrentEditable(seq); - const QVector &waypoints = WPM->getWaypointEditableList(); - - if (seq < waypoints.size()) - { - for(int i = 0; i < waypoints.size(); i++) - { - WaypointEditableView* widget = wpEditableViews.find(waypoints[i]).value(); - - if (waypoints[i]->getId() == seq) - { - widget->setCurrent(true); - } - else - { - widget->setCurrent(false); - } - } - } -} - -// Update waypointViews to correctly indicate the new current waypoint -void WaypointList::currentWaypointViewOnlyChanged(quint16 seq) -{ - const QVector &waypoints = WPM->getWaypointViewOnlyList(); - - if (seq < waypoints.size()) - { - for(int i = 0; i < waypoints.size(); i++) - { - WaypointViewOnlyView* widget = wpViewOnlyViews.find(waypoints[i]).value(); - - if (waypoints[i]->getId() == seq) - { - widget->setCurrent(true); - } - else - { - widget->setCurrent(false); - } - } - } -} - -void WaypointList::updateWaypointEditable(int uas, Waypoint* wp) -{ - Q_UNUSED(uas); - WaypointEditableView *wpv = wpEditableViews.value(wp); - wpv->updateValues(); -} - -void WaypointList::updateWaypointViewOnly(int uas, Waypoint* wp) -{ - Q_UNUSED(uas); - WaypointViewOnlyView *wpv = wpViewOnlyViews.value(wp); - wpv->updateValues(); -} - -void WaypointList::waypointViewOnlyListChanged() -{ - // Prevent updates to prevent visual flicker - this->setUpdatesEnabled(false); - const QVector &waypoints = WPM->getWaypointViewOnlyList(); - - if (!wpViewOnlyViews.empty()) { - QMapIterator viewIt(wpViewOnlyViews); - viewIt.toFront(); - while(viewIt.hasNext()) { - viewIt.next(); - Waypoint *cur = viewIt.key(); - int i; - for (i = 0; i < waypoints.size(); i++) { - if (waypoints[i] == cur) { - break; - } - } - if (i == waypoints.size()) { - WaypointViewOnlyView* widget = wpViewOnlyViews.find(cur).value(); - widget->hide(); - viewOnlyListLayout->removeWidget(widget); - wpViewOnlyViews.remove(cur); - } - } - } - - // then add/update the views for each waypoint in the list - for(int i = 0; i < waypoints.size(); i++) { - Waypoint *wp = waypoints[i]; - if (!wpViewOnlyViews.contains(wp)) { - WaypointViewOnlyView* wpview = new WaypointViewOnlyView(wp, this); - wpViewOnlyViews.insert(wp, wpview); - connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); - viewOnlyListLayout->insertWidget(i, wpview); - } - WaypointViewOnlyView *wpv = wpViewOnlyViews.value(wp); - - //check if ordering has changed - if(viewOnlyListLayout->itemAt(i)->widget() != wpv) { - viewOnlyListLayout->removeWidget(wpv); - viewOnlyListLayout->insertWidget(i, wpv); - } - - wpv->updateValues(); // update the values of the ui elements in the view - } - this->setUpdatesEnabled(true); - loadFileGlobalWP = false; - -} - - -void WaypointList::waypointEditableListChanged() -{ - // Prevent updates to prevent visual flicker - this->setUpdatesEnabled(false); - const QVector &waypoints = WPM->getWaypointEditableList(); - - if (!wpEditableViews.empty()) { - QMapIterator viewIt(wpEditableViews); - viewIt.toFront(); - while(viewIt.hasNext()) { - viewIt.next(); - Waypoint *cur = viewIt.key(); - int i; - for (i = 0; i < waypoints.size(); i++) { - if (waypoints[i] == cur) { - break; - } - } - if (i == waypoints.size()) { - WaypointEditableView* widget = wpEditableViews.find(cur).value(); - widget->hide(); - editableListLayout->removeWidget(widget); - wpEditableViews.remove(cur); - } - } - } - - // then add/update the views for each waypoint in the list - for(int i = 0; i < waypoints.size(); i++) { - Waypoint *wp = waypoints[i]; - if (!wpEditableViews.contains(wp)) { - WaypointEditableView* wpview = new WaypointEditableView(wp, this); - wpEditableViews.insert(wp, wpview); - connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); - connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); - connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); - //connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); - connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(currentWaypointEditableChanged(quint16))); - editableListLayout->insertWidget(i, wpview); - } - WaypointEditableView *wpv = wpEditableViews.value(wp); - - //check if ordering has changed - if(editableListLayout->itemAt(i)->widget() != wpv) { - editableListLayout->removeWidget(wpv); - editableListLayout->insertWidget(i, wpv); - } - - wpv->updateValues(); // update the values of the ui elements in the view - } - this->setUpdatesEnabled(true); - loadFileGlobalWP = false; - - -} - -void WaypointList::moveUp(Waypoint* wp) -{ - const QVector &waypoints = WPM->getWaypointEditableList(); - - //get the current position of wp in the local storage - int i; - for (i = 0; i < waypoints.size(); i++) { - if (waypoints[i] == wp) - break; - } - - // if wp was found and its not the first entry, move it - if (i < waypoints.size() && i > 0) { - WPM->moveWaypoint(i, i-1); - } -} - -void WaypointList::moveDown(Waypoint* wp) -{ - const QVector &waypoints = WPM->getWaypointEditableList(); - - //get the current position of wp in the local storage - int i; - for (i = 0; i < waypoints.size(); i++) { - if (waypoints[i] == wp) - break; - } - - // if wp was found and its not the last entry, move it - if (i < waypoints.size()-1) { - WPM->moveWaypoint(i, i+1); - } -} - -void WaypointList::removeWaypoint(Waypoint* wp) -{ - WPM->removeWaypoint(wp->getId()); -} - -void WaypointList::changeEvent(QEvent *e) -{ - switch (e->type()) { - case QEvent::LanguageChange: - m_ui->retranslateUi(this); - break; - default: - break; - } -} - - - -void WaypointList::on_clearWPListButton_clicked() -{ - - - if (uas) { - emit clearPathclicked(); - const QVector &waypoints = WPM->getWaypointEditableList(); - while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++) - WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value(); - widget->remove(); - } - } else { -// if(isGlobalWP) -// { -// emit clearPathclicked(); -// } - } -} - -///** @brief The MapWidget informs that a waypoint global was changed on the map */ - -//void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP) -//{ -// if (uas) -// { -// const QVector &waypoints = WPM->getWaypointEditableList(); -// if (waypoints.size() > 0) -// { -// Waypoint *temp = waypoints.at(indexWP); - -// temp->setX(coordinate.x()); -// temp->setY(coordinate.y()); - -// //WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value(); -// //widget->updateValues(); -// } -// } - - -//} - -///** @brief The MapWidget informs that a waypoint global was changed on the map */ - -//void WaypointList::waypointGlobalPositionChanged(Waypoint* wp) -//{ -// QPointF coordinate; -// coordinate.setX(wp->getX()); -// coordinate.setY(wp->getY()); - -// emit ChangeWaypointGlobalPosition(wp->getId(), coordinate); - - -//} - -void WaypointList::clearWPWidget() -{ - const QVector &waypoints = WPM->getWaypointEditableList(); - while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++) - WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value(); - widget->remove(); - } -} - -//void WaypointList::setIsLoadFileWP() -//{ -// loadFileGlobalWP = true; -//} - -//void WaypointList::setIsReadGlobalWP(bool value) -//{ -// // FIXME James Check this -// Q_UNUSED(value); -// // readGlobalWP = value; -//} diff --git a/src/ui/flightdisplay/FlightDisplay.cc b/src/ui/flightdisplay/FlightDisplay.cc index a154823b76d532aab283f8b631b0d10b10ffa3ad..6b2f87b666286cdcf06e4d838a0e75f599aea150 100644 --- a/src/ui/flightdisplay/FlightDisplay.cc +++ b/src/ui/flightdisplay/FlightDisplay.cc @@ -37,7 +37,6 @@ This file is part of the QGROUNDCONTROL project #include "ScreenToolsController.h" #include "FlightDisplay.h" -#include "UASManager.h" const char* kMainFlightDisplayGroup = "MainFlightDisplay"; diff --git a/src/ui/linechart/Linecharts.cc b/src/ui/linechart/Linecharts.cc index 6e97aaef4365d2e6a01747b8d95990a97fcf042c..f4f69a62561efd24955a97a0fd50193819195627 100644 --- a/src/ui/linechart/Linecharts.cc +++ b/src/ui/linechart/Linecharts.cc @@ -1,7 +1,7 @@ #include #include "Linecharts.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" #include "MainWindow.h" @@ -11,14 +11,12 @@ Linecharts::Linecharts(QWidget *parent) : active(true) { this->setVisible(false); - // Get current MAV list - QList systems = UASManager::instance()->getUASList(); - - // Add each of them - foreach (UASInterface* sys, systems) { - addSystem(sys); + + // Add each MAV + foreach (Vehicle* vehicle, MultiVehicleManager::instance()->vehicles()) { + addVehicle(vehicle); } - connect(UASManager::instance(), &UASManager::UASCreated, this, &Linecharts::addSystem); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleAdded, this, &Linecharts::addVehicle); } void Linecharts::showEvent(QShowEvent* event) @@ -56,8 +54,10 @@ void Linecharts::hideEvent(QHideEvent* event) emit visibilityChanged(false); } -void Linecharts::addSystem(UASInterface* uas) +void Linecharts::addVehicle(Vehicle* vehicle) { + UAS* uas = vehicle->uas(); + // FIXME Add removeSystem() call // Compatibility hack diff --git a/src/ui/linechart/Linecharts.h b/src/ui/linechart/Linecharts.h index ced7734ebaee1e585dae4ed82eed52c8196025cf..dfa608994ebc75afbd8d62e977be40ce367c7082 100644 --- a/src/ui/linechart/Linecharts.h +++ b/src/ui/linechart/Linecharts.h @@ -6,7 +6,7 @@ #include #include "LinechartWidget.h" -#include "UASInterface.h" +#include "Vehicle.h" class Linecharts : public QStackedWidget { @@ -21,7 +21,7 @@ signals: public slots: /** @brief Add a new system to the list of plots */ - void addSystem(UASInterface* uas); + void addVehicle(Vehicle* vehicle); /** @brief Add a new generic message source (not a system) */ void addSource(QObject* obj); diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc index cee22331d44c301ac8f8e26871abd12975515198..8f3bd0177f30aab4079ac711bbe6c65f7607cc1d 100644 --- a/src/ui/map/MAV2DIcon.cc +++ b/src/ui/map/MAV2DIcon.cc @@ -4,6 +4,7 @@ #include #include "QGC.h" +#include "MultiVehicleManager.h" MAV2DIcon::MAV2DIcon(mapcontrol::MapGraphicItem* map,mapcontrol::OPMapWidget* parent, UASInterface* uas, int radius, int type) : UAVItem(map,parent), @@ -12,7 +13,7 @@ MAV2DIcon::MAV2DIcon(mapcontrol::MapGraphicItem* map,mapcontrol::OPMapWidget* pa type(type), airframe(uas->getAirframe()), iconColor(uas->getColor()), - selected(uas->getSelected()), + selected(MultiVehicleManager::instance()->activeUas() == uas), uasid(uas->getUASID()) { size = QSize(radius, radius); diff --git a/src/ui/map/QGCMapWidget.cc b/src/ui/map/QGCMapWidget.cc index 156c4156b60352d2d2093a39994ed1f1b2aca369..d48f41697580ceac3efbf007bf9b3cfcd715b7e8 100644 --- a/src/ui/map/QGCMapWidget.cc +++ b/src/ui/map/QGCMapWidget.cc @@ -7,6 +7,7 @@ #include "Waypoint2DIcon.h" #include "UASWaypointManager.h" #include "QGCMessageBox.h" +#include "MultiVehicleManager.h" QGCMapWidget::QGCMapWidget(QWidget *parent) : mapcontrol::OPMapWidget(parent), @@ -20,14 +21,18 @@ QGCMapWidget::QGCMapWidget(QWidget *parent) : mapPositionInitialized(false), homeAltitude(0), zoomBlocked(false), - uas(NULL) + _uas(NULL) { - currWPManager = UASManager::instance()->getActiveUASWaypointManager(); + currWPManager = MultiVehicleManager::instance()->activeWaypointManager(); + waypointLines.insert(0, new QGraphicsItemGroup(map)); + connect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int))); connect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); + connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*))); connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*))); + offlineMode = true; // Widget is inactive until shown defaultGuidedAlt = -1; @@ -56,6 +61,7 @@ QGCMapWidget::QGCMapWidget(QWidget *parent) : guidedaction->setText("Go To Here Alt (Guided Mode)"); connect(guidedaction,SIGNAL(triggered()),this,SLOT(guidedAltActionTriggered())); this->addAction(guidedaction); + // Set home location option QAction *sethomeaction = new QAction(this); sethomeaction->setText("Set Home Location Here"); @@ -64,7 +70,7 @@ QGCMapWidget::QGCMapWidget(QWidget *parent) : } void QGCMapWidget::guidedActionTriggered() { - if (!uas) + if (!_uas) { QGCMessageBox::information(tr("Error"), tr("Please connect first")); return; @@ -90,7 +96,7 @@ void QGCMapWidget::guidedActionTriggered() } bool QGCMapWidget::guidedAltActionTriggered() { - if (!uas) + if (!_uas) { QGCMessageBox::information(tr("Error"), tr("Please connect first")); return false; @@ -112,7 +118,7 @@ bool QGCMapWidget::guidedAltActionTriggered() */ bool QGCMapWidget::setHomeActionTriggered() { - if (!uas) + if (!_uas) { QGCMessageBox::information(tr("Error"), tr("Please connect first")); return false; @@ -178,16 +184,15 @@ void QGCMapWidget::showEvent(QShowEvent* event) // Connect map updates to the adapter slots connect(this, SIGNAL(WPValuesChanged(WayPointItem*)), this, SLOT(handleMapWaypointEdit(WayPointItem*)), Qt::UniqueConnection); - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)), Qt::UniqueConnection); - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUASSet(UASInterface*)), Qt::UniqueConnection); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleAdded, this, &QGCMapWidget::_vehicleAdded); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &QGCMapWidget::_activeVehicleChanged); + connect(UASManager::instance(), SIGNAL(homePositionChanged(double,double,double)), this, SLOT(updateHomePosition(double,double,double)), Qt::UniqueConnection); - - foreach (UASInterface* uas, UASManager::instance()->getUASList()) - { - addUAS(uas); + + foreach (Vehicle* vehicle, MultiVehicleManager::instance()->vehicles()) { + _vehicleAdded(vehicle); } - if (!mapInitialized) { internals::PointLatLng pos_lat_lon = internals::PointLatLng(0, 0); @@ -203,7 +208,7 @@ void QGCMapWidget::showEvent(QShowEvent* event) setFrameStyle(QFrame::NoFrame); // no border frame setBackgroundBrush(QBrush(Qt::black)); // tile background - if (!UASManager::instance()->getActiveUAS()) { + if (!MultiVehicleManager::instance()->activeVehicle()) { SetCurrentPosition(pos_lat_lon); // set the map position to default } @@ -211,7 +216,7 @@ void QGCMapWidget::showEvent(QShowEvent* event) updateHomePosition(UASManager::instance()->getHomeLatitude(), UASManager::instance()->getHomeLongitude(), UASManager::instance()->getHomeAltitude()); // Set currently selected system - activeUASSet(UASManager::instance()->getActiveUAS()); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); setFocus(); // Start timer @@ -333,8 +338,10 @@ void QGCMapWidget::mouseDoubleClickEvent(QMouseEvent* event) * * @param uas the UAS/MAV to monitor/display with the map widget */ -void QGCMapWidget::addUAS(UASInterface* uas) +void QGCMapWidget::_vehicleAdded(Vehicle* vehicle) { + UAS* uas = vehicle->uas(); + connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,double,quint64)), Qt::UniqueConnection); connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int)), Qt::UniqueConnection); @@ -348,10 +355,9 @@ void QGCMapWidget::addUAS(UASInterface* uas) } } -void QGCMapWidget::activeUASSet(UASInterface* uas) +void QGCMapWidget::_activeVehicleChanged(Vehicle* vehicle) { - // Only execute if proper UAS is set - this->uas = uas; + _uas = NULL; // Disconnect old MAV manager if (currWPManager) @@ -365,13 +371,15 @@ void QGCMapWidget::activeUASSet(UASInterface* uas) // Attach the new waypoint manager if a new UAS was selected. Otherwise, indicate // that no such manager exists. - if (uas) + if (vehicle) { - currWPManager = uas->getWaypointManager(); + _uas = vehicle->uas(); + + currWPManager = _uas->getWaypointManager(); - updateSelectedSystem(uas->getUASID()); - followUAVID = uas->getUASID(); - updateWaypointList(uas->getUASID()); + updateSelectedSystem(vehicle->id()); + followUAVID = vehicle->id(); + updateWaypointList(vehicle->id()); // Connect the waypoint manager / data storage to the UI connect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int)), Qt::UniqueConnection); @@ -380,7 +388,7 @@ void QGCMapWidget::activeUASSet(UASInterface* uas) connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*)), Qt::UniqueConnection); if (!mapPositionInitialized) { - internals::PointLatLng pos_lat_lon = internals::PointLatLng(uas->getLatitude(), uas->getLongitude()); + internals::PointLatLng pos_lat_lon = internals::PointLatLng(_uas->getLatitude(), _uas->getLongitude()); SetCurrentPosition(pos_lat_lon); // Zoom in @@ -449,9 +457,10 @@ void QGCMapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lo */ void QGCMapWidget::updateGlobalPosition() { - QList systems = UASManager::instance()->getUASList(); - foreach (UASInterface* system, systems) + foreach (Vehicle* vehicle, MultiVehicleManager::instance()->vehicles()) { + UAS* system = vehicle->uas(); + // Get reference to graphic UAV item mapcontrol::UAVItem* uav = GetUAV(system->getUASID()); // Check if reference is valid, else create a new one @@ -477,9 +486,10 @@ void QGCMapWidget::updateGlobalPosition() void QGCMapWidget::updateLocalPosition() { - QList systems = UASManager::instance()->getUASList(); - foreach (UASInterface* system, systems) + foreach (Vehicle* vehicle, MultiVehicleManager::instance()->vehicles()) { + UAS* system = vehicle->uas(); + // Get reference to graphic UAV item mapcontrol::UAVItem* uav = GetUAV(system->getUASID()); // Check if reference is valid, else create a new one @@ -517,7 +527,7 @@ void QGCMapWidget::updateSystemSpecs(int uas) if (icon && icon->getUASId() == uas) { // Set new airframe - icon->setAirframe(UASManager::instance()->getUASForId(uas)->getAirframe()); + icon->setAirframe(MultiVehicleManager::instance()->getVehicleById(uas)->uas()->getAirframe()); icon->drawIcon(); } } @@ -664,7 +674,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp) } // Currently only accept waypoint updates from the UAS in focus // this has to be changed to accept read-only updates from other systems as well. - UASInterface* uasInstance = UASManager::instance()->getUASForId(uas); + UASInterface* uasInstance = MultiVehicleManager::instance()->getVehicleById(uas)->uas(); if (currWPManager) { // Only accept waypoints in global coordinate frame @@ -774,7 +784,7 @@ void QGCMapWidget::updateWaypointList(int uas) qDebug() << "UPDATE WP LIST IN 2D MAP CALLED FOR UAS" << uas; // Currently only accept waypoint updates from the UAS in focus // this has to be changed to accept read-only updates from other systems as well. - UASInterface* uasInstance = UASManager::instance()->getUASForId(uas); + UASInterface* uasInstance = MultiVehicleManager::instance()->getVehicleById(uas)->uas(); if (currWPManager) { // ORDER MATTERS HERE! diff --git a/src/ui/map/QGCMapWidget.h b/src/ui/map/QGCMapWidget.h index bddc19860d6dcedcc7dc9de691aeab6f972dde3a..6178369d5df201696427cac3b56bfa3a17653fea 100644 --- a/src/ui/map/QGCMapWidget.h +++ b/src/ui/map/QGCMapWidget.h @@ -3,7 +3,9 @@ #include #include + #include "opmapcontrol.h" +#include "Vehicle.h" // Choose one default map type //#define MAP_DEFAULT_TYPE_BING @@ -51,8 +53,6 @@ public slots: bool guidedAltActionTriggered(); /** @brief Action triggered when set home action is selected from the context menu. */ bool setHomeActionTriggered(); - /** @brief Add system to map view */ - void addUAS(UASInterface* uas); /** @brief Update the global position of a system */ void updateGlobalPosition(UASInterface* uas, double lat, double lon, double altAMSL, double altWGS84, quint64 usec); /** @brief Update the global position of all systems */ @@ -63,8 +63,6 @@ public slots: void updateLocalPositionEstimates(); /** @brief Update the type, size, etc. of this system */ void updateSystemSpecs(int uas); - /** @brief Change current system in focus / editing */ - void activeUASSet(UASInterface* uas); /** @brief Show a dialog to jump to given GPS coordinates */ void showGoToDialog(); /** @brief Jump to the home position on the map */ @@ -177,8 +175,11 @@ protected: QPoint contextMousePressPos; ///< Mouse position when context menu activated. int defaultGuidedAlt; ///< Default altitude for guided mode bool zoomBlocked; ///< Wether zooming is blocked - UASInterface *uas; ///< Currently selected UAS. + UASInterface* _uas; ///< Currently selected UAS. +private slots: + void _vehicleAdded(Vehicle* vehicle); + void _activeVehicleChanged(Vehicle* vehicle); }; #endif // QGCMAPWIDGET_H diff --git a/src/ui/mapdisplay/QGCMapDisplay.cc b/src/ui/mapdisplay/QGCMapDisplay.cc index 192427e03f79defd435dd31626b211681bd323c8..2c4ce9f04613200d72c878f761de1d309abc4c5e 100644 --- a/src/ui/mapdisplay/QGCMapDisplay.cc +++ b/src/ui/mapdisplay/QGCMapDisplay.cc @@ -33,7 +33,6 @@ This file is part of the QGROUNDCONTROL project #include "ScreenToolsController.h" #include "QGCMapDisplay.h" -#include "UASManager.h" const char* kMainMapDisplayGroup = "MainMapDisplay"; diff --git a/src/ui/toolbar/MainToolBar.cc b/src/ui/toolbar/MainToolBar.cc index 20aa1f0a5beec7eaaa2bdf9a8a28fa7455728d89..9199d639b400e120dd5c940554ba32688497c855 100644 --- a/src/ui/toolbar/MainToolBar.cc +++ b/src/ui/toolbar/MainToolBar.cc @@ -38,10 +38,11 @@ This file is part of the QGROUNDCONTROL project #include "FlightDisplay.h" #include "QGCApplication.h" #include "MavManager.h" -#include "AutoPilotPluginManager.h" +#include "MultiVehicleManager.h" MainToolBar::MainToolBar(QWidget* parent) : QGCQmlWidgetHolder(parent) + , _vehicle(NULL) , _mav(NULL) , _toolBar(NULL) , _currentView(ViewNone) @@ -83,17 +84,19 @@ MainToolBar::MainToolBar(QWidget* parent) setVisible(true); emit configListChanged(); emit connectionCountChanged(_connectionCount); - _setActiveUAS(UASManager::instance()->getActiveUAS()); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); + // Link signals connect(LinkManager::instance(), &LinkManager::linkConfigurationChanged, this, &MainToolBar::_updateConfigurations); connect(LinkManager::instance(), &LinkManager::linkConnected, this, &MainToolBar::_linkConnected); connect(LinkManager::instance(), &LinkManager::linkDisconnected, this, &MainToolBar::_linkDisconnected); + // RSSI (didn't like standard connection) connect(MAVLinkProtocol::instance(), SIGNAL(radioStatusChanged(LinkInterface*, unsigned, unsigned, unsigned, unsigned, unsigned, unsigned, unsigned)), this, SLOT(_telemetryChanged(LinkInterface*, unsigned, unsigned, unsigned, unsigned, unsigned, unsigned, unsigned))); - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(_setActiveUAS(UASInterface*))); - connect(UASManager::instance(), SIGNAL(UASDeleted(UASInterface*)), this, SLOT(_forgetUAS(UASInterface*))); + + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &MainToolBar::_activeVehicleChanged); connect(this, &MainToolBar::heightChanged, this, &MainToolBar::_heightChanged); } @@ -267,30 +270,23 @@ void MainToolBar::setCurrentView(int currentView) } } -void MainToolBar::_forgetUAS(UASInterface* uas) +void MainToolBar::_activeVehicleChanged(Vehicle* vehicle) { - if (_mav != NULL && _mav == uas) { + // Disconnect the previous one (if any) + if (_vehicle) { disconnect(_mav, &UASInterface::remoteControlRSSIChanged, this, &MainToolBar::_remoteControlRSSIChanged); - disconnect(AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_mav).data(), &AutoPilotPlugin::parameterListProgress, this, &MainToolBar::_setProgressBarValue); + disconnect(_vehicle->autopilotPlugin(), &AutoPilotPlugin::parameterListProgress, this, &MainToolBar::_setProgressBarValue); _mav = NULL; + _vehicle = NULL; } -} -void MainToolBar::_setActiveUAS(UASInterface* active) -{ - // Do nothing if system is the same - if (_mav == active) { - return; - } - // Disconnect the previous one (if any) - if(_mav) { - _forgetUAS(_mav); - } + // Connect new system - _mav = active; - if (_mav) + if (vehicle) { + _vehicle = vehicle; + _mav = vehicle->uas(); connect(_mav, &UASInterface::remoteControlRSSIChanged, this, &MainToolBar::_remoteControlRSSIChanged); - connect(AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_mav).data(), &AutoPilotPlugin::parameterListProgress, this, &MainToolBar::_setProgressBarValue); + connect(_vehicle->autopilotPlugin(), &AutoPilotPlugin::parameterListProgress, this, &MainToolBar::_setProgressBarValue); } } diff --git a/src/ui/toolbar/MainToolBar.h b/src/ui/toolbar/MainToolBar.h index c6e2b4ce2472160d5eca1aa8c3ce6ca688118dde..deb2ea4900c0bf7d3b88bbb093cb6d77f804ed6f 100644 --- a/src/ui/toolbar/MainToolBar.h +++ b/src/ui/toolbar/MainToolBar.h @@ -114,8 +114,7 @@ signals: void showMessage(const QString& message); private slots: - void _forgetUAS (UASInterface* uas); - void _setActiveUAS (UASInterface* uas); + void _activeVehicleChanged (Vehicle* vehicle); void _updateConfigurations (); void _linkConnected (LinkInterface* link); void _linkDisconnected (LinkInterface* link); @@ -131,6 +130,7 @@ private: void _setToolBarState (const QString& key, bool value); private: + Vehicle* _vehicle; UASInterface* _mav; QQuickItem* _toolBar; ViewType_t _currentView; diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc index c36a40b962a8a63d6117a73d6db862bd279e108b..668da4bfe879bfa985a56e59e46a92364bb5fad7 100644 --- a/src/ui/uas/UASControlWidget.cc +++ b/src/ui/uas/UASControlWidget.cc @@ -36,21 +36,21 @@ This file is part of the PIXHAWK project #include #include "UASControlWidget.h" -#include -#include +#include "MultiVehicleManager.h" +#include "UAS.h" #include "QGC.h" #include "AutoPilotPluginManager.h" #include "FirmwarePluginManager.h" UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent), - uasID(-1), + _uas(NULL), armed(false) { ui.setupUi(this); - this->setUAS(UASManager::instance()->getActiveUAS()); - - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); + + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &UASControlWidget::_activeVehicleChanged); connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode())); ui.liftoffButton->hide(); @@ -62,14 +62,10 @@ UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent), void UASControlWidget::updateModesList() { - if (this->uasID == 0) { + if (!_uas) { return; } - - UASInterface*uas = UASManager::instance()->getUASForId(this->uasID); - Q_ASSERT(uas); - - _modeList = FirmwarePluginManager::instance()->firmwarePluginForAutopilot((MAV_AUTOPILOT)uas->getAutopilotType())->flightModes(); + _modeList = FirmwarePluginManager::instance()->firmwarePluginForAutopilot((MAV_AUTOPILOT)_uas->getAutopilotType())->flightModes(); // Set combobox items ui.modeComboBox->clear(); @@ -82,37 +78,32 @@ void UASControlWidget::updateModesList() ui.modeComboBox->update(); } -void UASControlWidget::setUAS(UASInterface* uas) +void UASControlWidget::_activeVehicleChanged(Vehicle* vehicle) { - if (this->uasID > 0) { - UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uasID); - if (oldUAS) { - disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(armSystem())); - disconnect(ui.liftoffButton, SIGNAL(clicked()), oldUAS, SLOT(launch())); - disconnect(ui.landButton, SIGNAL(clicked()), oldUAS, SLOT(home())); - disconnect(ui.shutdownButton, SIGNAL(clicked()), oldUAS, SLOT(shutdown())); - disconnect(oldUAS, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); - } + if (_uas) { + disconnect(ui.controlButton, SIGNAL(clicked()), _uas, SLOT(armSystem())); + disconnect(ui.liftoffButton, SIGNAL(clicked()), _uas, SLOT(launch())); + disconnect(ui.landButton, SIGNAL(clicked()), _uas, SLOT(home())); + disconnect(ui.shutdownButton, SIGNAL(clicked()), _uas, SLOT(shutdown())); + disconnect(_uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); + _uas = NULL; } // Connect user interface controls - if (uas) { + if (vehicle) { + _uas = vehicle->uas(); connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton())); - connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch())); - connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home())); - connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown())); - connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); + connect(ui.liftoffButton, SIGNAL(clicked()), _uas, SLOT(launch())); + connect(ui.landButton, SIGNAL(clicked()), _uas, SLOT(home())); + connect(ui.shutdownButton, SIGNAL(clicked()), _uas, SLOT(shutdown())); + connect(_uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); - ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName()); + ui.controlStatusLabel->setText(tr("Connected to ") + _uas->getUASName()); - this->uasID = uas->getUASID(); - setBackgroundColor(uas->getColor()); + setBackgroundColor(_uas->getColor()); this->updateModesList(); this->updateArmText(); - - } else { - this->uasID = -1; } } @@ -167,40 +158,37 @@ void UASControlWidget::updateState(int state) void UASControlWidget::transmitMode() { - UAS* uas = dynamic_cast(UASManager::instance()->getUASForId(this->uasID)); - if (uas) { + if (_uas) { uint8_t base_mode; uint32_t custom_mode; QString flightMode = ui.modeComboBox->itemText(ui.modeComboBox->currentIndex()); - if (FirmwarePluginManager::instance()->firmwarePluginForAutopilot((MAV_AUTOPILOT)uas->getAutopilotType())->setFlightMode(flightMode, &base_mode, &custom_mode)) { + if (FirmwarePluginManager::instance()->firmwarePluginForAutopilot((MAV_AUTOPILOT)_uas->getAutopilotType())->setFlightMode(flightMode, &base_mode, &custom_mode)) { if (armed) { base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } - if (uas->isHilEnabled() || uas->isHilActive()) { + if (_uas->isHilEnabled() || _uas->isHilActive()) { base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - uas->setMode(base_mode, custom_mode); + _uas->setMode(base_mode, custom_mode); QString modeText = ui.modeComboBox->currentText(); - ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(flightMode).arg(uas->getUASName())); + ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(flightMode).arg(_uas->getUASName())); } } } void UASControlWidget::cycleContextButton() { - UAS* uas = dynamic_cast(UASManager::instance()->getUASForId(this->uasID)); - if (uas) { + if (_uas) { if (!armed) { - uas->armSystem(); - ui.lastActionLabel->setText(QString("Arm %1").arg(uas->getUASName())); + _uas->armSystem(); + ui.lastActionLabel->setText(QString("Arm %1").arg(_uas->getUASName())); } else { - uas->disarmSystem(); - ui.lastActionLabel->setText(QString("Disarm %1").arg(uas->getUASName())); + _uas->disarmSystem(); + ui.lastActionLabel->setText(QString("Disarm %1").arg(_uas->getUASName())); } } } - diff --git a/src/ui/uas/UASControlWidget.h b/src/ui/uas/UASControlWidget.h index fd17c75a84eb2dccf268942581c4cd75e53c286e..aa19493fb6ca23d7fb4d7d6cbf0b9d06ed9b2642 100644 --- a/src/ui/uas/UASControlWidget.h +++ b/src/ui/uas/UASControlWidget.h @@ -36,9 +36,10 @@ This file is part of the QGROUNDCONTROL project #include #include #include -#include -#include -#include "AutoPilotPluginManager.h" + +#include "ui_UASControl.h" +#include "UASInterface.h" +#include "Vehicle.h" /** * @brief Widget controlling one MAV @@ -54,8 +55,6 @@ public: public slots: /** @brief Update modes list for selected system */ void updateModesList(); - /** @brief Set the system this widget controls */ - void setUAS(UASInterface* uasID); /** @brief Trigger next context action */ void cycleContextButton(); /** @brief Transmit the operation mode */ @@ -70,9 +69,12 @@ protected slots: void setBackgroundColor(QColor color); protected: - int uasID; ///< Reference to the current uas + UAS* _uas; QStringList _modeList; ///< Mode list for the current UAS bool armed; ///< Engine state + +private slots: + void _activeVehicleChanged(Vehicle* vehicle); private: Ui::uasControl ui; diff --git a/src/ui/uas/UASInfoWidget.cc b/src/ui/uas/UASInfoWidget.cc index 3c766649c300ea389d59f4c47513a4a0c86d573e..3fce669452af8ffb5921d3fd5a37e244edbcd75d 100644 --- a/src/ui/uas/UASInfoWidget.cc +++ b/src/ui/uas/UASInfoWidget.cc @@ -33,7 +33,7 @@ This file is part of the PIXHAWK project #include #include -#include +#include #include #include #include @@ -48,8 +48,8 @@ UASInfoWidget::UASInfoWidget(QWidget *parent, QString name) : QWidget(parent) this->name = name; activeUAS = NULL; - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); - setActiveUAS(UASManager::instance()->getActiveUAS()); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &UASInfoWidget::_activeVehicleChanged); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); startTime = QGC::groundTimeMilliseconds(); @@ -94,23 +94,23 @@ void UASInfoWidget::hideEvent(QHideEvent* event) updateTimer->stop(); } -void UASInfoWidget::addUAS(UASInterface* uas) +void UASInfoWidget::_activeVehicleChanged(Vehicle* vehicle) { - if (uas != NULL) { - connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int))); - connect(uas, SIGNAL(dropRateChanged(int,float)), this, SLOT(updateReceiveLoss(int,float))); - connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateCPULoad(UASInterface*,double))); - connect(uas, SIGNAL(errCountChanged(int,QString,QString,int)), this, SLOT(updateErrorCount(int,QString,QString,int))); - - // Set this UAS as active if it is the first one - if (activeUAS == 0) activeUAS = uas; + if (activeUAS) { + disconnect(activeUAS, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int))); + disconnect(activeUAS, SIGNAL(dropRateChanged(int,float)), this, SLOT(updateReceiveLoss(int,float))); + disconnect(activeUAS, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateCPULoad(UASInterface*,double))); + disconnect(activeUAS, SIGNAL(errCountChanged(int,QString,QString,int)), this, SLOT(updateErrorCount(int,QString,QString,int))); + activeUAS = NULL; + } + + if (vehicle) { + activeUAS = vehicle->uas(); + connect(activeUAS, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int))); + connect(activeUAS, SIGNAL(dropRateChanged(int,float)), this, SLOT(updateReceiveLoss(int,float))); + connect(activeUAS, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateCPULoad(UASInterface*,double))); + connect(activeUAS, SIGNAL(errCountChanged(int,QString,QString,int)), this, SLOT(updateErrorCount(int,QString,QString,int))); } -} - -void UASInfoWidget::setActiveUAS(UASInterface* uas) -{ - if (uas) - activeUAS = uas; } void UASInfoWidget::updateBattery(UASInterface* uas, double voltage, double current, double percent, int seconds) diff --git a/src/ui/uas/UASInfoWidget.h b/src/ui/uas/UASInfoWidget.h index 003cdfce46b6a40aa420a473a5328ca823a7e69f..1e72d9dcd60d1e9e24fad1ecf32f4a9256a336fb 100644 --- a/src/ui/uas/UASInfoWidget.h +++ b/src/ui/uas/UASInfoWidget.h @@ -38,6 +38,7 @@ This file is part of the QGROUNDCONTROL project #include "UASInterface.h" #include "ui_UASInfo.h" +#include "Vehicle.h" /** * @brief Info indicator for the currently active UAS @@ -51,10 +52,6 @@ public: ~UASInfoWidget(); public slots: - void addUAS(UASInterface* uas); - - void setActiveUAS(UASInterface* uas); - void updateBattery(UASInterface* uas, double voltage, double current, double percent, int seconds); void updateCPULoad(UASInterface* uas, double load); /** @@ -109,6 +106,9 @@ protected: void showEvent(QShowEvent* event); void hideEvent(QHideEvent* event); + +private slots: + void _activeVehicleChanged(Vehicle* vehicle); private: Ui::uasInfo ui; diff --git a/src/ui/uas/UASListWidget.cc b/src/ui/uas/UASListWidget.cc index 3733a88ce06f47067e3c592dbc977a4362422263..e7a8b028c6d46dab4d868032199b66d4b9736a23 100644 --- a/src/ui/uas/UASListWidget.cc +++ b/src/ui/uas/UASListWidget.cc @@ -36,7 +36,7 @@ This file is part of the PIXHAWK project #include "MG.h" #include "UASListWidget.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" #include "UAS.h" #include "UASView.h" #include "QGCUnconnectedInfoWidget.h" @@ -64,17 +64,13 @@ UASListWidget::UASListWidget(QWidget *parent) : QWidget(parent), connect(LinkManager::instance(), SIGNAL(linkDeleted(LinkInterface*)), this, SLOT(removeLink(LinkInterface*))); - // Listen for when UASes are added or removed. This does not manage the UASView - // widgets that are displayed within this widget. - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), - this, SLOT(addUAS(UASInterface*))); - connect(UASManager::instance(), SIGNAL(UASDeleted(UASInterface*)), - this, SLOT(removeUAS(UASInterface*))); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleAdded, this, &UASListWidget::_vehicleAdded); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleRemoved, this, &UASListWidget::_vehicleRemoved); // Get a list of all existing UAS - foreach (UASInterface* uas, UASManager::instance()->getUASList()) + foreach (Vehicle* vehicle, MultiVehicleManager::instance()->vehicles()) { - addUAS(uas); + _vehicleAdded(vehicle); } } @@ -149,8 +145,10 @@ void UASListWidget::updateStatus() } } -void UASListWidget::addUAS(UASInterface* uas) +void UASListWidget::_vehicleAdded(Vehicle* vehicle) { + UAS* uas = vehicle->uas(); + // If the list was empty, remove the unconnected widget and start the update timer. if (uasViews.isEmpty()) { @@ -163,10 +161,11 @@ void UASListWidget::addUAS(UASInterface* uas) uWidget = NULL; } } + if (!uasViews.contains(uas)) { // Only display the UAS in a single link. - QList x = uas->getLinks(); + QList x = vehicle->links(); if (x.size()) { LinkInterface* li = x.first(); @@ -189,16 +188,13 @@ void UASListWidget::addUAS(UASInterface* uas) } // And add the new UAS to the UASList - UASView* newView = new UASView(uas, newBox); + UASView* newView = new UASView(vehicle, newBox); uasViews.insert(uas, newView); uasToBoxMapping[uas] = newBox; newBox->layout()->addWidget(newView); } } -} - -void UASListWidget::activeUAS(UASInterface* uas) -{ + UASView* view = uasViews.value(uas, NULL); if (view) { view->setUASasActive(true); @@ -209,8 +205,10 @@ void UASListWidget::activeUAS(UASInterface* uas) * If the UAS was removed, check to see if it was the last one in the QGroupBox and delete * the QGroupBox if so. */ -void UASListWidget::removeUAS(UASInterface* uas) +void UASListWidget::_vehicleRemoved(Vehicle* vehicle) { + UAS* uas = vehicle->uas(); + // Remove the UASView and check if its parent GroupBox has any other children, // delete it if it doesn't. QGroupBox* box = uasToBoxMapping[uas]; diff --git a/src/ui/uas/UASListWidget.h b/src/ui/uas/UASListWidget.h index 494d16fdb892c7db395ce1c49297b009aab8794e..f614c3025ac209acf64e473c9a44cc1730c14d65 100644 --- a/src/ui/uas/UASListWidget.h +++ b/src/ui/uas/UASListWidget.h @@ -40,6 +40,7 @@ This file is part of the QGROUNDCONTROL project #include "UASView.h" #include "QGCUnconnectedInfoWidget.h" #include "ui_UASList.h" +#include "Vehicle.h" class UASListWidget : public QWidget { @@ -50,9 +51,6 @@ public: ~UASListWidget(); public slots: - void addUAS(UASInterface* uas); - void activeUAS(UASInterface* uas); - void removeUAS(UASInterface* uas); void removeLink(LinkInterface* link); protected: @@ -65,6 +63,10 @@ protected: QGCUnconnectedInfoWidget* uWidget; QTimer* updateTimer; void changeEvent(QEvent *e); + +private slots: + void _vehicleAdded(Vehicle* vehicle); + void _vehicleRemoved(Vehicle* vehicle); private: Ui::UASList* m_ui; diff --git a/src/ui/uas/UASQuickView.cc b/src/ui/uas/UASQuickView.cc index be18b191bea6f9739063af482131f8ea98c333dd..5ec115f17f8f9826f6dbcf2224849b38f591867e 100644 --- a/src/ui/uas/UASQuickView.cc +++ b/src/ui/uas/UASQuickView.cc @@ -5,6 +5,7 @@ #include "UASQuickViewTextItem.h" #include #include +#include "MultiVehicleManager.h" UASQuickView::UASQuickView(QWidget *parent) : QWidget(parent), uas(NULL) { @@ -17,12 +18,8 @@ UASQuickView::UASQuickView(QWidget *parent) : QWidget(parent), m_verticalLayoutList.append(new QVBoxLayout()); ui.horizontalLayout->addItem(m_verticalLayoutList[0]); - connect(UASManager::instance(),SIGNAL(activeUASSet(UASInterface*)),this,SLOT(setActiveUAS(UASInterface*))); - connect(UASManager::instance(),SIGNAL(UASCreated(UASInterface*)),this,SLOT(addUAS(UASInterface*))); - if (UASManager::instance()->getActiveUAS()) - { - addUAS(UASManager::instance()->getActiveUAS()); - } + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &UASQuickView::_activeVehicleChanged); + _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); this->setContextMenuPolicy(Qt::ActionsContextMenu); loadSettings(); @@ -249,26 +246,13 @@ void UASQuickView::updateTimerTick() } } -void UASQuickView::addUAS(UASInterface* uas) -{ - if (uas) - { - if (!this->uas) - { - setActiveUAS(uas); - } - } -} - -void UASQuickView::setActiveUAS(UASInterface* uas) +void UASQuickView::_activeVehicleChanged(Vehicle* vehicle) { - if (!uas) - { + if (uas) { return; } - this->uas = uas; + this->uas = vehicle->uas(); connect(uas,SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)),this,SLOT(valueChanged(int,QString,QString,QVariant,quint64))); - //connect(uas,SIGNAL()) } void UASQuickView::addSource(MAVLinkDecoder *decoder) { diff --git a/src/ui/uas/UASQuickView.h b/src/ui/uas/UASQuickView.h index 1204f5952570b68615509c265d177138baf01518..69b6b64b112d72cc6cb743311740b3776bf00a7c 100644 --- a/src/ui/uas/UASQuickView.h +++ b/src/ui/uas/UASQuickView.h @@ -4,12 +4,12 @@ #include #include #include -#include "uas/UASManager.h" #include "uas/UASInterface.h" #include "ui_UASQuickView.h" #include "UASQuickViewItem.h" #include "MAVLinkDecoder.h" #include "UASQuickViewItemSelect.h" +#include "Vehicle.h" class UASQuickView : public QWidget { Q_OBJECT @@ -63,12 +63,13 @@ public slots: void actionTriggered(bool checked); void actionTriggered(); void updateTimerTick(); - void addUAS(UASInterface* uas); - void setActiveUAS(UASInterface* uas); void selectDialogClosed(); void valueEnabled(QString value); void valueDisabled(QString value); void columnActionTriggered(); + +private slots: + void _activeVehicleChanged(Vehicle* vehicle); }; #endif // UASQUICKVIEW_H diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index f3af3168e41dd2c77ec2955c7b51770344da74b5..47c69a4637f5c5e1deb8f946a8e3bed75cff4803 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -35,7 +35,7 @@ This file is part of the PIXHAWK project #include #include "QGC.h" -#include "UASManager.h" +#include "MultiVehicleManager.h" #include "UASView.h" #include "UASWaypointManager.h" #include "MainWindow.h" @@ -45,9 +45,10 @@ This file is part of the PIXHAWK project #include #endif -UASView::UASView(UASInterface* uas, QWidget *parent) : +UASView::UASView(Vehicle* vehicle, QWidget *parent) : QWidget(parent), - uas(uas), + vehicle(vehicle), + uas(vehicle->uas()), startTime(0), timeout(false), iconIsRed(true), @@ -104,10 +105,11 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int,int))); connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16))); connect(uas, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint))); - connect(UASManager::instance(), SIGNAL(activeUASStatusChanged(UASInterface*,bool)), this, SLOT(updateActiveUAS(UASInterface*,bool))); connect(uas, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(showStatusText(int, int, int, QString))); connect(uas, SIGNAL(navModeChanged(int, int, QString)), this, SLOT(updateNavMode(int, int, QString))); + connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &UASView::_activeVehicleChanged); + // Setup user interaction connect(m_ui->liftoffButton, SIGNAL(clicked()), uas, SLOT(launch())); connect(m_ui->haltButton, SIGNAL(clicked()), uas, SLOT(halt())); @@ -118,9 +120,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : connect(m_ui->shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown())); // Allow deleting this widget - connect(removeAction, SIGNAL(triggered()), this, SLOT(triggerUASDeletion())); connect(renameAction, SIGNAL(triggered()), this, SLOT(rename())); - connect(selectAction, SIGNAL(triggered()), uas, SLOT(setSelected())); + connect(selectAction, &QAction::triggered, this, &UASView::_activateVehicle); connect(selectAirframeAction, SIGNAL(triggered()), this, SLOT(selectAirframe())); connect(setBatterySpecsAction, SIGNAL(triggered()), this, SLOT(setBatterySpecs())); @@ -155,7 +156,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : heartbeatColor = uas->getColor(); QString colorstyle("QLabel { background-color: %1; }"); m_ui->typeLabel->setStyleSheet(colorstyle.arg(heartbeatColor.name())); - updateActiveUAS(uas, false); + _activeVehicleChanged(vehicle); // Hide kill and shutdown buttons by default m_ui->killButton->hide(); @@ -172,6 +173,11 @@ UASView::~UASView() delete m_ui; } +void UASView::_activateVehicle(void) +{ + MultiVehicleManager::instance()->setActiveVehicle(vehicle); +} + void UASView::heartbeatTimeout(bool timeout, unsigned int ms) { Q_UNUSED(ms); @@ -198,23 +204,26 @@ void UASView::setUASasActive(bool active) { if (active) { - UASManager::instance()->setActiveUAS(this->uas); + MultiVehicleManager::instance()->setActiveVehicle(vehicle); } } -void UASView::updateActiveUAS(UASInterface* uas, bool active) +void UASView::_activeVehicleChanged(Vehicle* vehicle) { - if (uas == this->uas) + bool active = false; + + if (vehicle) { + active = vehicle->uas() == this->uas; + } + + this->isActive = active; + if (isActive) { - this->isActive = active; - if (active) - { - setStyleSheet("UASView { border-width: 3px}"); - } - else - { - setStyleSheet(QString("UASView { border-color: %1}").arg(heartbeatColor.name())); - } + setStyleSheet("UASView { border-width: 3px}"); + } + else + { + setStyleSheet(QString("UASView { border-color: %1}").arg(heartbeatColor.name())); } } @@ -231,7 +240,7 @@ void UASView::updateMode(int sysId, QString status, QString description) void UASView::mouseDoubleClickEvent (QMouseEvent * event) { Q_UNUSED(event); - UASManager::instance()->setActiveUAS(uas); + MultiVehicleManager::instance()->setActiveVehicle(vehicle); } void UASView::receiveHeartbeat(UASInterface* uas) @@ -243,7 +252,7 @@ void UASView::receiveHeartbeat(UASInterface* uas) // If we're returning from a disconnection, recolor things properly. if (disconnected) { - updateActiveUAS(this->uas, this->isActive); + _activeVehicleChanged(vehicle); disconnected = false; } timeout = false; @@ -520,12 +529,6 @@ void UASView::selectAirframe() } } -void UASView::triggerUASDeletion() -{ - refreshTimer->stop(); - UASManager::instance()->removeUAS(uas); -} - void UASView::refresh() { if (generalUpdateCount == 4) diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h index bf9170a88d01236915208cef7608838905f11238..f46f03154873c865ff2ccff0f2541041c4e2b944 100644 --- a/src/ui/uas/UASView.h +++ b/src/ui/uas/UASView.h @@ -36,7 +36,9 @@ This file is part of the QGROUNDCONTROL project #include #include #include -#include + +#include "Vehicle.h" +#include "UASInterface.h" namespace Ui { @@ -47,8 +49,9 @@ class UASView : public QWidget { Q_OBJECT public: - UASView(UASInterface* uas, QWidget *parent = 0); + UASView(Vehicle* vehicle, QWidget *parent = 0); ~UASView(); + Vehicle* vehicle; UASInterface* uas; public slots: @@ -65,11 +68,6 @@ public slots: void updateMode(int sysId, QString status, QString description); void updateLoad(UASInterface* uas, double load); //void receiveValue(int uasid, QString id, double value, quint64 time); - /** - * Request that the UASManager deletes this UAS. This doesn't delete this widget - * yet, it waits for the approprait uasDeleted signal. - */ - void triggerUASDeletion(); void refresh(); /** @brief Receive new waypoint information */ void setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current); @@ -81,8 +79,6 @@ public slots: void setSystemType(UASInterface* uas, unsigned int systemType); /** @brief Set the current UAS as the globally active system */ void setUASasActive(bool); - /** @brief Update the view if an UAS has been set to active */ - void updateActiveUAS(UASInterface* uas, bool active); /** @brief Set the widget into critical mode */ void heartbeatTimeout(bool timeout, unsigned int ms); /** @brief Bring up the dialog to rename the system */ @@ -143,6 +139,10 @@ protected: * with the UAS. */ void contextMenuEvent(QContextMenuEvent *event); + +private slots: + void _activeVehicleChanged(Vehicle* vehicle); + void _activateVehicle(void); private: Ui::UASView *m_ui;