Merge branch 'master' of github.com:mavlink/qgroundcontrol

.gitignore

@@ -47,6 +47,8 @@ user_config.pri
 *.cproject
 *.sln
 *.suo
+*.uhf.txt
+*.opensdf
 
 thirdParty/qserialport-build-desktop/
 thirdParty/qserialport/bin/

libs/thirdParty/3DMouse/win/I3dMouseParams.h

+#ifndef I3D_MOUSE_PARAMS_H
+#define I3D_MOUSE_PARAMS_H
+
+/*
+   Parameters for the 3D mouse based on the SDK from 3Dconnexion
+*/
+
+class I3dMouseSensor
+{
+public:
+
+	enum ESpeed {
+	   kLowSpeed = 0,
+	   kMidSpeed,
+	   kHighSpeed
+	};
+
+virtual bool	IsPanZoom() const = 0;
+virtual bool	IsRotate() const  = 0;
+virtual ESpeed	GetSpeed() const  = 0;
+
+virtual void	SetPanZoom(bool isPanZoom) = 0;
+virtual void	SetRotate(bool isRotate) = 0;
+virtual void	SetSpeed(ESpeed speed) = 0;
+
+protected:
+
+virtual ~I3dMouseSensor() {}
+
+};
+
+
+class I3dMouseNavigation
+{
+public:
+
+   enum EPivot {
+	  kManualPivot = 0,
+	  kAutoPivot,
+	  kAutoPivotOverride
+   };
+
+   enum ENavigation {
+	  kObjectMode = 0,
+	  kCameraMode,
+	  kFlyMode,
+	  kWalkMode,
+	  kHelicopterMode
+   };
+
+   enum EPivotVisibility {
+	  kHidePivot = 0,
+	  kShowPivot,
+	  kShowMovingPivot
+   };
+
+
+virtual ENavigation		GetNavigationMode() const  = 0;
+virtual EPivot			GetPivotMode() const  = 0;
+virtual EPivotVisibility GetPivotVisibility()  const = 0;
+virtual bool			IsLockHorizon()  const = 0;
+
+virtual void			SetLockHorizon(bool bOn) = 0;
+virtual void			SetNavigationMode(ENavigation navigation) = 0;
+virtual void			SetPivotMode(EPivot pivot) = 0;
+virtual void			SetPivotVisibility(EPivotVisibility visibility) = 0;
+
+protected:
+
+virtual ~I3dMouseNavigation(){}
+
+
+};
+
+
+class I3dMouseParam : public I3dMouseSensor, public I3dMouseNavigation
+{
+public:
+
+virtual ~I3dMouseParam() {}
+
+};
+
+#endif

libs/thirdParty/3DMouse/win/Mouse3DInput.cpp

