From a148e3e9f092d85e04964bb46b54b8a54761ab77 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Thu, 11 May 2017 17:59:41 -0400 Subject: [PATCH] Use QList to expose firmwareplugin factgroups --- .../APM/ArduSubFirmwarePlugin.cc | 15 ++-- .../APM/ArduSubFirmwarePlugin.h | 69 ++++++++++--------- src/FirmwarePlugin/FirmwarePlugin.cc | 4 ++ src/FirmwarePlugin/FirmwarePlugin.h | 6 +- src/Vehicle/Vehicle.cc | 5 +- 5 files changed, 53 insertions(+), 46 deletions(-) diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc index 9ccccca96..2af431eb3 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc @@ -40,7 +40,8 @@ APMSubMode::APMSubMode(uint32_t mode, bool settable) : setEnumToStringMapping(enumToString); } -ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void) +ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void): + _infoFactGroup(this) { QList supportedFlightModes; supportedFlightModes << APMSubMode(APMSubMode::MANUAL ,true); @@ -100,7 +101,7 @@ ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void) _remapParamNameIntialized = true; } - fwFactGroup = new APMSubmarineFactGroup(this); + _factGroups.append(&_infoFactGroup); } int ArduSubFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const @@ -166,15 +167,15 @@ void ArduSubFirmwarePlugin::_handleNamedValueFloat(mavlink_message_t* message) QString name = QString(value.name); if (name == "CamTilt") { - fwFactGroup->getFact("camera tilt")->setRawValue(value.value * 100); + _infoFactGroup.getFact("camera tilt")->setRawValue(value.value * 100); } else if (name == "TetherTrn") { - fwFactGroup->getFact("tether turns")->setRawValue(value.value); + _infoFactGroup.getFact("tether turns")->setRawValue(value.value); } else if (name == "Lights1") { - fwFactGroup->getFact("lights 1")->setRawValue(value.value * 100); + _infoFactGroup.getFact("lights 1")->setRawValue(value.value * 100); } else if (name == "Lights2") { - fwFactGroup->getFact("lights 2")->setRawValue(value.value * 100); + _infoFactGroup.getFact("lights 2")->setRawValue(value.value * 100); } else if (name == "PilotGain") { - fwFactGroup->getFact("pilot gain")->setRawValue(value.value * 100); + _infoFactGroup.getFact("pilot gain")->setRawValue(value.value * 100); } } diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h index 87af47c28..a1c50156f 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h @@ -28,6 +28,40 @@ #define ArduSubFirmwarePlugin_H #include "APMFirmwarePlugin.h" +class APMSubmarineFactGroup : public FactGroup +{ + Q_OBJECT + +public: + APMSubmarineFactGroup(QObject* parent = NULL); + + Q_PROPERTY(Fact* camTilt READ camTilt CONSTANT) + Q_PROPERTY(Fact* tetherTurns READ tetherTurns CONSTANT) + Q_PROPERTY(Fact* lightsLevel1 READ lightsLevel1 CONSTANT) + Q_PROPERTY(Fact* lightsLevel2 READ lightsLevel2 CONSTANT) + Q_PROPERTY(Fact* pilotGain READ pilotGain CONSTANT) + + Fact* camTilt (void) { return &_camTiltFact; } + Fact* tetherTurns (void) { return &_tetherTurnsFact; } + Fact* lightsLevel1 (void) { return &_lightsLevel1Fact; } + Fact* lightsLevel2 (void) { return &_lightsLevel2Fact; } + Fact* pilotGain (void) { return &_pilotGainFact; } + + static const char* _camTiltFactName; + static const char* _tetherTurnsFactName; + static const char* _lightsLevel1FactName; + static const char* _lightsLevel2FactName; + static const char* _pilotGainFactName; + + static const char* _settingsGroup; + +private: + Fact _camTiltFact; + Fact _tetherTurnsFact; + Fact _lightsLevel1Fact; + Fact _lightsLevel2Fact; + Fact _pilotGainFact; +}; class APMSubMode : public APMCustomMode { @@ -97,40 +131,7 @@ private: static FirmwarePlugin::remapParamNameMajorVersionMap_t _remapParamName; void _handleNamedValueFloat(mavlink_message_t* message); void _handleMavlinkMessage(mavlink_message_t* message); -}; - -class APMSubmarineFactGroup : public FactGroup -{ - Q_OBJECT - -public: - APMSubmarineFactGroup(QObject* parent = NULL); - - Q_PROPERTY(Fact* camTilt READ camTilt CONSTANT) - Q_PROPERTY(Fact* tetherTurns READ tetherTurns CONSTANT) - Q_PROPERTY(Fact* lightsLevel1 READ lightsLevel1 CONSTANT) - Q_PROPERTY(Fact* lightsLevel2 READ lightsLevel2 CONSTANT) - Q_PROPERTY(Fact* pilotGain READ pilotGain CONSTANT) - - Fact* camTilt (void) { return &_camTiltFact; } - Fact* tetherTurns (void) { return &_tetherTurnsFact; } - Fact* lightsLevel1 (void) { return &_lightsLevel1Fact; } - Fact* lightsLevel2 (void) { return &_lightsLevel2Fact; } - Fact* pilotGain (void) { return &_pilotGainFact; } - - static const char* _camTiltFactName; - static const char* _tetherTurnsFactName; - static const char* _lightsLevel1FactName; - static const char* _lightsLevel2FactName; - static const char* _pilotGainFactName; - static const char* _settingsGroup; - -private: - Fact _camTiltFact; - Fact _tetherTurnsFact; - Fact _lightsLevel1Fact; - Fact _lightsLevel2Fact; - Fact _pilotGainFact; + APMSubmarineFactGroup _infoFactGroup; }; #endif diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index d07ace1ed..9e0e5fa60 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -419,6 +419,10 @@ const QVariantList& FirmwarePlugin::cameraList(const Vehicle* vehicle) return _cameraList; } +QList FirmwarePlugin::factGroups(void) { + return _factGroups; +} + bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const { return vehicle->multiRotor() ? false : true; diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 704dc3935..641d38c05 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -273,6 +273,8 @@ public: /// Returns a list of CameraMetaData objects for available cameras on the vehicle. virtual const QVariantList& cameraList(const Vehicle* vehicle); + virtual QList factGroups(void); + /// @true: When flying a mission the vehicle is always facing towards the next waypoint virtual bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const; @@ -295,13 +297,11 @@ public: // FIXME: Hack workaround for non pluginize FollowMe support static const char* px4FollowMeFlightMode; - /// Used to add additional firmware-specific facts to the vehicle values widget - FactGroup* fwFactGroup = NULL; - protected: // Arms the vehicle with validation and retries // @return: true - vehicle armed, false - vehicle failed to arm bool _armVehicleAndValidate(Vehicle* vehicle); + QList _factGroups; // Sets the vehicle to the specified flight mode with validation and retries // @return: true - vehicle in specified flight mode, false - flight mode change failed diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index f28c1d558..0033a10a4 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -373,8 +373,9 @@ void Vehicle::_commonInit(void) _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName); // Add firmware-specific fact group, if provided - if (_firmwarePlugin->fwFactGroup) { - _addFactGroup(_firmwarePlugin->fwFactGroup, "FWPlugin"); + QList fwFactGroups = _firmwarePlugin->factGroups(); + for (int i = 0; i < fwFactGroups.count(); i++) { + _addFactGroup(fwFactGroups[i], QString("FWPlugin%1").arg(i)); } _flightDistanceFact.setRawValue(0); -- 2.22.0