Add rangefinder distance fact for sub vehicles

src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc

@@ -220,6 +220,14 @@ void ArduSubFirmwarePlugin::_handleMavlinkMessage(mavlink_message_t* message)
     switch (message->msgid) {
     case (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT):
         _handleNamedValueFloat(message);
+        break;
+    case (MAVLINK_MSG_ID_RANGEFINDER):
+    {
+        mavlink_rangefinder_t msg;
+        mavlink_msg_rangefinder_decode(message, &msg);
+        _infoFactGroup.getFact("rangefinder distance")->setRawValue(msg.distance);
+        break;
+    }
     }
 }
 
@@ -233,36 +241,41 @@ 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";
-const char* APMSubmarineFactGroup::_inputHoldFactName = "input hold";
+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";
+const char* APMSubmarineFactGroup::_inputHoldFactName           = "input hold";
+const char* APMSubmarineFactGroup::_rangefinderDistanceFactName = "rangefinder distance";
 
 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)
-    , _inputHoldFact     (0, _inputHoldFactName,     FactMetaData::valueTypeDouble)
+    , _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)
+    , _inputHoldFact           (0, _inputHoldFactName,           FactMetaData::valueTypeDouble)
+    , _rangefinderDistanceFact (0, _rangefinderDistanceFactName, FactMetaData::valueTypeDouble)
 {
-    _addFact(&_camTiltFact,       _camTiltFactName);
-    _addFact(&_tetherTurnsFact,   _tetherTurnsFactName);
-    _addFact(&_lightsLevel1Fact,  _lightsLevel1FactName);
-    _addFact(&_lightsLevel2Fact,  _lightsLevel2FactName);
-    _addFact(&_pilotGainFact,     _pilotGainFactName);
-    _addFact(&_inputHoldFact,     _inputHoldFactName);
+    _addFact(&_camTiltFact,             _camTiltFactName);
+    _addFact(&_tetherTurnsFact,         _tetherTurnsFactName);
+    _addFact(&_lightsLevel1Fact,        _lightsLevel1FactName);
+    _addFact(&_lightsLevel2Fact,        _lightsLevel2FactName);
+    _addFact(&_pilotGainFact,           _pilotGainFactName);
+    _addFact(&_inputHoldFact,           _inputHoldFactName);
+    _addFact(&_rangefinderDistanceFact, _rangefinderDistanceFactName);
 
     // 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());
-    _inputHoldFact.setRawValue     (std::numeric_limits<float>::quiet_NaN());
+    _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());
+    _inputHoldFact.setRawValue           (std::numeric_limits<float>::quiet_NaN());
+    _rangefinderDistanceFact.setRawValue (std::numeric_limits<float>::quiet_NaN());
+
 }
 
 QString ArduSubFirmwarePlugin::vehicleImageOpaque(const Vehicle* vehicle) const

src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h

@@ -35,19 +35,21 @@ class APMSubmarineFactGroup : public FactGroup
 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)
-    Q_PROPERTY(Fact* inputHold     READ inputHold     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; }
-    Fact* inputHold     (void) { return &_inputHoldFact; }
+    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)
+    Q_PROPERTY(Fact* inputHold           READ inputHold     CONSTANT)
+    Q_PROPERTY(Fact* rangefinderDistance READ rangefinderDistance 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; }
+    Fact* inputHold           (void) { return &_inputHoldFact; }
+    Fact* rangefinderDistance (void) { return &_rangefinderDistanceFact; }
 
     static const char* _camTiltFactName;
     static const char* _tetherTurnsFactName;
@@ -55,6 +57,7 @@ public:
     static const char* _lightsLevel2FactName;
     static const char* _pilotGainFactName;
     static const char* _inputHoldFactName;
+    static const char* _rangefinderDistanceFactName;
 
     static const char* _settingsGroup;
 
@@ -65,6 +68,7 @@ private:
     Fact            _lightsLevel2Fact;
     Fact            _pilotGainFact;
     Fact            _inputHoldFact;
+    Fact            _rangefinderDistanceFact;
 };
 
 class APMSubMode : public APMCustomMode

src/Vehicle/SubmarineFact.json

@@ -35,5 +35,11 @@
     "type":             "int16",
     "enumStrings":      "Disabled,Enabled",
     "enumValues":       "0,1"
+},
+{    "name":             "rangefinder distance",
+    "shortDescription": "Rangefinder",
+    "type":             "float",
+    "decimalPlaces":    2,
+    "units":            "meters"
 }
 ]