+
+#include "Mouse3DInput.h"
+
+#include <QApplication>
+
+#define LOGITECH_VENDOR_ID 0x46d
+#define _CONSTANT_INPUT_PERIOD 0
+
+#ifndef RIDEV_DEVNOTIFY
+#define RIDEV_DEVNOTIFY 0x00002000
+#endif
+
+#define _TRACE_WM_INPUT_PERIOD 0
+#define _TRACE_RI_TYPE 0
+#define _TRACE_RIDI_DEVICENAME 0
+#define _TRACE_RIDI_DEVICEINFO 0
+#define _TRACE_RI_RAWDATA 0
+#define _TRACE_3DINPUT_PERIOD 0
+
+#ifdef _WIN64
+typedef unsigned __int64 QWORD;
+#endif
+
+// object angular velocity per mouse tick 0.008 milliradians per second per count
+static const double k3dmouseAngularVelocity = 8.0e-6; // radians per second per count
+
+static const int kTimeToLive = 5;
+
+enum e3dconnexion_pid {
+   eSpacePilot = 0xc625,
+   eSpaceNavigator = 0xc626,
+   eSpaceExplorer = 0xc627,
+   eSpaceNavigatorForNotebooks = 0xc628,
+   eSpacePilotPRO = 0xc629
+};
+
+enum e3dmouse_virtual_key
+{
+   V3DK_INVALID=0
+   , V3DK_MENU=1, V3DK_FIT
+   , V3DK_TOP, V3DK_LEFT, V3DK_RIGHT, V3DK_FRONT, V3DK_BOTTOM, V3DK_BACK
+   , V3DK_CW, V3DK_CCW
+   , V3DK_ISO1, V3DK_ISO2
+   , V3DK_1, V3DK_2, V3DK_3, V3DK_4, V3DK_5, V3DK_6, V3DK_7, V3DK_8, V3DK_9, V3DK_10
+   , V3DK_ESC, V3DK_ALT, V3DK_SHIFT, V3DK_CTRL
+   , V3DK_ROTATE, V3DK_PANZOOM, V3DK_DOMINANT
+   , V3DK_PLUS, V3DK_MINUS
+};
+
+struct tag_VirtualKeys
+{
+   e3dconnexion_pid pid;
+   size_t nKeys;
+   e3dmouse_virtual_key *vkeys;
+};
+
+static const e3dmouse_virtual_key SpaceExplorerKeys [] =
+{
+   V3DK_INVALID     // there is no button 0
+   , V3DK_1, V3DK_2
+   , V3DK_TOP, V3DK_LEFT, V3DK_RIGHT, V3DK_FRONT
+   , V3DK_ESC, V3DK_ALT, V3DK_SHIFT, V3DK_CTRL
+   , V3DK_FIT, V3DK_MENU
+   , V3DK_PLUS, V3DK_MINUS
+   , V3DK_ROTATE
+};
+
+static const e3dmouse_virtual_key SpacePilotKeys [] =
+{
+   V3DK_INVALID
+   , V3DK_1, V3DK_2, V3DK_3, V3DK_4, V3DK_5, V3DK_6
+   , V3DK_TOP, V3DK_LEFT, V3DK_RIGHT, V3DK_FRONT
+   , V3DK_ESC, V3DK_ALT, V3DK_SHIFT, V3DK_CTRL
+   , V3DK_FIT, V3DK_MENU
+   , V3DK_PLUS, V3DK_MINUS
+   , V3DK_DOMINANT, V3DK_ROTATE
+};
+
+static const struct tag_VirtualKeys _3dmouseVirtualKeys[]=
+{
+   eSpacePilot
+   , sizeof(SpacePilotKeys)/sizeof(SpacePilotKeys[0])
+   , const_cast<e3dmouse_virtual_key *>(SpacePilotKeys),
+   eSpaceExplorer
+   , sizeof(SpaceExplorerKeys)/sizeof(SpaceExplorerKeys[0])
+   , const_cast<e3dmouse_virtual_key *>(SpaceExplorerKeys)
+};
+
+/*!
+	Converts a hid device keycode (button identifier) of a pre-2009 3Dconnexion USB device to the standard 3d mouse virtual key definition.
+
+	\a pid USB Product ID (PID) of 3D mouse device
+	\a hidKeyCode  Hid keycode as retrieved from a Raw Input packet
+
+	\return The standard 3d mouse virtual key (button identifier) or zero if an error occurs.
+
+	Converts a hid device keycode (button identifier) of a pre-2009 3Dconnexion USB device
+	to the standard 3d mouse virtual key definition.
+*/
+
+unsigned short HidToVirtualKey(unsigned long pid, unsigned short hidKeyCode)
+{
+   unsigned short virtualkey=hidKeyCode;
+   for (size_t i=0; i<sizeof(_3dmouseVirtualKeys)/sizeof(_3dmouseVirtualKeys[0]); ++i)
+   {
+	  if (pid == _3dmouseVirtualKeys[i].pid)
+	  {
+		 if (hidKeyCode < _3dmouseVirtualKeys[i].nKeys)
+			virtualkey = _3dmouseVirtualKeys[i].vkeys[hidKeyCode];
+		 else
+			virtualkey = V3DK_INVALID;
+		 break;
+	  }
+   }
+   // Remaining devices are unchanged
+   return virtualkey;
+}
+
+
+static Mouse3DInput* gMouseInput = 0;
+
+bool Mouse3DInput::RawInputEventFilter(void* msg, long* result)
+{
+	if (gMouseInput == 0) return false;
+
+	MSG* message = (MSG*)(msg);
+
+	if (message->message == WM_INPUT) {
+		HRAWINPUT hRawInput = reinterpret_cast<HRAWINPUT>(message->lParam);
+		gMouseInput->OnRawInput(RIM_INPUT,hRawInput);
+		if (result != 0)  {
+			result = 0;
+		}
+		return true;
+	}
+
+	return false;
+}
+
+
+Mouse3DInput::Mouse3DInput(QWidget* widget) :
+									QObject(widget)
+{
+	fLast3dmouseInputTime = 0;
+
+	InitializeRawInput(widget->winId());
+
+	gMouseInput = this;
+	qApp->setEventFilter(Mouse3DInput::RawInputEventFilter);
+}
+
+Mouse3DInput::~Mouse3DInput()
+{
+	if (gMouseInput == this) {
+		gMouseInput = 0;
+	}
+}
+
+/*!
+	Access the mouse parameters structure
+*/
+I3dMouseParam& Mouse3DInput::MouseParams()
+{
+	return f3dMouseParams;
+}
+
+/*!
+	Access the mouse parameters structure
+*/
+const I3dMouseParam& Mouse3DInput::MouseParams() const
+{
+	return f3dMouseParams;
+}
+
+/*!
+	Called with the processed motion data when a 3D mouse event is received
+
+	The default implementation emits a Move3d signal with the motion data
+*/
+void Mouse3DInput::Move3d(HANDLE device, std::vector<float>& motionData)
+{
+	Q_UNUSED(device);
+	emit Move3d(motionData);
+}
+
+/*!
+	Called when a 3D mouse key is pressed
+
+	The default implementation emits a On3dmouseKeyDown signal with the key code.
+*/
+void Mouse3DInput::On3dmouseKeyDown(HANDLE device, int virtualKeyCode)
+{
+	Q_UNUSED(device);
+	emit On3dmouseKeyDown(virtualKeyCode);
+}
+
+/*!
+	Called when a 3D mouse key is released
+
+	The default implementation emits a On3dmouseKeyUp signal with the key code.
+*/
+void Mouse3DInput::On3dmouseKeyUp(HANDLE device, int virtualKeyCode)
+{
+	Q_UNUSED(device);
+	emit On3dmouseKeyUp(virtualKeyCode);
+}
+
+/*!
+	Get an initialized array of PRAWINPUTDEVICE for the 3D devices
+
+	pNumDevices returns the number of devices to register. Currently this is always 1.
+ */
+static PRAWINPUTDEVICE GetDevicesToRegister(unsigned int* pNumDevices)
+{
+	// Array of raw input devices to register
+	static RAWINPUTDEVICE sRawInputDevices[] = {
+		{0x01, 0x08, 0x00, 0x00} // Usage Page = 0x01 Generic Desktop Page, Usage Id= 0x08 Multi-axis Controller
+	};
+
+	if (pNumDevices) {
+		*pNumDevices = sizeof(sRawInputDevices) / sizeof(sRawInputDevices[0]);
+	}
+
+	return sRawInputDevices;
+}
+
+/*!
+	Detect the 3D mouse
+*/
+bool Mouse3DInput::Is3dmouseAttached()
+{
+	unsigned int numDevicesOfInterest = 0;
+	PRAWINPUTDEVICE devicesToRegister = GetDevicesToRegister(&numDevicesOfInterest);
+
+	unsigned int nDevices = 0;
+
+	if (::GetRawInputDeviceList(NULL, &nDevices, sizeof(RAWINPUTDEVICELIST)) != 0) {
+		return false;
+	}
+
+	if (nDevices == 0) return false;
+
+	std::vector<RAWINPUTDEVICELIST> rawInputDeviceList(nDevices);
+	if (::GetRawInputDeviceList(&rawInputDeviceList[0], &nDevices, sizeof(RAWINPUTDEVICELIST)) == static_cast<unsigned int>(-1)) {
+		return false;
+	}
+
+	for (unsigned int i = 0; i < nDevices; ++i) {
+		RID_DEVICE_INFO rdi = {sizeof(rdi)};
+		unsigned int cbSize = sizeof(rdi);
+
+		if (GetRawInputDeviceInfo(rawInputDeviceList[i].hDevice, RIDI_DEVICEINFO, &rdi, &cbSize) > 0) {
+			//skip non HID and non logitec (3DConnexion) devices
+			if (rdi.dwType != RIM_TYPEHID || rdi.hid.dwVendorId != LOGITECH_VENDOR_ID) {
+				continue;
+			}
+
+			//check if devices matches Multi-axis Controller
+			for (unsigned int j = 0; j < numDevicesOfInterest; ++j) {
+				if (devicesToRegister[j].usUsage == rdi.hid.usUsage
+						&& devicesToRegister[j].usUsagePage == rdi.hid.usUsagePage) {
+					return true;
+				}
+			}
+		}
+	}
+	return false;
+}
+
+
+
+/*!
+	Initialize the window to recieve raw-input messages
+
+	This needs to be called initially so that Windows will send the messages from the 3D mouse to the window.
+*/
+bool Mouse3DInput::InitializeRawInput(HWND hwndTarget)
+{
+	fWindow = hwndTarget;
+
+	// Simply fail if there is no window
+	if (!hwndTarget)  return false;
+
+	unsigned int numDevices = 0;
+	PRAWINPUTDEVICE devicesToRegister = GetDevicesToRegister(&numDevices);
+
+	if (numDevices == 0) return false;
+
+	// Get OS version.
+	OSVERSIONINFO osvi = {sizeof(OSVERSIONINFO),0};
+	::GetVersionEx(&osvi);
+
+	unsigned int cbSize = sizeof (devicesToRegister[0]);
+	for (size_t i = 0; i < numDevices; i++) {
+		// Set the target window to use
+		//devicesToRegister[i].hwndTarget = hwndTarget;
+
+		// If Vista or newer, enable receiving the WM_INPUT_DEVICE_CHANGE message.
+		if (osvi.dwMajorVersion >= 6) {
+			devicesToRegister[i].dwFlags |= RIDEV_DEVNOTIFY;
+		}
+	}
+	return (::RegisterRawInputDevices(devicesToRegister, numDevices, cbSize) != FALSE);
+}
+
+
+/*!
+	Get the raw input data from Windows
+
+	Includes workaround for incorrect alignment of the RAWINPUT structure on x64 os
+	when running as Wow64 (copied directly from 3DConnexion code)
+*/
+
+UINT Mouse3DInput::GetRawInputBuffer(PRAWINPUT pData, PUINT pcbSize, UINT cbSizeHeader)
+{
+#ifdef _WIN64
+	return ::GetRawInputBuffer(pData, pcbSize, cbSizeHeader);
+#else
+	BOOL bIsWow64 = FALSE;
+	::IsWow64Process(GetCurrentProcess(), &bIsWow64);
+	if (!bIsWow64 || pData==NULL) {
+		return ::GetRawInputBuffer(pData, pcbSize, cbSizeHeader);
+	} else {
+		HWND hwndTarget = fWindow; //fParent->winId();
+
+		size_t cbDataSize=0;
+		UINT nCount=0;
+		PRAWINPUT pri = pData;
+
+		MSG msg;
+		while (PeekMessage(&msg, hwndTarget, WM_INPUT, WM_INPUT, PM_NOREMOVE)) {
+			HRAWINPUT hRawInput = reinterpret_cast<HRAWINPUT>(msg.lParam);
+			size_t cbSize = *pcbSize - cbDataSize;
+			if (::GetRawInputData(hRawInput, RID_INPUT, pri, &cbSize, cbSizeHeader) == static_cast<UINT>(-1)) {
+				if (nCount==0) {
+					return static_cast<UINT>(-1);
+				}  else {
+					break;
+				}
+			}
+			++nCount;
+
+			// Remove the message for the data just read
+			PeekMessage(&msg, hwndTarget, WM_INPUT, WM_INPUT, PM_REMOVE);
+
+			pri = NEXTRAWINPUTBLOCK(pri);
+			cbDataSize = reinterpret_cast<ULONG_PTR>(pri) - reinterpret_cast<ULONG_PTR>(pData);
+			if (cbDataSize >= *pcbSize) {
+				cbDataSize = *pcbSize;
+				break;
+			}
+		}
+		return nCount;
+	}
+#endif
+}
+
+/*!
+	Process the raw input device data
+
+	On3dmouseInput() does all the preprocessing of the rawinput device data before
+	finally calling the Move3d method.
+*/
+
+void Mouse3DInput::On3dmouseInput()
+{
+	// Don't do any data processing in background
+	bool bIsForeground = (::GetActiveWindow() != NULL);
+	if (!bIsForeground) {
+		// set all cached data to zero so that a zero event is seen and the cached data deleted
+		for (std::map<HANDLE, TInputData>::iterator it = fDevice2Data.begin(); it != fDevice2Data.end(); it++) {
+			it->second.fAxes.assign(6, .0);
+			it->second.fIsDirty = true;
+		}
+	}
+
+	DWORD dwNow = ::GetTickCount();           // Current time;
+	DWORD dwElapsedTime;                      // Elapsed time since we were last here
+
+	if (0 == fLast3dmouseInputTime) {
+		dwElapsedTime = 10;                    // System timer resolution
+	} else {
+		dwElapsedTime = dwNow - fLast3dmouseInputTime;
+		if (fLast3dmouseInputTime > dwNow) {
+			dwElapsedTime = ~dwElapsedTime+1;
+		}
+		if (dwElapsedTime<1) {
+			dwElapsedTime=1;
+		} else if (dwElapsedTime > 500) {
+			// Check for wild numbers because the device was removed while sending data
+			dwElapsedTime = 10;
+		}
+	}
+
+#if _TRACE_3DINPUT_PERIOD
+	qDebug("On3DmouseInput() period is %dms\n", dwElapsedTime);
+#endif
+
+	float mouseData2Rotation = k3dmouseAngularVelocity;
+	// v = w * r,  we don't know r yet so lets assume r=1.)
+	float mouseData2PanZoom = k3dmouseAngularVelocity;
+
+	// Grab the I3dmouseParam interface
+	 I3dMouseParam& i3dmouseParam = f3dMouseParams;
+	 // Take a look at the users preferred speed setting and adjust the sensitivity accordingly
+	 I3dMouseSensor::ESpeed speedSetting = i3dmouseParam.GetSpeed();
+	 // See "Programming for the 3D Mouse", Section 5.1.3
+	float speed = (speedSetting == I3dMouseSensor::kLowSpeed ? 0.25f : speedSetting == I3dMouseSensor::kHighSpeed ?  4.f : 1.f);
+
+	 // Multiplying by the following will convert the 3d mouse data to real world units
+	mouseData2PanZoom *= speed;
+	mouseData2Rotation *= speed;
+
+	std::map<HANDLE, TInputData>::iterator iterator=fDevice2Data.begin();
+	while (iterator != fDevice2Data.end()) {
+
+		// If we have not received data for a while send a zero event
+		if ((--(iterator->second.fTimeToLive)) == 0) {
+			iterator->second.fAxes.assign(6, .0);
+		} else if (/*!t_bPoll3dmouse &&*/ !iterator->second.fIsDirty) {
+		  // If we are not polling then only handle the data that was actually received
+			++iterator;
+			continue;
+		}
+		iterator->second.fIsDirty=false;
+
+		// get a copy of the device
+		HANDLE hdevice = iterator->first;
+
+		// get a copy of the motion vectors and apply the user filters
+		std::vector<float> motionData = iterator->second.fAxes;
+
+		// apply the user filters
+
+		// Pan Zoom filter
+		// See "Programming for the 3D Mouse", Section 5.1.2
+		if (!i3dmouseParam.IsPanZoom()) {
+			// Pan zoom is switched off so set the translation vector values to zero
+			motionData[0] =  motionData[1] =  motionData[2] = 0.;
+		}
+
+		// Rotate filter
+		// See "Programming for the 3D Mouse", Section 5.1.1
+		if (!i3dmouseParam.IsRotate()) {
+			// Rotate is switched off so set the rotation vector values to zero
+			motionData[3] =  motionData[4] =  motionData[5] = 0.;
+		}
+
+		// convert the translation vector into physical data
+		for (int axis = 0; axis < 3; axis++) {
+			 motionData[axis] *= mouseData2PanZoom;
+		 }
+		// convert the directed Rotate vector into physical data
+		// See "Programming for the 3D Mouse", Section 7.2.2
+		for (int axis = 3; axis < 6; axis++) {
+			motionData[axis] *= mouseData2Rotation;
+		}
+
+		// Now that the data has had the filters and sensitivty settings applied
+		// calculate the displacements since the last view update
+		for (int axis = 0; axis < 6; axis++) {
+			motionData[axis] *= dwElapsedTime;
+		}
+
+
+		// Now a bit of book keeping before passing on the data
+		if (iterator->second.IsZero()) {
+			iterator = fDevice2Data.erase(iterator);
+		} else {
+			++iterator;
+		}
+
+		// Work out which will be the next device
+		HANDLE hNextDevice = 0;
+		if (iterator != fDevice2Data.end()) {
+			hNextDevice = iterator->first;
+		}
+
+		 // Pass the 3dmouse input to the view controller
+		 Move3d(hdevice, motionData);
+
+		// Because we don't know what happened in the previous call, the cache might have
+		// changed so reload the iterator
+		iterator = fDevice2Data.find(hNextDevice);
+   }
+
+	if (!fDevice2Data.empty()) {
+		fLast3dmouseInputTime = dwNow;
+	} else {
+		fLast3dmouseInputTime = 0;
+	}
+}
+
+/*!
+	Called when new raw input data is available
+*/
+void Mouse3DInput::OnRawInput(UINT nInputCode, HRAWINPUT hRawInput)
+{
+	const size_t cbSizeOfBuffer=1024;
+	BYTE pBuffer[cbSizeOfBuffer];
+
+	PRAWINPUT pRawInput = reinterpret_cast<PRAWINPUT>(pBuffer);
+	UINT cbSize = cbSizeOfBuffer;
+
+	if (::GetRawInputData(hRawInput, RID_INPUT, pRawInput, &cbSize, sizeof(RAWINPUTHEADER)) == static_cast<UINT>(-1)) {
+		return;
+	}
+
+	bool b3dmouseInput = TranslateRawInputData(nInputCode, pRawInput);
+	::DefRawInputProc(&pRawInput, 1, sizeof(RAWINPUTHEADER));
+
+	// Check for any buffered messages
+	cbSize = cbSizeOfBuffer;
+	UINT nCount = this->GetRawInputBuffer(pRawInput, &cbSize, sizeof(RAWINPUTHEADER));
+	if (nCount == (UINT)-1) {
+		 qDebug ("GetRawInputBuffer returned error %d\n", GetLastError());
+	}
+
+	while (nCount>0 && nCount !=  static_cast<UINT>(-1)) {
+		PRAWINPUT pri = pRawInput;
+		UINT nInput;
+		for (nInput=0; nInput<nCount; ++nInput) {
+			b3dmouseInput |= TranslateRawInputData(nInputCode, pri);
+			// clean the buffer
+			::DefRawInputProc(&pri, 1, sizeof(RAWINPUTHEADER));
+
+			pri = NEXTRAWINPUTBLOCK(pri);
+		}
+		cbSize = cbSizeOfBuffer;
+		nCount = this->GetRawInputBuffer(pRawInput, &cbSize, sizeof(RAWINPUTHEADER));
+	}
+
+	// If we have mouse input data for the app then tell tha app about it
+	if (b3dmouseInput) {
+		On3dmouseInput();
+	}
+}
+
+
+
+bool Mouse3DInput::TranslateRawInputData(UINT nInputCode, PRAWINPUT pRawInput)
+{
+	bool bIsForeground = (nInputCode == RIM_INPUT);
+
+#if _TRACE_RI_TYPE
+	qDebug("Rawinput.header.dwType=0x%x\n", pRawInput->header.dwType);
+#endif
+	// We are not interested in keyboard or mouse data received via raw input
+	if (pRawInput->header.dwType != RIM_TYPEHID) return false;
+
+#if _TRACE_RIDI_DEVICENAME
+	UINT dwSize=0;
+	if (::GetRawInputDeviceInfo(pRawInput->header.hDevice, RIDI_DEVICENAME, NULL, &dwSize) == 0)  {
+		std::vector<wchar_t> szDeviceName(dwSize+1);
+		if (::GetRawInputDeviceInfo(pRawInput->header.hDevice, RIDI_DEVICENAME, &szDeviceName[0],	&dwSize) >0) {
+			qDebug("Device Name = %s\nDevice handle = 0x%x\n", &szDeviceName[0], pRawInput->header.hDevice);
+		}
+   }
+#endif
+
+	RID_DEVICE_INFO sRidDeviceInfo;
+	sRidDeviceInfo.cbSize = sizeof(RID_DEVICE_INFO);
+	UINT cbSize = sizeof(RID_DEVICE_INFO);
+
+	if (::GetRawInputDeviceInfo(pRawInput->header.hDevice, RIDI_DEVICEINFO, &sRidDeviceInfo, &cbSize) == cbSize) {
+#if _TRACE_RIDI_DEVICEINFO
+		switch (sRidDeviceInfo.dwType)  {
+			case RIM_TYPEMOUSE:
+				qDebug("\tsRidDeviceInfo.dwType=RIM_TYPEMOUSE\n");
+				break;
+			case RIM_TYPEKEYBOARD:
+				qDebug("\tsRidDeviceInfo.dwType=RIM_TYPEKEYBOARD\n");
+				break;
+			case RIM_TYPEHID:
+				qDebug("\tsRidDeviceInfo.dwType=RIM_TYPEHID\n");
+				qDebug("\tVendor=0x%x\n\tProduct=0x%x\n\tUsagePage=0x%x\n\tUsage=0x%x\n",
+						sRidDeviceInfo.hid.dwVendorId,
+						sRidDeviceInfo.hid.dwProductId,
+						sRidDeviceInfo.hid.usUsagePage,
+						sRidDeviceInfo.hid.usUsage);
+				break;
+		}
+#endif
+
+		if (sRidDeviceInfo.hid.dwVendorId == LOGITECH_VENDOR_ID) {
+			if (pRawInput->data.hid.bRawData[0] == 0x01) { // Translation vector
+
+				TInputData& deviceData = fDevice2Data[pRawInput->header.hDevice];
+				deviceData.fTimeToLive = kTimeToLive;
+				if (bIsForeground) {
+					short* pnRawData = reinterpret_cast<short*>(&pRawInput->data.hid.bRawData[1]);
+					// Cache the pan zoom data
+					deviceData.fAxes[0] = static_cast<float>(pnRawData[0]);
+					deviceData.fAxes[1] = static_cast<float>(pnRawData[1]);
+					deviceData.fAxes[2] = static_cast<float>(pnRawData[2]);
+
+#if _TRACE_RI_RAWDATA
+					qDebug("Pan/Zoom RI Data =\t0x%x,\t0x%x,\t0x%x\n",
+									pnRawData[0],  pnRawData[1],  pnRawData[2]);
+#endif
+					if (pRawInput->data.hid.dwSizeHid >= 13) {// Highspeed package
+						// Cache the rotation data
+						deviceData.fAxes[3] = static_cast<float>(pnRawData[3]);
+						deviceData.fAxes[4] = static_cast<float>(pnRawData[4]);
+						deviceData.fAxes[5] = static_cast<float>(pnRawData[5]);
+						deviceData.fIsDirty = true;
+#if _TRACE_RI_RAWDATA
+						qDebug("Rotation RI Data =\t0x%x,\t0x%x,\t0x%x\n",
+							 pnRawData[3], pnRawData[4], pnRawData[5]);
+#endif
+						return true;
+					}
+				} else { // Zero out the data if the app is not in forground
+					deviceData.fAxes.assign(6, 0.f);
+				}
+			} else if (pRawInput->data.hid.bRawData[0] == 0x02) { // Rotation vector
+				// If we are not in foreground do nothing
+				// The rotation vector was zeroed out with the translation vector in the previous message
+				if (bIsForeground) {
+					TInputData& deviceData = fDevice2Data[pRawInput->header.hDevice];
+					deviceData.fTimeToLive = kTimeToLive;
+
+					short* pnRawData = reinterpret_cast<short*>(&pRawInput->data.hid.bRawData[1]);
+					// Cache the rotation data
+					deviceData.fAxes[3] = static_cast<float>(pnRawData[0]);
+					deviceData.fAxes[4] = static_cast<float>(pnRawData[1]);
+					deviceData.fAxes[5] = static_cast<float>(pnRawData[2]);
+					deviceData.fIsDirty = true;
+
+#if _TRACE_RI_RAWDATA
+					qDebug("Rotation RI Data =\t0x%x,\t0x%x,\t0x%x\n",
+						pnRawData[0],  pnRawData[1], pnRawData[2]);
+#endif
+					return true;
+				}
+			} else if (pRawInput->data.hid.bRawData[0] == 0x03)  { // Keystate change
+				// this is a package that contains 3d mouse keystate information
+				// bit0=key1, bit=key2 etc.
+
+
+				unsigned long dwKeystate = *reinterpret_cast<unsigned long*>(&pRawInput->data.hid.bRawData[1]);
+#if _TRACE_RI_RAWDATA
+				qDebug("ButtonData =0x%x\n", dwKeystate);
+#endif
+				// Log the keystate changes
+				unsigned long dwOldKeystate = fDevice2Keystate[pRawInput->header.hDevice];
+				if (dwKeystate != 0) {
+					fDevice2Keystate[pRawInput->header.hDevice] = dwKeystate;
+				} else {
+					fDevice2Keystate.erase(pRawInput->header.hDevice);
+				}
+
+				//  Only call the keystate change handlers if the app is in foreground
+				if (bIsForeground) {
+					unsigned long dwChange = dwKeystate ^ dwOldKeystate;
+
+
+					for (int nKeycode=1; nKeycode<33; nKeycode++) {
+						if (dwChange & 0x01) {
+							int nVirtualKeyCode = HidToVirtualKey(sRidDeviceInfo.hid.dwProductId, nKeycode);
+							if (nVirtualKeyCode) {
+								if (dwKeystate&0x01) {
+									On3dmouseKeyDown(pRawInput->header.hDevice, nVirtualKeyCode);
+								} else {
+									On3dmouseKeyUp(pRawInput->header.hDevice, nVirtualKeyCode);
+								}
+							}
+						}
+						dwChange >>=1;
+						dwKeystate >>=1;
+					}
+				}
+			}
+		}
+   }
+   return false;
+}

