Introduce manual control and position setpoints from Joystick inputs

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());
 
@@ -74,6 +71,7 @@ void JoystickInput::loadGeneralSettings()
     settings.beginGroup("JOYSTICK_INPUT");
     isEnabled = settings.value("ENABLED", false).toBool();
     joystickName = settings.value("JOYSTICK_NAME", "").toString();
+    mode = (JOYSTICK_MODE)settings.value("JOYSTICK_MODE", JOYSTICK_MODE_ATTITUDE).toInt();
     settings.endGroup();
 }
 
@@ -109,6 +107,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.
@@ -133,6 +133,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++)
@@ -159,6 +181,7 @@ void JoystickInput::storeGeneralSettings() const
     settings.beginGroup("JOYSTICK_INPUT");
     settings.setValue("ENABLED", isEnabled);
     settings.setValue("JOYSTICK_NAME", joystickName);
+    settings.setValue("JOYSTICK_MODE", mode);
     settings.endGroup();
 }
 
@@ -196,6 +219,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");
@@ -215,6 +240,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())
@@ -268,7 +319,7 @@ void JoystickInput::setActiveUAS(UASInterface* uas)
         tmp = dynamic_cast<UAS*>(this->uas);
         if(tmp)
         {
-            disconnect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16)), tmp, SLOT(setManualControlCommands(float,float,float,float,qint8,qint8,quint16)));
+            disconnect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16,quint8)), tmp, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,quint8)));
             disconnect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int)));
         }
         uasCanReverse = false;
@@ -284,7 +335,8 @@ void JoystickInput::setActiveUAS(UASInterface* uas)
 
     if (this->uas && (tmp = dynamic_cast<UAS*>(this->uas)))
     {
-        connect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16)), tmp, SLOT(setManualControlCommands(float,float,float,float,qint8,qint8,quint16)));
+        connect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16,quint8)), tmp, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,quint8)));
+        qDebug() << "connected joystick";
         connect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int)));
         uasCanReverse = tmp->systemCanReverse();
 
@@ -397,26 +449,47 @@ 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;
 
             // For non-throttle axes or if the UAS can reverse, go ahead and convert this into the range [-1:1].
-            if (uasCanReverse || throttleAxis != i)
+
+            //if (uasCanReverse || throttleAxis != i)
+            // don't take into account if UAS can reverse. This means to reverse position but not throttle
+            // therefore deactivated for now
+            if (throttleAxis != i)
             {
                 axisValue = axisValue * 2.0f - 1.0f;
             }
-            // Otherwise if this vehicle can only go forward, but the axis is limited to only the positive range,
-            // scale this so the negative values are ignored for this axis and it's clamped to [0:1].
+            // Otherwise if this vehicle can only go forward, scale it to [0:1].
             else if (throttleAxis == i && joystickSettings[autopilotType][systemType].axesLimited.value(i))
             {
-                axisValue = axisValue * 2.0f - 1.0f;
                 if (axisValue < 0.0f)
                 {
                     axisValue = 0.0f;
@@ -476,11 +549,11 @@ void JoystickInput::run()
             float pitch = pitchAxis > -1?joystickAxes[pitchAxis]:numeric_limits<float>::quiet_NaN();
             float yaw = yawAxis > -1?joystickAxes[yawAxis]:numeric_limits<float>::quiet_NaN();
             float throttle = throttleAxis > -1?joystickAxes[throttleAxis]:numeric_limits<float>::quiet_NaN();
-            emit joystickChanged(roll, pitch, yaw, throttle, xHat, yHat, joystickButtons);
+            emit joystickChanged(roll, pitch, yaw, throttle, xHat, yHat, joystickButtons, mode);
         }
 
-        // Sleep, update rate of joystick is approx. 50 Hz (1000 ms / 50 = 20 ms)
-        QGC::SLEEP::msleep(20);
+        // Sleep, update rate of joystick is approx. 25 Hz (1000 ms / 25 = 40 ms)
+        QGC::SLEEP::msleep(40);
     }
 }
 
@@ -531,6 +604,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)
@@ -589,6 +694,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)
@@ -625,6 +748,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)
@@ -87,6 +90,19 @@ public:
     };
 
     /**
+     * @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.
      */
     void loadJoystickSettings();
@@ -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.cc

@@ -2401,7 +2401,7 @@ void UAS::toggleAutonomy()
 * Set the manual control commands.
 * This can only be done if the system has manual inputs enabled and is armed.
 */
