Introduce manual control and position setpoints from Joystick inputs

src/input/JoystickInput.h

@@ -37,6 +37,7 @@ This file is part of the PIXHAWK project
  *
  * @author Lorenz Meier <mavteam@student.ethz.ch>
  * @author Andreas Romer <mavteam@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef _JOYSTICKINPUT_H_
@@ -56,6 +57,8 @@ This file is part of the PIXHAWK project
 struct JoystickSettings {
     QMap<int, bool> axesInverted; ///< Whether each axis should be used inverted from what was reported.
     QMap<int, bool> axesLimited; ///< Whether each axis should be limited to only the positive range. Currently this only applies to the throttle axis, but is kept generic here to possibly support other axes.
+    QMap<int, float> axesMaxRange; ///< The maximum values per axis
+    QMap<int, float> axesMinRange; ///< The minimum values per axis
     QMap<int, int> buttonActions; ///< The index of the action associated with every button.
 };
 Q_DECLARE_METATYPE(JoystickSettings)
@@ -86,6 +89,19 @@ public:
         JOYSTICK_INPUT_MAPPING_THROTTLE = 4
     };
 
+    /**
+     * @brief The JOYSTICK_MODE enum stores the values for each item in the mode combobox.
+     * This should match the order of items in the mode combobox in JoystickWidget.ui.
+     */
+    enum JOYSTICK_MODE
+    {
+        JOYSTICK_MODE_ATTITUDE     = 0,
+        JOYSTICK_MODE_POSITION     = 1,
+        JOYSTICK_MODE_FORCE        = 2,
+        JOYSTICK_MODE_VELOCITY     = 3,
+        JOYSTICK_MODE_MANUAL       = 4
+    };
+
     /**
      * @brief Load joystick-specific settings.
      */
@@ -109,6 +125,11 @@ public:
         return isEnabled;
     }
 
+    bool calibrating() const
+    {
+        return isCalibrating;
+    }
+
     int getMappingThrottleAxis() const
     {
         return throttleAxis;
@@ -154,6 +175,11 @@ public:
         return numJoysticks;
     }
 
+    JOYSTICK_MODE getMode() const
+    {
+        return mode;
+    }
+
     QString getJoystickNameById(int id) const
     {
         return QString(SDL_JoystickName(id));
@@ -162,16 +188,17 @@ public:
     float getCurrentValueForAxis(int axis) const;
     bool getInvertedForAxis(int axis) const;
     bool getRangeLimitForAxis(int axis) const;
+    float getAxisRangeLimitMinForAxis(int axis) const;
+    float getAxisRangeLimitMaxForAxis(int axis) const;
     int getActionForButton(int button) const;
 
     const double sdlJoystickMin;
     const double sdlJoystickMax;
 
 protected:
-    double calibrationPositive[10];
-    double calibrationNegative[10];
 
     bool isEnabled; ///< Track whether the system should emit the higher-level signals: joystickChanged & actionTriggered.
+    bool isCalibrating; ///< Track if calibration in progress
     bool done;
 
     SDL_Joystick* joystick;
@@ -194,6 +221,9 @@ protected:
     int joystickNumAxes;
     int joystickNumButtons;
 
+    // mode of joystick (attitude, position, force, ... (see JOYSTICK_MODE enum))
+    JOYSTICK_MODE mode;
+
     // Track axis/button settings based on a Joystick/AutopilotType/SystemType triplet.
     // This is only a double-map, because settings are stored/loaded based on joystick
     // name first, so only the settings for the current joystick need to be stored at any given time.
@@ -223,8 +253,9 @@ signals:
      * @param throttle Throttle, -100%:-1.0, 0%: 0.0, 100%: 1.0. If the roll axis isn't defined, NaN is transmit instead.
      * @param xHat hat vector in forward-backward direction, +1 forward, 0 center, -1 backward
      * @param yHat hat vector in left-right direction, -1 left, 0 center, +1 right
+     * @param mode (setpoint type) see JOYSTICK_MODE enum
      */
-    void joystickChanged(float roll, float pitch, float yaw, float throttle, qint8 xHat, qint8 yHat, quint16 buttons);
+    void joystickChanged(float roll, float pitch, float yaw, float throttle, qint8 xHat, qint8 yHat, quint16 buttons, quint8 mode);
 
     /**
       * @brief Emit a new value for an axis
@@ -287,6 +318,8 @@ public slots:
     void setActiveUAS(UASInterface* uas);
     /** @brief Switch to a new joystick by ID number. Both buttons and axes are updated with the proper signals emitted. */
     void setActiveJoystick(int id);
+    /** @brief Switch calibration mode active */
+    void setCalibrating(bool active);
     /**
      * @brief Change the control mapping for a given joystick axis.
      * @param axisID The axis to modify (0-indexed)
@@ -300,12 +333,28 @@ public slots:
      * @param inverted True indicates inverted from normal. Varies by controller.
      */
     void setAxisInversion(int axis, bool inverted);
+
     /**
      * @brief Specify that an axis should only transmit the positive values. Useful for controlling throttle from auto-centering axes.
      * @param axis Which axis has its range limited.
      * @param limitRange If true only the positive half of this axis will be read.
      */
     void setAxisRangeLimit(int axis, bool limitRange);
+
+    /**
+     * @brief Specify minimum value for axis.
+     * @param axis Which axis should be set.
+     * @param min Value to be set.
+     */
+    void setAxisRangeLimitMin(int axis, float min);
+
+    /**
+     * @brief Specify maximum value for axis.
+     * @param axis Which axis should be set.
+     * @param max Value to be set.
+     */
+    void setAxisRangeLimitMax(int axis, float max);
+
     /**
      * @brief Specify a button->action mapping for the given uas.
      * This mapping is applied based on UAS autopilot type and UAS system type.
@@ -314,6 +363,15 @@ public slots:
      * @param action The numeric ID of the action for this UAS to map to.
      */
     void setButtonAction(int button, int action);
+
+    /**
+     * @brief Specify which setpoints should be sent to the UAS when moving the joystick
+     * @param newMode the mode (setpoint type) see the JOYSTICK_MODE enum
+     */
+    void setMode(int newMode)
+    {
+        mode = (JOYSTICK_MODE)newMode;
+    }
 };
 
 #endif // _JOYSTICKINPUT_H_