libs/thirdParty/3DMouse/win/Mouse3DInput.h

+#ifndef T3DMOUSE_INPUT_H
+#define T3DMOUSE_INPUT_H
+
+#include "MouseParameters.h"
+
+#include <QWidget>
+#include <vector>
+#include <map>
+
+#ifndef _WIN32_WINNT
+#define _WIN32_WINNT 0x0501  //target at least windows XP
+#endif
+
+#include <windows.h>
+
+
+/*
+	A class for connecting to and receiving data from a 3D Connexion 3D mouse
+
+	This helper class manages the connection to a 3D mouse,  collecting WM_INPUT
+	messages from Windows and converting the data into 3D motion data.
+
+	This class is based on the SDK from 3D Connexion but is modified to work with Qt.
+
+	It is Windows only since it uses the WM_INPUT messages from windows directly
+	rather than Qt events.
+
+	Note that it needs to be compiled with _WIN32_WINNT defined as 0x0501 (windows XP)
+	or later because the raw input API was added in Windows XP. This also means that
+	Qt needs to be compiled with this enabled otherwise the QEventDispatcherWin32 blocks
+	in processEvents because the raw input messages do not cause the thread to be woken if
+	Qt is compiled for Win 2000 targets.
+*/
+
+class Mouse3DInput : public QObject
+{
+	Q_OBJECT
+public:
+		Mouse3DInput(QWidget* widget);
+		~Mouse3DInput();
+
+static	bool		Is3dmouseAttached();
+
+		I3dMouseParam&			MouseParams();
+		const I3dMouseParam&	MouseParams() const;
+
+virtual	void		Move3d(HANDLE device, std::vector<float>& motionData);
+virtual void		On3dmouseKeyDown(HANDLE device, int virtualKeyCode);
+virtual void		On3dmouseKeyUp(HANDLE device, int virtualKeyCode);
+
+signals:
+
+		void		Move3d(std::vector<float>& motionData);
+		void		On3dmouseKeyDown(int virtualKeyCode);
+		void		On3dmouseKeyUp(int virtualKeyCode);
+
+private:
+
+		bool		InitializeRawInput(HWND hwndTarget);
+
+static	bool		RawInputEventFilter(void* msg, long* result);
+
+		void		OnRawInput(UINT nInputCode, HRAWINPUT hRawInput);
+		UINT		GetRawInputBuffer(PRAWINPUT pData, PUINT pcbSize, UINT cbSizeHeader);
+		bool		TranslateRawInputData(UINT nInputCode, PRAWINPUT pRawInput);
+		void		On3dmouseInput();
+
+		class TInputData
+		{
+		public:
+		   TInputData() : fAxes(6) {}
+
+		   bool IsZero() {
+			  return (0.==fAxes[0] && 0.==fAxes[1] && 0.==fAxes[2] &&
+				 0.==fAxes[3] && 0.==fAxes[4] && 0.==fAxes[5]);
+		   }
+
+		   int                   fTimeToLive; // For telling if the device was unplugged while sending data
+		   bool                  fIsDirty;
+		   std::vector<float>	 fAxes;
+
+		};
+
+		HWND								fWindow;
+
+		// Data cache to handle multiple rawinput devices
+		std::map< HANDLE, TInputData>       fDevice2Data;
+		std::map< HANDLE, unsigned long>    fDevice2Keystate;
+
+		// 3dmouse parameters
+		MouseParameters						f3dMouseParams;     // Rotate, Pan Zoom etc.
+
+		// use to calculate distance traveled since last event
+		DWORD								fLast3dmouseInputTime;
+
+};
+
+#endif

