Revert "Joystick: update gui and send new mavlink target messages"

src/input/JoystickInput.h

@@ -37,7 +37,6 @@ 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_
@@ -57,8 +56,6 @@ 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)
@@ -89,17 +86,6 @@ 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
-    };
-
     /**
      * @brief Load joystick-specific settings.
      */
@@ -123,11 +109,6 @@ public:
         return isEnabled;
     }
 
-    bool calibrating() const
-    {
-        return isCalibrating;
-    }
-
     int getMappingThrottleAxis() const
     {
         return throttleAxis;
@@ -173,11 +154,6 @@ public:
         return numJoysticks;
     }
 
-    JOYSTICK_MODE getMode() const
-    {
-        return mode;
-    }
-
     QString getJoystickNameById(int id) const
     {
         return QString(SDL_JoystickName(id));
@@ -186,17 +162,16 @@ 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;
@@ -219,9 +194,6 @@ 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.
@@ -251,9 +223,8 @@ 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, quint8 mode);
+    void joystickChanged(float roll, float pitch, float yaw, float throttle, qint8 xHat, qint8 yHat, quint16 buttons);
 
     /**
       * @brief Emit a new value for an axis
@@ -316,8 +287,6 @@ 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)
@@ -331,28 +300,12 @@ 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.
@@ -361,15 +314,6 @@ 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

@@ -2713,7 +2713,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::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons, quint8 joystickMode)
+void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons)
 {
     Q_UNUSED(xHat);
     Q_UNUSED(yHat);
@@ -2726,126 +2726,57 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
     static quint16 manualButtons = 0;
     static quint8 countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission
 
-    // 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)
+    // 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))
     {
-        sendCommand = true;
 
-        // Ensure that another message will be sent the next time this function is called
-        countSinceLastTransmission = 10;
-    }
+        // 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;
 
-    // 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;
+            // Ensure that another message will be sent the next time this function is called
+            countSinceLastTransmission = 10;
+        }
 
-        mavlink_message_t message;
+        // 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;
 
-        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 = 0b111; // 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 = 0b0000000111111000; // 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
-                    );
-        }
-        else if (joystickMode == JoystickInput::JOYSTICK_MODE_FORCE)
-        {
-            // Send the the force setpoint (local pos sp external message)
-            // XXX: scale with thrust
-            float dcm[3][3];
-            mavlink_euler_to_dcm(roll, pitch, yaw, dcm);
-            const float fx = -dcm[0][2];
-            const float fy = -dcm[1][2];
-            const float fz = -dcm[2][2];
-            uint16_t typeMask = 0b0000001000111111; // 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
-                    );
-        }
-        else {
-            return;
-        }
+            // Store scaling values for all 3 axes
+            const float axesScaling = 1.0 * 1000.0;
 
-        sendMessage(message);
+            // 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;
 
-        // Emit an update in control values to other UI elements, like the HSI display
-        emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
+            // 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());
+        }
     }
 }
 

src/uas/UAS.h

@@ -42,7 +42,6 @@ This file is part of the QGROUNDCONTROL project
 #include "QGCJSBSimLink.h"
 #include "QGCXPlaneLink.h"
 #include "QGCUASParamManager.h"
-#include "JoystickInput.h"
 #include "QGCUASFileManager.h"
 
 
@@ -846,7 +845,7 @@ public slots:
     void toggleAutonomy();
 
     /** @brief Set the values for the manual control of the vehicle */
-    void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons, quint8);
+    void setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons);
 
     /** @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,8 +45,6 @@ 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,9 +45,6 @@ JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) :
     styleChanged(MainWindow::instance()->getStyle());
     connect(MainWindow::instance(), SIGNAL(styleChanged(MainWindow::QGC_MAINWINDOW_STYLE)), this, SLOT(styleChanged(MainWindow::QGC_MAINWINDOW_STYLE)));
 
-    // 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();
@@ -71,12 +68,6 @@ 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->setCurrentIndex(joystick->getMode());
-
         // Create the initial UI.
         createUIForJoystick();
     }
@@ -237,8 +228,6 @@ void JoystickWidget::createUIForJoystick()
     {
         m_ui->axesBox->hide();
     }
-
-    connect(m_ui->calibrationButton, SIGNAL(clicked()), this, SLOT(cycleCalibrationButton()));
 }
 
 void JoystickWidget::updateAxisValue(int axis, float value)
@@ -264,14 +253,3 @@ 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,8 +70,6 @@ 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

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

src/ui/designer/QGCCommandButton.cc

@@ -83,7 +83,6 @@ QGCCommandButton::QGCCommandButton(QWidget *parent) :
     ui->editCommandComboBox->addItem("MAV_CMD_OVERRIDE_GOTO", MAV_CMD_OVERRIDE_GOTO);
     ui->editCommandComboBox->addItem("MAV_CMD_MISSION_START", MAV_CMD_MISSION_START);
     ui->editCommandComboBox->addItem("MAV_CMD_COMPONENT_ARM_DISARM", MAV_CMD_COMPONENT_ARM_DISARM);
-    ui->editCommandComboBox->addItem("MAV_CMD_NAV_GUIDED_ENABLE", MAV_CMD_NAV_GUIDED_ENABLE);
     ui->editCommandComboBox->addItem("MAV_CMD_DO_FLIGHTTERMINATION", MAV_CMD_DO_FLIGHTTERMINATION);
     ui->editCommandComboBox->setEditable(true);