Servo controll v2

custom-example/qgroundcontrol.qrc

@@ -10,6 +10,8 @@
 		<file alias="JoystickIndicator.qml">../src/ui/toolbar/JoystickIndicator.qml</file>
 		<file alias="DayLightActions.qml">../src/ui/toolbar/DayLightActions.qml</file>
 		<file alias="NightLightActions.qml">../src/ui/toolbar/NightLightActions.qml</file>
+		<file alias="RebActions.qml">../src/ui/toolbar/RebActions.qml</file>
+		<file alias="UniversalActions.qml">../src/ui/toolbar/UniversalActions.qml</file>
 		<file alias="LinkIndicator.qml">../src/ui/toolbar/LinkIndicator.qml</file>
 		<file alias="MainToolBarIndicators.qml">../src/ui/toolbar/MainToolBarIndicators.qml</file>
 		<file alias="MessageIndicator.qml">../src/ui/toolbar/MessageIndicator.qml</file>

qgcimages.qrc

@@ -103,6 +103,8 @@
         <file alias="Joystick.png">src/ui/toolbar/Images/Joystick.png</file>
         <file alias="SimpleLight.png">src/ui/toolbar/Images/SimpleLight.png</file>
         <file alias="NightLight.png">src/ui/toolbar/Images/NightLight.png</file>
+        <file alias="Reb.png">src/ui/toolbar/Images/Reb.png</file>
+        <file alias="SimpleDot.png">src/ui/toolbar/Images/SimpleDot.png</file>
         <file alias="LandMode.svg">src/AutoPilotPlugins/PX4/Images/LandMode.svg</file>
         <file alias="LandModeCopter.svg">src/AutoPilotPlugins/PX4/Images/LandModeCopter.svg</file>
         <file alias="LightsComponentIcon.png">src/AutoPilotPlugins/APM/Images/LightsComponentIcon.png</file>

qgroundcontrol.qrc

@@ -10,6 +10,8 @@
         <file alias="JoystickIndicator.qml">src/ui/toolbar/JoystickIndicator.qml</file>
 		<file alias="DayLightActions.qml">src/ui/toolbar/DayLightActions.qml</file>
 		<file alias="NightLightActions.qml">src/ui/toolbar/NightLightActions.qml</file>
+        <file alias="RebActions.qml">src/ui/toolbar/RebActions.qml</file>
+		<file alias="UniversalActions.qml">src/ui/toolbar/UniversalActions.qml</file>
         <file alias="LinkIndicator.qml">src/ui/toolbar/LinkIndicator.qml</file>
         <file alias="MainToolBarIndicators.qml">src/ui/toolbar/MainToolBarIndicators.qml</file>
         <file alias="MessageIndicator.qml">src/ui/toolbar/MessageIndicator.qml</file>

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -336,6 +336,8 @@ const QVariantList& FirmwarePlugin::toolIndicators(const Vehicle*)
             QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/RemoteIDIndicator.qml")),
             QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/DayLightActions.qml")),
             QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/NightLightActions.qml")),
+            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/RebActions.qml")),
+            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/UniversalActions.qml")),
         });
     }
     return _toolIndicatorList;

src/FlightDisplay/GuidedActionsController.qml

