Joystick: added simple range calibration

src/input/JoystickInput.cc

@@ -7,6 +7,7 @@
  *
  *   @author Lorenz Meier <mavteam@student.ethz.ch>
  *   @author Andreas Romer <mavteam@student.ethz.ch>
+ *   @author Julian Oes <julian@oes.ch
  *
  */
 
@@ -32,6 +33,7 @@ JoystickInput::JoystickInput() :
     sdlJoystickMin(-32768.0f),
     sdlJoystickMax(32767.0f),
     isEnabled(false),
+    isCalibrating(false),
     done(false),
     uas(NULL),
     autopilotType(0),
@@ -46,11 +48,6 @@ JoystickInput::JoystickInput() :
     joystickNumButtons(0)
 {
 
-    for (int i = 0; i < 10; i++) {
-        calibrationPositive[i] = sdlJoystickMax;
-        calibrationNegative[i] = sdlJoystickMin;
-    }
-
     // Make sure we initialize with the correct UAS.
     setActiveUAS(UASManager::instance()->getActiveUAS());
 
@@ -111,6 +108,8 @@ void JoystickInput::loadJoystickSettings()
             // Now that both the autopilot and system type are available, update some references.
             QMap<int, bool>* joystickAxesInverted = &joystickSettings[autopilotType][systemType].axesInverted;
             QMap<int, bool>* joystickAxesLimited = &joystickSettings[autopilotType][systemType].axesLimited;
+            QMap<int, float>* joystickAxesMinRange = &joystickSettings[autopilotType][systemType].axesMaxRange;
+            QMap<int, float>* joystickAxesMaxRange = &joystickSettings[autopilotType][systemType].axesMinRange;
             QMap<int, int>* joystickButtonActions = &joystickSettings[autopilotType][systemType].buttonActions;
 
             // Read back the joystickAxesInverted QList one element at a time.
@@ -135,6 +134,28 @@ void JoystickInput::loadJoystickSettings()
             }
             settings.endArray();
 
+            // Read back the joystickAxesMinRange QList one element at a time.
+            axesStored = settings.beginReadArray("AXES_MIN_RANGE");
+            for (int k = 0; k < axesStored; k++)
+            {
+                settings.setArrayIndex(k);
+                int index = settings.value("INDEX", 0).toInt();
+                float min = settings.value("MIN_RANGE", false).toFloat();
+                joystickAxesMinRange->insert(index, min);
+            }
+            settings.endArray();
+
+            // Read back the joystickAxesMaxRange QList one element at a time.
+            axesStored = settings.beginReadArray("AXES_MAX_RANGE");
+            for (int k = 0; k < axesStored; k++)
+            {
+                settings.setArrayIndex(k);
+                int index = settings.value("INDEX", 0).toInt();
+                float max = settings.value("MAX_RANGE", false).toFloat();
+                joystickAxesMaxRange->insert(index, max);
+            }
+            settings.endArray();
+
             // Read back the button->action mapping.
             int buttonsStored = settings.beginReadArray("BUTTONS_ACTIONS");
             for (int k = 0; k < buttonsStored; k++)
@@ -199,6 +220,8 @@ void JoystickInput::storeJoystickSettings() const
             // Now that both the autopilot and system type are available, update some references.
             QMapIterator<int, bool> joystickAxesInverted(joystickSettings[autopilotType][systemType].axesInverted);
             QMapIterator<int, bool> joystickAxesLimited(joystickSettings[autopilotType][systemType].axesLimited);
+            QMapIterator<int, float> joystickAxesMinRange(joystickSettings[autopilotType][systemType].axesMaxRange);
+            QMapIterator<int, float> joystickAxesMaxRange(joystickSettings[autopilotType][systemType].axesMinRange);
             QMapIterator<int, int> joystickButtonActions(joystickSettings[autopilotType][systemType].buttonActions);
 
             settings.beginWriteArray("AXES_INVERTED");
@@ -218,6 +241,32 @@ void JoystickInput::storeJoystickSettings() const
             }
             settings.endArray();
 
+            settings.beginWriteArray("AXES_MIN_RANGE");
+            k = 0;
+            while (joystickAxesMinRange.hasNext())
+            {
+                joystickAxesMinRange.next();
+                float min = joystickAxesMinRange.value();
+                settings.setArrayIndex(k++);
+                int index = joystickAxesMinRange.key();
+                settings.setValue("INDEX", index);
+                settings.setValue("MIN_RANGE", min);
+            }
+            settings.endArray();
+
+            settings.beginWriteArray("AXES_MAX_RANGE");
+            k = 0;
+            while (joystickAxesMaxRange.hasNext())
+            {
+                joystickAxesMaxRange.next();
+                float max = joystickAxesMaxRange.value();
+                settings.setArrayIndex(k++);
+                int index = joystickAxesMaxRange.key();
+                settings.setValue("INDEX", index);
+                settings.setValue("MAX_RANGE", max);
+            }
+            settings.endArray();
+
             settings.beginWriteArray("AXES_LIMITED");
             k = 0;
             while (joystickAxesLimited.hasNext())
@@ -401,13 +450,32 @@ void JoystickInput::run()
             // This is generally not used for controlling a vehicle, but a UI representation, so it being slightly off is fine.
             // Here we map the joystick axis value into the initial range of [0:1].
             float axisValue = SDL_JoystickGetAxis(joystick, i);