libs/thirdParty/3DMouse/win/MouseParameters.cpp

+#include "MouseParameters.h"
+
+MouseParameters::MouseParameters(): fNavigation(kObjectMode)
+	  , fPivot(kAutoPivot)
+	  , fPivotVisibility(kShowPivot)
+	  , fIsLockHorizon(true)
+	  , fIsPanZoom(true)
+	  , fIsRotate(true)
+	  , fSpeed(kLowSpeed)
+{
+}
+
+MouseParameters::~MouseParameters()
+{
+}
+
+bool MouseParameters::IsPanZoom()  const
+{
+	return fIsPanZoom;
+}
+
+bool MouseParameters::IsRotate()  const
+{
+	return fIsRotate;
+}
+
+MouseParameters::ESpeed MouseParameters::GetSpeed()  const
+{
+	return fSpeed;
+}
+
+void MouseParameters::SetPanZoom(bool isPanZoom)
+{
+	fIsPanZoom=isPanZoom;
+}
+
+void MouseParameters::SetRotate(bool isRotate)
+{
+	fIsRotate=isRotate;
+}
+
+void MouseParameters::SetSpeed(ESpeed speed)
+{
+	fSpeed=speed;
+}
+
+
+MouseParameters::ENavigation MouseParameters::GetNavigationMode()  const
+{
+	return fNavigation;
+}
+
+MouseParameters::EPivot MouseParameters::GetPivotMode()  const
+{
+	return fPivot;
+}
+
+MouseParameters::EPivotVisibility MouseParameters::GetPivotVisibility()  const
+{
+	return fPivotVisibility;
+}
+
+bool MouseParameters::IsLockHorizon()  const
+{
+	return fIsLockHorizon;
+}
+
+void MouseParameters::SetLockHorizon(bool bOn)
+{
+	fIsLockHorizon=bOn;
+}
+
+void MouseParameters::SetNavigationMode(ENavigation navigation)
+{
+	fNavigation=navigation;
+}
+
+void MouseParameters::SetPivotMode(EPivot pivot)
+{
+   if (fPivot!=kManualPivot || pivot!=kAutoPivotOverride)
+	  fPivot = pivot;
+}
+
+void MouseParameters::SetPivotVisibility(EPivotVisibility visibility)
+{
+	fPivotVisibility = visibility;
+}
+

