actually fix FG this time

Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>

6 files changed, 21 insertions, 250 deletions

diff --git a/facetracknoir/clientfiles/FlightGear/Linux/usage.txt b/facetracknoir/clientfiles/FlightGear/Linux/usage.txt
index 14c77ad0..48cee837 100644
--- a/facetracknoir/clientfiles/FlightGear/Linux/usage.txt
+++ b/facetracknoir/clientfiles/FlightGear/Linux/usage.txt
@@ -1,4 +1,6 @@
-fgfs --generic=socket,in,25,localhost,5542,udp,headtracker ~/opt/fgdata/Nasal/headtracker.xml
+Copy Protocol/headtracker.xml to fgdata/Protocol/headtracker.xml
+
+$ fgfs --generic=socket,in,25,localhost,5542,udp,headtracker
 
 Adjust paths as necessary.
 
diff --git a/facetracknoir/clientfiles/FlightGear/Nasal/headtracker.xml b/facetracknoir/clientfiles/FlightGear/Nasal/headtracker.xml
deleted file mode 100644
index d8bd1d0a..00000000
--- a/facetracknoir/clientfiles/FlightGear/Nasal/headtracker.xml
+++ /dev/null
@@ -1,83 +0,0 @@
-<?xml version="1.0"?>
-
-<PropertyList>
-	<nasal>
-		<headtracker>
-			<script>
-				var Value = {
-					new: func(prop) {
-						var m = { parents: [Value] };
-						m.prop = props.globals.getNode(prop, 1);
-						m.value = 0;
-						return m;
-					},
-					apply: func(value) {
-						me.prop.setDoubleValue(me.prop.getValue() - me.value + value);
-						me.value = value;
-					},
-				};
-
-
-				var x = Value.new("/sim/current-view/x-offset-m");
-				var y = Value.new("/sim/current-view/y-offset-m");
-				var z = Value.new("/sim/current-view/z-offset-m");
-				var h = Value.new("/sim/current-view/heading-offset-deg");
-				var p = Value.new("/sim/current-view/pitch-offset-deg");
-				var r = Value.new("/sim/current-view/roll-offset-deg");
-
-
-				var resetting = 0;
-				var status = nil;
-
-				var reset = func {
-					if (status != 1) {
-						setprop("/sim/headtracker/control", 1);
-						resetting = 1;
-						view.resetViewPos();
-						view.resetViewDir();
-						x.value = y.value = z.value = h.value = p.value = r.value = 0;
-					}
-				}
-
-				setlistener("/sim/headtracker/status", func(n) {
-					var s = n.getValue();
-					if (!status and s) {
-						setprop("/sim/headtracker/control", 0);
-						resetting = 0;
-					}
-					status = s;
-				}, 1, 0);
-
-
-				var loop = func {
-					if (!view.index and !resetting) {
-						x.apply(getprop("/sim/headtracker/x-m"));
-						y.apply(getprop("/sim/headtracker/y-m"));
-						z.apply(getprop("/sim/headtracker/z-m"));
-
-						h.apply(getprop("/sim/headtracker/heading-deg"));
-						p.apply(getprop("/sim/headtracker/pitch-deg"));
-						r.apply(-1 * getprop("/sim/headtracker/roll-deg"));
-					}
-					settimer(loop, 0);
-				}
-
-				loop();
-
-			</script>
-		</headtracker>
-	</nasal>
-
-	<sim>
-		<headtracker>
-			<x-m type="double">0</x-m>
-			<y-m type="double">0</y-m>
-			<z-m type="double">0</z-m>
-			<heading-deg type="double">0</heading-deg>
-			<pitch-deg type="double">0</pitch-deg>
-			<roll-deg type="double">0</roll-deg>
-			<status type="int">0</status>
-			<control type="int">0</control>
-		</headtracker>
-	</sim>
-</PropertyList>
diff --git a/facetracknoir/clientfiles/FlightGear/Protocol/headtracker.xml b/facetracknoir/clientfiles/FlightGear/Protocol/headtracker.xml
index cd1d0dad..8c14119a 100644
--- a/facetracknoir/clientfiles/FlightGear/Protocol/headtracker.xml
+++ b/facetracknoir/clientfiles/FlightGear/Protocol/headtracker.xml
@@ -11,57 +11,44 @@
 			<chunk>
 				<name>x</name>
 				<type>double</type>