+
+            // during calibration save min and max values
+            if (isCalibrating)
+            {
+                if (joystickSettings[autopilotType][systemType].axesMinRange.value(i) > axisValue)
+                {
+                    joystickSettings[autopilotType][systemType].axesMinRange[i] = axisValue;
+                }
+
+                if (joystickSettings[autopilotType][systemType].axesMaxRange.value(i) < axisValue)
+                {
+                    joystickSettings[autopilotType][systemType].axesMaxRange[i] = axisValue;
+                }
+            }
+
             if (joystickSettings[autopilotType][systemType].axesInverted[i])
             {
-                axisValue = (axisValue - calibrationNegative[i]) / (calibrationPositive[i] - calibrationNegative[i]);
+                axisValue = (axisValue - joystickSettings[autopilotType][systemType].axesMinRange.value(i)) /
+                        (joystickSettings[autopilotType][systemType].axesMaxRange.value(i) -
+                         joystickSettings[autopilotType][systemType].axesMinRange.value(i));
             }
             else
             {
-                axisValue = (axisValue - calibrationPositive[i]) / (calibrationNegative[i] - calibrationPositive[i]);
+                axisValue = (axisValue - joystickSettings[autopilotType][systemType].axesMaxRange.value(i)) /
+                        (joystickSettings[autopilotType][systemType].axesMinRange.value(i) -
+                         joystickSettings[autopilotType][systemType].axesMaxRange.value(i));
             }
             axisValue = 1.0f - axisValue;
 
@@ -535,6 +603,38 @@ void JoystickInput::setActiveJoystick(int id)
     emit joystickSettingsChanged();
 }
 
+void JoystickInput::setCalibrating(bool active)
+{
+    if (active)
+    {
+        setEnabled(false);
+        isCalibrating = true;
+
+        // set range small so that limits can be re-found
+        for (int i = 0; i < joystickNumAxes; i++)
+        {
+            joystickSettings[autopilotType][systemType].axesMinRange[i] = -10.0f;
+            joystickSettings[autopilotType][systemType].axesMaxRange[i] = 10.0f;
+        }
+
+    } else {
+
+        // store calibration values
+        storeJoystickSettings();
+
+        qDebug() << "Calibration result:";
+        for (int i = 0; i < joystickNumAxes; i++)
+        {
+            qDebug() << i << ": " <<
+                        joystickSettings[autopilotType][systemType].axesMinRange[i] <<
+                        " - " <<
+                        joystickSettings[autopilotType][systemType].axesMaxRange[i];
+        }
+        setEnabled(true);
+        isCalibrating = false;
+    }
+}
+
 void JoystickInput::setAxisMapping(int axis, JOYSTICK_INPUT_MAPPING newMapping)
 {
     switch (newMapping)
@@ -593,6 +693,24 @@ void JoystickInput::setAxisRangeLimit(int axis, bool limitRange)
     }
 }
 
+void JoystickInput::setAxisRangeLimitMin(int axis, float min)
+{
+    if (axis < joystickNumAxes)
+    {
+        joystickSettings[autopilotType][systemType].axesMinRange[axis] = min;
+        storeJoystickSettings();
+    }
+}
+
+void JoystickInput::setAxisRangeLimitMax(int axis, float max)
+{
+    if (axis < joystickNumAxes)
+    {
+        joystickSettings[autopilotType][systemType].axesMaxRange[axis] = max;
+        storeJoystickSettings();
+    }
+}
+
 void JoystickInput::setButtonAction(int button, int action)
 {
     if (button < joystickNumButtons)
@@ -629,6 +747,24 @@ bool JoystickInput::getRangeLimitForAxis(int axis) const
     return false;
 }
 
+float JoystickInput::getAxisRangeLimitMinForAxis(int axis) const
+{
+    if (axis < joystickNumAxes)
+    {
+        return joystickSettings[autopilotType][systemType].axesMinRange.value(axis);
+    }
+    return sdlJoystickMin;
+}
+
+float JoystickInput::getAxisRangeLimitMaxForAxis(int axis) const
+{
+    if (axis < joystickNumAxes)
+    {
+        return joystickSettings[autopilotType][systemType].axesMaxRange.value(axis);
+    }
+    return sdlJoystickMax;
+}
+
 int JoystickInput::getActionForButton(int button) const
 {
     if (button < joystickNumButtons && joystickSettings[autopilotType][systemType].buttonActions.contains(button))

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)
@@ -109,6 +112,11 @@ public:
         return isEnabled;
     }
 
+    bool calibrating() const
+    {
+        return isCalibrating;
+    }
+
     int getMappingThrottleAxis() const
     {
         return throttleAxis;
@@ -162,16 +170,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;
@@ -287,6 +296,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 +311,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.

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

@@ -228,6 +228,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 +255,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(MainWindow::QGC_MAINWINDOW_STYLE);
     /** Update the UI assuming the joystick has stayed the same. */

src/ui/JoystickWidget.ui

@@ -78,7 +78,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,0">
       <property name="sizeConstraint">
        <enum>QLayout::SetMinimumSize</enum>
       </property>
@@ -171,6 +171,20 @@
         </property>
        </widget>
       </item>
+      <item>
+       <widget class="QPushButton" name="calibrationButton">
+        <property name="text">
+         <string>Calibrate range</string>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <widget class="QLabel" name="calibrationStatusLabel">
+        <property name="text">
+         <string>Range status</string>
+        </property>
+       </widget>
+      </item>
      </layout>
     </widget>
    </item>