libs/thirdParty/3DMouse/win/MouseParameters.h

+
+#ifndef T3D_MOUSE_PARAMS_H
+#define T3D_MOUSE_PARAMS_H
+
+#include "I3dMouseParams.h"
+
+
+class MouseParameters : public I3dMouseParam
+{
+public:
+	MouseParameters();
+	~MouseParameters();
+
+	// I3dmouseSensor interface
+	bool		IsPanZoom()  const;
+	bool		IsRotate()  const;
+	ESpeed		GetSpeed()  const;
+
+	void		SetPanZoom(bool isPanZoom);
+	void		SetRotate(bool isRotate);
+	void		SetSpeed(ESpeed speed);
+
+	// I3dmouseNavigation interface
+	ENavigation			GetNavigationMode()  const;
+	EPivot				GetPivotMode()  const;
+	EPivotVisibility	GetPivotVisibility()  const;
+	bool				IsLockHorizon()  const;
+
+	void				SetLockHorizon(bool bOn);
+	void				SetNavigationMode(ENavigation navigation);
+	void				SetPivotMode(EPivot pivot);
+	void				SetPivotVisibility(EPivotVisibility visibility);
+
+private:
+
+	MouseParameters (const MouseParameters&);
+	const MouseParameters& operator =(const MouseParameters&);
+
+	ENavigation			fNavigation;
+	EPivot				fPivot;
+	EPivotVisibility	fPivotVisibility;
+	bool				fIsLockHorizon;
+
+	bool				fIsPanZoom;
+	bool				fIsRotate;
+	ESpeed				fSpeed;
+};
+
+#endif

qgroundcontrol.pro

@@ -624,3 +624,36 @@ win32-msvc2008|win32-msvc2010|linux {
     LIBS += -Llibs/thirdParty/libxbee/lib \
         -llibxbee
 }
+
+###################################################################
+#### --- 3DConnexion 3d Mice support (e.g. spacenavigator) --- ####
+###################################################################
+
+# xdrvlib only supported by linux (theoretical all X11) systems
+# You have to install the official 3DxWare driver for linux to use 3D mouse support on linux systems!
+linux-g++|linux-g++-64{
+    exists(/usr/local/lib/libxdrvlib.so){
+        message("Including support for Magellan 3DxWare for linux system.")
+        SOURCES  += src/input/Mouse6dofInput.cpp
+        HEADERS  += src/input/Mouse6dofInput.h
+        LIBS += -L/usr/local/lib/ -lxdrvlib
+        INCLUDEPATH *= /usr/local/include
+        DEFINES += MOUSE_ENABLED_LINUX \
+                    ParameterCheck                      # Hack: Has to be defined for magellan usage
+    }
+}
+
+# Support for Windows systems
+# You have to install the official 3DxWare driver for Windows to use the 3D mouse support on Windows systems!
+win32-msvc2008|win32-msvc2010 {
+    message("Including support for 3DxWare for Windows system.")
+    SOURCES  += libs/thirdParty/3DMouse/win/MouseParameters.cpp \
+                libs/thirdParty/3DMouse/win/Mouse3DInput.cpp \
+                src/input/Mouse6dofInput.cpp
+    HEADERS  += libs/thirdParty/3DMouse/win/I3dMouseParams.h \
+                libs/thirdParty/3DMouse/win/MouseParameters.h \
+                libs/thirdParty/3DMouse/win/Mouse3DInput.h \
+                src/input/Mouse6dofInput.h
+    INCLUDEPATH += libs/thirdParty/3DMouse/win
+    DEFINES += MOUSE_ENABLED_WIN
+}

src/input/Mouse6dofInput.cpp

