From a0feb94b670a150e4574f1921cfcfe59049be822 Mon Sep 17 00:00:00 2001
From: Wim Vriend <facetracknoir@gmail.com>
Date: Fri, 11 Jun 2010 18:09:28 +0000
Subject: PPJoy 1st success

git-svn-id: svn+ssh://svn.code.sf.net/p/facetracknoir/code@10 19e81ba0-9b1a-49c3-bd6c-561e1906d5fb
---
 FaceTrackNoIR/PPJoyServer.cpp | 130 ++++++++++++++++++++++++++++++++++--------
 1 file changed, 107 insertions(+), 23 deletions(-)

(limited to 'FaceTrackNoIR/PPJoyServer.cpp')

diff --git a/FaceTrackNoIR/PPJoyServer.cpp b/FaceTrackNoIR/PPJoyServer.cpp
index 59c6f675..46a66c44 100644
--- a/FaceTrackNoIR/PPJoyServer.cpp
+++ b/FaceTrackNoIR/PPJoyServer.cpp
@@ -30,6 +30,10 @@
 #include "Tracker.h"
 #include <Winsock.h>
 
+long PPJoyServer::PPJoyCorrection = 1470;
+long PPJoyServer::analogDefault = (PPJOY_AXIS_MIN+PPJOY_AXIS_MAX)/2 - PPJoyServer::PPJoyCorrection;
+static const char* DevName = "\\\\.\\PPJoyIOCTL1";
+
 /** constructor **/
 PPJoyServer::PPJoyServer( Tracker *parent ) {
 
@@ -39,36 +43,116 @@ PPJoyServer::PPJoyServer( Tracker *parent ) {
 	// Create events
 	m_StopThread = CreateEvent(0, TRUE, FALSE, 0);
 	m_WaitThread = CreateEvent(0, TRUE, FALSE, 0);
+
+	// Initialize arrays
+	for (int i = 0;i < 3;i++) {
+		centerPos[i] = 0;
+		centerRot[i] = 0;
+	}
+
+	/* Open a handle to the control device for the first virtual joystick. */
+	/* Virtual joystick devices are names PPJoyIOCTL1 to PPJoyIOCTL16. */
+	h = CreateFileA((LPCSTR) DevName,GENERIC_WRITE,FILE_SHARE_WRITE,NULL,OPEN_EXISTING,0,NULL);
+
+	/* Make sure we could open the device! */
+	if (h == INVALID_HANDLE_VALUE)
+	{
+		QMessageBox::critical(0, "Connection Failed", QString("FaceTrackNoIR failed to connect to Virtual Joystick.\nCheck if it was properly installed!"));
+		return;
+	}
+}
+
+/** destructor **/
+PPJoyServer::~PPJoyServer() {
+
+	// Trigger thread to stop
+	::SetEvent(m_StopThread);
+
+	// Wait until thread finished
+	::WaitForSingleObject(m_WaitThread, INFINITE);
+
+	// Close handles
+	::CloseHandle(m_StopThread);
+	::CloseHandle(m_WaitThread);
+
+	//
+	// Free the Virtual Joystick
+	//
+	CloseHandle(h);
+	//terminates the QThread and waits for finishing the QThread
+	terminate();
+	wait();
 }
 
 /** QThread run @override **/
 void PPJoyServer::run() {
 
-	//// Init. the data
-	//TestData.x = 0.0f;
-	//TestData.y = 0.0f;
-	//TestData.z = 0.0f;
-	//TestData.h = 0.0f;
-	//TestData.p = 0.0f;
-	//TestData.r = 0.0f;
-	//TestData.status = 0;
-	//fg_cmd = 0;
-
-	// Create UDP-sockets
-	//inSocket = new QUdpSocket();
-	//outSocket = new QUdpSocket();
-
-	//// Connect the inSocket to the port, to receive readyRead messages
-	//inSocket->bind(QHostAddress::LocalHost, 5551);
-
- //   // Connect the inSocket to the member-function, to read FlightGear commands
-	//connect(inSocket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams()), Qt::DirectConnection);
-	exec();								// Exec only returns, when the thread terminates...
-}
 
-/** QThread terminate @override **/
-void PPJoyServer::terminate() {
+	/* Initialise the IOCTL data structure */
+	JoyState.Signature= JOYSTICK_STATE_V1;
+	JoyState.NumAnalog= NUM_ANALOG;					// Number of analog values
+	Analog= JoyState.Analog;						// Keep a pointer to the analog array for easy updating
+	Digital= JoyState.Digital;						// Keep a pointer to the digital array for easy updating
+	JoyState.NumDigital= NUM_DIGITAL;				// Number of digital values
 
+
+	forever
+	{
+	    // Check event for stop thread
+		if(::WaitForSingleObject(m_StopThread, 0) == WAIT_OBJECT_0)
+		{
+			// Set event
+			::SetEvent(m_WaitThread);
+			return;
+		}
+
+		Analog[0] = scale2AnalogLimits( virtRotX, -90.0f, 90.0f );	// Pitch
+		qDebug() << "PPJoyServer says: Pitch =" << Analog[0] << " VirtRotX =" << virtRotX ;
+		Analog[1] = scale2AnalogLimits( virtRotY, -90.0f, 90.0f );	// Yaw
+		Analog[2] = scale2AnalogLimits( virtRotZ, -90.0f, 90.0f );	// Roll
+		Analog[3] = virtPosX + analogDefault;						// X
+
+		Analog[5] = virtPosY + analogDefault;						// Y (5?)
+		Analog[6] = virtPosZ + analogDefault;						// Z (6?)
+
+		checkAnalogLimits();
+
+		/* Send request to PPJoy for processing. */
+		/* Currently there is no Return Code from PPJoy, this may be added at a */
+		/* later stage. So we pass a 0 byte output buffer.                      */
+		if (!DeviceIoControl( h, IOCTL_PPORTJOY_SET_STATE, &JoyState, sizeof(JoyState), NULL, 0, &RetSize, NULL))
+		{
+			return;
+		}
+		// just for lower cpu load
+		msleep(15);	
+		yieldCurrentThread();
+	}
 }
 
+//
+// Limit the Joystick values
+//
+void PPJoyServer::checkAnalogLimits() {
+	for (int i = 0;i < NUM_ANALOG;i++) {
+		if (Analog[i]>PPJOY_AXIS_MAX) {
+			Analog[i]=PPJOY_AXIS_MAX;
+		}
+		else if (Analog[i]<PPJOY_AXIS_MIN) {
+			Analog[i]=PPJOY_AXIS_MIN;
+		}
+	}
+}
+
+//
+// Scale the measured value to the Joystick values
+//
+long PPJoyServer::scale2AnalogLimits( float x, float min_x, float max_x ) {
+double y;
+
+	y = ((PPJOY_AXIS_MAX - PPJOY_AXIS_MIN)/(max_x - min_x)) * x + ((PPJOY_AXIS_MAX - PPJOY_AXIS_MIN)/2) + PPJOY_AXIS_MIN;
+	qDebug() << "scale2AnalogLimits says: long_y =" << y;
+
+	return (long) y;
+}
 //END
-- 
cgit v1.2.3