Add support for surftrak mode

mavlink · Apr 8, 2024 · a8e230f · a8e230f
1 parent 7ead1de
commit a8e230f
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 0 deletions.
diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc
@@ -43,6 +43,7 @@ APMSubMode::APMSubMode(uint32_t mode, bool settable) :
         {SURFACE, "Surface"},
         {POSHOLD, "Position Hold"},
         {MOTORDETECTION, "Motor Detection"},
+        {SURFTRAK, "Surftrak"},
     });
 }
 
@@ -60,6 +61,7 @@ ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void):
         APMSubMode(APMSubMode::SURFACE ,false),
         APMSubMode(APMSubMode::POSHOLD ,true),
         APMSubMode(APMSubMode::MOTORDETECTION, false),
+        APMSubMode(APMSubMode::SURFTRAK, true),
     });
 
     if (!_remapParamNameIntialized) {
@@ -236,6 +238,8 @@ void ArduSubFirmwarePlugin::_handleNamedValueFloat(mavlink_message_t* message)
         _infoFactGroup.getFact("inputHold")->setRawValue(value.value);
     } else if (name == "RollPitch") {
         _infoFactGroup.getFact("rollPitchToggle")->setRawValue(value.value);
+    } else if (name == "RFTarget") {
+      _infoFactGroup.getFact("rangefinderTarget")->setRawValue(value.value);
     }
 }
 
@@ -273,6 +277,7 @@ const char* APMSubmarineFactGroup::_pilotGainFactName           = "pilotGain";
 const char* APMSubmarineFactGroup::_inputHoldFactName           = "inputHold";
 const char* APMSubmarineFactGroup::_rollPitchToggleFactName     = "rollPitchToggle";
 const char* APMSubmarineFactGroup::_rangefinderDistanceFactName = "rangefinderDistance";
+const char* APMSubmarineFactGroup::_rangefinderTargetFactName   = "rangefinderTarget";
 
 APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
     : FactGroup(300, ":/json/Vehicle/SubmarineFact.json", parent)
@@ -284,6 +289,7 @@ APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
     , _inputHoldFact           (0, _inputHoldFactName,           FactMetaData::valueTypeDouble)
     , _rollPitchToggleFact     (0, _rollPitchToggleFactName,     FactMetaData::valueTypeDouble)
     , _rangefinderDistanceFact (0, _rangefinderDistanceFactName, FactMetaData::valueTypeDouble)
+    , _rangefinderTargetFact   (0, _rangefinderTargetFactName,   FactMetaData::valueTypeDouble)
 {
     _addFact(&_camTiltFact,             _camTiltFactName);
     _addFact(&_tetherTurnsFact,         _tetherTurnsFactName);
@@ -293,6 +299,7 @@ APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
     _addFact(&_inputHoldFact,           _inputHoldFactName);
     _addFact(&_rollPitchToggleFact    , _rollPitchToggleFactName);
     _addFact(&_rangefinderDistanceFact, _rangefinderDistanceFactName);
+    _addFact(&_rangefinderTargetFact,   _rangefinderTargetFactName);
 
     // Start out as not available "--.--"
     _camTiltFact.setRawValue             (std::numeric_limits<float>::quiet_NaN());
@@ -303,6 +310,7 @@ APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
     _inputHoldFact.setRawValue           (std::numeric_limits<float>::quiet_NaN());
     _rollPitchToggleFact.setRawValue     (2); // 2 shows "Unavailable" in older firmwares
     _rangefinderDistanceFact.setRawValue (std::numeric_limits<float>::quiet_NaN());
+    _rangefinderTargetFact.setRawValue   (std::numeric_limits<float>::quiet_NaN());
 
 }
 

diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h
@@ -42,6 +42,7 @@ class APMSubmarineFactGroup : public FactGroup
     Q_PROPERTY(Fact* pilotGain           READ pilotGain           CONSTANT)
     Q_PROPERTY(Fact* inputHold           READ inputHold     CONSTANT)
     Q_PROPERTY(Fact* rangefinderDistance READ rangefinderDistance CONSTANT)
+    Q_PROPERTY(Fact* rangefinderTarget   READ rangefinderTarget CONSTANT)
 
     Fact* camTilt             (void) { return &_camTiltFact; }
     Fact* tetherTurns         (void) { return &_tetherTurnsFact; }
@@ -50,6 +51,7 @@ class APMSubmarineFactGroup : public FactGroup
     Fact* pilotGain           (void) { return &_pilotGainFact; }
     Fact* inputHold           (void) { return &_inputHoldFact; }
     Fact* rangefinderDistance (void) { return &_rangefinderDistanceFact; }
+    Fact* rangefinderTarget   (void) { return &_rangefinderTargetFact; }
 
     static const char* _camTiltFactName;
     static const char* _tetherTurnsFactName;
@@ -59,6 +61,7 @@ class APMSubmarineFactGroup : public FactGroup
     static const char* _inputHoldFactName;
     static const char* _rollPitchToggleFactName;
     static const char* _rangefinderDistanceFactName;
+    static const char* _rangefinderTargetFactName;
 
     static const char* _settingsGroup;
 
@@ -71,6 +74,7 @@ class APMSubmarineFactGroup : public FactGroup
     Fact            _inputHoldFact;
     Fact            _rollPitchToggleFact;
     Fact            _rangefinderDistanceFact;
+    Fact            _rangefinderTargetFact;
 };
 
 class APMSubMode : public APMCustomMode
@@ -98,6 +102,7 @@ class APMSubMode : public APMCustomMode
         RESERVED_18       = 18,
         MANUAL            = 19,
         MOTORDETECTION    = 20,
+        SURFTRAK          = 21,  // Surface (seafloor) tracking, aka hold range
     };
 
     APMSubMode(uint32_t mode, bool settable);

diff --git a/src/Vehicle/SubmarineFact.json b/src/Vehicle/SubmarineFact.json
@@ -46,6 +46,12 @@
     "decimalPlaces":    2,
     "units":            "m"
 },
+{    "name":            "rangefinderTarget",
+    "shortDesc": "RFTarget",
+    "type":             "float",
+    "decimalPlaces":    2,
+    "units":            "m"
+},
 {   "name":             "rollPitchToggle",
     "shortDesc": "Roll/Pitch Toggle",
     "type":             "int16",