+/**
+ * @file
+ *   @brief 3dConnexion 3dMouse interface for QGroundControl
+ *
+ *   @author Matthias Krebs <makrebs@student.ethz.ch>
+ *
+ */
+
+#include "Mouse6dofInput.h"
+#include "UAS.h"
+#include "UASManager.h"
+#include "QMessageBox"
+
+#ifdef MOUSE_ENABLED_LINUX
+#include <QX11Info>
+#include <X11/Xlib.h>
+#ifdef Success
+#undef Success              // Eigen library doesn't work if Success is defined
+#endif //Success
+extern "C"
+{
+#include "xdrvlib.h"
+}
+#endif // MOUSE_ENABLED_LINUX
+
+#ifdef MOUSE_ENABLED_WIN
+Mouse6dofInput::Mouse6dofInput(Mouse3DInput* mouseInput) :
+    mouse3DMax(0.075),   // TODO: check maximum value fot plugged device
+    uas(NULL),
+    done(false),
+    mouseActive(false),
+    translationActive(true),
+    rotationActive(true),
+    xValue(0.0),
+    yValue(0.0),
+    zValue(0.0),
+    aValue(0.0),
+    bValue(0.0),
+    cValue(0.0)
+{
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
+    // Connect 3DxWare SDK MotionEvent
+    connect(mouseInput, SIGNAL(Move3d(std::vector<float>&)), this, SLOT(motion3DMouse(std::vector<float>&)));
+    connect(mouseInput, SIGNAL(On3dmouseKeyDown(int)), this, SLOT(button3DMouseDown(int)));
+    //connect(mouseInput, SIGNAL(On3dmouseKeyUp(int)), this, SLOT(FUNCTION(int)));
+
+}
+#endif //MOUSE_ENABLED_WIN
+
+#ifdef MOUSE_ENABLED_LINUX
+Mouse6dofInput::Mouse6dofInput(QWidget* parent) :
+    mouse3DMax(350.0),   // TODO: check maximum value fot plugged device
+    uas(NULL),
+    done(false),
+    mouseActive(false),
+    translationActive(true),
+    rotationActive(true),
+    xValue(0.0),
+    yValue(0.0),
+    zValue(0.0),
+    aValue(0.0),
+    bValue(0.0),
+    cValue(0.0)
+{
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
+
+    if (!mouseActive)
+    {
+//        // man visudo --> then you can omit giving password (success not guarantied..)
+//        qDebug() << "Starting 3DxWare Daemon for 3dConnexion 3dMouse";
+//        QString processProgramm = "gksudo";
+//        QStringList processArguments;
+//        processArguments << "/etc/3DxWare/daemon/3dxsrv -d usb";
+//        process3dxDaemon = new QProcess();
+//        process3dxDaemon->start(processProgramm, processArguments);
+//    //    process3dxDaemon->waitForFinished();
+//    //    {
+//    //        qDebug() << "... continuing without 3DxWare. May not be initialized properly!";
+//    //        qDebug() << "Try in terminal as user root:" << processArguments.last();
+//    //    }
+
+        Display *display = QX11Info::display();
+        if(!display)
+        {
+            qDebug() << "Cannot open display!" << endl;
+        }
+        if ( !MagellanInit( display, parent->winId() ) )
+        {
+            QMessageBox msgBox;
+            msgBox.setIcon(QMessageBox::Information);
+            msgBox.setText(tr("No 3DxWare driver is running."));
+            msgBox.setInformativeText(tr("Enter in Terminal 'sudo /etc/3DxWare/daemon/3dxsrv -d usb' and then restart QGroundControl."));
+            msgBox.setStandardButtons(QMessageBox::Ok);
+            msgBox.setDefaultButton(QMessageBox::Ok);
+            msgBox.exec();
+
+            qDebug() << "No 3DxWare driver is running!";
+            return;
+        }
+        else
+        {
+            qDebug() << "Initialized 3dMouse";
+            mouseActive = true;
+        }
+    }
+    else
+    {
+        qDebug() << "3dMouse already initialized..";
+    }
+
+}
+#endif //MOUSE_ENABLED_LINUX
+
+
+Mouse6dofInput::~Mouse6dofInput()
+{
+    done = true;
+}
+
+void Mouse6dofInput::setActiveUAS(UASInterface* uas)
+{
+    // Only connect / disconnect is the UAS is of a controllable UAS class
+    UAS* tmp = 0;
+    if (this->uas)
+    {
+        tmp = dynamic_cast<UAS*>(this->uas);
+        if(tmp)
+        {
+            disconnect(this, SIGNAL(mouse6dofChanged(double,double,double,double,double,double)), tmp, SLOT(setManual6DOFControlCommands(double,double,double,double,double,double)));
+            // Todo: disconnect button mapping
+        }
+    }
+
+    this->uas = uas;
+
+    tmp = dynamic_cast<UAS*>(this->uas);
+    if(tmp) {
+                connect(this, SIGNAL(mouse6dofChanged(double,double,double,double,double,double)), tmp, SLOT(setManual6DOFControlCommands(double,double,double,double,double,double)));
+                // Todo: connect button mapping
+    }
+    if (!isRunning())
+    {
+        start();
+    }
+}
+
+void Mouse6dofInput::init()
+{
+    // Make sure active UAS is set
+    setActiveUAS(UASManager::instance()->getActiveUAS());
+}
+
+void Mouse6dofInput::run()
+{
+    init();
+
+    forever
+    {
+        if (done)
+        {
+           done = false;
+           exit();
+        }
+
+        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);
+    }
+}
+
+#ifdef MOUSE_ENABLED_WIN
+void Mouse6dofInput::motion3DMouse(std::vector<float> &motionData)
+{
+    if (motionData.size() < 6) return;
+    mouseActive = true;
+
+    if (translationActive)
+    {
+        xValue = (double)1.0e2f*motionData[ 1 ] / mouse3DMax;
+        yValue = (double)1.0e2f*motionData[ 0 ] / mouse3DMax;
+        zValue = (double)1.0e2f*motionData[ 2 ] / mouse3DMax;
+    }else{
+        xValue = 0;
+        yValue = 0;
+        zValue = 0;
+    }
+    if (rotationActive)
+    {
+        aValue = (double)1.0e2f*motionData[ 4 ] / mouse3DMax;
+        bValue = (double)1.0e2f*motionData[ 3 ] / mouse3DMax;
+        cValue = (double)1.0e2f*motionData[ 5 ] / mouse3DMax;
+    }else{
+        aValue = 0;
+        bValue = 0;
+        cValue = 0;
+    }
+
+    //qDebug() << "NEW 3D MOUSE VALUES -- X" << xValue << " -- Y" << yValue << " -- Z" << zValue << " -- A" << aValue << " -- B" << bValue << " -- C" << cValue;
+}
+#endif //MOUSE_ENABLED_WIN
+
+#ifdef MOUSE_ENABLED_WIN
+void Mouse6dofInput::button3DMouseDown(int button)
+{
+    switch(button)
+    {
+    case 1:
+    {
+            rotationActive = !rotationActive;
+            emit mouseRotationActiveChanged(rotationActive);
+            qDebug() << "Changed 3DMouse Rotation to " << (bool)rotationActive;
+        break;
+    }
+    case 2:
+    {
+            translationActive = !translationActive;
+            emit mouseTranslationActiveChanged(translationActive);
+            qDebug() << "Changed 3DMouse Translation to" << (bool)translationActive;
+        break;
+    }
+    default:
+        break;
+    }
+}
+#endif //MOUSE_ENABLED_WIN
+
+#ifdef MOUSE_ENABLED_LINUX
+void Mouse6dofInput::handleX11Event(XEvent *event)
+{
+    //qDebug("XEvent occured...");
+    if (!mouseActive)
+    {
+        qDebug() << "3dMouse not initialized. Cancelled handling X11event for 3dMouse";
+        return;
+    }
+
+    MagellanFloatEvent MagellanEvent;
+
+    Display *display = QX11Info::display();
+    if(!display)
+    {
+        qDebug() << "Cannot open display!" << endl;
+    }
+
+    switch (event->type)
+    {
+    case ClientMessage:
+      switch( MagellanTranslateEvent( display, event, &MagellanEvent, 1.0, 1.0 ) )
+      {
+        case MagellanInputMotionEvent :
+             MagellanRemoveMotionEvents( display );
+             for (int i = 0; i < 6; i++) {  // Saturation
+                 MagellanEvent.MagellanData[i] = (abs(MagellanEvent.MagellanData[i]) < mouse3DMax) ? MagellanEvent.MagellanData[i] : (mouse3DMax*MagellanEvent.MagellanData[i]/abs(MagellanEvent.MagellanData[i]));
+             }
+
+             // Check whether translational motions are enabled
+             if (translationActive)
+             {
+                 xValue = MagellanEvent.MagellanData[ MagellanZ ] / mouse3DMax;
+                 yValue = MagellanEvent.MagellanData[ MagellanX ] / mouse3DMax;
+                 zValue = - MagellanEvent.MagellanData[ MagellanY ] / mouse3DMax;
+             }else{
+                 xValue = 0;
+                 yValue = 0;
+                 zValue = 0;
+             }
+             // Check whether rotational motions are enabled
+             if (rotationActive)
+             {
+                 aValue = MagellanEvent.MagellanData[ MagellanC ] / mouse3DMax;
+                 bValue = MagellanEvent.MagellanData[ MagellanA ] / mouse3DMax;
+                 cValue = - MagellanEvent.MagellanData[ MagellanB ] / mouse3DMax;
+             }else{
+                 aValue = 0;
+                 bValue = 0;
+                 cValue = 0;
+             }
+             //qDebug() << "NEW 3D MOUSE VALUES -- X" << xValue << " -- Y" << yValue << " -- Z" << zValue << " -- A" << aValue << " -- B" << bValue << " -- C" << cValue;
+        break;
+
+        case MagellanInputButtonPressEvent :
+            qDebug() << "MagellanInputButtonPressEvent called with button " << MagellanEvent.MagellanButton;
+            switch (MagellanEvent.MagellanButton)
+            {
+            case 1:
+            {
+                    rotationActive = !rotationActive;
+                    emit mouseRotationActiveChanged(rotationActive);
+                    qDebug() << "Changed 3DMouse Rotation to " << (bool)rotationActive;
+                break;
+            }
+            case 2:
+            {
+                    translationActive = !translationActive;
+                    emit mouseTranslationActiveChanged(translationActive);
+                    qDebug() << "Changed 3DMouse Translation to" << (bool)translationActive;
+                break;
+            }
+            default:
+                break;
+            }
+        default:
+            break;
+        }
+    }
+}
+#endif //MOUSE_ENABLED_LINUX

