Dynamically allocate Submarine fact group

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/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":            "%"
+}
+]

src/Vehicle/Vehicle.cc

@@ -59,6 +59,7 @@ const char* Vehicle::_batteryFactGroupName =    "battery";
 const char* Vehicle::_windFactGroupName =       "wind";
 const char* Vehicle::_vibrationFactGroupName =  "vibration";
 const char* Vehicle::_temperatureFactGroupName = "temperature";
+const char* Vehicle::_submarineFactGroupName = "sub";
 
 Vehicle::Vehicle(LinkInterface*             link,
                  int                        vehicleId,
@@ -372,6 +373,11 @@ void Vehicle::_commonInit(void)
     _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName);
     _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName);
 
+    if (_vehicleType == MAV_TYPE_SUBMARINE) {
+        _submarineFactGroup = new VehicleSubmarineFactGroup(this);
+        _addFactGroup(_submarineFactGroup, _submarineFactGroupName);
+    }
+
     _flightDistanceFact.setRawValue(0);
     _flightTimeFact.setRawValue(0);
 }
@@ -597,6 +603,10 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_WIND:
         _handleWind(message);
         break;
+
+    case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
+        _handleNamedValueFloat(message);
+        break;
     }
 
     emit mavlinkMessageReceived(message);
@@ -1164,6 +1174,27 @@ void Vehicle::_handleScaledPressure3(mavlink_message_t& message) {
     _temperatureFactGroup.temperature3()->setRawValue(pressure.temperature / 100.0);
 }
 
+void Vehicle::_handleNamedValueFloat(mavlink_message_t &message) {
+    if (!_submarineFactGroup) {
+        return;
+    }
+
+    mavlink_named_value_float_t value;
+    mavlink_msg_named_value_float_decode(&message, &value);
+
+    QString name = QString(value.name);
+
+    if (name == "CamTilt") {
+        _submarineFactGroup->camTilt()->setRawValue(value.value * 100);
+    } else if (name == "TetherTrn") {
+        _submarineFactGroup->tetherTurns()->setRawValue(value.value);
+    } else if (name == "Lights1") {
+        _submarineFactGroup->lightsLevel1()->setRawValue(value.value * 100);
+    } else if (name == "Lights2") {
+        _submarineFactGroup->lightsLevel2()->setRawValue(value.value * 100);
+    }
+}
+
 bool Vehicle::_containsLink(LinkInterface* link)
 {
     return _links.contains(link);
@@ -2712,3 +2743,27 @@ VehicleTemperatureFactGroup::VehicleTemperatureFactGroup(QObject* parent)
     _temperature2Fact.setRawValue      (std::numeric_limits<float>::quiet_NaN());
     _temperature3Fact.setRawValue      (std::numeric_limits<float>::quiet_NaN());
 }
+
+const char* VehicleSubmarineFactGroup::_camTiltFactName = "camera tilt";
+const char* VehicleSubmarineFactGroup::_tetherTurnsFactName = "tether turns";
+const char* VehicleSubmarineFactGroup::_lightsLevel1FactName = "lights 1";
+const char* VehicleSubmarineFactGroup::_lightsLevel2FactName = "lights 2";
+
+VehicleSubmarineFactGroup::VehicleSubmarineFactGroup(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)
+{
+    _addFact(&_camTiltFact,       _camTiltFactName);
+    _addFact(&_tetherTurnsFact,   _tetherTurnsFactName);
+    _addFact(&_lightsLevel1Fact,  _lightsLevel1FactName);
+    _addFact(&_lightsLevel2Fact,  _lightsLevel2FactName);
+
+    // 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());
+}

src/Vehicle/Vehicle.h

@@ -214,6 +214,37 @@ private:
     Fact            _temperature3Fact;
 };
 
+class VehicleSubmarineFactGroup : public FactGroup
+{
+    Q_OBJECT
+
+public:
+    VehicleSubmarineFactGroup(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)
+
+    Fact* camTilt       (void) { return &_camTiltFact; }
+    Fact* tetherTurns   (void) { return &_tetherTurnsFact; }
+    Fact* lightsLevel1  (void) { return &_lightsLevel1Fact; }
+    Fact* lightsLevel2  (void) { return &_lightsLevel2Fact; }
+
+    static const char* _camTiltFactName;
+    static const char* _tetherTurnsFactName;
+    static const char* _lightsLevel1FactName;
+    static const char* _lightsLevel2FactName;
+
+    static const char* _settingsGroup;
+
+private:
+    Fact            _camTiltFact;
+    Fact            _tetherTurnsFact;
+    Fact            _lightsLevel1Fact;
+    Fact            _lightsLevel2Fact;
+};
+
 class Vehicle : public FactGroup
 {
     Q_OBJECT
@@ -350,6 +381,7 @@ public:
     Q_PROPERTY(FactGroup* wind        READ windFactGroup        CONSTANT)
     Q_PROPERTY(FactGroup* vibration   READ vibrationFactGroup   CONSTANT)
     Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT)
+    Q_PROPERTY(FactGroup* submarineFactGroup   READ submarineFactGroup   CONSTANT)
 
     Q_PROPERTY(int      firmwareMajorVersion        READ firmwareMajorVersion       NOTIFY firmwareVersionChanged)
     Q_PROPERTY(int      firmwareMinorVersion        READ firmwareMinorVersion       NOTIFY firmwareVersionChanged)
@@ -612,6 +644,7 @@ public:
     FactGroup* windFactGroup        (void) { return &_windFactGroup; }
     FactGroup* vibrationFactGroup   (void) { return &_vibrationFactGroup; }
     FactGroup* temperatureFactGroup (void) { return &_temperatureFactGroup; }
+    FactGroup* submarineFactGroup   (void) { return _submarineFactGroup; }
 
     void setConnectionLostEnabled(bool connectionLostEnabled);
 
@@ -832,6 +865,7 @@ private:
     void _handleScaledPressure(mavlink_message_t& message);
     void _handleScaledPressure2(mavlink_message_t& message);
     void _handleScaledPressure3(mavlink_message_t& message);
+    void _handleNamedValueFloat(mavlink_message_t& message);
     void _handleCameraFeedback(const mavlink_message_t& message);
     void _handleCameraImageCaptured(const mavlink_message_t& message);
     void _missionManagerError(int errorCode, const QString& errorMsg);
@@ -1019,6 +1053,7 @@ private:
     VehicleWindFactGroup        _windFactGroup;
     VehicleVibrationFactGroup   _vibrationFactGroup;
     VehicleTemperatureFactGroup _temperatureFactGroup;
+    VehicleSubmarineFactGroup*  _submarineFactGroup;
 
     static const char* _rollFactName;
     static const char* _pitchFactName;
@@ -1036,6 +1071,7 @@ private:
     static const char* _windFactGroupName;
     static const char* _vibrationFactGroupName;
     static const char* _temperatureFactGroupName;
+    static const char* _submarineFactGroupName;
 
     static const int _vehicleUIUpdateRateMSecs = 100;