src/uas/UAS.h

@@ -41,6 +41,7 @@ This file is part of the QGROUNDCONTROL project
 #include "QGCJSBSimLink.h"
 #include "QGCXPlaneLink.h"
 #include "FileManager.h"
+#include "JoystickInput.h"
 
 Q_DECLARE_LOGGING_CATEGORY(UASLog)
 
@@ -780,7 +781,7 @@ public slots:
     void toggleAutonomy();
 
     /** @brief Set the values for the manual control of the vehicle */
-    void setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons);
+    void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons, quint8);
 
     /** @brief Set the values for the 6dof manual control of the vehicle */
     void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw);

src/ui/JoystickAxis.h

@@ -45,6 +45,8 @@ public:
     void setMapping(JoystickInput::JOYSTICK_INPUT_MAPPING newMapping);
     void setInverted(bool newValue);
     void setRangeLimit(bool newValue);
+    void setAxisRangeMin(float min);
+    void setAxisRangeMax(float max);
 
 signals:
     /** @brief Signal a change in this axis' yaw/pitch/roll mapping */

src/ui/JoystickWidget.cc

@@ -45,6 +45,9 @@ JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) :
     styleChanged(qgcApp()->styleIsDark());
     connect(qgcApp(), &QGCApplication::styleChanged, this, &JoystickWidget::styleChanged);
 
+    // change mode when mode combobox is changed
+    connect(m_ui->joystickModeComboBox, SIGNAL(currentIndexChanged(int)), this->joystick, SLOT(setMode(int)));
+
     // Display the widget above all other windows.
     this->raise();
     this->show();
@@ -68,6 +71,14 @@ void JoystickWidget::initUI()
             m_ui->joystickFrame->setEnabled(true);
         }
 
+        // mode combo box
+        m_ui->joystickModeComboBox->addItem("Attitude");
+        m_ui->joystickModeComboBox->addItem("Position");
+        m_ui->joystickModeComboBox->addItem("Force");
+        m_ui->joystickModeComboBox->addItem("Velocity");
+        m_ui->joystickModeComboBox->addItem("Manual");
+        m_ui->joystickModeComboBox->setCurrentIndex(joystick->getMode());
+
         // Create the initial UI.
         createUIForJoystick();
     }
@@ -228,6 +239,8 @@ void JoystickWidget::createUIForJoystick()
     {
         m_ui->axesBox->hide();
     }
+
+    connect(m_ui->calibrationButton, SIGNAL(clicked()), this, SLOT(cycleCalibrationButton()));
 }
 
 void JoystickWidget::updateAxisValue(int axis, float value)
@@ -253,3 +266,14 @@ void JoystickWidget::joystickButtonReleased(int key)
 {
     buttons.at(key)->setStyleSheet("");
 }
+
+void JoystickWidget::cycleCalibrationButton()
+{
+    if (this->joystick->calibrating()) {
+        this->joystick->setCalibrating(false);
+        m_ui->calibrationButton->setText("Calibrate range");
+    } else {
+        this->joystick->setCalibrating(true);
+        m_ui->calibrationButton->setText("End calibration");
+    }
+}

src/ui/JoystickWidget.h

@@ -70,6 +70,8 @@ public slots:
     void joystickButtonPressed(int key);
     /** @brief Trigger a UI change based on a button being released */
     void joystickButtonReleased(int key);
+    /** @brief Toggle the calibration button */
+    void cycleCalibrationButton();
     /** @brief Update the UI color scheme when the MainWindow theme changes. */
     void styleChanged(bool styleIsDark);
     /** Update the UI assuming the joystick has stayed the same. */

src/ui/JoystickWidget.ui

@@ -67,6 +67,9 @@
      </property>
     </widget>
    </item>
+   <item>
+    <widget class="QComboBox" name="joystickModeComboBox"/>
+   </item>
    <item>
     <widget class="QFrame" name="joystickFrame">
      <property name="enabled">
@@ -78,7 +81,7 @@
      <property name="frameShadow">
       <enum>QFrame::Sunken</enum>
      </property>
-     <layout class="QVBoxLayout" name="verticalLayout_2" stretch="0,0">
+     <layout class="QVBoxLayout" name="verticalLayout_2" stretch="0,0,0">
       <property name="sizeConstraint">
        <enum>QLayout::SetMinimumSize</enum>
       </property>
@@ -171,6 +174,13 @@
         </property>
        </widget>
       </item>
+      <item>
+       <widget class="QPushButton" name="calibrationButton">
+        <property name="text">
+         <string>Calibrate range</string>
+        </property>
+       </widget>
+      </item>
      </layout>
     </widget>
    </item>