From fb43c44204bbf2cadf763318b5ca090b3d3de433 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 29 Oct 2015 20:06:13 -0700 Subject: [PATCH] More works towards removing UAS completely --- .../APM/APMAirframeComponent.cc | 4 +- .../APM/APMAirframeComponent.h | 2 +- .../APM/APMAutoPilotPlugin.cc | 2 +- src/AutoPilotPlugins/APM/APMComponent.cc | 6 +- src/AutoPilotPlugins/APM/APMComponent.h | 2 +- src/AutoPilotPlugins/AutoPilotPlugin.cc | 2 +- src/AutoPilotPlugins/PX4/AirframeComponent.cc | 4 +- src/AutoPilotPlugins/PX4/AirframeComponent.h | 2 +- .../PX4/FlightModesComponent.cc | 4 +- .../PX4/FlightModesComponent.h | 2 +- .../PX4/FlightModesComponentController.cc | 2 +- .../PX4/PX4AutoPilotPlugin.cc | 12 +- src/AutoPilotPlugins/PX4/PX4Component.cc | 6 +- src/AutoPilotPlugins/PX4/PX4Component.h | 2 +- src/AutoPilotPlugins/PX4/PowerComponent.cc | 4 +- src/AutoPilotPlugins/PX4/PowerComponent.h | 2 +- src/AutoPilotPlugins/PX4/RadioComponent.cc | 4 +- src/AutoPilotPlugins/PX4/RadioComponent.h | 2 +- src/AutoPilotPlugins/PX4/SafetyComponent.cc | 4 +- src/AutoPilotPlugins/PX4/SafetyComponent.h | 2 +- src/AutoPilotPlugins/PX4/SensorsComponent.cc | 35 ++-- src/AutoPilotPlugins/PX4/SensorsComponent.h | 2 +- .../PX4/SensorsComponentController.cc | 15 +- src/FactSystem/ParameterLoader.cc | 4 +- src/FactSystem/ParameterLoader.h | 2 +- src/Vehicle/Vehicle.cc | 126 ------------- src/Vehicle/Vehicle.h | 111 +++++------- src/VehicleSetup/VehicleComponent.cc | 6 +- src/VehicleSetup/VehicleComponent.h | 8 +- src/comm/QGCFlightGearLink.cc | 52 +++--- src/comm/QGCFlightGearLink.h | 36 ++-- src/comm/QGCJSBSimLink.cc | 32 ++-- src/comm/QGCJSBSimLink.h | 12 +- src/comm/QGCXPlaneLink.cc | 82 +++------ src/comm/QGCXPlaneLink.h | 6 +- src/uas/UAS.cc | 166 +----------------- src/uas/UAS.h | 44 ----- src/uas/UASInterface.h | 46 ----- src/ui/QGCHilConfiguration.cc | 5 +- src/ui/QGCHilFlightGearConfiguration.cc | 21 ++- src/ui/QGCHilFlightGearConfiguration.h | 6 +- src/ui/QGCHilJSBSimConfiguration.cc | 17 +- src/ui/QGCHilJSBSimConfiguration.h | 9 +- src/ui/QGCMAVLinkInspector.cc | 11 +- 44 files changed, 246 insertions(+), 678 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMAirframeComponent.cc b/src/AutoPilotPlugins/APM/APMAirframeComponent.cc index 7519398a7..468baf03c 100644 --- a/src/AutoPilotPlugins/APM/APMAirframeComponent.cc +++ b/src/AutoPilotPlugins/APM/APMAirframeComponent.cc @@ -27,8 +27,8 @@ #include "APMAirframeComponent.h" #include "QGCQmlWidgetHolder.h" -APMAirframeComponent::APMAirframeComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) : - APMComponent(uas, autopilot, parent), +APMAirframeComponent::APMAirframeComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : + APMComponent(vehicle, autopilot, parent), _name(tr("Airframe")) { diff --git a/src/AutoPilotPlugins/APM/APMAirframeComponent.h b/src/AutoPilotPlugins/APM/APMAirframeComponent.h index 4a763c66e..4231af7cc 100644 --- a/src/AutoPilotPlugins/APM/APMAirframeComponent.h +++ b/src/AutoPilotPlugins/APM/APMAirframeComponent.h @@ -31,7 +31,7 @@ class APMAirframeComponent : public APMComponent Q_OBJECT public: - APMAirframeComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent = NULL); + APMAirframeComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); // Virtuals from APMComponent virtual QStringList setupCompleteChangedTriggerList(void) const; diff --git a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc index ad623ca5b..d79966a96 100644 --- a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc @@ -48,7 +48,7 @@ const QVariantList& APMAutoPilotPlugin::vehicleComponents(void) Q_ASSERT(_vehicle); if (parametersReady()) { - _airframeComponent = new APMAirframeComponent(_vehicle->uas(), this); + _airframeComponent = new APMAirframeComponent(_vehicle, this); Q_CHECK_PTR(_airframeComponent); _airframeComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent)); diff --git a/src/AutoPilotPlugins/APM/APMComponent.cc b/src/AutoPilotPlugins/APM/APMComponent.cc index 2a0b652fe..ec3cbf9b6 100644 --- a/src/AutoPilotPlugins/APM/APMComponent.cc +++ b/src/AutoPilotPlugins/APM/APMComponent.cc @@ -28,10 +28,10 @@ #include "Fact.h" #include "AutoPilotPlugin.h" -APMComponent::APMComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) : - VehicleComponent(uas, autopilot, parent) +APMComponent::APMComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : + VehicleComponent(vehicle, autopilot, parent) { - Q_ASSERT(uas); + Q_ASSERT(vehicle); Q_ASSERT(autopilot); } diff --git a/src/AutoPilotPlugins/APM/APMComponent.h b/src/AutoPilotPlugins/APM/APMComponent.h index 88097c18a..558c8d99f 100644 --- a/src/AutoPilotPlugins/APM/APMComponent.h +++ b/src/AutoPilotPlugins/APM/APMComponent.h @@ -37,7 +37,7 @@ class APMComponent : public VehicleComponent Q_OBJECT public: - APMComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent = NULL); + APMComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); /// @brief Returns an list of parameter names for which a change should cause the setupCompleteChanged /// signal to be emitted. Last element is signalled by NULL. diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.cc b/src/AutoPilotPlugins/AutoPilotPlugin.cc index 08d7a5d27..1878ca9bd 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc @@ -170,7 +170,7 @@ const QMap >& AutoPilotPlugin::getGroupMap(void) void AutoPilotPlugin::writeParametersToStream(QTextStream &stream) { - _vehicle->getParameterLoader()->writeParametersToStream(stream, _vehicle->uas()->getUASName()); + _vehicle->getParameterLoader()->writeParametersToStream(stream); } QString AutoPilotPlugin::readParametersFromStream(QTextStream &stream) diff --git a/src/AutoPilotPlugins/PX4/AirframeComponent.cc b/src/AutoPilotPlugins/PX4/AirframeComponent.cc index f24e4fce1..e99236ee2 100644 --- a/src/AutoPilotPlugins/PX4/AirframeComponent.cc +++ b/src/AutoPilotPlugins/PX4/AirframeComponent.cc @@ -68,8 +68,8 @@ static const struct mavType mavTypeInfo[] = { static size_t cMavTypes = sizeof(mavTypeInfo) / sizeof(mavTypeInfo[0]); #endif -AirframeComponent::AirframeComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) : - PX4Component(uas, autopilot, parent), +AirframeComponent::AirframeComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : + PX4Component(vehicle, autopilot, parent), _name(tr("Airframe")) { #if 0 diff --git a/src/AutoPilotPlugins/PX4/AirframeComponent.h b/src/AutoPilotPlugins/PX4/AirframeComponent.h index 8c02eb4cb..b43d5907b 100644 --- a/src/AutoPilotPlugins/PX4/AirframeComponent.h +++ b/src/AutoPilotPlugins/PX4/AirframeComponent.h @@ -35,7 +35,7 @@ class AirframeComponent : public PX4Component Q_OBJECT public: - AirframeComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent = NULL); + AirframeComponent(Vehicle* vehicles, AutoPilotPlugin* autopilot, QObject* parent = NULL); // Virtuals from PX4Component virtual QStringList setupCompleteChangedTriggerList(void) const; diff --git a/src/AutoPilotPlugins/PX4/FlightModesComponent.cc b/src/AutoPilotPlugins/PX4/FlightModesComponent.cc index 6eac6335e..cef48ab81 100644 --- a/src/AutoPilotPlugins/PX4/FlightModesComponent.cc +++ b/src/AutoPilotPlugins/PX4/FlightModesComponent.cc @@ -43,8 +43,8 @@ static const SwitchListItem switchList[] = { }; static const size_t cSwitchList = sizeof(switchList) / sizeof(switchList[0]); -FlightModesComponent::FlightModesComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) : - PX4Component(uas, autopilot, parent), +FlightModesComponent::FlightModesComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : + PX4Component(vehicle, autopilot, parent), _name(tr("Flight Modes")) { } diff --git a/src/AutoPilotPlugins/PX4/FlightModesComponent.h b/src/AutoPilotPlugins/PX4/FlightModesComponent.h index 19e772f82..e5a3adcf4 100644 --- a/src/AutoPilotPlugins/PX4/FlightModesComponent.h +++ b/src/AutoPilotPlugins/PX4/FlightModesComponent.h @@ -35,7 +35,7 @@ class FlightModesComponent : public PX4Component Q_OBJECT public: - FlightModesComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent = NULL); + FlightModesComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); // Virtuals from PX4Component virtual QStringList setupCompleteChangedTriggerList(void) const; diff --git a/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc b/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc index 5fa55e4e4..1c55ff127 100644 --- a/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc +++ b/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc @@ -66,7 +66,7 @@ FlightModesComponentController::~FlightModesComponentController() void FlightModesComponentController::_init(void) { // FIXME: What about VTOL? That confuses the whole Flight Mode naming scheme - _fixedWing = _uas->getSystemType() == MAV_TYPE_FIXED_WING; + _fixedWing = _vehicle->vehicleType() == MAV_TYPE_FIXED_WING; // We need to know min and max for channel in order to calculate percentage range for (int channel=0; channel<_chanMax; channel++) { diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc index c4ef8a386..014350998 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc @@ -94,32 +94,32 @@ const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void) Q_ASSERT(_vehicle); if (parametersReady()) { - _airframeComponent = new AirframeComponent(_vehicle->uas(), this); + _airframeComponent = new AirframeComponent(_vehicle, this); Q_CHECK_PTR(_airframeComponent); _airframeComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent)); - _radioComponent = new RadioComponent(_vehicle->uas(), this); + _radioComponent = new RadioComponent(_vehicle, this); Q_CHECK_PTR(_radioComponent); _radioComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_radioComponent)); - _flightModesComponent = new FlightModesComponent(_vehicle->uas(), this); + _flightModesComponent = new FlightModesComponent(_vehicle, this); Q_CHECK_PTR(_flightModesComponent); _flightModesComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_flightModesComponent)); - _sensorsComponent = new SensorsComponent(_vehicle->uas(), this); + _sensorsComponent = new SensorsComponent(_vehicle, this); Q_CHECK_PTR(_sensorsComponent); _sensorsComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_sensorsComponent)); - _powerComponent = new PowerComponent(_vehicle->uas(), this); + _powerComponent = new PowerComponent(_vehicle, this); Q_CHECK_PTR(_powerComponent); _powerComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_powerComponent)); - _safetyComponent = new SafetyComponent(_vehicle->uas(), this); + _safetyComponent = new SafetyComponent(_vehicle, this); Q_CHECK_PTR(_safetyComponent); _safetyComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_safetyComponent)); diff --git a/src/AutoPilotPlugins/PX4/PX4Component.cc b/src/AutoPilotPlugins/PX4/PX4Component.cc index 85fd90983..567869bf9 100644 --- a/src/AutoPilotPlugins/PX4/PX4Component.cc +++ b/src/AutoPilotPlugins/PX4/PX4Component.cc @@ -28,10 +28,10 @@ #include "Fact.h" #include "AutoPilotPlugin.h" -PX4Component::PX4Component(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) : - VehicleComponent(uas, autopilot, parent) +PX4Component::PX4Component(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : + VehicleComponent(vehicle, autopilot, parent) { - Q_ASSERT(uas); + Q_ASSERT(vehicle); Q_ASSERT(autopilot); } diff --git a/src/AutoPilotPlugins/PX4/PX4Component.h b/src/AutoPilotPlugins/PX4/PX4Component.h index 74455ceda..0283506e8 100644 --- a/src/AutoPilotPlugins/PX4/PX4Component.h +++ b/src/AutoPilotPlugins/PX4/PX4Component.h @@ -37,7 +37,7 @@ class PX4Component : public VehicleComponent Q_OBJECT public: - PX4Component(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent = NULL); + PX4Component(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); /// @brief Returns an list of parameter names for which a change should cause the setupCompleteChanged /// signal to be emitted. Last element is signalled by NULL. diff --git a/src/AutoPilotPlugins/PX4/PowerComponent.cc b/src/AutoPilotPlugins/PX4/PowerComponent.cc index c0883eba8..16be86cbf 100644 --- a/src/AutoPilotPlugins/PX4/PowerComponent.cc +++ b/src/AutoPilotPlugins/PX4/PowerComponent.cc @@ -28,8 +28,8 @@ #include "QGCQmlWidgetHolder.h" #include "PX4AutoPilotPlugin.h" -PowerComponent::PowerComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) : - PX4Component(uas, autopilot, parent), +PowerComponent::PowerComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : + PX4Component(vehicle, autopilot, parent), _name(tr("Power")) { } diff --git a/src/AutoPilotPlugins/PX4/PowerComponent.h b/src/AutoPilotPlugins/PX4/PowerComponent.h index ca0841c8d..7d2105524 100644 --- a/src/AutoPilotPlugins/PX4/PowerComponent.h +++ b/src/AutoPilotPlugins/PX4/PowerComponent.h @@ -35,7 +35,7 @@ class PowerComponent : public PX4Component Q_OBJECT public: - PowerComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent = NULL); + PowerComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); // Virtuals from PX4Component virtual QStringList setupCompleteChangedTriggerList(void) const; diff --git a/src/AutoPilotPlugins/PX4/RadioComponent.cc b/src/AutoPilotPlugins/PX4/RadioComponent.cc index 02e9f8320..0f95fb1a4 100644 --- a/src/AutoPilotPlugins/PX4/RadioComponent.cc +++ b/src/AutoPilotPlugins/PX4/RadioComponent.cc @@ -28,8 +28,8 @@ #include "QGCQmlWidgetHolder.h" #include "PX4AutoPilotPlugin.h" -RadioComponent::RadioComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) : - PX4Component(uas, autopilot, parent), +RadioComponent::RadioComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : + PX4Component(vehicle, autopilot, parent), _name(tr("Radio")) { } diff --git a/src/AutoPilotPlugins/PX4/RadioComponent.h b/src/AutoPilotPlugins/PX4/RadioComponent.h index 7e4bf5776..15f718140 100644 --- a/src/AutoPilotPlugins/PX4/RadioComponent.h +++ b/src/AutoPilotPlugins/PX4/RadioComponent.h @@ -36,7 +36,7 @@ class RadioComponent : public PX4Component Q_OBJECT public: - RadioComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent = NULL); + RadioComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); // Virtuals from PX4Component virtual QStringList setupCompleteChangedTriggerList(void) const; diff --git a/src/AutoPilotPlugins/PX4/SafetyComponent.cc b/src/AutoPilotPlugins/PX4/SafetyComponent.cc index e7b5da65e..27dfb9fd8 100644 --- a/src/AutoPilotPlugins/PX4/SafetyComponent.cc +++ b/src/AutoPilotPlugins/PX4/SafetyComponent.cc @@ -28,8 +28,8 @@ #include "QGCQmlWidgetHolder.h" #include "PX4AutoPilotPlugin.h" -SafetyComponent::SafetyComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) : - PX4Component(uas, autopilot, parent), +SafetyComponent::SafetyComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : + PX4Component(vehicle, autopilot, parent), _name(tr("Safety")) { } diff --git a/src/AutoPilotPlugins/PX4/SafetyComponent.h b/src/AutoPilotPlugins/PX4/SafetyComponent.h index cdcc68bfa..7b11f59b8 100644 --- a/src/AutoPilotPlugins/PX4/SafetyComponent.h +++ b/src/AutoPilotPlugins/PX4/SafetyComponent.h @@ -36,7 +36,7 @@ class SafetyComponent : public PX4Component Q_OBJECT public: - SafetyComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent = NULL); + SafetyComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); // Virtuals from PX4Component virtual QStringList setupCompleteChangedTriggerList(void) const; diff --git a/src/AutoPilotPlugins/PX4/SensorsComponent.cc b/src/AutoPilotPlugins/PX4/SensorsComponent.cc index c6311fcf5..5fb1d00de 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponent.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponent.cc @@ -31,8 +31,8 @@ // These two list must be kept in sync -SensorsComponent::SensorsComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) : - PX4Component(uas, autopilot, parent), +SensorsComponent::SensorsComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : + PX4Component(vehicle, autopilot, parent), _name(tr("Sensors")) { @@ -87,11 +87,15 @@ QStringList SensorsComponent::setupCompleteChangedTriggerList(void) const QStringList triggers; triggers << "CAL_MAG0_ID" << "CAL_GYRO0_ID" << "CAL_ACC0_ID"; - if (_uas->getSystemType() == MAV_TYPE_FIXED_WING || - _uas->getSystemType() == MAV_TYPE_VTOL_DUOROTOR || - _uas->getSystemType() == MAV_TYPE_VTOL_QUADROTOR || - _uas->getSystemType() == MAV_TYPE_VTOL_TILTROTOR) { - triggers << "SENS_DPRES_OFF"; + switch (_vehicle->vehicleType()) { + case MAV_TYPE_FIXED_WING: + case MAV_TYPE_VTOL_DUOROTOR: + case MAV_TYPE_VTOL_QUADROTOR: + case MAV_TYPE_VTOL_TILTROTOR: + triggers << "SENS_DPRES_OFF"; + break; + default: + break; } return triggers; @@ -115,13 +119,16 @@ QUrl SensorsComponent::summaryQmlSource(void) const { QString summaryQml; - if (_uas->getSystemType() == MAV_TYPE_FIXED_WING || - _uas->getSystemType() == MAV_TYPE_VTOL_DUOROTOR || - _uas->getSystemType() == MAV_TYPE_VTOL_QUADROTOR || - _uas->getSystemType() == MAV_TYPE_VTOL_TILTROTOR) { - summaryQml = "qrc:/qml/SensorsComponentSummaryFixedWing.qml"; - } else { - summaryQml = "qrc:/qml/SensorsComponentSummary.qml"; + switch (_vehicle->vehicleType()) { + case MAV_TYPE_FIXED_WING: + case MAV_TYPE_VTOL_DUOROTOR: + case MAV_TYPE_VTOL_QUADROTOR: + case MAV_TYPE_VTOL_TILTROTOR: + summaryQml = "qrc:/qml/SensorsComponentSummaryFixedWing.qml"; + break; + default: + summaryQml = "qrc:/qml/SensorsComponentSummary.qml"; + break; } return QUrl::fromUserInput(summaryQml); diff --git a/src/AutoPilotPlugins/PX4/SensorsComponent.h b/src/AutoPilotPlugins/PX4/SensorsComponent.h index 5ce0e00fc..aa79968d3 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponent.h +++ b/src/AutoPilotPlugins/PX4/SensorsComponent.h @@ -35,7 +35,7 @@ class SensorsComponent : public PX4Component Q_OBJECT public: - SensorsComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent = NULL); + SensorsComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); // Virtuals from PX4Component virtual QStringList setupCompleteChangedTriggerList(void) const; diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc index 583dc6963..9988d9898 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc @@ -445,12 +445,15 @@ void SensorsComponentController::_refreshParams(void) bool SensorsComponentController::fixedWing(void) { - UASInterface* uas = _autopilot->vehicle()->uas(); - Q_ASSERT(uas); - return uas->getSystemType() == MAV_TYPE_FIXED_WING || - uas->getSystemType() == MAV_TYPE_VTOL_DUOROTOR || - uas->getSystemType() == MAV_TYPE_VTOL_QUADROTOR || - uas->getSystemType() == MAV_TYPE_VTOL_TILTROTOR; + switch (_vehicle->vehicleType()) { + case MAV_TYPE_FIXED_WING: + case MAV_TYPE_VTOL_DUOROTOR: + case MAV_TYPE_VTOL_QUADROTOR: + case MAV_TYPE_VTOL_TILTROTOR: + return true; + default: + return false; + } } void SensorsComponentController::_updateAndEmitShowOrientationCalArea(bool show) diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index 81b07f758..57a80deff 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -708,9 +708,9 @@ QString ParameterLoader::readParametersFromStream(QTextStream& stream) return errors; } -void ParameterLoader::writeParametersToStream(QTextStream &stream, const QString& name) +void ParameterLoader::writeParametersToStream(QTextStream &stream) { - stream << "# Onboard parameters for system " << name << "\n"; + stream << "# Onboard parameters for vehicle " << _vehicle->id() << "\n"; stream << "#\n"; stream << "# MAV ID COMPONENT ID PARAM NAME VALUE (FLOAT)\n"; diff --git a/src/FactSystem/ParameterLoader.h b/src/FactSystem/ParameterLoader.h index 483111daa..5a82e81fd 100644 --- a/src/FactSystem/ParameterLoader.h +++ b/src/FactSystem/ParameterLoader.h @@ -85,7 +85,7 @@ public: /// Returns error messages from loading QString readParametersFromStream(QTextStream& stream); - void writeParametersToStream(QTextStream &stream, const QString& name); + void writeParametersToStream(QTextStream &stream); signals: /// Signalled when the full set of facts are ready diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index f83189622..90849bcc2 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -90,8 +90,6 @@ Vehicle::Vehicle(LinkInterface* link, , _batteryPercent(0.0) , _batteryConsumed(-1.0) , _currentHeartbeatTimeout(0) - , _waypointDistance(0.0) - , _currentWaypoint(0) , _satelliteCount(-1) , _satelliteLock(0) , _updateCount(0) @@ -154,15 +152,12 @@ Vehicle::Vehicle(LinkInterface* link, connect(_mav, &UASInterface::heartbeatTimeout, this, &Vehicle::_heartbeatTimeout); connect(_mav, &UASInterface::batteryChanged, this, &Vehicle::_updateBatteryRemaining); connect(_mav, &UASInterface::batteryConsumedChanged, this, &Vehicle::_updateBatteryConsumedChanged); - connect(_mav, &UASInterface::nameChanged, this, &Vehicle::_updateName); - connect(_mav, &UASInterface::systemTypeSet, this, &Vehicle::_setSystemType); connect(_mav, &UASInterface::localizationChanged, this, &Vehicle::_setSatLoc); UAS* pUas = dynamic_cast(_mav); if(pUas) { _setSatelliteCount(pUas->getSatelliteCount(), QString("")); connect(pUas, &UAS::satelliteCountChanged, this, &Vehicle::_setSatelliteCount); } - _setSystemType(_mav, _mav->getSystemType()); _loadSettings(); @@ -188,8 +183,6 @@ Vehicle::Vehicle(LinkInterface* link, Vehicle::~Vehicle() { - qDebug() << "~Vehicle"; - delete _missionManager; _missionManager = NULL; @@ -519,12 +512,6 @@ void Vehicle::_updateNavigationControllerData(UASInterface *uas, float, float, f * Internal */ -bool Vehicle::_isAirplane() { - if (_mav) - return _mav->isAirplane(); - return false; -} - void Vehicle::_addChange(int id) { if(!_changes.contains(id)) { @@ -639,86 +626,6 @@ void Vehicle::_updateState(UASInterface*, QString name, QString) } } -void Vehicle::_updateName(const QString& name) -{ - if (_systemName != name) { - _systemName = name; - emit systemNameChanged(); - } -} - -/** - * The current system type is represented through the system icon. - * - * @param uas Source system, has to be the same as this->uas - * @param systemType type ID, following the MAVLink system type conventions - * @see http://pixhawk.ethz.ch/software/mavlink - */ -void Vehicle::_setSystemType(UASInterface*, unsigned int systemType) -{ - _systemPixmap = "qrc:/res/mavs/"; - switch (systemType) { - case MAV_TYPE_GENERIC: - _systemPixmap += "Generic"; - break; - case MAV_TYPE_FIXED_WING: - _systemPixmap += "FixedWing"; - break; - case MAV_TYPE_QUADROTOR: - _systemPixmap += "QuadRotor"; - break; - case MAV_TYPE_COAXIAL: - _systemPixmap += "Coaxial"; - break; - case MAV_TYPE_HELICOPTER: - _systemPixmap += "Helicopter"; - break; - case MAV_TYPE_ANTENNA_TRACKER: - _systemPixmap += "AntennaTracker"; - break; - case MAV_TYPE_GCS: - _systemPixmap += "Groundstation"; - break; - case MAV_TYPE_AIRSHIP: - _systemPixmap += "Airship"; - break; - case MAV_TYPE_FREE_BALLOON: - _systemPixmap += "FreeBalloon"; - break; - case MAV_TYPE_ROCKET: - _systemPixmap += "Rocket"; - break; - case MAV_TYPE_GROUND_ROVER: - _systemPixmap += "GroundRover"; - break; - case MAV_TYPE_SURFACE_BOAT: - _systemPixmap += "SurfaceBoat"; - break; - case MAV_TYPE_SUBMARINE: - _systemPixmap += "Submarine"; - break; - case MAV_TYPE_HEXAROTOR: - _systemPixmap += "HexaRotor"; - break; - case MAV_TYPE_OCTOROTOR: - _systemPixmap += "OctoRotor"; - break; - case MAV_TYPE_TRICOPTER: - _systemPixmap += "TriCopter"; - break; - case MAV_TYPE_FLAPPING_WING: - _systemPixmap += "FlappingWing"; - break; - case MAV_TYPE_KITE: - _systemPixmap += "Kite"; - break; - default: - _systemPixmap += "Unknown"; - break; - } - emit systemPixmapChanged(); -} - void Vehicle::_heartbeatTimeout(bool timeout, unsigned int ms) { unsigned int elapsed = ms; @@ -756,39 +663,6 @@ void Vehicle::_setSatLoc(UASInterface*, int fix) } } -void Vehicle::_updateWaypointDistance(double distance) -{ - if (_waypointDistance != distance) { - _waypointDistance = distance; - emit waypointDistanceChanged(); - } -} - -void Vehicle::_updateCurrentWaypoint(quint16 id) -{ - if (_currentWaypoint != id) { - _currentWaypoint = id; - emit currentWaypointChanged(); - } -} - -void Vehicle::_updateWaypointViewOnly(int, MissionItem* /*wp*/) -{ - /* - bool changed = false; - for(int i = 0; i < _waypoints.count(); i++) { - if(_waypoints[i].sequenceNumber() == wp->sequenceNumber()) { - _waypoints[i] = *wp; - changed = true; - break; - } - } - if(changed) { - emit waypointListChanged(); - } - */ -} - void Vehicle::_handleTextMessage(int newCount) { // Reset? diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 0a0e83306..34f0b4b84 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -63,9 +63,8 @@ public: JoystickManager* joystickManager); ~Vehicle(); - Q_PROPERTY(int id READ id CONSTANT) - Q_PROPERTY(AutoPilotPlugin* autopilot MEMBER _autopilotPlugin CONSTANT) - + Q_PROPERTY(int id READ id CONSTANT) + Q_PROPERTY(AutoPilotPlugin* autopilot MEMBER _autopilotPlugin CONSTANT) Q_PROPERTY(QGeoCoordinate coordinate READ coordinate NOTIFY coordinateChanged) Q_PROPERTY(bool coordinateValid READ coordinateValid NOTIFY coordinateValidChanged) Q_PROPERTY(MissionManager* missionManager MEMBER _missionManager CONSTANT) @@ -78,54 +77,51 @@ public: Q_PROPERTY(bool hilMode READ hilMode WRITE setHilMode NOTIFY hilModeChanged) Q_PROPERTY(bool missingParameters READ missingParameters NOTIFY missingParametersChanged) Q_PROPERTY(QmlObjectListModel* trajectoryPoints READ trajectoryPoints CONSTANT) + Q_PROPERTY(float roll READ roll NOTIFY rollChanged) + Q_PROPERTY(float pitch READ pitch NOTIFY pitchChanged) + Q_PROPERTY(float heading READ heading NOTIFY headingChanged) + Q_PROPERTY(float groundSpeed READ groundSpeed NOTIFY groundSpeedChanged) + Q_PROPERTY(float airSpeed READ airSpeed NOTIFY airSpeedChanged) + Q_PROPERTY(float climbRate READ climbRate NOTIFY climbRateChanged) + Q_PROPERTY(float altitudeRelative READ altitudeRelative NOTIFY altitudeRelativeChanged) + Q_PROPERTY(float altitudeWGS84 READ altitudeWGS84 NOTIFY altitudeWGS84Changed) + Q_PROPERTY(float altitudeAMSL READ altitudeAMSL NOTIFY altitudeAMSLChanged) + Q_PROPERTY(float latitude READ latitude NOTIFY coordinateChanged) + Q_PROPERTY(float longitude READ longitude NOTIFY coordinateChanged) + Q_PROPERTY(double batteryVoltage READ batteryVoltage NOTIFY batteryVoltageChanged) + Q_PROPERTY(double batteryPercent READ batteryPercent NOTIFY batteryPercentChanged) + Q_PROPERTY(double batteryConsumed READ batteryConsumed NOTIFY batteryConsumedChanged) + Q_PROPERTY(int satelliteCount READ satelliteCount NOTIFY satelliteCountChanged) + Q_PROPERTY(QString currentState READ currentState NOTIFY currentStateChanged) + Q_PROPERTY(int satelliteLock READ satelliteLock NOTIFY satelliteLockChanged) + Q_PROPERTY(unsigned int heartbeatTimeout READ heartbeatTimeout NOTIFY heartbeatTimeoutChanged) + Q_PROPERTY(QmlObjectListModel* missionItems READ missionItemsModel CONSTANT) + Q_PROPERTY(bool messageTypeNone READ messageTypeNone NOTIFY messageTypeChanged) + Q_PROPERTY(bool messageTypeNormal READ messageTypeNormal NOTIFY messageTypeChanged) + Q_PROPERTY(bool messageTypeWarning READ messageTypeWarning NOTIFY messageTypeChanged) + Q_PROPERTY(bool messageTypeError READ messageTypeError NOTIFY messageTypeChanged) + Q_PROPERTY(int newMessageCount READ newMessageCount NOTIFY newMessageCountChanged) + Q_PROPERTY(int messageCount READ messageCount NOTIFY messageCountChanged) + Q_PROPERTY(QString latestError READ latestError NOTIFY latestErrorChanged) + Q_PROPERTY(int joystickMode READ joystickMode WRITE setJoystickMode NOTIFY joystickModeChanged) + Q_PROPERTY(QStringList joystickModes READ joystickModes CONSTANT) + Q_PROPERTY(bool joystickEnabled READ joystickEnabled WRITE setJoystickEnabled NOTIFY joystickEnabledChanged) + Q_PROPERTY(bool active READ active WRITE setActive NOTIFY activeChanged) + + /// Returns the number of buttons which are reserved for firmware use in the MANUAL_CONTROL mavlink + /// message. For example PX4 Flight Stack reserves the first 8 buttons to simulate rc switches. + /// The remainder can be assigned to Vehicle actions. + /// @return -1: reserver all buttons, >0 number of buttons to reserve + Q_PROPERTY(int manualControlReservedButtonCount READ manualControlReservedButtonCount CONSTANT) + + Q_INVOKABLE QString getMavIconColor(); // Property accessors QGeoCoordinate coordinate(void) { return _coordinate; } bool coordinateValid(void) { return _coordinateValid; } - - Q_INVOKABLE QString getMavIconColor(); - - //-- System Messages - Q_PROPERTY(bool messageTypeNone READ messageTypeNone NOTIFY messageTypeChanged) - Q_PROPERTY(bool messageTypeNormal READ messageTypeNormal NOTIFY messageTypeChanged) - Q_PROPERTY(bool messageTypeWarning READ messageTypeWarning NOTIFY messageTypeChanged) - Q_PROPERTY(bool messageTypeError READ messageTypeError NOTIFY messageTypeChanged) - Q_PROPERTY(int newMessageCount READ newMessageCount NOTIFY newMessageCountChanged) - Q_PROPERTY(int messageCount READ messageCount NOTIFY messageCountChanged) - Q_PROPERTY(QString latestError READ latestError NOTIFY latestErrorChanged) - //-- UAV Stats - Q_PROPERTY(float roll READ roll NOTIFY rollChanged) - Q_PROPERTY(float pitch READ pitch NOTIFY pitchChanged) - Q_PROPERTY(float heading READ heading NOTIFY headingChanged) - Q_PROPERTY(float groundSpeed READ groundSpeed NOTIFY groundSpeedChanged) - Q_PROPERTY(float airSpeed READ airSpeed NOTIFY airSpeedChanged) - Q_PROPERTY(float climbRate READ climbRate NOTIFY climbRateChanged) - Q_PROPERTY(float altitudeRelative READ altitudeRelative NOTIFY altitudeRelativeChanged) - Q_PROPERTY(float altitudeWGS84 READ altitudeWGS84 NOTIFY altitudeWGS84Changed) - Q_PROPERTY(float altitudeAMSL READ altitudeAMSL NOTIFY altitudeAMSLChanged) - Q_PROPERTY(float latitude READ latitude NOTIFY coordinateChanged) - Q_PROPERTY(float longitude READ longitude NOTIFY coordinateChanged) - Q_PROPERTY(double batteryVoltage READ batteryVoltage NOTIFY batteryVoltageChanged) - Q_PROPERTY(double batteryPercent READ batteryPercent NOTIFY batteryPercentChanged) - Q_PROPERTY(double batteryConsumed READ batteryConsumed NOTIFY batteryConsumedChanged) - Q_PROPERTY(QString systemPixmap READ systemPixmap NOTIFY systemPixmapChanged) - Q_PROPERTY(int satelliteCount READ satelliteCount NOTIFY satelliteCountChanged) - Q_PROPERTY(QString currentState READ currentState NOTIFY currentStateChanged) - Q_PROPERTY(QString systemName READ systemName NOTIFY systemNameChanged) - Q_PROPERTY(int satelliteLock READ satelliteLock NOTIFY satelliteLockChanged) - Q_PROPERTY(double waypointDistance READ waypointDistance NOTIFY waypointDistanceChanged) - Q_PROPERTY(uint16_t currentWaypoint READ currentWaypoint NOTIFY currentWaypointChanged) - Q_PROPERTY(unsigned int heartbeatTimeout READ heartbeatTimeout NOTIFY heartbeatTimeoutChanged) - - Q_PROPERTY(QmlObjectListModel* missionItems READ missionItemsModel CONSTANT) QmlObjectListModel* missionItemsModel(void); - /// Returns the number of buttons which are reserved for firmware use in the MANUAL_CONTROL mavlink - /// message. For example PX4 Flight Stack reserves the first 8 buttons to simulate rc switches. - /// The remainder can be assigned to Vehicle actions. - /// @return -1: reserver all buttons, >0 number of buttons to reserve - Q_PROPERTY(int manualControlReservedButtonCount READ manualControlReservedButtonCount CONSTANT) typedef enum { JoystickModeRC, ///< Joystick emulates an RC Transmitter @@ -136,22 +132,16 @@ public: JoystickModeMax } JoystickMode_t; - /// The joystick mode associated with this vehicle. Joystick modes are stored keyed by mavlink system id. - Q_PROPERTY(int joystickMode READ joystickMode WRITE setJoystickMode NOTIFY joystickModeChanged) int joystickMode(void); void setJoystickMode(int mode); /// List of joystick mode names - Q_PROPERTY(QStringList joystickModes READ joystickModes CONSTANT) QStringList joystickModes(void); - // Enable/Disable joystick for this vehicle - Q_PROPERTY(bool joystickEnabled READ joystickEnabled WRITE setJoystickEnabled NOTIFY joystickEnabledChanged) bool joystickEnabled(void); void setJoystickEnabled(bool enabled); // Is vehicle active with respect to current active vehicle in QGC - Q_PROPERTY(bool active READ active WRITE setActive NOTIFY activeChanged) bool active(void); void setActive(bool active); @@ -227,7 +217,6 @@ public: // Called when the message drop-down is invoked to clear current count void resetMessages(); - bool messageTypeNone () { return _currentMessageType == MessageNone; } bool messageTypeNormal () { return _currentMessageType == MessageNormal; } bool messageTypeWarning () { return _currentMessageType == MessageWarning; } @@ -251,12 +240,8 @@ public: double batteryVoltage () { return _batteryVoltage; } double batteryPercent () { return _batteryPercent; } double batteryConsumed () { return _batteryConsumed; } - QString systemPixmap () { return _systemPixmap; } QString currentState () { return _currentState; } - QString systemName () { return _systemName; } int satelliteLock () { return _satelliteLock; } - double waypointDistance () { return _waypointDistance; } - uint16_t currentWaypoint () { return _currentWaypoint; } unsigned int heartbeatTimeout () { return _currentHeartbeatTimeout; } ParameterLoader* getParameterLoader(void); @@ -302,13 +287,9 @@ signals: void batteryConsumedChanged (); void heartbeatTimeoutChanged(); void currentConfigChanged (); - void systemPixmapChanged (); void satelliteCountChanged (); void currentStateChanged (); - void systemNameChanged (); void satelliteLockChanged (); - void waypointDistanceChanged(); - void currentWaypointChanged (); private slots: void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); @@ -324,9 +305,7 @@ private slots: void _updateAttitude (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp); /** @brief Attitude from one specific component / redundant autopilot */ void _updateAttitude (UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp); - /** @brief Speed */ void _updateSpeed (UASInterface* uas, double _groundSpeed, double _airSpeed, quint64 timestamp); - /** @brief Altitude */ 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); @@ -334,14 +313,9 @@ private slots: void _updateBatteryRemaining (UASInterface*, double voltage, double, double percent, int); void _updateBatteryConsumedChanged (UASInterface*, double current_consumed); void _updateState (UASInterface* system, QString name, QString description); - void _updateName (const QString& name); - void _setSystemType (UASInterface* uas, unsigned int systemType); void _heartbeatTimeout (bool timeout, unsigned int ms); - void _updateCurrentWaypoint (quint16 id); - void _updateWaypointDistance (double distance); void _setSatelliteCount (double val, QString name); void _setSatLoc (UASInterface* uas, int fix); - void _updateWaypointViewOnly (int uas, MissionItem* wp); private: bool _containsLink(LinkInterface* link); @@ -355,7 +329,6 @@ private: void _mapTrajectoryStart(void); void _mapTrajectoryStop(void); - bool _isAirplane (); void _addChange (int id); float _oneDecimal (float value); @@ -412,11 +385,7 @@ private: double _batteryPercent; double _batteryConsumed; QString _currentState; - QString _systemName; - QString _systemPixmap; unsigned int _currentHeartbeatTimeout; - double _waypointDistance; - quint16 _currentWaypoint; int _satelliteCount; int _satelliteLock; int _updateCount; diff --git a/src/VehicleSetup/VehicleComponent.cc b/src/VehicleSetup/VehicleComponent.cc index d9cf5f0d2..16df91fe2 100644 --- a/src/VehicleSetup/VehicleComponent.cc +++ b/src/VehicleSetup/VehicleComponent.cc @@ -27,12 +27,12 @@ #include "VehicleComponent.h" #include "AutoPilotPlugin.h" -VehicleComponent::VehicleComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) : +VehicleComponent::VehicleComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : QObject(parent), - _uas(uas), + _vehicle(vehicle), _autopilot(autopilot) { - Q_ASSERT(uas); + Q_ASSERT(vehicle); Q_ASSERT(autopilot); } diff --git a/src/VehicleSetup/VehicleComponent.h b/src/VehicleSetup/VehicleComponent.h index 37c520ff4..d777e7ae9 100644 --- a/src/VehicleSetup/VehicleComponent.h +++ b/src/VehicleSetup/VehicleComponent.h @@ -31,7 +31,7 @@ #include #include -#include "UASInterface.h" +#include "Vehicle.h" class AutoPilotPlugin; @@ -53,7 +53,7 @@ class VehicleComponent : public QObject Q_PROPERTY(QString prerequisiteSetup READ prerequisiteSetup) public: - VehicleComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent = NULL); + VehicleComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); ~VehicleComponent(); virtual QString name(void) const = 0; @@ -73,8 +73,8 @@ signals: void setupCompleteChanged(bool setupComplete); protected: - UASInterface* _uas; - AutoPilotPlugin* _autopilot; + Vehicle* _vehicle; + AutoPilotPlugin* _autopilot; }; #endif diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index cf70786c3..2b3d82380 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -47,28 +47,30 @@ This file is part of the QGROUNDCONTROL project #include "QGCMessageBox.h" #include "HomePositionManager.h" #include "QGCApplication.h" +#include "Vehicle.h" +#include "UAS.h" // FlightGear _fgProcess start and connection is quite fragile. Uncomment the define below to get higher level of debug output // for tracking down problems. //#define DEBUG_FLIGHTGEAR_CONNECT -QGCFlightGearLink::QGCFlightGearLink(UASInterface* mav, QString startupArguments, QString remoteHost, QHostAddress host, quint16 port) : - flightGearVersion(3), - startupArguments(startupArguments), - _sensorHilEnabled(true), - barometerOffsetkPa(0.0f), - _udpCommSocket(NULL), - _fgProcess(NULL) +QGCFlightGearLink::QGCFlightGearLink(Vehicle* vehicle, QString startupArguments, QString remoteHost, QHostAddress host, quint16 port) + : _vehicle(vehicle) + , _udpCommSocket(NULL) + , _fgProcess(NULL) + , flightGearVersion(3) + , startupArguments(startupArguments) + , _sensorHilEnabled(true) + , barometerOffsetkPa(0.0f) { // We're doing it wrong - because the Qt folks got the API wrong: // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/ moveToThread(this); this->host = host; - this->port = port+mav->getUASID(); + this->port = port + _vehicle->id(); this->connectState = false; - this->currentPort = 49000+mav->getUASID(); - this->mav = mav; + this->currentPort = 49000 + _vehicle->id(); this->name = tr("FlightGear 3.0+ Link (port:%1)").arg(port); setRemoteHost(remoteHost); @@ -88,7 +90,7 @@ QGCFlightGearLink::~QGCFlightGearLink() /// @brief Runs the simulation thread. We do setup work here which needs to happen in the seperate thread. void QGCFlightGearLink::run() { - Q_ASSERT(mav); + Q_ASSERT(_vehicle); Q_ASSERT(!_fgProcessName.isEmpty()); // We communicate with FlightGear over a UDP _udpCommSocket @@ -100,16 +102,16 @@ void QGCFlightGearLink::run() // Connect to the various HIL signals that we use to then send information across the UDP protocol to FlightGear. - connect(mav, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), + connect(_vehicle->uas(), SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8))); connect(this, SIGNAL(hilStateChanged(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)), - mav, SLOT(sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float))); + _vehicle->uas(), SLOT(sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float))); connect(this, SIGNAL(sensorHilGpsChanged(quint64, double, double, double, int, float, float, float, float, float, float, float, int)), - mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int))); + _vehicle->uas(), SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int))); connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), - mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); + _vehicle->uas(), SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); connect(this, SIGNAL(sensorHilOpticalFlowChanged(quint64, qint16, qint16, float,float, quint8, float)), - mav, SLOT(sendHilOpticalFlow(quint64, qint16, qint16, float, float, quint8, float))); + _vehicle->uas(), SLOT(sendHilOpticalFlow(quint64, qint16, qint16, float, float, quint8, float))); // Start a new QProcess to run FlightGear in _fgProcess = new QProcess(this); @@ -430,7 +432,7 @@ void QGCFlightGearLink::readBytes() zmag_body = mag_body(2); emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed, - xmag_body, ymag_body, zmag_body, abs_pressure*1e-2f, diff_pressure*1e-2f, pressure_alt, temperature, fields_changed); //Pressure in hPa for mavlink + xmag_body, ymag_body, zmag_body, abs_pressure*1e-2f, diff_pressure*1e-2f, pressure_alt, temperature, fields_changed); //Pressure in hPa for _vehicle->uas()link // qDebug() << "sensorHilRawImuChanged " << xacc << yacc << zacc << rollspeed << pitchspeed << yawspeed << xmag << ymag << zmag << abs_pressure << diff_pressure << pressure_alt << temperature; int gps_fix_type = 3; @@ -491,12 +493,12 @@ bool QGCFlightGearLink::disconnectSimulation() { disconnect(_fgProcess, SIGNAL(error(QProcess::ProcessError)), this, SLOT(processError(QProcess::ProcessError))); - disconnect(mav, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8))); - disconnect(this, SIGNAL(hilStateChanged(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)), mav, SLOT(sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float))); - disconnect(this, SIGNAL(sensorHilGpsChanged(quint64, double, double, double, int, float, float, float, float, float, float, float, int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int))); - disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); + disconnect(_vehicle->uas(), SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8))); + disconnect(this, SIGNAL(hilStateChanged(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)), _vehicle->uas(), SLOT(sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float))); + disconnect(this, SIGNAL(sensorHilGpsChanged(quint64, double, double, double, int, float, float, float, float, float, float, float, int)), _vehicle->uas(), SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int))); + disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), _vehicle->uas(), SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); disconnect(this, SIGNAL(sensorHilOpticalFlowChanged(quint64, qint16, qint16, float,float, quint8, float)), - mav, SLOT(sendHilOpticalFlow(quint64, qint16, qint16, float, float, quint8, float))); + _vehicle->uas(), SLOT(sendHilOpticalFlow(quint64, qint16, qint16, float, float, quint8, float))); if (_fgProcess) { @@ -629,7 +631,7 @@ bool QGCFlightGearLink::connectSimulation() // have that information setup we start the thread which will call run, which will in turn // start the various FG processes on the seperate thread. - if (!mav) { + if (!_vehicle->uas()) { return false; } @@ -846,7 +848,7 @@ bool QGCFlightGearLink::connectSimulation() _fgArgList += "--fg-aircraft=" + qgcAircraftDir; // Setup protocol we will be using to communicate with FlightGear - QString fgProtocol(mav->getSystemType() == MAV_TYPE_QUADROTOR ? "qgroundcontrol-quadrotor" : "qgroundcontrol-fixed-wing"); + QString fgProtocol(_vehicle->vehicleType() == MAV_TYPE_QUADROTOR ? "qgroundcontrol-quadrotor" : "qgroundcontrol-fixed-wing"); QString fgProtocolArg("--generic=socket,%1,300,127.0.0.1,%2,udp,%3"); _fgArgList << fgProtocolArg.arg("out").arg(port).arg(fgProtocol); _fgArgList << fgProtocolArg.arg("in").arg(currentPort).arg(fgProtocol); @@ -947,7 +949,7 @@ bool QGCFlightGearLink::connectSimulation() } // Start the engines to save a startup step - if (mav->getSystemType() == MAV_TYPE_QUADROTOR) { + if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR) { // Start all engines of the quad _fgArgList << "--prop:/engines/engine[0]/running=true"; _fgArgList << "--prop:/engines/engine[1]/running=true"; diff --git a/src/comm/QGCFlightGearLink.h b/src/comm/QGCFlightGearLink.h index 269f4ac1c..9a9fe5aa0 100644 --- a/src/comm/QGCFlightGearLink.h +++ b/src/comm/QGCFlightGearLink.h @@ -38,18 +38,20 @@ This file is part of the QGROUNDCONTROL project #include #include #include -#include + +#include "LinkInterface.h" #include "QGCConfig.h" #include "UASInterface.h" #include "QGCHilLink.h" -#include +#include "QGCHilFlightGearConfiguration.h" +#include "Vehicle.h" class QGCFlightGearLink : public QGCHilLink { Q_OBJECT public: - QGCFlightGearLink(UASInterface* mav, QString startupArguments, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress host = QHostAddress::Any, quint16 port = 49005); + QGCFlightGearLink(Vehicle* vehicle, QString startupArguments, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress host = QHostAddress::Any, quint16 port = 49005); ~QGCFlightGearLink(); bool isConnected(); @@ -135,20 +137,6 @@ public slots: void processError(QProcess::ProcessError err); protected: - QString name; - QHostAddress host; - QHostAddress currentHost; - quint16 currentPort; - quint16 port; - int id; - bool connectState; - - UASInterface* mav; - unsigned int flightGearVersion; - QString startupArguments; - bool _sensorHilEnabled; - float barometerOffsetkPa; - void setName(QString name); private slots: @@ -158,6 +146,7 @@ private slots: private: static bool _findUIArgument(const QStringList& uiArgList, const QString& argLabel, QString& argValue); + Vehicle* _vehicle; QString _fgProcessName; ///< FlightGear process to start QString _fgProcessWorkingDirPath; ///< Working directory to start FG process in, empty for none QStringList _fgArgList; ///< Arguments passed to FlightGear process @@ -166,6 +155,19 @@ private: QProcess* _fgProcess; ///< FlightGear process QString _fgProtocolFileFullyQualified; ///< Fully qualified file name for protocol file + + QString name; + QHostAddress host; + QHostAddress currentHost; + quint16 currentPort; + quint16 port; + int id; + bool connectState; + + unsigned int flightGearVersion; + QString startupArguments; + bool _sensorHilEnabled; + float barometerOffsetkPa; }; #endif // QGCFLIGHTGEARLINK_H diff --git a/src/comm/QGCJSBSimLink.cc b/src/comm/QGCJSBSimLink.cc index 0322b2295..bf5c77f5d 100644 --- a/src/comm/QGCJSBSimLink.cc +++ b/src/comm/QGCJSBSimLink.cc @@ -41,20 +41,20 @@ This file is part of the QGROUNDCONTROL project #include "QGC.h" #include "QGCMessageBox.h" -QGCJSBSimLink::QGCJSBSimLink(UASInterface* mav, QString startupArguments, QString remoteHost, QHostAddress host, quint16 port) : - socket(NULL), - process(NULL), - startupArguments(startupArguments) +QGCJSBSimLink::QGCJSBSimLink(Vehicle* vehicle, QString startupArguments, QString remoteHost, QHostAddress host, quint16 port) + : _vehicle(vehicle) + , socket(NULL) + , process(NULL) + , startupArguments(startupArguments) { // We're doing it wrong - because the Qt folks got the API wrong: // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/ moveToThread(this); this->host = host; - this->port = port+mav->getUASID(); + this->port = port + _vehicle->id(); this->connectState = false; - this->currentPort = 49000+mav->getUASID(); - this->mav = mav; + this->currentPort = 49000 + _vehicle->id(); this->name = tr("JSBSim Link (port:%1)").arg(port); setRemoteHost(remoteHost); } @@ -75,7 +75,7 @@ void QGCJSBSimLink::run() { qDebug() << "STARTING FLIGHTGEAR LINK"; - if (!mav) return; + if (!_vehicle) return; socket = new QUdpSocket(this); socket->moveToThread(this); connectState = socket->bind(host, port, QAbstractSocket::ReuseAddressHint); @@ -84,15 +84,11 @@ void QGCJSBSimLink::run() process = new QProcess(this); - connect(mav, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8))); - connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); + connect(_vehicle->uas(), SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8))); + connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), _vehicle->uas(), SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); - UAS* uas = dynamic_cast(mav); - if (uas) - { - uas->startHil(); - } + _vehicle->uas()->startHil(); //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate())); // Catch process error @@ -139,7 +135,7 @@ void QGCJSBSimLink::run() /*Prepare JSBSim Arguments */ - if (mav->getSystemType() == MAV_TYPE_QUADROTOR) + if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR) { arguments << QString("--realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=%s/flightgear.xml --script=%s/%s").arg(rootJSB).arg(rootJSB).arg(script); } @@ -364,8 +360,8 @@ bool QGCJSBSimLink::disconnectSimulation() { disconnect(process, SIGNAL(error(QProcess::ProcessError)), this, SLOT(processError(QProcess::ProcessError))); - disconnect(mav, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8))); - disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); + disconnect(_vehicle->uas(), SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8))); + disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), _vehicle->uas(), SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); if (process) { diff --git a/src/comm/QGCJSBSimLink.h b/src/comm/QGCJSBSimLink.h index bb293cd51..409d78aa4 100644 --- a/src/comm/QGCJSBSimLink.h +++ b/src/comm/QGCJSBSimLink.h @@ -40,8 +40,8 @@ This file is part of the QGROUNDCONTROL project #include #include #include "QGCConfig.h" -#include "UASInterface.h" #include "QGCHilLink.h" +#include "Vehicle.h" class QGCJSBSimLink : public QGCHilLink { @@ -49,7 +49,7 @@ class QGCJSBSimLink : public QGCHilLink //Q_INTERFACES(QGCJSBSimLinkInterface:LinkInterface) public: - QGCJSBSimLink(UASInterface* mav, QString startupArguments, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress host = QHostAddress::Any, quint16 port = 49005); + QGCJSBSimLink(Vehicle* vehicle, QString startupArguments, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress host = QHostAddress::Any, quint16 port = 49005); ~QGCJSBSimLink(); bool isConnected(); @@ -127,7 +127,8 @@ public slots: void setStartupArguments(QString startupArguments); -protected: +private: + Vehicle* _vehicle; QString name; QHostAddress host; QHostAddress currentHost; @@ -147,7 +148,6 @@ protected: QMutex statisticsMutex; QMutex dataMutex; QTimer refreshTimer; - UASInterface* mav; QProcess* process; unsigned int flightGearVersion; QString startupArguments; @@ -155,10 +155,6 @@ protected: bool _sensorHilEnabled; void setName(QString name); - -signals: - - }; #endif // QGCJSBSimLink_H diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index ef85146bf..d150bb539 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -45,8 +45,8 @@ This file is part of the QGROUNDCONTROL project #include "QGCMessageBox.h" #include "HomePositionManager.h" -QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress localHost, quint16 localPort) : - mav(mav), +QGCXPlaneLink::QGCXPlaneLink(Vehicle* vehicle, QString remoteHost, QHostAddress localHost, quint16 localPort) : + _vehicle(vehicle), remoteHost(QHostAddress("127.0.0.1")), remotePort(49000), socket(NULL), @@ -156,7 +156,7 @@ void QGCXPlaneLink::setVersion(unsigned int version) **/ void QGCXPlaneLink::run() { - if (!mav) { + if (!_vehicle) { emit statusMessage("No MAV present"); return; } @@ -182,22 +182,15 @@ void QGCXPlaneLink::run() QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes())); - UAS* uas = dynamic_cast(mav); - if (uas) - { - connect(uas, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8)), Qt::QueuedConnection); - connect(uas, SIGNAL(hilActuatorsChanged(quint64, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(quint64,float,float,float,float,float,float,float,float)), Qt::QueuedConnection); + connect(_vehicle->uas(), SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8)), Qt::QueuedConnection); + connect(_vehicle->uas(), SIGNAL(hilActuatorsChanged(quint64, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(quint64,float,float,float,float,float,float,float,float)), Qt::QueuedConnection); - connect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), uas, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), Qt::QueuedConnection); - connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), uas, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), Qt::QueuedConnection); - connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), uas, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), Qt::QueuedConnection); - connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), uas, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), Qt::QueuedConnection); + connect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), _vehicle->uas(), SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), Qt::QueuedConnection); + connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), _vehicle->uas(), SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), Qt::QueuedConnection); + connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), _vehicle->uas(), SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), Qt::QueuedConnection); + connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), _vehicle->uas(), SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), Qt::QueuedConnection); - uas->startHil(); - } else { - emit statusMessage(tr("Failed to connect to drone instance")); - return; - } + _vehicle->uas()->startHil(); #pragma pack(push, 1) struct iset_struct @@ -246,20 +239,13 @@ void QGCXPlaneLink::run() QGC::SLEEP::msleep(5); } - uas = dynamic_cast(mav); - if (uas) - { - disconnect(uas, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8))); - disconnect(uas, SIGNAL(hilActuatorsChanged(quint64, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(quint64,float,float,float,float,float,float,float,float))); + disconnect(_vehicle->uas(), SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8))); + disconnect(_vehicle->uas(), SIGNAL(hilActuatorsChanged(quint64, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(quint64,float,float,float,float,float,float,float,float))); - disconnect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), uas, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); - disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), uas, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); - disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), uas, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int))); - disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), uas, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); - - // Do not toggle HIL state on the UAS - this is not the job of this link, but of the - // UAS object - } + disconnect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), _vehicle->uas(), SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); + disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), _vehicle->uas(), SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); + disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), _vehicle->uas(), SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int))); + disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), _vehicle->uas(), SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); connectState = false; @@ -368,7 +354,7 @@ void QGCXPlaneLink::setRemoteHost(const QString& newHost) void QGCXPlaneLink::updateActuators(quint64 time, float act1, float act2, float act3, float act4, float act5, float act6, float act7, float act8) { - if (mav->getSystemType() == MAV_TYPE_QUADROTOR) + if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR) // Only update this for multirotors { @@ -450,19 +436,7 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch bool isFixedWing = true; - if (mav->getAirframe() == UASInterface::QGC_AIRFRAME_X8 || - mav->getAirframe() == UASInterface::QGC_AIRFRAME_VIPER_2_0 || - mav->getAirframe() == UASInterface::QGC_AIRFRAME_CAMFLYER_Q) - { - // de-mix delta-mixed inputs - // pitch input - mixed roll and pitch channels - p.f[0] = 0.5f * (rollAilerons - pitchElevator); - // roll input - mixed roll and pitch channels - p.f[1] = 0.5f * (rollAilerons + pitchElevator); - // yaw - p.f[2] = 0.0f; - } - else if (mav->getSystemType() == MAV_TYPE_QUADROTOR) + if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR) { qDebug() << "MAV_TYPE_QUADROTOR"; @@ -1021,17 +995,17 @@ void QGCXPlaneLink::setRandomPosition() double offLon = rand() / static_cast(RAND_MAX) / 500.0 + 1.0/500.0; double offAlt = rand() / static_cast(RAND_MAX) * 200.0 + 100.0; - if (mav->getAltitudeAMSL() + offAlt < 0) + if (_vehicle->altitudeAMSL() + offAlt < 0) { offAlt *= -1.0; } - setPositionAttitude(mav->getLatitude() + offLat, - mav->getLongitude() + offLon, - mav->getAltitudeAMSL() + offAlt, - mav->getRoll(), - mav->getPitch(), - mav->getYaw()); + setPositionAttitude(_vehicle->latitude() + offLat, + _vehicle->longitude() + offLon, + _vehicle->altitudeAMSL() + offAlt, + _vehicle->roll(), + _vehicle->pitch(), + _vehicle->uas()->getYaw()); } void QGCXPlaneLink::setRandomAttitude() @@ -1043,9 +1017,9 @@ void QGCXPlaneLink::setRandomAttitude() double pitch = rand() / static_cast(RAND_MAX) * 2.0 - 1.0; double yaw = rand() / static_cast(RAND_MAX) * 2.0 - 1.0; - setPositionAttitude(mav->getLatitude(), - mav->getLongitude(), - mav->getAltitudeAMSL(), + setPositionAttitude(_vehicle->latitude(), + _vehicle->longitude(), + _vehicle->altitudeAMSL(), roll, pitch, yaw); diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h index af0fb750a..faee17102 100644 --- a/src/comm/QGCXPlaneLink.h +++ b/src/comm/QGCXPlaneLink.h @@ -40,8 +40,8 @@ This file is part of the QGROUNDCONTROL project #include #include #include "QGCConfig.h" -#include "UASInterface.h" #include "QGCHilLink.h" +#include "Vehicle.h" class QGCXPlaneLink : public QGCHilLink { @@ -49,7 +49,7 @@ class QGCXPlaneLink : public QGCHilLink //Q_INTERFACES(QGCXPlaneLinkInterface:LinkInterface) public: - QGCXPlaneLink(UASInterface* mav, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress localHost = QHostAddress::Any, quint16 localPort = 49005); + QGCXPlaneLink(Vehicle* vehicle, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress localHost = QHostAddress::Any, quint16 localPort = 49005); ~QGCXPlaneLink(); /** @@ -164,7 +164,7 @@ public slots: void setRandomAttitude(); protected: - UASInterface* mav; + Vehicle* _vehicle; QString name; QHostAddress localHost; quint16 localPort; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index a2b560333..7d6749c96 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -59,9 +59,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi receiveDropRate(0), sendDropRate(0), - name(""), - type(MAV_TYPE_GENERIC), - airframe(QGC_AIRFRAME_GENERIC), autopilot(vehicle->firmwareType()), base_mode(0), custom_mode(0), @@ -91,13 +88,8 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi manualThrust(0), positionLock(false), - isLocalPositionKnown(false), isGlobalPositionKnown(false), - localX(0.0), - localY(0.0), - localZ(0.0), - latitude(0.0), longitude(0.0), altitudeAMSL(0.0), @@ -191,9 +183,7 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi color = UASInterface::getNextColor(); connect(&statusTimeout, SIGNAL(timeout()), this, SLOT(updateState())); - connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings())); statusTimeout.start(500); - readSettings(); } /** @@ -211,33 +201,6 @@ UAS::~UAS() simulation->deleteLater(); } #endif - writeSettings(); -} - -/** -* Saves the settings of name, airframe, autopilot type and battery specifications -* for the next instantiation of UAS. -*/ -void UAS::writeSettings() -{ - QSettings settings; - settings.beginGroup(QString("MAV%1").arg(uasId)); - settings.setValue("NAME", this->name); - settings.setValue("AIRFRAME", this->airframe); - settings.endGroup(); -} - -/** -* Reads in the settings: name, airframe, autopilot type, and battery specifications -* for the new UAS. -*/ -void UAS::readSettings() -{ - QSettings settings; - settings.beginGroup(QString("MAV%1").arg(uasId)); - this->name = settings.value("NAME", this->name).toString(); - this->airframe = settings.value("AIRFRAME", this->airframe).toInt(); - settings.endGroup(); } /** @@ -385,13 +348,6 @@ void UAS::receiveMessage(mavlink_message_t message) emit valueChanged(uasId, name.arg("custom_mode"), "bits", state.custom_mode, time); emit valueChanged(uasId, name.arg("system_status"), "-", state.system_status, time); - // Set new type if it has changed - if (this->type != state.type) - { - this->autopilot = state.autopilot; - setSystemType(state.type); - } - QString audiostring = QString("System %1").arg(uasId); QString stateAudio = ""; QString modeAudio = ""; @@ -683,25 +639,16 @@ void UAS::receiveMessage(mavlink_message_t message) mavlink_msg_local_position_ned_decode(&message, &pos); quint64 time = getUnixTime(pos.time_boot_ms); - // Emit position always with component ID - emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time); - if (!wrongComponent) { - setLocalX(pos.x); - setLocalY(pos.y); - setLocalZ(pos.z); - speedX = pos.vx; speedY = pos.vy; speedZ = pos.vz; // Emit - emit localPositionChanged(this, localX, localY, localZ, time); emit velocityChanged_NED(this, speedX, speedY, speedZ, time); positionLock = true; - isLocalPositionKnown = true; } } break; @@ -710,7 +657,6 @@ void UAS::receiveMessage(mavlink_message_t message) mavlink_global_vision_position_estimate_t pos; mavlink_msg_global_vision_position_estimate_decode(&message, &pos); quint64 time = getUnixTime(pos.usec); - emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time); emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time); } break; @@ -1076,11 +1022,7 @@ void UAS::setHomePosition(double lat, double lon, double alt) if (!_vehicle || blockHomePositionChanges) return; - QString uasName = (getUASName() == "")? - tr("UAS") + QString::number(getUASID()) - : getUASName(); - - QMessageBox::StandardButton button = QGCMessageBox::question(tr("Set a new home position for vehicle %1").arg(uasName), + QMessageBox::StandardButton button = QGCMessageBox::question(tr("Set a new home position for vehicle %1").arg(getUASID()), tr("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location"), QMessageBox::Yes | QMessageBox::Cancel, QMessageBox::Cancel); @@ -1533,32 +1475,6 @@ quint64 UAS::getUptime() const } } -bool UAS::isRotaryWing() -{ - switch (type) { - case MAV_TYPE_QUADROTOR: - /* fallthrough */ - case MAV_TYPE_COAXIAL: - case MAV_TYPE_HELICOPTER: - case MAV_TYPE_HEXAROTOR: - case MAV_TYPE_OCTOROTOR: - case MAV_TYPE_TRICOPTER: - return true; - default: - return false; - } -} - -bool UAS::isFixedWing() -{ - switch (type) { - case MAV_TYPE_FIXED_WING: - return true; - default: - return false; - } -} - //TODO update this to use the parameter manager / param data model instead void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramUnion) { @@ -1602,40 +1518,6 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, emit parameterUpdate(uasId, compId, paramName, rawValue.param_count, rawValue.param_index, rawValue.param_type, paramValue); } -/** -* @param systemType Type of MAV. -*/ -void UAS::setSystemType(int systemType) -{ - if((systemType >= MAV_TYPE_GENERIC) && (systemType < MAV_TYPE_ENUM_END)) - { - type = systemType; - - // If the airframe is still generic, change it to a close default type - if (airframe == 0) - { - switch (type) - { - case MAV_TYPE_FIXED_WING: - setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR); - break; - case MAV_TYPE_QUADROTOR: - setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH); - break; - case MAV_TYPE_HEXAROTOR: - setAirframe(UASInterface::QGC_AIRFRAME_HEXCOPTER); - break; - default: - // Do nothing - break; - } - } - emit systemSpecsChanged(uasId); - emit systemTypeSet(this, type); - qDebug() << "TYPE CHANGED TO:" << type; - } -} - void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) { if (!_vehicle) { @@ -1887,29 +1769,6 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll } #endif -/** -* @return the type of the system -*/ -int UAS::getSystemType() -{ - return this->type; -} - -/** @brief Is it an airplane (or like one)?,..)*/ -bool UAS::isAirplane() -{ - switch(this->type) { - case MAV_TYPE_GENERIC: - case MAV_TYPE_FIXED_WING: - case MAV_TYPE_AIRSHIP: - case MAV_TYPE_FLAPPING_WING: - return true; - default: - break; - } - return false; -} - /** * Order the robot to start receiver pairing */ @@ -1940,7 +1799,7 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj stopHil(); delete simulation; } - simulation = new QGCFlightGearLink(this, options); + simulation = new QGCFlightGearLink(_vehicle, options); } float noise_scaler = 0.002f; @@ -1988,7 +1847,7 @@ void UAS::enableHilJSBSim(bool enable, QString options) stopHil(); delete simulation; } - simulation = new QGCJSBSimLink(this, options); + simulation = new QGCJSBSimLink(_vehicle, options); } // Connect Flight Gear Link link = dynamic_cast(simulation); @@ -2016,7 +1875,7 @@ void UAS::enableHilXPlane(bool enable) stopHil(); delete simulation; } - simulation = new QGCXPlaneLink(this); + simulation = new QGCXPlaneLink(_vehicle); float noise_scaler = 0.0002f; xacc_var = noise_scaler * 0.2914f; @@ -2341,23 +2200,6 @@ void UAS::stopHil() } #endif -/** - * @return The name of this system as string in human-readable form - */ -QString UAS::getUASName(void) const -{ - QString result; - if (name == "") - { - result = tr("MAV ") + result.sprintf("%03d", getUASID()); - } - else - { - result = name; - } - return result; -} - /** * @rerturn the map of the components */ diff --git a/src/uas/UAS.h b/src/uas/UAS.h index cd52382c4..b43de731a 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -71,15 +71,8 @@ public: /* MANAGEMENT */ - /** @brief The name of the robot */ - QString getUASName(void) const; /** @brief Get the unique system id */ int getUASID() const; - /** @brief Get the airframe */ - int getAirframe() const - { - return airframe; - } /** @brief Get the components */ QMap getComponents(); @@ -88,13 +81,9 @@ public: /** @brief Add one measurement and get low-passed voltage */ float filterVoltage(float value); - Q_PROPERTY(double localX READ getLocalX WRITE setLocalX NOTIFY localXChanged) - Q_PROPERTY(double localY READ getLocalY WRITE setLocalY NOTIFY localYChanged) - Q_PROPERTY(double localZ READ getLocalZ WRITE setLocalZ NOTIFY localZChanged) Q_PROPERTY(double latitude READ getLatitude WRITE setLatitude NOTIFY latitudeChanged) Q_PROPERTY(double longitude READ getLongitude WRITE setLongitude NOTIFY longitudeChanged) Q_PROPERTY(double satelliteCount READ getSatelliteCount WRITE setSatelliteCount NOTIFY satelliteCountChanged) - Q_PROPERTY(bool isLocalPositionKnown READ localPositionKnown) Q_PROPERTY(bool isGlobalPositionKnown READ globalPositionKnown) Q_PROPERTY(double roll READ getRoll WRITE setRoll NOTIFY rollChanged) Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged) @@ -248,11 +237,6 @@ public: return satelliteCount; } - virtual bool localPositionKnown() const - { - return isLocalPositionKnown; - } - virtual bool globalPositionKnown() const { return isGlobalPositionKnown; @@ -369,9 +353,6 @@ public: temperature_var = var; } - bool isRotaryWing(); - bool isFixedWing(); - friend class FileManager; protected: //COMMENTS FOR TEST UNIT @@ -387,9 +368,6 @@ protected: //COMMENTS FOR TEST UNIT QTimer statusTimeout; ///< Timer for various status timeouts /// BASIC UAS TYPE, NAME AND STATE - QString name; ///< Human-friendly name of the vehicle, e.g. bravo - unsigned char type; ///< UAS type (from type enum) - int airframe; ///< The airframe type int autopilot; ///< Type of the Autopilot: -1: None, 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM uint8_t base_mode; ///< The current mode of the MAV uint32_t custom_mode; ///< The current mode of the MAV @@ -427,7 +405,6 @@ protected: //COMMENTS FOR TEST UNIT /// POSITION bool positionLock; ///< Status if position information is available or not - bool isLocalPositionKnown; ///< If the local position has been received for this MAV bool isGlobalPositionKnown; ///< If the global position has been received for this MAV double localX; @@ -520,9 +497,6 @@ public: } #endif - int getSystemType(); - bool isAirplane(); - QImage getImage(); void requestImage(); int getAutopilotType(){ @@ -536,18 +510,6 @@ public slots: autopilot = apType; emit systemSpecsChanged(uasId); } - /** @brief Set the type of airframe */ - void setSystemType(int systemType); - /** @brief Set the specific airframe type */ - void setAirframe(int airframe) - { - if((airframe >= QGC_AIRFRAME_GENERIC) && (airframe < QGC_AIRFRAME_END_OF_ENUM)) - { - this->airframe = airframe; - emit systemSpecsChanged(uasId); - } - - } /** @brief Executes a command with 7 params */ void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component); @@ -686,12 +648,6 @@ protected: quint64 lastSendTimeSensors; ///< Last HIL Sensors message sent quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent -protected slots: - /** @brief Write settings to disk */ - void writeSettings(); - /** @brief Read settings from disk */ - void readSettings(); - private: void _say(const QString& text, int severity = 6); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 8121e819b..fc90c9a4d 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -67,17 +67,10 @@ public: /* MANAGEMENT */ - /** @brief The name of the robot **/ - virtual QString getUASName() const = 0; virtual int getUASID() const = 0; ///< Get the ID of the connected UAS /** @brief The time interval the robot is switched on **/ virtual quint64 getUptime() const = 0; - virtual double getLocalX() const = 0; - virtual double getLocalY() const = 0; - virtual double getLocalZ() const = 0; - virtual bool localPositionKnown() const = 0; - virtual double getLatitude() const = 0; virtual double getLongitude() const = 0; virtual double getAltitudeAMSL() const = 0; @@ -88,30 +81,8 @@ public: virtual double getPitch() const = 0; virtual double getYaw() const = 0; - /** @brief Set the airframe of this MAV */ - virtual int getAirframe() const = 0; - virtual FileManager* getFileManager() = 0; - enum Airframe { - QGC_AIRFRAME_GENERIC = 0, - QGC_AIRFRAME_EASYSTAR, - QGC_AIRFRAME_TWINSTAR, - QGC_AIRFRAME_MERLIN, - QGC_AIRFRAME_CHEETAH, - QGC_AIRFRAME_MIKROKOPTER, - QGC_AIRFRAME_REAPER, - QGC_AIRFRAME_PREDATOR, - QGC_AIRFRAME_COAXIAL, - QGC_AIRFRAME_PTERYX, - QGC_AIRFRAME_TRICOPTER, - QGC_AIRFRAME_HEXCOPTER, - QGC_AIRFRAME_X8, - QGC_AIRFRAME_VIPER_2_0, - QGC_AIRFRAME_CAMFLYER_Q, - QGC_AIRFRAME_END_OF_ENUM - }; - /** * @brief Get the color for this UAS * @@ -151,11 +122,6 @@ public: return colors[nextColor];//return the next color } - /** @brief Get the type of the system (airplane, quadrotor, helicopter,..)*/ - virtual int getSystemType() = 0; - /** @brief Is it an airplane (or like one)?,..)*/ - virtual bool isAirplane() = 0; - /** @brief Get the type of the autopilot (PIXHAWK, APM, UDB, PPZ,..) */ virtual int getAutopilotType() = 0; virtual void setAutopilotType(int apType) = 0; @@ -204,19 +170,11 @@ public slots: /** @brief Executes a command **/ virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) = 0; - /** @brief Selects the airframe */ - virtual void setAirframe(int airframe) = 0; - /** @brief Order the robot to pair its receiver **/ virtual void pairRX(int rxType, int rxSubType) = 0; virtual void setHomePosition(double lat, double lon, double alt) = 0; - /** @brief Return if this a rotary wing */ - virtual bool isRotaryWing() = 0; - /** @brief Return if this is a fixed wing */ - virtual bool isFixedWing() = 0; - /** @brief Send the full HIL state to the MAV */ #ifndef __mobile__ virtual void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, @@ -322,8 +280,6 @@ signals: void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec); /** @brief A user (or an autonomous mission or obstacle avoidance planner) requested to set a new setpoint */ void userPositionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired); - void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec); - void localPositionChanged(UASInterface*, int component, double x, double y, double z, quint64 usec); void globalPositionChanged(UASInterface*, double lat, double lon, double altAMSL, double altWGS84, quint64 usec); void altitudeChanged(UASInterface*, double altitudeAMSL, double altitudeWGS84, double altitudeRelative, double climbRate, quint64 usec); /** @brief Update the status of one satellite used for localization */ @@ -339,8 +295,6 @@ signals: void imageStarted(int imgid, int width, int height, int depth, int channels); void imageDataReceived(int imgid, const unsigned char* imageData, int length, int startIndex); - /** @brief Emit the new system type */ - void systemTypeSet(UASInterface* uas, unsigned int type); /** @brief Attitude control enabled/disabled */ void attitudeControlEnabled(bool enabled); diff --git a/src/ui/QGCHilConfiguration.cc b/src/ui/QGCHilConfiguration.cc index 85a30b162..bdd163fe3 100644 --- a/src/ui/QGCHilConfiguration.cc +++ b/src/ui/QGCHilConfiguration.cc @@ -29,6 +29,7 @@ #include "QGCHilFlightGearConfiguration.h" #include "QGCHilJSBSimConfiguration.h" #include "QGCHilXPlaneConfiguration.h" +#include "UAS.h" QGCHilConfiguration::QGCHilConfiguration(Vehicle* vehicle, QWidget *parent) : QWidget(parent) @@ -87,7 +88,7 @@ void QGCHilConfiguration::on_simComboBox_currentIndexChanged(int index) { // Ensure the sim exists and is disabled _vehicle->uas()->enableHilFlightGear(false, "", true, this); - QGCHilFlightGearConfiguration* hfgconf = new QGCHilFlightGearConfiguration(_vehicle->uas(), this); + QGCHilFlightGearConfiguration* hfgconf = new QGCHilFlightGearConfiguration(_vehicle, this); hfgconf->show(); ui->simulatorConfigurationLayout->addWidget(hfgconf); QGCFlightGearLink* fg = dynamic_cast(_vehicle->uas()->getHILSimulation()); @@ -117,7 +118,7 @@ void QGCHilConfiguration::on_simComboBox_currentIndexChanged(int index) { // Ensure the sim exists and is disabled _vehicle->uas()->enableHilJSBSim(false, ""); - QGCHilJSBSimConfiguration* hfgconf = new QGCHilJSBSimConfiguration(_vehicle->uas(), this); + QGCHilJSBSimConfiguration* hfgconf = new QGCHilJSBSimConfiguration(_vehicle, this); hfgconf->show(); ui->simulatorConfigurationLayout->addWidget(hfgconf); QGCJSBSimLink* jsb = dynamic_cast(_vehicle->uas()->getHILSimulation()); diff --git a/src/ui/QGCHilFlightGearConfiguration.cc b/src/ui/QGCHilFlightGearConfiguration.cc index 33004797d..d183e300f 100644 --- a/src/ui/QGCHilFlightGearConfiguration.cc +++ b/src/ui/QGCHilFlightGearConfiguration.cc @@ -1,5 +1,6 @@ #include "QGCHilFlightGearConfiguration.h" #include "MainWindow.h" +#include "UAS.h" #include @@ -16,19 +17,17 @@ const char* QGCHilFlightGearConfiguration::_sensorHilKey = "SEN // the QGCFlightGearLink code instead. const char* QGCHilFlightGearConfiguration::_defaultOptions = "--roll=0 --pitch=0 --vc=0 --heading=300 --timeofday=noon --disable-hud-3d --disable-fullscreen --geometry=400x300 --disable-anti-alias-hud --wind=0@0 --turbulence=0.0 --disable-sound --disable-random-objects --disable-ai-traffic --shading-flat --fog-disable --disable-specular-highlight --disable-panel --disable-clouds --fdm=jsb --units-meters --enable-terrasync"; -QGCHilFlightGearConfiguration::QGCHilFlightGearConfiguration(UAS* mav, QWidget *parent) : - QWidget(parent), - _mav(mav), - _mavSettingsSubGroup(NULL), - _resetOptionsAction(tr("Reset to default options"), this) +QGCHilFlightGearConfiguration::QGCHilFlightGearConfiguration(Vehicle* vehicle, QWidget *parent) + : QWidget(parent) + , _vehicle(vehicle) + , _mavSettingsSubGroup(NULL) + , _resetOptionsAction(tr("Reset to default options"), this) { - Q_ASSERT(_mav); - _ui.setupUi(this); QStringList items; - if (_mav->getSystemType() == MAV_TYPE_FIXED_WING) + if (_vehicle->vehicleType() == MAV_TYPE_FIXED_WING) { items << "EasyStar"; items << "Rascal110-JSBSim"; @@ -37,7 +36,7 @@ QGCHilFlightGearConfiguration::QGCHilFlightGearConfiguration(UAS* mav, QWidget * items << "Malolo1"; _mavSettingsSubGroup = _mavSettingsSubGroupFixedWing; } - else if (_mav->getSystemType() == MAV_TYPE_QUADROTOR) + else if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR) { items << "arducopter"; _mavSettingsSubGroup = _mavSettingsSubGroupQuadRotor; @@ -118,12 +117,12 @@ void QGCHilFlightGearConfiguration::on_startButton_clicked() //XXX check validity of inputs QString options = _ui.optionsPlainTextEdit->toPlainText(); options.append(" --aircraft=" + _ui.aircraftComboBox->currentText()); - _mav->enableHilFlightGear(true, options, _ui.sensorHilCheckBox->isChecked(), this); + _vehicle->uas()->enableHilFlightGear(true, options, _ui.sensorHilCheckBox->isChecked(), this); } void QGCHilFlightGearConfiguration::on_stopButton_clicked() { - _mav->stopHil(); + _vehicle->uas()->stopHil(); } void QGCHilFlightGearConfiguration::on_barometerOffsetLineEdit_textChanged(const QString& baroOffset) diff --git a/src/ui/QGCHilFlightGearConfiguration.h b/src/ui/QGCHilFlightGearConfiguration.h index 0b72996c6..6ebd999b6 100644 --- a/src/ui/QGCHilFlightGearConfiguration.h +++ b/src/ui/QGCHilFlightGearConfiguration.h @@ -5,7 +5,7 @@ #include "QGCHilLink.h" #include "QGCFlightGearLink.h" -#include "UAS.h" +#include "Vehicle.h" #include "ui_QGCHilFlightGearConfiguration.h" @@ -18,7 +18,7 @@ class QGCHilFlightGearConfiguration : public QWidget Q_OBJECT public: - explicit QGCHilFlightGearConfiguration(UAS* mav, QWidget *parent = 0); + explicit QGCHilFlightGearConfiguration(Vehicle* vehicle, QWidget *parent = 0); ~QGCHilFlightGearConfiguration(); protected: @@ -31,8 +31,8 @@ private slots: void _showContextMenu(const QPoint& pt); private: + Vehicle* _vehicle; Ui::QGCHilFlightGearConfiguration _ui; - UAS* _mav; /// mav associated with this ui static const char* _settingsGroup; /// Top level settings group const char* _mavSettingsSubGroup; /// We maintain a settings sub group per mav type diff --git a/src/ui/QGCHilJSBSimConfiguration.cc b/src/ui/QGCHilJSBSimConfiguration.cc index d37602731..4e958ae0b 100644 --- a/src/ui/QGCHilJSBSimConfiguration.cc +++ b/src/ui/QGCHilJSBSimConfiguration.cc @@ -2,16 +2,17 @@ #include "ui_QGCHilJSBSimConfiguration.h" #include "MainWindow.h" +#include "UAS.h" -QGCHilJSBSimConfiguration::QGCHilJSBSimConfiguration(UAS* mav,QWidget *parent) : - QWidget(parent), - mav(mav), - ui(new Ui::QGCHilJSBSimConfiguration) +QGCHilJSBSimConfiguration::QGCHilJSBSimConfiguration(Vehicle* vehicle, QWidget *parent) + : QWidget(parent) + , _vehicle(vehicle) + , ui(new Ui::QGCHilJSBSimConfiguration) { ui->setupUi(this); QStringList items = QStringList(); - if (mav->getSystemType() == MAV_TYPE_FIXED_WING) + if (_vehicle->vehicleType() == MAV_TYPE_FIXED_WING) { items << "EasyStar"; items << "Rascal110-JSBSim"; @@ -19,7 +20,7 @@ QGCHilJSBSimConfiguration::QGCHilJSBSimConfiguration(UAS* mav,QWidget *parent) : items << "YardStik"; items << "Malolo1"; } - else if (mav->getSystemType() == MAV_TYPE_QUADROTOR) + else if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR) { items << "arducopter"; } @@ -40,10 +41,10 @@ void QGCHilJSBSimConfiguration::on_startButton_clicked() //XXX check validity of inputs QString options = ui->optionsPlainTextEdit->toPlainText(); options.append(" --script=" + ui->aircraftComboBox->currentText()); - mav->enableHilJSBSim(true, options); + _vehicle->uas()->enableHilJSBSim(true, options); } void QGCHilJSBSimConfiguration::on_stopButton_clicked() { - mav->stopHil(); + _vehicle->uas()->stopHil(); } diff --git a/src/ui/QGCHilJSBSimConfiguration.h b/src/ui/QGCHilJSBSimConfiguration.h index d31560006..b03bdf4a0 100644 --- a/src/ui/QGCHilJSBSimConfiguration.h +++ b/src/ui/QGCHilJSBSimConfiguration.h @@ -5,7 +5,7 @@ #include "QGCHilLink.h" #include "QGCFlightGearLink.h" -#include "UAS.h" +#include "Vehicle.h" namespace Ui { class QGCHilJSBSimConfiguration; @@ -16,17 +16,16 @@ class QGCHilJSBSimConfiguration : public QWidget Q_OBJECT public: - explicit QGCHilJSBSimConfiguration(UAS* mav, QWidget *parent = 0); + explicit QGCHilJSBSimConfiguration(Vehicle* vehicle, QWidget *parent = 0); ~QGCHilJSBSimConfiguration(); -protected: - UAS* mav; - private slots: void on_startButton_clicked(); void on_stopButton_clicked(); private: + Vehicle* _vehicle; + Ui::QGCHilJSBSimConfiguration *ui; }; diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc index 41a665d46..0ab169cd3 100644 --- a/src/ui/QGCMAVLinkInspector.cc +++ b/src/ui/QGCMAVLinkInspector.cc @@ -65,7 +65,7 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(const QString& title, QAction* action, void QGCMAVLinkInspector::_vehicleAdded(Vehicle* vehicle) { - ui->systemComboBox->addItem(vehicle->uas()->getUASName(), vehicle->id()); + ui->systemComboBox->addItem(QString("Vehicle %1").arg(vehicle->id()), vehicle->id()); // Add a tree for a new UAS addUAStoTree(vehicle->id()); @@ -340,14 +340,7 @@ void QGCMAVLinkInspector::addUAStoTree(int sysId) { UASInterface* uas = vehicle->uas(); QStringList idstring; - if (uas->getUASName() == "") - { - idstring << tr("UAS ") + QString::number(uas->getUASID()); - } - else - { - idstring << uas->getUASName(); - } + idstring << QString("Vehicle %1").arg(uas->getUASID()); QTreeWidgetItem* uasWidget = new QTreeWidgetItem(idstring); uasWidget->setFirstColumnSpanned(true); uasTreeWidgetItems.insert(sysId,uasWidget); -- 2.22.0