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;
+    }
     }
 }
 
@@ -239,6 +247,7 @@ 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)
@@ -248,6 +257,7 @@ APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
     , _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);
@@ -255,6 +265,7 @@ APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
     _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());
@@ -263,6 +274,8 @@ APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
     _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

@@ -41,6 +41,7 @@ public:
     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; }
@@ -48,6 +49,7 @@ public:
     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"
 }
 ]