src/input/Mouse6dofInput.h

+/**
+ * @file
+ *   @brief Definition of 3dConnexion 3dMouse interface for QGroundControl
+ *
+ *   @author Matthias Krebs <makrebs@student.ethz.ch>
+ *
+ */
+
+#ifndef MOUSE6DOFINPUT_H
+#define MOUSE6DOFINPUT_H
+
+#include <QThread>
+
+#ifdef MOUSE_ENABLED_WIN
+#include "Mouse3DInput.h"
+#endif //MOUSE_ENABLED_WIN
+
+#include "UASInterface.h"
+
+class Mouse6dofInput : public QThread
+{
+    Q_OBJECT
+
+public:
+#ifdef MOUSE_ENABLED_WIN
+    Mouse6dofInput(Mouse3DInput* mouseInput);
+#endif //MOUSE_ENABLED_WIN
+#ifdef MOUSE_ENABLED_LINUX
+    Mouse6dofInput(QWidget* parent);
+#endif //MOUSE_ENABLED_LINUX
+
+    ~Mouse6dofInput();
+    void run();
+
+    const double mouse3DMax;
+
+protected:
+    void init();
+
+    UASInterface* uas;
+    bool done;
+    bool mouseActive;
+    bool translationActive;
+    bool rotationActive;
+
+    double xValue;
+    double yValue;
+    double zValue;
+    double aValue;
+    double bValue;
+    double cValue;
+
+
+signals:
+    /**
+     * @brief Input of the 3d mouse has changed
+     *
+     * @param x Input x direction, range [-1, 1]
+     * @param y Input y direction, range [-1, 1]
+     * @param z Input z direction, range [-1, 1]
+     * @param a Input x rotation,  range [-1, 1]
+     * @param b Input y rotation,  range [-1, 1]
+     * @param c Input z rotation,  range [-1, 1]
+     */
+    void mouse6dofChanged(double x, double y, double z, double a, double b, double c);
+
+   /**
+     * @brief Activity of translational 3DMouse inputs changed
+     * @param translationEnable, true: translational inputs active; false: translational inputs ingored
+     */
+    void mouseTranslationActiveChanged(bool translationEnable);
+
+   /**
+     * @brief Activity of rotational 3DMouse inputs changed
+     * @param rotationEnable, true: rotational inputs active; false: rotational inputs ingored
+     */
+    void mouseRotationActiveChanged(bool rotationEnable);
+
+public slots:
+    void setActiveUAS(UASInterface* uas);
+#ifdef MOUSE_ENABLED_WIN
+    /** @brief Get a motion input from 3DMouse */
+    void motion3DMouse(std::vector<float> &motionData);
+    /** @brief Get a button input from 3DMouse */
+    void button3DMouseDown(int button);
+#endif //MOUSE_ENABLED_WIN
+#ifdef MOUSE_ENABLED_LINUX
+    /** @brief Get an XEvent to check it for an 3DMouse event (motion or button) */
+    void handleX11Event(XEvent* event);
+#endif //MOUSE_ENABLED_LINUX
+
+};
+
+#endif // MOUSE6DOFINPUT_H

src/uas/ArduPilotMegaMAV.cc

@@ -32,6 +32,8 @@ ArduPilotMegaMAV::ArduPilotMegaMAV(MAVLinkProtocol* mavlink, int id) :
     UAS(mavlink, id)//,
     // place other initializers here
 {
+    // Ask for all streams at 4 Hz
+    enableAllDataTransmission(4);
 }
 
 /**

src/uas/UAS.cc

@@ -2454,7 +2454,25 @@ 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)
+{
+    // 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";
     }
 }
 

src/uas/UAS.h

@@ -573,6 +573,9 @@ public slots:
     /** @brief Receive a button pressed event from an input device, e.g. joystick */
     void receiveButton(int buttonIndex);
 
+    /** @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);
+
     /** @brief Add a link associated with this robot */
     void addLink(LinkInterface* link);
     /** @brief Remove a link associated with this robot */

src/uas/UASInterface.h

@@ -475,6 +475,7 @@ signals:
     void imageDataReceived(int imgid, const unsigned char* imageData, int length, int startIndex);
     /** @brief Emit the new system type */
     void systemTypeSet(UASInterface* uas, unsigned int type);
+
     /** @brief Attitude control enabled/disabled */
     void attitudeControlEnabled(bool enabled);
     /** @brief Position 2D control enabled/disabled */
@@ -483,6 +484,30 @@ signals:
     void positionZControlEnabled(bool enabled);
     /** @brief Heading control enabled/disabled */
     void positionYawControlEnabled(bool enabled);
+    /** @brief Optical flow status changed */
+    void opticalFlowStatusChanged(bool supported, bool enabled, bool ok);
+    /** @brief Vision based localization status changed */
+    void visionLocalizationStatusChanged(bool supported, bool enabled, bool ok);
+    /** @brief Infrared / Ultrasound status changed */
+    void distanceSensorStatusChanged(bool supported, bool enabled, bool ok);
+    /** @brief Gyroscope status changed */
+    void gyroStatusChanged(bool supported, bool enabled, bool ok);
+    /** @brief Accelerometer status changed */
+    void accelStatusChanged(bool supported, bool enabled, bool ok);
+    /** @brief Magnetometer status changed */
+    void magSensorStatusChanged(bool supported, bool enabled, bool ok);
+    /** @brief Barometer status changed */
+    void baroStatusChanged(bool supported, bool enabled, bool ok);
+    /** @brief Differential pressure / airspeed status changed */
+    void airspeedStatusChanged(bool supported, bool enabled, bool ok);
+    /** @brief Actuator status changed */
+    void actuatorStatusChanged(bool supported, bool enabled, bool ok);
+    /** @brief Laser scanner status changed */
+    void laserStatusChanged(bool supported, bool enabled, bool ok);
+    /** @brief Vicon / Leica Geotracker status changed */
+    void groundTruthSensorStatusChanged(bool supported, bool enabled, bool ok);
+
+
     /** @brief Value of a remote control channel (raw) */
     void remoteControlChannelRawChanged(int channelId, float raw);
     /** @brief Value of a remote control channel (scaled)*/

src/ui/HSIDisplay.cc

@@ -123,7 +123,34 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
     userSetPointSet(false),
     userXYSetPointSet(false),
     userZSetPointSet(false),
-    userYawSetPointSet(false)
+    userYawSetPointSet(false),
+    gyroKnown(false),
+    gyroON(false),
+    gyroOK(false),
+    accelKnown(false),
+    accelON(false),
+    accelOK(false),
+    magKnown(false),
+    magON(false),
+    magOK(false),
+    pressureKnown(false),
+    pressureON(false),
+    pressureOK(false),
+    diffPressureKnown(false),
+    diffPressureON(false),
+    diffPressureOK(false),
+    flowKnown(false),
+    flowON(false),
+    flowOK(false),
+    laserKnown(false),
+    laserON(false),
+    laserOK(false),
+    viconKnown(false),
+    viconON(false),
+    viconOK(false),
+    actuatorsKnown(false),
+    actuatorsON(false),
+    actuatorsOK(false)
 {
     refreshTimer->setInterval(updateInterval);
 
@@ -845,6 +872,16 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
         disconnect(this->uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int)));
         disconnect(this->uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int)));
         disconnect(this->uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float)));
+
+        disconnect(this->uas, SIGNAL(gyroStatusChanged(bool,bool,bool)), this, SLOT(updateGyroStatus(bool,bool,bool)));
+        disconnect(this->uas, SIGNAL(accelStatusChanged(bool,bool,bool)), this, SLOT(updateAccelStatus(bool,bool,bool)));
+        disconnect(this->uas, SIGNAL(magSensorStatusChanged(bool,bool,bool)), this, SLOT(updateMagSensorStatus(bool,bool,bool)));
+        disconnect(this->uas, SIGNAL(baroStatusChanged(bool,bool,bool)), this, SLOT(updateBaroStatus(bool,bool,bool)));
+        disconnect(this->uas, SIGNAL(airspeedStatusChanged(bool,bool,bool)), this, SLOT(updateAirspeedStatus(bool,bool,bool)));
+        disconnect(this->uas, SIGNAL(opticalFlowStatusChanged(bool,bool,bool)), this, SLOT(updateOpticalFlowStatus(bool,bool,bool)));
+        disconnect(this->uas, SIGNAL(laserStatusChanged(bool,bool,bool)), this, SLOT(updateLaserStatus(bool,bool,bool)));
+        disconnect(this->uas, SIGNAL(groundTruthSensorStatusChanged(bool,bool,bool)), this, SLOT(updateGroundTruthSensorStatus(bool,bool,bool)));
+        disconnect(this->uas, SIGNAL(actuatorStatusChanged(bool,bool,bool)), this, SLOT(updateActuatorStatus(bool,bool,bool)));
     }
 
     connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