-				<node>/sim/headtracker/x-m</node>
+				<node>/sim/current-view/x-offset-m</node>
 			</chunk>
 
 			<chunk>
 				<name>y</name>
 				<type>double</type>
-				<node>/sim/headtracker/y-m</node>
+				<node>/sim/current-view/y-offset-m</node>
 			</chunk>
 
 			<chunk>
 				<name>z</name>
 				<type>double</type>
-				<node>/sim/headtracker/z-m</node>
+				<node>/sim/current-view/z-offset-m</node>
 			</chunk>
 
 			<chunk>
 				<name>heading</name>
 				<type>double</type>
-				<node>/sim/headtracker/heading-deg</node>
+				<node>/sim/current-view/heading-offset-deg</node>
 			</chunk>
 
 			<chunk>
 				<name>pitch</name>
 				<type>double</type>
-				<node>/sim/headtracker/pitch-deg</node>
+				<node>/sim/current-view/pitch-offset-deg</node>
 			</chunk>
 
 			<chunk>
 				<name>roll</name>
 				<type>double</type>
-				<node>/sim/headtracker/roll-deg</node>
+				<node>/sim/current-view/roll-offset-deg</node>
 			</chunk>
 
 			<chunk>
 				<name>status</name>
 				<type>int</type>
-				<node>/sim/headtracker/status</node>
+				<node>/sim/current-view/headtracker-debug-status</node>
 			</chunk>
 		</input>
-
-		<output>
-			<binary_mode>true</binary_mode>
-			<binary_footer>none</binary_footer>
-			<byte_order>host</byte_order>
-			<record_length>4</record_length>
-
-			<chunk>
-				<name>control</name>
-				<type>int</type>
-				<node>/sim/headtracker/control</node>
-			</chunk>
-		</output>
 	</generic>
 </PropertyList>
diff --git a/ftnoir_protocol_fg/fgtypes.h b/ftnoir_protocol_fg/fgtypes.h
index 049de318..0f29be3d 100644
--- a/ftnoir_protocol_fg/fgtypes.h
+++ b/ftnoir_protocol_fg/fgtypes.h
@@ -17,7 +17,7 @@
 //
 // x,y,z position in metres, heading, pitch and roll in degrees...
 //
