Commit a148e3e9 authored by Jacob Walser's avatar Jacob Walser

Use QList<FactGroup*> to expose firmwareplugin factgroups

parent eba10f0c
...@@ -40,7 +40,8 @@ APMSubMode::APMSubMode(uint32_t mode, bool settable) : ...@@ -40,7 +40,8 @@ APMSubMode::APMSubMode(uint32_t mode, bool settable) :
setEnumToStringMapping(enumToString); setEnumToStringMapping(enumToString);
} }
ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void) ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void):
_infoFactGroup(this)
{ {
QList<APMCustomMode> supportedFlightModes; QList<APMCustomMode> supportedFlightModes;
supportedFlightModes << APMSubMode(APMSubMode::MANUAL ,true); supportedFlightModes << APMSubMode(APMSubMode::MANUAL ,true);
...@@ -100,7 +101,7 @@ ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void) ...@@ -100,7 +101,7 @@ ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void)
_remapParamNameIntialized = true; _remapParamNameIntialized = true;
} }
fwFactGroup = new APMSubmarineFactGroup(this); _factGroups.append(&_infoFactGroup);
} }
int ArduSubFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const int ArduSubFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const
...@@ -166,15 +167,15 @@ void ArduSubFirmwarePlugin::_handleNamedValueFloat(mavlink_message_t* message) ...@@ -166,15 +167,15 @@ void ArduSubFirmwarePlugin::_handleNamedValueFloat(mavlink_message_t* message)
QString name = QString(value.name); QString name = QString(value.name);
if (name == "CamTilt") { if (name == "CamTilt") {
fwFactGroup->getFact("camera tilt")->setRawValue(value.value * 100); _infoFactGroup.getFact("camera tilt")->setRawValue(value.value * 100);
} else if (name == "TetherTrn") { } else if (name == "TetherTrn") {
fwFactGroup->getFact("tether turns")->setRawValue(value.value); _infoFactGroup.getFact("tether turns")->setRawValue(value.value);
} else if (name == "Lights1") { } else if (name == "Lights1") {
fwFactGroup->getFact("lights 1")->setRawValue(value.value * 100); _infoFactGroup.getFact("lights 1")->setRawValue(value.value * 100);
} else if (name == "Lights2") { } else if (name == "Lights2") {
fwFactGroup->getFact("lights 2")->setRawValue(value.value * 100); _infoFactGroup.getFact("lights 2")->setRawValue(value.value * 100);
} else if (name == "PilotGain") { } else if (name == "PilotGain") {
fwFactGroup->getFact("pilot gain")->setRawValue(value.value * 100); _infoFactGroup.getFact("pilot gain")->setRawValue(value.value * 100);
} }
} }
......
...@@ -28,6 +28,40 @@ ...@@ -28,6 +28,40 @@
#define ArduSubFirmwarePlugin_H #define ArduSubFirmwarePlugin_H
#include "APMFirmwarePlugin.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 class APMSubMode : public APMCustomMode
{ {
...@@ -97,40 +131,7 @@ private: ...@@ -97,40 +131,7 @@ private:
static FirmwarePlugin::remapParamNameMajorVersionMap_t _remapParamName; static FirmwarePlugin::remapParamNameMajorVersionMap_t _remapParamName;
void _handleNamedValueFloat(mavlink_message_t* message); void _handleNamedValueFloat(mavlink_message_t* message);
void _handleMavlinkMessage(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; APMSubmarineFactGroup _infoFactGroup;
private:
Fact _camTiltFact;
Fact _tetherTurnsFact;
Fact _lightsLevel1Fact;
Fact _lightsLevel2Fact;
Fact _pilotGainFact;
}; };
#endif #endif
...@@ -419,6 +419,10 @@ const QVariantList& FirmwarePlugin::cameraList(const Vehicle* vehicle) ...@@ -419,6 +419,10 @@ const QVariantList& FirmwarePlugin::cameraList(const Vehicle* vehicle)
return _cameraList; return _cameraList;
} }
QList<FactGroup*> FirmwarePlugin::factGroups(void) {
return _factGroups;
}
bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
{ {
return vehicle->multiRotor() ? false : true; return vehicle->multiRotor() ? false : true;
......
...@@ -273,6 +273,8 @@ public: ...@@ -273,6 +273,8 @@ public:
/// Returns a list of CameraMetaData objects for available cameras on the vehicle. /// Returns a list of CameraMetaData objects for available cameras on the vehicle.
virtual const QVariantList& cameraList(const Vehicle* vehicle); virtual const QVariantList& cameraList(const Vehicle* vehicle);
virtual QList<FactGroup*> factGroups(void);
/// @true: When flying a mission the vehicle is always facing towards the next waypoint /// @true: When flying a mission the vehicle is always facing towards the next waypoint
virtual bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const; virtual bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const;
...@@ -295,13 +297,11 @@ public: ...@@ -295,13 +297,11 @@ public:
// FIXME: Hack workaround for non pluginize FollowMe support // FIXME: Hack workaround for non pluginize FollowMe support
static const char* px4FollowMeFlightMode; static const char* px4FollowMeFlightMode;
/// Used to add additional firmware-specific facts to the vehicle values widget
FactGroup* fwFactGroup = NULL;
protected: protected:
// Arms the vehicle with validation and retries // Arms the vehicle with validation and retries
// @return: true - vehicle armed, false - vehicle failed to arm // @return: true - vehicle armed, false - vehicle failed to arm
bool _armVehicleAndValidate(Vehicle* vehicle); bool _armVehicleAndValidate(Vehicle* vehicle);
QList<FactGroup*> _factGroups;
// Sets the vehicle to the specified flight mode with validation and retries // Sets the vehicle to the specified flight mode with validation and retries
// @return: true - vehicle in specified flight mode, false - flight mode change failed // @return: true - vehicle in specified flight mode, false - flight mode change failed
......
...@@ -373,8 +373,9 @@ void Vehicle::_commonInit(void) ...@@ -373,8 +373,9 @@ void Vehicle::_commonInit(void)
_addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName); _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName);
// Add firmware-specific fact group, if provided // Add firmware-specific fact group, if provided
if (_firmwarePlugin->fwFactGroup) { QList<FactGroup*> fwFactGroups = _firmwarePlugin->factGroups();
_addFactGroup(_firmwarePlugin->fwFactGroup, "FWPlugin"); for (int i = 0; i < fwFactGroups.count(); i++) {
_addFactGroup(fwFactGroups[i], QString("FWPlugin%1").arg(i));
} }
_flightDistanceFact.setRawValue(0); _flightDistanceFact.setRawValue(0);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment