Merge pull request #5121 from bluerobotics/sub-vehicle-values

Add firmware-specific factgroups

qgroundcontrol.qrc

@@ -212,6 +212,7 @@
         <file alias="Vehicle/WindFact.json">src/Vehicle/WindFact.json</file>
         <file alias="Vehicle/VibrationFact.json">src/Vehicle/VibrationFact.json</file>
         <file alias="Vehicle/TemperatureFact.json">src/Vehicle/TemperatureFact.json</file>
+        <file alias="Vehicle/SubmarineFact.json">src/Vehicle/SubmarineFact.json</file>
     </qresource>
     <qresource prefix="/MockLink">
         <file alias="APMArduCopterMockLink.params">src/comm/APMArduCopterMockLink.params</file>

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -76,27 +76,27 @@ public:
     QList<MAV_CMD> supportedMissionCommands(void) final;
 
     AutoPilotPlugin*    autopilotPlugin                 (Vehicle* vehicle) final;
-    bool                isCapable                       (const Vehicle *vehicle, FirmwareCapabilities capabilities);
+    bool                isCapable                       (const Vehicle *vehicle, FirmwareCapabilities capabilities) override;
     QStringList         flightModes                     (Vehicle* vehicle) final;
     QString             flightMode                      (uint8_t base_mode, uint32_t custom_mode) const final;
     bool                setFlightMode                   (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final;
     bool                isGuidedMode                    (const Vehicle* vehicle) const final;
-    void                pauseVehicle                    (Vehicle* vehicle);
-    int                 manualControlReservedButtonCount(void);
-    bool                adjustIncomingMavlinkMessage    (Vehicle* vehicle, mavlink_message_t* message) final;
+    void                pauseVehicle                    (Vehicle* vehicle) override;
+    int                 manualControlReservedButtonCount(void) override;
+    bool                adjustIncomingMavlinkMessage    (Vehicle* vehicle, mavlink_message_t* message) override;
     void                adjustOutgoingMavlinkMessage    (Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) final;
     void                initializeVehicle               (Vehicle* vehicle) final;
     bool                sendHomePositionToVehicle       (void) final;
     void                addMetaDataToFact               (QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) final;
-    QString             missionCommandOverrides         (MAV_TYPE vehicleType) const;
+    QString             missionCommandOverrides         (MAV_TYPE vehicleType) const override;
     QString             getVersionParam                 (void) final { return QStringLiteral("SYSID_SW_MREV"); }
     QString             internalParameterMetaDataFile   (Vehicle* vehicle) final;
     void                getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
-    QObject*            loadParameterMetaData           (const QString& metaDataFile);
-    GeoFenceManager*    newGeoFenceManager              (Vehicle* vehicle) { return new APMGeoFenceManager(vehicle); }
-    RallyPointManager*  newRallyPointManager            (Vehicle* vehicle) { return new APMRallyPointManager(vehicle); }
-    QString             brandImageIndoor                (const Vehicle* vehicle) const { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
-    QString             brandImageOutdoor               (const Vehicle* vehicle) const { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
+    QObject*            loadParameterMetaData           (const QString& metaDataFile) final;
+    GeoFenceManager*    newGeoFenceManager              (Vehicle* vehicle) final { return new APMGeoFenceManager(vehicle); }
+    RallyPointManager*  newRallyPointManager            (Vehicle* vehicle) final { return new APMRallyPointManager(vehicle); }
+    QString             brandImageIndoor                (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
+    QString             brandImageOutdoor               (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
 
 protected:
     /// All access to singleton is through stack specific implementation

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<APMCustomMode> supportedFlightModes;
     supportedFlightModes << APMSubMode(APMSubMode::MANUAL ,true);
@@ -99,6 +100,8 @@ ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void)
 
         _remapParamNameIntialized = true;
     }
+
+    _nameToFactGroupMap.insert("APMSubInfo", &_infoFactGroup);
 }
 
 int ArduSubFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const
@@ -150,3 +153,69 @@ const QVariantList& ArduSubFirmwarePlugin::toolBarIndicators(const Vehicle* vehi
     }
     return _toolBarIndicators;
 }
+
+void ArduSubFirmwarePlugin::_handleNamedValueFloat(mavlink_message_t* message)
+{
+    mavlink_named_value_float_t value;
+    mavlink_msg_named_value_float_decode(message, &value);
+
+    QString name = QString(value.name);
+
+    if (name == "CamTilt") {
+        _infoFactGroup.getFact("camera tilt")->setRawValue(value.value * 100);
+    } else if (name == "TetherTrn") {
+        _infoFactGroup.getFact("tether turns")->setRawValue(value.value);
+    } else if (name == "Lights1") {
+        _infoFactGroup.getFact("lights 1")->setRawValue(value.value * 100);
+    } else if (name == "Lights2") {
+        _infoFactGroup.getFact("lights 2")->setRawValue(value.value * 100);
+    } else if (name == "PilotGain") {
+        _infoFactGroup.getFact("pilot gain")->setRawValue(value.value * 100);
+    }
+}
+
+void ArduSubFirmwarePlugin::_handleMavlinkMessage(mavlink_message_t* message)
+{
+    switch (message->msgid) {
+    case (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT):
+        _handleNamedValueFloat(message);
+    }
+}
+
+bool ArduSubFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
+{
+    _handleMavlinkMessage(message);
+    return APMFirmwarePlugin::adjustIncomingMavlinkMessage(vehicle, message);
+}
+
+QMap<QString, FactGroup*>* ArduSubFirmwarePlugin::factGroups(void) {
+    return &_nameToFactGroupMap;
+}
+
+const char* APMSubmarineFactGroup::_camTiltFactName = "camera tilt";
+const char* APMSubmarineFactGroup::_tetherTurnsFactName = "tether turns";
+const char* APMSubmarineFactGroup::_lightsLevel1FactName = "lights 1";
+const char* APMSubmarineFactGroup::_lightsLevel2FactName = "lights 2";
+const char* APMSubmarineFactGroup::_pilotGainFactName = "pilot gain";
+
+APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
+    : FactGroup(300, ":/json/Vehicle/SubmarineFact.json", parent)
+    , _camTiltFact       (0, _camTiltFactName,       FactMetaData::valueTypeDouble)
+    , _tetherTurnsFact   (0, _tetherTurnsFactName,   FactMetaData::valueTypeDouble)
+    , _lightsLevel1Fact  (0, _lightsLevel1FactName,  FactMetaData::valueTypeDouble)
+    , _lightsLevel2Fact  (0, _lightsLevel2FactName,  FactMetaData::valueTypeDouble)
+    , _pilotGainFact     (0, _pilotGainFactName,     FactMetaData::valueTypeDouble)
+{
+    _addFact(&_camTiltFact,       _camTiltFactName);
+    _addFact(&_tetherTurnsFact,   _tetherTurnsFactName);
+    _addFact(&_lightsLevel1Fact,  _lightsLevel1FactName);
+    _addFact(&_lightsLevel2Fact,  _lightsLevel2FactName);
+    _addFact(&_pilotGainFact,     _pilotGainFactName);
+
+    // Start out as not available "--.--"
+    _camTiltFact.setRawValue       (std::numeric_limits<float>::quiet_NaN());
+    _tetherTurnsFact.setRawValue   (std::numeric_limits<float>::quiet_NaN());
+    _lightsLevel1Fact.setRawValue  (std::numeric_limits<float>::quiet_NaN());
+    _lightsLevel2Fact.setRawValue  (std::numeric_limits<float>::quiet_NaN());
+    _pilotGainFact.setRawValue     (std::numeric_limits<float>::quiet_NaN());
+}

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
 {
@@ -86,11 +120,18 @@ public:
     const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
     int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
     const QVariantList& toolBarIndicators(const Vehicle* vehicle) final;
+    bool  adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
+    virtual QMap<QString, FactGroup*>* factGroups(void);
+
 
 private:
     QVariantList _toolBarIndicators;
     static bool _remapParamNameIntialized;
     static FirmwarePlugin::remapParamNameMajorVersionMap_t  _remapParamName;
-};
+    void _handleNamedValueFloat(mavlink_message_t* message);
+    void _handleMavlinkMessage(mavlink_message_t* message);
 
+    QMap<QString, FactGroup*> _nameToFactGroupMap;
+    APMSubmarineFactGroup _infoFactGroup;
+};
 #endif

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -414,6 +414,11 @@ const QVariantList& FirmwarePlugin::cameraList(const Vehicle* vehicle)
     return _cameraList;
 }
 
+QMap<QString, FactGroup*>* FirmwarePlugin::factGroups(void) {
+    // Generic plugin has no FactGroups
+    return NULL;
+}
+
 bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
 {
     return vehicle->multiRotor() ? false : true;

src/FirmwarePlugin/FirmwarePlugin.h

@@ -269,6 +269,9 @@ public:
     /// Returns a list of CameraMetaData objects for available cameras on the vehicle.
     virtual const QVariantList& cameraList(const Vehicle* vehicle);
 
+    /// Returns a pointer to a dictionary of firmware-specific FactGroups
+    virtual QMap<QString, FactGroup*>* factGroups(void);
+
     /// @true: When flying a mission the vehicle is always facing towards the next waypoint
     virtual bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const;
 
@@ -303,7 +306,6 @@ protected:
 private:
     QVariantList _toolBarIndicatorList;
     static QVariantList _cameraList;    ///< Standard QGC camera list
-
 };
 
 class FirmwarePluginFactory : public QObject

src/FlightMap/Widgets/ValuesWidget.qml

@@ -65,11 +65,18 @@ QGCFlickable {
 
         Repeater {
             model: _activeVehicle ? controller.largeValues : 0
+            Loader {
+                sourceComponent: fact ? largeValue : undefined
+                property Fact fact: _activeVehicle.getFact(modelData.replace("Vehicle.", ""))
+            }
+        } // Repeater - Large
+    } // Column - Large
+
+    Component {
+        id: largeValue
 
         Column {
             width:  _largeColumn.width
-
-                property Fact fact: _activeVehicle.getFact(modelData.replace("Vehicle.", ""))
             property bool largeValue: _root.listContains(controller.altitudeProperties, fact.name)
 
             QGCLabel {
@@ -89,8 +96,7 @@ QGCFlickable {
                 text:                   fact.valueString
             }
         }
-        } // Repeater - Large
-    } // Column - Large
+    }
 
     Flow {
         id:                 _smallFlow
@@ -102,13 +108,20 @@ QGCFlickable {
 
         Repeater {
             model: _activeVehicle ? controller.smallValues : 0
+            Loader {
+                sourceComponent: fact ? smallValue : undefined
+                property Fact fact: _activeVehicle.getFact(modelData.replace("Vehicle.", ""))
+            }
+        } // Repeater - Small
+    } // Flow
+
+    Component {
+        id: smallValue
 
         Column {
             width:  (_root.width / 2) - (_margins / 2) - 0.1
             clip:   true
 
-                property Fact fact: _activeVehicle.getFact(modelData.replace("Vehicle.", ""))
-
             QGCLabel {
                 width:                  parent.width
                 horizontalAlignment:    Text.AlignHCenter
@@ -133,8 +146,7 @@ QGCFlickable {
                 text:                   fact.units
             }
         }
-        } // Repeater - Small
-    } // Flow
+    }
 
     Component {
         id: propertyPicker

src/Vehicle/SubmarineFact.json

+[
+{
+    "name":             "camera tilt",
+    "shortDescription": "Camera Tilt",
+    "type":             "int16",
+    "units":            "%"
+},
+{
+    "name":             "tether turns",
+    "shortDescription": "Tether Turns",
+    "type":             "int16",
+    "units":            "clockwise"
+},
+{
+    "name":             "lights 1",
+    "shortDescription": "Lights 1 level",
+    "type":             "int16",
+    "units":            "%"
+},
+{
+    "name":             "lights 2",
+    "shortDescription": "Lights 2 level",
+    "type":             "int16",
+    "units":            "%"
+},
+{
+    "name":             "pilot gain",
+    "shortDescription": "Pilot Gain",
+    "type":             "int16",
+    "units":            "%"
+}
+]

src/Vehicle/Vehicle.cc

@@ -372,6 +372,16 @@ void Vehicle::_commonInit(void)
     _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName);
     _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName);
 
+    // Add firmware-specific fact groups, if provided
+    QMap<QString, FactGroup*>* fwFactGroups = _firmwarePlugin->factGroups();
+    if (fwFactGroups) {
+        QMapIterator<QString, FactGroup*> i(*fwFactGroups);
+        while(i.hasNext()) {
+            i.next();
+            _addFactGroup(i.value(), i.key());
+        }
+    }
+
     _flightDistanceFact.setRawValue(0);
     _flightTimeFact.setRawValue(0);
 }