From 1b88e1427f3d86c5b2497ba61929cda57c0642b5 Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Thu, 20 Feb 2020 15:26:45 -0300
Subject: [PATCH] Sub: Add fact for roll/pitch toggle
This will allow sub users to see if the ROV is in the Roll/pitch toggle
mode using the APMSubInfo section in the info widget
---
src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc | 6 ++++++
src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h | 2 ++
src/Vehicle/SubmarineFact.json | 6 ++++++
3 files changed, 14 insertions(+)
diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc
index b13e66b..ead513c 100644
--- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc
@@ -252,6 +252,8 @@ void ArduSubFirmwarePlugin::_handleNamedValueFloat(mavlink_message_t* message)
_infoFactGroup.getFact("pilot gain")->setRawValue(value.value * 100);
} else if (name == "InputHold") {
_infoFactGroup.getFact("input hold")->setRawValue(value.value);
+ } else if (name == "RollPitch") {
+ _infoFactGroup.getFact("roll pitch toggle")->setRawValue(value.value);
}
}
@@ -287,6 +289,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::_rollPitchToggleFactName = "roll pitch toggle";
const char* APMSubmarineFactGroup::_rangefinderDistanceFactName = "rangefinder distance";
APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
@@ -297,6 +300,7 @@ APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
, _lightsLevel2Fact (0, _lightsLevel2FactName, FactMetaData::valueTypeDouble)
, _pilotGainFact (0, _pilotGainFactName, FactMetaData::valueTypeDouble)
, _inputHoldFact (0, _inputHoldFactName, FactMetaData::valueTypeDouble)
+ , _rollPitchToggleFact (0, _rollPitchToggleFactName, FactMetaData::valueTypeDouble)
, _rangefinderDistanceFact (0, _rangefinderDistanceFactName, FactMetaData::valueTypeDouble)
{
_addFact(&_camTiltFact, _camTiltFactName);
@@ -305,6 +309,7 @@ APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
_addFact(&_lightsLevel2Fact, _lightsLevel2FactName);
_addFact(&_pilotGainFact, _pilotGainFactName);
_addFact(&_inputHoldFact, _inputHoldFactName);
+ _addFact(&_rollPitchToggleFact , _rollPitchToggleFactName);
_addFact(&_rangefinderDistanceFact, _rangefinderDistanceFactName);
// Start out as not available "--.--"
@@ -314,6 +319,7 @@ 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());
+ _rollPitchToggleFact.setRawValue (2); // 2 shows "Unavailable" in older firmwares
_rangefinderDistanceFact.setRawValue (std::numeric_limits<float>::quiet_NaN());
}
diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h
index f29c233..ca74431 100644
--- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h
@@ -57,6 +57,7 @@ public:
static const char* _lightsLevel2FactName;
static const char* _pilotGainFactName;
static const char* _inputHoldFactName;
+ static const char* _rollPitchToggleFactName;
static const char* _rangefinderDistanceFactName;
static const char* _settingsGroup;
@@ -68,6 +69,7 @@ private:
Fact _lightsLevel2Fact;
Fact _pilotGainFact;
Fact _inputHoldFact;
+ Fact _rollPitchToggleFact;
Fact _rangefinderDistanceFact;
};
diff --git a/src/Vehicle/SubmarineFact.json b/src/Vehicle/SubmarineFact.json
index 2c5961d..be7f04c 100644
--- a/src/Vehicle/SubmarineFact.json
+++ b/src/Vehicle/SubmarineFact.json
@@ -45,6 +45,12 @@
"type": "float",
"decimalPlaces": 2,
"units": "meters"
+},
+{ "name": "roll pitch toggle",
+ "shortDescription": "Roll/Pitch Toggle",
+ "type": "int16",
+ "enumStrings": "Disabled,Enabled,Unavailable",
+ "enumValues": "0,1,2"
}
]
}