@@ -867,6 +904,16 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
     connect(uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int)));
     connect(uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float)));
 
+    connect(uas, SIGNAL(gyroStatusChanged(bool,bool,bool)), this, SLOT(updateGyroStatus(bool,bool,bool)));
+    connect(uas, SIGNAL(accelStatusChanged(bool,bool,bool)), this, SLOT(updateAccelStatus(bool,bool,bool)));
+    connect(uas, SIGNAL(magSensorStatusChanged(bool,bool,bool)), this, SLOT(updateMagSensorStatus(bool,bool,bool)));
+    connect(uas, SIGNAL(baroStatusChanged(bool,bool,bool)), this, SLOT(updateBaroStatus(bool,bool,bool)));
+    connect(uas, SIGNAL(airspeedStatusChanged(bool,bool,bool)), this, SLOT(updateAirspeedStatus(bool,bool,bool)));
+    connect(uas, SIGNAL(opticalFlowStatusChanged(bool,bool,bool)), this, SLOT(updateOpticalFlowStatus(bool,bool,bool)));
+    connect(uas, SIGNAL(laserStatusChanged(bool,bool,bool)), this, SLOT(updateLaserStatus(bool,bool,bool)));
+    connect(uas, SIGNAL(groundTruthSensorStatusChanged(bool,bool,bool)), this, SLOT(updateGroundTruthSensorStatus(bool,bool,bool)));
+    connect(uas, SIGNAL(actuatorStatusChanged(bool,bool,bool)), this, SLOT(updateActuatorStatus(bool,bool,bool)));
+
     this->uas = uas;
 
     resetMAVState();

src/ui/HSIDisplay.h

@@ -66,6 +66,83 @@ public slots:
     void updateAttitudeControllerEnabled(bool enabled);
     void updatePositionXYControllerEnabled(bool enabled);
     void updatePositionZControllerEnabled(bool enabled);
+
+    /** @brief Optical flow status changed */
+    void updateOpticalFlowStatus(bool supported, bool enabled, bool ok) {
+        if (supported && enabled && ok) {
+            visionFix = true;
+        } else {
+            visionFix = false;
+        }
+    }
+
+    /** @brief Vision based localization status changed */
+    void updateVisionLocalizationStatus(bool supported, bool enabled, bool ok) {
+        if (enabled && ok) {
+            visionFix = true;
+        } else {
+            visionFix = false;
+        }
+        visionFixKnown = supported;
+    }
+    /** @brief Infrared / Ultrasound status changed */
+    void updateDistanceSensorStatus(bool supported, bool enabled, bool ok) {
+        if (enabled && ok) {
+            iruFix = true;
+        } else {
+            iruFix = false;
+        }
+        iruFixKnown = supported;
+    }
+    /** @brief Gyroscope status changed */
+    void updateGyroStatus(bool supported, bool enabled, bool ok) {
+        gyroKnown = supported;
+        gyroON = enabled;
+        gyroOK = ok;
+    }
+    /** @brief Accelerometer status changed */
+    void updateAccelStatus(bool supported, bool enabled, bool ok) {
+        accelKnown = supported;
+        accelON = enabled;
+        accelOK = ok;
+    }
+    /** @brief Magnetometer status changed */
+    void updateMagSensorStatus(bool supported, bool enabled, bool ok) {
+        magKnown = supported;
+        magON = enabled;
+        magOK = ok;
+    }
+    /** @brief Barometer status changed */
+    void updateBaroStatus(bool supported, bool enabled, bool ok) {
+        pressureKnown = supported;
+        pressureON = enabled;
+        pressureOK = ok;
+    }
+    /** @brief Differential pressure / airspeed status changed */
+    void updateAirspeedStatus(bool supported, bool enabled, bool ok) {
+        diffPressureKnown = supported;
+        diffPressureON = enabled;
+        diffPressureOK = ok;
+    }
+    /** @brief Actuator status changed */
+    void updateActuatorStatus(bool supported, bool enabled, bool ok) {
+        actuatorsKnown = supported;
+        actuatorsON = enabled;
+        actuatorsOK = ok;
+    }
+    /** @brief Laser scanner status changed */
+    void updateLaserStatus(bool supported, bool enabled, bool ok) {
+        laserKnown = supported;
+        laserON = enabled;
+        laserOK = ok;
+    }
+    /** @brief Vicon / Leica Geotracker status changed */
+    void updateGroundTruthSensorStatus(bool supported, bool enabled, bool ok) {
+        viconKnown = supported;
+        viconON = enabled;
+        viconOK = ok;
+    }
+
     void updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance);
     /** @brief Heading control enabled/disabled */
     void updatePositionYawControllerEnabled(bool enabled);

src/ui/MainWindow.cc

@@ -182,6 +182,20 @@ MainWindow::MainWindow(QWidget *parent):
     joystickWidget = 0;
     joystick = new JoystickInput();
 
+#ifdef MOUSE_ENABLED_WIN
+    emit initStatusChanged("Initializing 3D mouse interface.");
+
+    mouseInput = new Mouse3DInput(this);
+    mouse = new Mouse6dofInput(mouseInput);
+#endif //MOUSE_ENABLED_WIN
+
+#if MOUSE_ENABLED_LINUX
+    emit initStatusChanged("Initializing 3D mouse interface.");
+
+    mouse = new Mouse6dofInput(this);
+    connect(this, SIGNAL(x11EventOccured(XEvent*)), mouse, SLOT(handleX11Event(XEvent*)));
+#endif //MOUSE_ENABLED_LINUX
+
     // Connect link
     if (autoReconnect)
     {
@@ -1719,3 +1733,12 @@ QList<QAction*> MainWindow::listLinkMenuActions(void)
 {
     return ui.menuNetwork->actions();
 }
+
+#ifdef MOUSE_ENABLED_LINUX
+bool MainWindow::x11Event(XEvent *event)
+{
+    emit x11EventOccured(event);
+    //qDebug("XEvent occured...");
+    return false;
+}
+#endif // MOUSE_ENABLED_LINUX

src/ui/MainWindow.h

@@ -54,6 +54,9 @@ This file is part of the QGROUNDCONTROL project
 #include "HUD.h"
 #include "JoystickWidget.h"
 #include "input/JoystickInput.h"
+#if (defined MOUSE_ENABLED_WIN) | (defined MOUSE_ENABLED_LINUX)
+#include "Mouse6dofInput.h"
+#endif // MOUSE_ENABLED_WIN
 #include "DebugConsole.h"
 #include "ParameterInterface.h"
 #include "XMLCommProtocolWidget.h"
@@ -232,6 +235,10 @@ public slots:
 
 signals:
     void initStatusChanged(const QString& message);
+#ifdef MOUSE_ENABLED_LINUX
+    /** @brief Forward X11Event to catch 3DMouse inputs */
+    void x11EventOccured(XEvent *event);
+#endif //MOUSE_ENABLED_LINUX
 
 public:
     QGCMAVLinkLogPlayer* getLogPlayer()
@@ -371,6 +378,18 @@ protected:
 
     JoystickInput* joystick;
 
+#ifdef MOUSE_ENABLED_WIN
+    /** @brief 3d Mouse support (WIN only) */
+    Mouse3DInput* mouseInput;               ///< 3dConnexion 3dMouse SDK
+    Mouse6dofInput* mouse;                  ///< Implementation for 3dMouse input
+#endif // MOUSE_ENABLED_WIN
+
+#ifdef MOUSE_ENABLED_LINUX
+    /** @brief Reimplementation of X11Event to handle 3dMouse Events (magellan) */
+    bool x11Event(XEvent *event);
+    Mouse6dofInput* mouse;                  ///< Implementation for 3dMouse input
+#endif // MOUSE_ENABLED_LINUX
+
     /** User interface actions **/
     QAction* connectUASAct;
     QAction* disconnectUASAct;