-void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons)
+void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons, quint8 joystickMode)
 {
     Q_UNUSED(xHat);
     Q_UNUSED(yHat);
@@ -2414,56 +2414,198 @@ void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thr
     static quint16 manualButtons = 0;
     static quint8 countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission
 
-    // We only transmit manual command messages if the system has manual inputs enabled and is armed
-    if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
-    {
+    // Transmit the external setpoints only if they've changed OR if it's been a little bit since they were last transmit. To make sure there aren't issues with
+    // response rate, we make sure that a message is transmit when the commands have changed, then one more time, and then switch to the lower transmission rate
+    // if no command inputs have changed.
+
+    // The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz.
+    bool sendCommand = false;
+    if (countSinceLastTransmission++ >= 5) {
+        sendCommand = true;
+        countSinceLastTransmission = 0;
+    } else if ((!isnan(roll) && roll != manualRollAngle) || (!isnan(pitch) && pitch != manualPitchAngle) ||
+             (!isnan(yaw) && yaw != manualYawAngle) || (!isnan(thrust) && thrust != manualThrust) ||
+             buttons != manualButtons) {
+        sendCommand = true;
+
+        // Ensure that another message will be sent the next time this function is called
+        countSinceLastTransmission = 10;
+    }
+
+    // Now if we should trigger an update, let's do that
+    if (sendCommand) {
+        // Save the new manual control inputs
+        manualRollAngle = roll;
+        manualPitchAngle = pitch;
+        manualYawAngle = yaw;
+        manualThrust = thrust;
+        manualButtons = buttons;
 
-        // Transmit the manual commands only if they've changed OR if it's been a little bit since they were last transmit. To make sure there aren't issues with
-        // response rate, we make sure that a message is transmit when the commands have changed, then one more time, and then switch to the lower transmission rate
-        // if no command inputs have changed.
-        // The default transmission rate is 50Hz, but when no inputs have changed it drops down to 5Hz.
-        bool sendCommand = false;
-        if (countSinceLastTransmission++ >= 10)
-        {
-            sendCommand = true;
-            countSinceLastTransmission = 0;
-        }
-        else if ((!isnan(roll) && roll != manualRollAngle) || (!isnan(pitch) && pitch != manualPitchAngle) ||
-                   (!isnan(yaw) && yaw != manualYawAngle) || (!isnan(thrust) && thrust != manualThrust) ||
-                   buttons != manualButtons)
-        {
-            sendCommand = true;
+        mavlink_message_t message;
 
-            // Ensure that another message will be sent the next time this function is called
-            countSinceLastTransmission = 10;
+        if (joystickMode == JoystickInput::JOYSTICK_MODE_ATTITUDE) {
+            // send an external attitude setpoint command (rate control disabled)
+            float attitudeQuaternion[4];
+            mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion);
+            uint8_t typeMask = 0x7; // disable rate control
+            mavlink_msg_set_attitude_target_pack(mavlink->getSystemId(),
+                mavlink->getComponentId(),
+                &message,
+                QGC::groundTimeUsecs(),
+                this->uasId,
+                0,
+                typeMask,
+                attitudeQuaternion,
+                0,
+                0,
+                0,
+                thrust
+                );
+        } else if (joystickMode == JoystickInput::JOYSTICK_MODE_POSITION) {
+            // Send the the local position setpoint (local pos sp external message)
+            static float px = 0;
+            static float py = 0;
+            static float pz = 0;
+            //XXX: find decent scaling
+            px -= pitch;
+            py += roll;
+            pz -= 2.0f*(thrust-0.5);
+            uint16_t typeMask = (1<<11)|(7<<6)|(7<<3); // select only POSITION control
+            mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(),
+                    mavlink->getComponentId(),
+                    &message,
+                    QGC::groundTimeUsecs(),
+                    this->uasId,
+                    0,
+                    MAV_FRAME_LOCAL_NED,
+                    typeMask,
+                    px,
+                    py,
+                    pz,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                    yaw,
+                    0
+                    );
+        } else if (joystickMode == JoystickInput::JOYSTICK_MODE_FORCE) {
+            // Send the the force setpoint (local pos sp external message)
+            float dcm[3][3];
+            mavlink_euler_to_dcm(roll, pitch, yaw, dcm);
+            const float fx = -dcm[0][2] * thrust;
+            const float fy = -dcm[1][2] * thrust;
+            const float fz = -dcm[2][2] * thrust;
+            uint16_t typeMask = (3<<10)|(7<<3)|(7<<0)|(1<<9); // select only FORCE control (disable everything else)
+            mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(),
+                    mavlink->getComponentId(),
+                    &message,
+                    QGC::groundTimeUsecs(),
+                    this->uasId,
+                    0,
+                    MAV_FRAME_LOCAL_NED,
+                    typeMask,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                    fx,
+                    fy,
+                    fz,
+                    0,
+                    0
+                    );
+        } else if (joystickMode == JoystickInput::JOYSTICK_MODE_VELOCITY) {
+            // Send the the local velocity setpoint (local pos sp external message)
+            static float vx = 0;
+            static float vy = 0;
+            static float vz = 0;
+            static float yawrate = 0;
+            //XXX: find decent scaling
+            vx -= pitch;
+            vy += roll;
+            vz -= 2.0f*(thrust-0.5);
+            yawrate += yaw; //XXX: not sure what scale to apply here
+            uint16_t typeMask = (1<<10)|(7<<6)|(7<<0); // select only VELOCITY control
+            mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(),
+                    mavlink->getComponentId(),
+                    &message,
+                    QGC::groundTimeUsecs(),
+                    this->uasId,
+                    0,
+                    MAV_FRAME_LOCAL_NED,
+                    typeMask,
+                    0,
+                    0,
+                    0,
+                    vx,
+                    vy,
+                    vz,
+                    0,
+                    0,
+                    0,
+                    0,
+                    yawrate
+                    );
+        } else if (joystickMode == JoystickInput::JOYSTICK_MODE_MANUAL) {
+            if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED)) {
+
+                // Transmit the manual commands only if they've changed OR if it's been a little bit since they were last transmit. To make sure there aren't issues with
+                // response rate, we make sure that a message is transmit when the commands have changed, then one more time, and then switch to the lower transmission rate
+                // if no command inputs have changed.
+                // The default transmission rate is 50Hz, but when no inputs have changed it drops down to 5Hz.
+                bool sendCommand = false;
+                if (countSinceLastTransmission++ >= 10) {
+                    sendCommand = true;
+                    countSinceLastTransmission = 0;
+                } else if ((!isnan(roll) && roll != manualRollAngle) || (!isnan(pitch) && pitch != manualPitchAngle) ||
+                        (!isnan(yaw) && yaw != manualYawAngle) || (!isnan(thrust) && thrust != manualThrust) ||
+                        buttons != manualButtons) {
+                    sendCommand = true;
+
+                    // Ensure that another message will be sent the next time this function is called
+                    countSinceLastTransmission = 10;
+                }
+
+                // Now if we should trigger an update, let's do that
+                if (sendCommand) {
+                    // Save the new manual control inputs
+                    manualRollAngle = roll;
+                    manualPitchAngle = pitch;
+                    manualYawAngle = yaw;
+                    manualThrust = thrust;
+                    manualButtons = buttons;
+
+                    // Store scaling values for all 3 axes
+                    const float axesScaling = 1.0 * 1000.0;
+
+                    // Calculate the new commands for roll, pitch, yaw, and thrust
+                    const float newRollCommand = roll * axesScaling;
+                    // negate pitch value because pitch is negative for pitching forward but mavlink message argument is positive for forward
+                    const float newPitchCommand = -pitch * axesScaling;
+                    const float newYawCommand = yaw * axesScaling;
+                    const float newThrustCommand = thrust * axesScaling;
+
+                    // Send the MANUAL_COMMAND message
+                    mavlink_message_t message;
+                    mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
+                    sendMessage(message);
+
+                    // Emit an update in control values to other UI elements, like the HSI display
+                    emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
+                }
+        } else {
+            return;
         }
 
