From 1b88e1427f3d86c5b2497ba61929cda57c0642b5 Mon Sep 17 00:00:00 2001 From: Willian Galvani 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 b13e66bd4..ead513c40 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::quiet_NaN()); _pilotGainFact.setRawValue (std::numeric_limits::quiet_NaN()); _inputHoldFact.setRawValue (std::numeric_limits::quiet_NaN()); + _rollPitchToggleFact.setRawValue (2); // 2 shows "Unavailable" in older firmwares _rangefinderDistanceFact.setRawValue (std::numeric_limits::quiet_NaN()); } diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h index f29c23305..ca744313a 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 2c5961dc9..be7f04c0e 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" } ] } -- 2.22.0