Send out mavlink setpoint 6dof messages for manual control

src/input/Mouse6dofInput.cpp

@@ -11,9 +11,10 @@
 #include "UASManager.h"
 
 Mouse6dofInput::Mouse6dofInput(Mouse3DInput* mouseInput) :
-    mouse3DMax(0.01),   // TODO: check maximum value fot plugged device
+    mouse3DMax(0.075),   // TODO: check maximum value fot plugged device
     uas(NULL),
     done(false),
+    mouseActive(false),
     xValue(0.0),
     yValue(0.0),
     zValue(0.0),
@@ -79,26 +80,30 @@ void Mouse6dofInput::run()
            done = false;
            exit();
         }
-        // Bound x value
-        if (xValue > 1.0) xValue = 1.0;
-        if (xValue < -1.0) xValue = -1.0;
-        // Bound x value
-        if (yValue > 1.0) yValue = 1.0;
-        if (yValue < -1.0) yValue = -1.0;
-        // Bound x value
-        if (zValue > 1.0) zValue = 1.0;
-        if (zValue < -1.0) zValue = -1.0;
-        // Bound x value
-        if (aValue > 1.0) aValue = 1.0;
-        if (aValue < -1.0) aValue = -1.0;
-        // Bound x value
-        if (bValue > 1.0) bValue = 1.0;
-        if (bValue < -1.0) bValue = -1.0;
-        // Bound x value
-        if (cValue > 1.0) cValue = 1.0;
-        if (cValue < -1.0) cValue = -1.0;
-
-        emit mouse6dofChanged(xValue, yValue, zValue, aValue, bValue, cValue);
+
+        if (mouseActive)
+        {
+            // Bound x value
+            if (xValue > 1.0) xValue = 1.0;
+            if (xValue < -1.0) xValue = -1.0;
+            // Bound x value
+            if (yValue > 1.0) yValue = 1.0;
+            if (yValue < -1.0) yValue = -1.0;
+            // Bound x value
+            if (zValue > 1.0) zValue = 1.0;
+            if (zValue < -1.0) zValue = -1.0;
+            // Bound x value
+            if (aValue > 1.0) aValue = 1.0;
+            if (aValue < -1.0) aValue = -1.0;
+            // Bound x value
+            if (bValue > 1.0) bValue = 1.0;
+            if (bValue < -1.0) bValue = -1.0;
+            // Bound x value
+            if (cValue > 1.0) cValue = 1.0;
+            if (cValue < -1.0) cValue = -1.0;
+
+            emit mouse6dofChanged(xValue, yValue, zValue, aValue, bValue, cValue);
+        }
 
         // Sleep, update rate of 3d mouse is approx. 50 Hz (1000 ms / 50 = 20 ms)
         QGC::SLEEP::msleep(20);
@@ -108,6 +113,8 @@ void Mouse6dofInput::run()
 void Mouse6dofInput::motion3DMouse(std::vector<float> &motionData)
 {
     if (motionData.size() < 6) return;
+    mouseActive = true;
+
     xValue = (double)1.0e2f*motionData[ 1 ] / mouse3DMax;
     yValue = (double)1.0e2f*motionData[ 0 ] / mouse3DMax;
     zValue = (double)1.0e2f*motionData[ 2 ] / mouse3DMax;
@@ -115,5 +122,5 @@ void Mouse6dofInput::motion3DMouse(std::vector<float> &motionData)
     bValue = (double)1.0e2f*motionData[ 3 ] / mouse3DMax;
     cValue = (double)1.0e2f*motionData[ 5 ] / mouse3DMax;
 
-    qDebug() << "NEW 3D MOUSE VALUES -- X" << xValue << " -- Y" << yValue << " -- Z" << zValue << " -- A" << aValue << " -- B" << bValue << " -- C" << cValue;
+    //qDebug() << "NEW 3D MOUSE VALUES -- X" << xValue << " -- Y" << yValue << " -- Z" << zValue << " -- A" << aValue << " -- B" << bValue << " -- C" << cValue;
 }

src/input/Mouse6dofInput.h

@@ -31,6 +31,7 @@ protected:
     Mouse3DInput* mouse3D;
     UASInterface* uas;
     bool done;
+    bool mouseActive;
 
     double xValue;
     double yValue;

src/uas/UAS.cc

@@ -2454,13 +2454,26 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
     }
     else
     {
-        qDebug() << "JOYSTICK/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send joystick commands first";
+        //qDebug() << "JOYSTICK/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send joystick commands first";
     }
 }
 
 void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)
 {
-// TODO: send mavlink 6DOF manual control message
+    // If system has manual inputs enabled and is armed
+    if(((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (mode & MAV_MODE_FLAG_HIL_ENABLED))
+    {
+        mavlink_message_t message;
+        mavlink_msg_setpoint_6dof_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)x, (float)y, (float)z, (float)roll, (float)pitch, (float)yaw);
+        sendMessage(message);
+        qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGE: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw;
+
+        //emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
+    }
+    else
+    {
+        qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first";
+    }
 }
 
 /**