i joystick: mavlink message depending on mode

src/input/JoystickInput.cc

@@ -323,7 +323,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,JOYSTICK_MODE)), tmp, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16, JOYSTICK_MODE)));
             disconnect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int)));
         }
         uasCanReverse = false;
@@ -339,7 +339,7 @@ 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,JOYSTICK_MODE)), tmp, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,JOYSTICK_MODE)));
         connect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int)));
         uasCanReverse = tmp->systemCanReverse();
 
@@ -553,7 +553,7 @@ 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. 25 Hz (1000 ms / 25 = 40 ms)

src/input/JoystickInput.h

@@ -251,8 +251,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, JOYSTICK_MODE mode);
 
     /**
       * @brief Emit a new value for an axis

src/uas/UAS.cc

@@ -2933,7 +2933,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, JoystickInput::JOYSTICK_MODE joystickMode)
 {
     Q_UNUSED(xHat);
     Q_UNUSED(yHat);
@@ -2979,53 +2979,57 @@ void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thr
 
         mavlink_message_t message;
 
-        // 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
-            );
-
-// TODO: This is a prototype to be used depending on the joystick mode
-//        // 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 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,
-//                0,
-//                0,
-//                0,
-//                0,
-//                0,
-//                0,
-//                fx,
-//                fy,
-//                fz
-//                );
-
+        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_attitude_setpoint_external_pack(mavlink->getSystemId(),
+                mavlink->getComponentId(),
+                &message,
+                QGC::groundTimeUsecs(),
+                this->uasId,
+                0,
+                typeMask,
+                attitudeQuaternion,
+                0,
+                0,
+                0,
+                thrust
+                );
+        }
+        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];
+            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_local_ned_position_setpoint_external_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;
+        }
 
         sendMessage(message);
 

src/uas/UAS.h

@@ -42,6 +42,7 @@ This file is part of the QGROUNDCONTROL project
 #include "QGCJSBSimLink.h"
 #include "QGCXPlaneLink.h"
 #include "QGCUASParamManager.h"
+#include "JoystickInput.h"
 
 
 /**
@@ -838,7 +839,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, JoystickInput::JOYSTICK_MODE joystickMode);
 
     /** @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);