-        // Now if we should trigger an update, let's do that
-        if (sendCommand)
-        {
-            // Save the new manual control inputs
-            manualRollAngle = roll;
-            manualPitchAngle = pitch;
-            manualYawAngle = yaw;
-            manualThrust = thrust;
-            manualButtons = buttons;
-
-            // Store scaling values for all 3 axes
-            const float axesScaling = 1.0 * 1000.0;
-
-            // Calculate the new commands for roll, pitch, yaw, and thrust
-            const float newRollCommand = roll * axesScaling;
-            const float newPitchCommand = pitch * axesScaling;
-            const float newYawCommand = yaw * axesScaling;
-            const float newThrustCommand = thrust * axesScaling;
-
-            // Send the MANUAL_COMMAND message
-            mavlink_message_t message;
-            mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
-            sendMessage(message);
-
-            // Emit an update in control values to other UI elements, like the HSI display
-            emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
+        sendMessage(message);
+
+        // Emit an update in control values to other UI elements, like the HSI display
+        emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
         }
     }
 }

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

@@ -68,6 +68,9 @@
     </widget>
    </item>
    <item>
+    <widget class="QComboBox" name="joystickModeComboBox"/>
+   </item>
+   <item>
     <widget class="QFrame" name="joystickFrame">
      <property name="enabled">
       <bool>false</bool>
@@ -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>