@@ -114,6 +114,8 @@ Item {
     readonly property int actionJoystickChangeStatus:       28
     readonly property int actionDayLight:                   29
     readonly property int actionNightLight:                 30
+    readonly property int actionReb:                        31
+    readonly property int actionUniversal:                  32
 
     property var    _activeVehicle:             QGroundControl.multiVehicleManager.activeVehicle
     property bool   _useChecklist:              QGroundControl.settingsManager.appSettings.useChecklist.rawValue && QGroundControl.corePlugin.options.preFlightChecklistUrl.toString().length
@@ -627,6 +629,12 @@ Item {
         case actionNightLight:
             _activeVehicle.setNightLightEnabled(actionData)
             break
+        case actionReb:
+             _activeVehicle.setRebEnabled(actionData)
+             break
+        case actionUniversal:
+             _activeVehicle.setUniversalEnabled(actionData)
+             break
         default:
             console.warn(qsTr("Internal error: unknown actionCode"), actionCode)
             break

src/Servo/ServoControl.cc

@@ -17,6 +17,8 @@
 
 #define DAY_LIGHT_SERVO   11
 #define NIGHT_LIGHT_SERVO 3
+#define REB_SERVO 10
+#define UNIVERSAL_SERVO 7
 
 
 ServoControl::ServoControl(QObject *parent, Vehicle *vehicle)
@@ -40,6 +42,22 @@ void ServoControl::setNightLightEnabled(bool enabled, int _defaultComponentId) {
     }
 };
 
+void ServoControl::setRebEnabled(bool enabled, int _defaultComponentId) {
+    if (!enabled) {
+        rebDisable(_defaultComponentId);
+    } else {
+        rebEnable(_defaultComponentId);
+    }
+};
+
+void ServoControl::setUniversalEnabled(bool enabled, int _defaultComponentId) {
+    if (!enabled) {
+        universalDisable(_defaultComponentId);
+    } else {
+        universalEnable(_defaultComponentId);
+    }
+};
+
 // ----------------------------------------------------------------------------
 void ServoControl::dayLightEnable(int _defaultComponentId) {
     // _defaultComponentId
@@ -87,3 +105,51 @@ void ServoControl::nightLightDisable(int _defaultComponentId) {
         SERVO_PWM_OFF
     );                
 };
+
+// ----------------------------------------------------------------------------
+void ServoControl::rebEnable(int _defaultComponentId) {
+    // _defaultComponentId
+    _vehicle->sendCommand(
+        _defaultComponentId,
+        MAV_CMD_DO_SET_SERVO,
+        true,
+        REB_SERVO,
+        SERVO_PWM_ON
+    );                
+};
+
+// ----------------------------------------------------------------------------
+void ServoControl::rebDisable(int _defaultComponentId) {
+    // _defaultComponentId
+    _vehicle->sendCommand(
+        _defaultComponentId,
+        MAV_CMD_DO_SET_SERVO,
+        true,
+        REB_SERVO,
+        SERVO_PWM_OFF
+    );                
+};
+
+// ----------------------------------------------------------------------------
+void ServoControl::universalEnable(int _defaultComponentId) {
+    // _defaultComponentId
+    _vehicle->sendCommand(
+        _defaultComponentId,
+        MAV_CMD_DO_SET_SERVO,
+        true,
+        UNIVERSAL_SERVO,
+        SERVO_PWM_ON
+    );                
+};
+
+// ----------------------------------------------------------------------------
+void ServoControl::universalDisable(int _defaultComponentId) {
+    // _defaultComponentId
+    _vehicle->sendCommand(
+        _defaultComponentId,
+        MAV_CMD_DO_SET_SERVO,
+        true,
+        UNIVERSAL_SERVO,
+        SERVO_PWM_OFF
+    );                
+};

src/Servo/ServoControl.h

@@ -26,11 +26,18 @@ Q_OBJECT
 
     Q_INVOKABLE void setDayLightEnabled(bool, int);
     Q_INVOKABLE void setNightLightEnabled(bool, int);
+    Q_INVOKABLE void setRebEnabled(bool, int);
+    Q_INVOKABLE void setUniversalEnabled(bool, int);
 
     void dayLightEnable(int);
     void dayLightDisable(int);
     void nightLightEnable(int);
     void nightLightDisable(int);
+    void rebEnable(int);
+    void rebDisable(int);
+    void universalEnable(int);
+    void universalDisable(int);
+
 private:
     Vehicle* _vehicle;
 };

src/Vehicle/ServoStatusFactGroup.json

@@ -14,6 +14,18 @@
     "shortDesc":    "Status of night light",
     "type":         "bool",
     "default":      false
+},
+{
+    "name":         "reb",
+    "shortDesc":    "Status of reb activation",
+    "type":         "bool",
+    "default":      false
+},
+{
+    "name":         "universal",
+    "shortDesc":    "Status of universal servo",
+    "type":         "bool",
+    "default":      false
 }
 ]
 }

src/Vehicle/Vehicle.cc

@@ -2136,6 +2136,15 @@ void Vehicle::setNightLightEnabled(bool enabled)
     _servoControl->setNightLightEnabled(enabled, _defaultComponentId);
 }
 
+void Vehicle::setRebEnabled(bool enabled)
+{
+    _servoControl->setRebEnabled(enabled, _defaultComponentId);
+}
+void Vehicle::setUniversalEnabled(bool enabled)
+{
+    _servoControl->setUniversalEnabled(enabled, _defaultComponentId);
+}
+
 // this function called in three cases:
 // 1. On constructor of vehicle, to see if we should enable a joystick
 // 2. When there is a new active joystick

src/Vehicle/Vehicle.h

@@ -493,6 +493,8 @@ class Vehicle : public FactGroup
 
     Q_INVOKABLE void setDayLightEnabled   (bool enabled);
     Q_INVOKABLE void setNightLightEnabled (bool enabled);
+    Q_INVOKABLE void setRebEnabled (bool enabled);
+    Q_INVOKABLE void setUniversalEnabled (bool enabled);
 
     bool joystickEnabled            () const;
     // void setJoystickEnabled         (bool enabled);

src/Vehicle/VehicleServoFactGroup.cc

@@ -16,18 +16,26 @@
 
 #define DAY_LIGHT_SERVO   11
 #define NIGHT_LIGHT_SERVO 3
+#define REB_SERVO 10
+#define UNIVERSAL_SERVO 7
 #define PWM_VALUE         1500
 
 const char* VehicleServoFactGroup::_dayLightFactName   = "dayLight";
 const char* VehicleServoFactGroup::_nightLightFactName = "nightLight";
