joystick mode ui option

this adds an option to select the mode of the joystick in the joystick
config widget. The mode sets the setpoint type which is sent when moving
the joystick.

src/input/JoystickInput.cc

@@ -72,6 +72,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();
 }
 
@@ -182,6 +183,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();
     settings.sync();
 }

src/input/JoystickInput.h

@@ -89,6 +89,17 @@ 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.
      */
@@ -162,6 +173,11 @@ public:
         return numJoysticks;
     }
 
+    JOYSTICK_MODE getMode() const
+    {
+        return mode;
+    }
+
     QString getJoystickNameById(int id) const
     {
         return QString(SDL_JoystickName(id));
@@ -203,6 +219,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.
@@ -341,6 +360,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

@@ -2995,29 +2995,57 @@ void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thr
             //const int16_t yawCommand = (int16_t)(yaw * axesScaling);
             //const uint16_t thrustCommand = (uint16_t)(thrust * axesScaling);
 
-
-            uint8_t mode = 3; // for velocity setpoint (OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY)
-
-            // Send the MANUAL_COMMAND message
             mavlink_message_t message;
+
+            //// Send the attitude setpoint external message
+            ////mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
+            //// 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_attitude_setpoint_external_pack(mavlink->getSystemId(),
+                    //mavlink->getComponentId(),
+                    //&message,
+                    //QGC::groundTimeUsecs(),
+                    //this->uasId,
+                    //0,
+                    //typeMask,
+                    //attitudeQuaternion,
+                    //0,
+                    //0,
+                    //0,
+                    //thrust
+                    //);
+
+
+            // Send the the force setpoint (local pos sp external message)
             //mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
             // 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_attitude_setpoint_external_pack(mavlink->getSystemId(),
+            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; // disable rate control
+            mavlink_msg_local_ned_position_setpoint_external_pack(mavlink->getSystemId(),
                     mavlink->getComponentId(),
                     &message,
                     QGC::groundTimeUsecs(),
                     this->uasId,
                     0,
+                    MAV_FRAME_LOCAL_NED,
                     typeMask,
-                    attitudeQuaternion,
                     0,
                     0,
                     0,
-                    thrust
+                    0,
+                    0,
+                    0,
+                    fx,
+                    fy,
+                    fz
                     );
+
             sendMessage(message);
 
             // Emit an update in control values to other UI elements, like the HSI display

src/ui/JoystickWidget.cc

@@ -45,6 +45,9 @@ 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();
@@ -68,6 +71,12 @@ 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();
     }

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">