-#pragma pack(push, 1)
+#pragma pack(push, 2)
 struct TFlightGearData {
 	double x, y, z, h, p, r;
 	int status;
diff --git a/ftnoir_protocol_fg/ftnoir_protocol_fg.cpp b/ftnoir_protocol_fg/ftnoir_protocol_fg.cpp
index 643f9dd5..5aa3487e 100644
--- a/ftnoir_protocol_fg/ftnoir_protocol_fg.cpp
+++ b/ftnoir_protocol_fg/ftnoir_protocol_fg.cpp
@@ -31,33 +31,16 @@
 #include <ftnoir_tracker_base/ftnoir_tracker_types.h>
 
 // For Todd and Arda Kutlu
-//#define SEND_ASCII_DATA
-//#define LOG_OUTPUT
 
-/** constructor **/
 FTNoIR_Protocol::FTNoIR_Protocol()
 {
-	blnConnectionActive = false;
-	loadSettings();
+    loadSettings();
 }
 
-/** destructor **/
 FTNoIR_Protocol::~FTNoIR_Protocol()
 {
-	if (inSocket != 0) {
-		inSocket->close();
-		delete inSocket;
-	}
-	
-	if (outSocket != 0) {
-		outSocket->close();
-		delete outSocket;
-	}
 }
 
-//
-// Load the current Settings from the currently 'active' INI-file.
-//
 void FTNoIR_Protocol::loadSettings() {
 	QSettings settings("opentrack");	// Registry settings (in HK_USER)
 
@@ -68,7 +51,7 @@ void FTNoIR_Protocol::loadSettings() {
 
 	bool blnLocalPC = iniFile.value ( "LocalPCOnly", 1 ).toBool();
 	if (blnLocalPC) {
-		destIP = QHostAddress::LocalHost;
+        destIP = QHostAddress::LocalHost;
 	}
 	else {
 		QString destAddr = iniFile.value ( "IP-1", 192 ).toString() + "." + iniFile.value ( "IP-2", 168 ).toString() + "." + iniFile.value ( "IP-3", 2 ).toString() + "." + iniFile.value ( "IP-4", 1 ).toString();
@@ -80,87 +63,19 @@ void FTNoIR_Protocol::loadSettings() {
 
 }
 
-//
-// Update Headpose in Game.
-//
 void FTNoIR_Protocol::sendHeadposeToGame( double *headpose, double *rawheadpose ) {
-int no_bytes;
-QHostAddress sender;
-quint16 senderPort;
+    int no_bytes;
+    QHostAddress sender;
+    quint16 senderPort;
 
-	//
-	// Copy the Raw measurements directly to the client.
-	//
     FlightData.x = headpose[TX] * 1e-2;
     FlightData.y = headpose[TY] * 1e-2;
     FlightData.z = headpose[TZ] * 1e-2;
     FlightData.p = headpose[Pitch];
     FlightData.h = headpose[Yaw];
     FlightData.r = headpose[Roll];
-	FlightData.status = fg_cmd;
-
-	//
-	// Try to send an UDP-message to the FlightGear
-	//
-
-#ifdef SEND_ASCII_DATA
-	sprintf_s(data, "%.2f %.2f %.2f %.2f %.2f %.2f\n\0", FlightData.x, FlightData.y, FlightData.z, FlightData.p, FlightData.h, FlightData.r);
-
-	if (outSocket != 0) {
-		no_bytes = outSocket->writeDatagram((const char *) &data, strlen( data ), destIP, destPort);
-		if ( no_bytes > 0) {
-		qDebug() << "FGServer::writePendingDatagrams says: bytes send =" << data;
-		}
-		else {
-			qDebug() << "FGServer::writePendingDatagrams says: nothing sent!";
-		}
-	}
-
-#endif
-
-	#ifdef LOG_OUTPUT
-	//	Use this for some debug-output to file...
-	QFile datafile(QCoreApplication::applicationDirPath() + "\\FG_output.txt");
-	if (datafile.open(QFile::WriteOnly | QFile::Append)) {
-		QTextStream out(&datafile);
-		out << "output:\t" << FlightData.x << "\t" << FlightData.y << "\t" << FlightData.z << "\t" << FlightData.p << "\t" << FlightData.h << "\t" << FlightData.r << '\n';
-	}
-	#endif
-
-	#ifndef SEND_ASCII_DATA
-	//! [1]
-	if (outSocket != 0) {
-        no_bytes = outSocket->writeDatagram((const char *) &FlightData, sizeof( FlightData ), destIP, destPort);
-		if ( no_bytes > 0) {
-		}
-		else {
-			qDebug() << "FGServer::writePendingDatagrams says: nothing sent!";
-		}
-	}
-	#endif
-
-	//
-	// FlightGear keeps sending data, so we must read that here.
-	//
-	if (inSocket != 0) {
-		while (inSocket->hasPendingDatagrams()) {
-
-			QByteArray datagram;
-			datagram.resize(inSocket->pendingDatagramSize());
-
-			inSocket->readDatagram( (char * ) &cmd, sizeof(cmd), &sender, &senderPort);
-
-			fg_cmd = cmd;									// Let's just accept that command for now...
-			if ( cmd > 0 ) {
-				qDebug() << "FGServer::sendHeadposeToGame hasPendingDatagrams, cmd = " << cmd;
-//				headTracker->handleGameCommand ( cmd );		// Send it upstream, for the Tracker to handle
-			}
-
-			if (!blnConnectionActive) {
-				blnConnectionActive = true;
-			}
-		}
-	}
+    FlightData.status = 1;
+    (void) outSocket.writeDatagram(reinterpret_cast<const char*>(&FlightData), sizeof(FlightData), destIP, static_cast<quint16>(destPort));
 }
 
 //
@@ -169,51 +84,9 @@ quint16 senderPort;
 //
 bool FTNoIR_Protocol::checkServerInstallationOK()
 {   
-	// Init. the data
-	FlightData.x = 0.0f;
-	FlightData.y = 0.0f;
-	FlightData.z = 0.0f;
-	FlightData.h = 0.0f;
-	FlightData.p = 0.0f;
-	FlightData.r = 0.0f;
-	FlightData.status = 0;
-	fg_cmd = 1;
-
-	inSocket = 0;
-	outSocket = 0;
-
-	//
-	// Create UDP-sockets.
-	//
-	if (inSocket == 0) {
-		qDebug() << "FGServer::sendHeadposeToGame creating insocket";
-		inSocket = new QUdpSocket();
-
-		// Connect the inSocket to the port, to receive messages
-		if (!inSocket->bind(QHostAddress::Any, destPort+1)) {
-			QMessageBox::warning(0,"FaceTrackNoIR Error", "Unable to bind UDP-port",QMessageBox::Ok,QMessageBox::NoButton);
-			delete inSocket;
-			inSocket = 0;
-			return false;
-		}
-	}
-
-	if (outSocket == 0) {
-		outSocket = new QUdpSocket();
-	}
-
-	return true;
+    return outSocket.bind(QHostAddress::Any, 0, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
 }
 
-////////////////////////////////////////////////////////////////////////////////
-// Factory function that creates instances if the Protocol object.
-
-// Export both decorated and undecorated names.
-//   GetProtocol     - Undecorated name, which can be easily used with GetProcAddress
-//                Win32 API function.
-//   _GetProtocol@0  - Common name decoration for __stdcall functions in C language.
-//#pragma comment(linker, "/export:GetProtocol=_GetProtocol@0")
-
 extern "C" FTNOIR_PROTOCOL_BASE_EXPORT IProtocol* CALLING_CONVENTION GetConstructor()
 {
     return new FTNoIR_Protocol;
diff --git a/ftnoir_protocol_fg/ftnoir_protocol_fg.h b/ftnoir_protocol_fg/ftnoir_protocol_fg.h
index 40a83384..f399904b 100644
--- a/ftnoir_protocol_fg/ftnoir_protocol_fg.h
+++ b/ftnoir_protocol_fg/ftnoir_protocol_fg.h
@@ -36,7 +36,6 @@
 #include <QUdpSocket>
 #include <QMessageBox>
 #include <QSettings>
-#include <math.h>
 #include "facetracknoir/global-settings.h"
 
 #define FT_PROGRAMID "FT_ProgramID"
@@ -52,15 +51,8 @@ public:
         return "FlightGear";
     }
 private:
-
-	bool blnConnectionActive;
-
-	//	Tracker *headTracker;								// For upstream messages...
-	TFlightGearData FlightData;
-	QUdpSocket *inSocket;									// Receive from FligthGear
-	QUdpSocket *outSocket;									// Send to FligthGear
-	qint32 cmd;
-	qint32 fg_cmd;											// Command from FlightGear
+    TFlightGearData FlightData;
+    QUdpSocket outSocket;									// Send to FligthGear
 	QHostAddress destIP;									// Destination IP-address
 	int destPort;											// Destination port-number
 	void loadSettings();