+const char* VehicleServoFactGroup::_rebFactName = "reb contol";
+const char* VehicleServoFactGroup::_universalFactName = "universal control";
 
 VehicleServoFactGroup::VehicleServoFactGroup(QObject* parent)
     : FactGroup(1000, ":/json/Vehicle/ServoStatusFactGroup.json", parent)
     , _dayLightFact   (0, _dayLightFactName,   FactMetaData::valueTypeBool)
     , _nightLightFact (0, _nightLightFactName, FactMetaData::valueTypeBool)
+    , _rebFact (0, _rebFactName, FactMetaData::valueTypeBool)
+    , _universalFact (0, _universalFactName, FactMetaData::valueTypeBool)
 {
     _addFact(&_dayLightFact,   _dayLightFactName);
     _addFact(&_nightLightFact, _nightLightFactName);
+    _addFact(&_rebFact, _rebFactName);
+    _addFact(&_universalFact, _universalFactName);
 
     // _dayLightFact.setRawValue(false);
     // _nightLightFact.setRawValue(false);
@@ -71,4 +79,6 @@ void VehicleServoFactGroup::_handleServoOutputRaw(mavlink_message_t& message)
 
     dayLight()   -> setRawValue ((_rgChannelvalues[DAY_LIGHT_SERVO - 1]   > PWM_VALUE) ? true : false);
     nightLight() -> setRawValue ((_rgChannelvalues[NIGHT_LIGHT_SERVO - 1] > PWM_VALUE) ? true : false);
-}
+    reb() -> setRawValue ((_rgChannelvalues[REB_SERVO - 1] > PWM_VALUE) ? true : false);
+    universal() -> setRawValue ((_rgChannelvalues[UNIVERSAL_SERVO - 1] > PWM_VALUE) ? true : false);
+}

src/Vehicle/VehicleServoFactGroup.h

@@ -23,19 +23,27 @@ class VehicleServoFactGroup : public FactGroup
 
     Q_PROPERTY(Fact* dayLight   READ dayLight   CONSTANT)
     Q_PROPERTY(Fact* nightLight READ nightLight CONSTANT)
+    Q_PROPERTY(Fact* reb READ reb CONSTANT)
+    Q_PROPERTY(Fact* universal READ universal CONSTANT)
 
     Fact* dayLight         () { return &_dayLightFact; }
     Fact* nightLight       () { return &_nightLightFact; }
+    Fact* reb              () { return &_rebFact; }
+    Fact* universal        () { return &_universalFact; }
 
     // Overrides from FactGroup
     virtual void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override;
 
     static const char* _dayLightFactName;
     static const char* _nightLightFactName;
+    static const char* _rebFactName;
+    static const char* _universalFactName;
 
 protected:
     void _handleServoOutputRaw(mavlink_message_t& message);
 
     Fact _dayLightFact;
     Fact _nightLightFact;
+    Fact _rebFact;
+    Fact _universalFact;
 };

src/ui/toolbar/CMakeLists.txt

@@ -8,6 +8,8 @@ add_custom_target(UiToolbarQml
 		JoystickIndicator.qml
 		DayLightActions.qml
 		NightLightActions.qml
+		RebActions.qml
+		UniversalActions.qml
 		LinkIndicator.qml
 		MainStatusIndicator.qml
 		MainToolBarIndicators.qml

src/ui/toolbar/RebActions.qml

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+import QtQuick          2.11
+import QtQuick.Layouts  1.11
+
+import QGroundControl                       1.0
+import QGroundControl.Controls              1.0
+import QGroundControl.MultiVehicleManager   1.0
+import QGroundControl.ScreenTools           1.0
+import QGroundControl.Palette               1.0
+
+// Joystick Indicator
+Item {
+    id:             _root
+    width:          rebRow.width * 1.1
+    anchors.top:    parent.top
+    anchors.bottom: parent.bottom
+
+    property var _activeVehicle:    QGroundControl.multiVehicleManager.activeVehicle
+    property bool showIndicator:    true
+    property bool _rebEnabled: _activeVehicle ? _activeVehicle.servo.reb.value : false
+
+    Row {
+        id:             rebRow
+        anchors.top:    parent.top
+        anchors.bottom: parent.bottom
+        spacing:        ScreenTools.defaultFontPixelWidth
+        QGCColoredImage {
+            width:              height
+            anchors.top:        parent.top
+            anchors.bottom:     parent.bottom
+            sourceSize.height:  height
+            source:             "/qmlimages/Reb.png"
+            fillMode:           Image.PreserveAspectFit
+            color: {
+                if (_activeVehicle.servo.reb.value) {
+                    return "yellow"
+                }
+
+                return qgcPal.buttonText
+            }
+        }
+    }
+
+    MouseArea {
+        anchors.fill:   parent
+        onClicked: {
+            globals.guidedControllerFlyView.executeAction(
+                globals.guidedControllerFlyView.actionReb,
+                !_activeVehicle.servo.reb.value,
+                null,
+                null
+            )
+        }
+    }
+}