maybe fix simconnect lag

2 files changed, 70 insertions, 75 deletions

diff --git a/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp b/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp
index b1d84f9b..41fcf1b0 100644
--- a/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp
+++ b/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp
@@ -29,43 +29,39 @@
 #include "ftnoir_protocol_sc.h"
 #include "opentrack/plugin-api.hpp"
 
-importSimConnect_CameraSetRelative6DOF FTNoIR_Protocol::simconnect_set6DOF;
-HANDLE FTNoIR_Protocol::hSimConnect = 0;			// Handle to SimConnect
-
-float FTNoIR_Protocol::virtSCPosX = 0.0f;			// Headpose
-float FTNoIR_Protocol::virtSCPosY = 0.0f;
-float FTNoIR_Protocol::virtSCPosZ = 0.0f;
-	
-float FTNoIR_Protocol::virtSCRotX = 0.0f;
-float FTNoIR_Protocol::virtSCRotY = 0.0f;
-float FTNoIR_Protocol::virtSCRotZ = 0.0f;
-
-float FTNoIR_Protocol::prevSCPosX = 0.0f;			// previous Headpose
-float FTNoIR_Protocol::prevSCPosY = 0.0f;
-float FTNoIR_Protocol::prevSCPosZ = 0.0f;
-	
-float FTNoIR_Protocol::prevSCRotX = 0.0f;
-float FTNoIR_Protocol::prevSCRotY = 0.0f;
-float FTNoIR_Protocol::prevSCRotZ = 0.0f;
-
 static QLibrary SCClientLib;
 
-FTNoIR_Protocol::FTNoIR_Protocol()
+FTNoIR_Protocol::FTNoIR_Protocol() : should_stop(false), hSimConnect(nullptr)
 {
-	blnSimConnectActive = false;
-	hSimConnect = 0;
+    start();
 }
 
 FTNoIR_Protocol::~FTNoIR_Protocol()
 {
-	qDebug() << "~FTNoIR_Protocol says: inside" << FTNoIR_Protocol::hSimConnect;
+    should_stop = true;
+    wait();
 
-	if (hSimConnect != 0) {
-		qDebug() << "~FTNoIR_Protocol says: before simconnect_close";
-		if (SUCCEEDED( simconnect_close( FTNoIR_Protocol::hSimConnect ) ) ) {
-			qDebug() << "~FTNoIR_Protocol says: close SUCCEEDED";
-		}
-	}
+	if (hSimConnect)
+		(void) simconnect_close(hSimConnect);
+}
+
+void FTNoIR_Protocol::run()
+{
+    if (SUCCEEDED(simconnect_open(&hSimConnect, "FaceTrackNoIR", NULL, 0, 0, 0))) {
+        simconnect_subscribetosystemevent(hSimConnect, EVENT_PING, "Frame");
+
+        simconnect_mapclienteventtosimevent(hSimConnect, EVENT_INIT, "");
+        simconnect_addclienteventtonotificationgroup(hSimConnect, GROUP0, EVENT_INIT, false);
+        simconnect_setnotificationgrouppriority(hSimConnect, GROUP0, SIMCONNECT_GROUP_PRIORITY_HIGHEST);
+    }
+    else
+        return;
+    
+    while (!should_stop)
+    {
+        (void) (simconnect_calldispatch(hSimConnect, processNextSimconnectEvent, reinterpret_cast<void*>(this)));
+        Sleep(3);
+    }
 }
 
 void FTNoIR_Protocol::pose( const double *headpose ) {
@@ -76,19 +72,6 @@ void FTNoIR_Protocol::pose( const double *headpose ) {
     virtSCPosX = headpose[TX]/100.f;						// cm to meters
     virtSCPosY = headpose[TY]/100.f;
     virtSCPosZ = -headpose[TZ]/100.f;
-
-	if (!blnSimConnectActive) {
-        if (SUCCEEDED(simconnect_open(&hSimConnect, "FaceTrackNoIR", NULL, 0, 0, 0))) {
-            simconnect_subscribetosystemevent(hSimConnect, EVENT_PING, "Frame");
-
-            simconnect_mapclienteventtosimevent(hSimConnect, EVENT_INIT, "");
-            simconnect_addclienteventtonotificationgroup(hSimConnect, GROUP0, EVENT_INIT, false);
-            simconnect_setnotificationgrouppriority(hSimConnect, GROUP0, SIMCONNECT_GROUP_PRIORITY_HIGHEST);
-            blnSimConnectActive = true;
-        }
-	}
-    else
-        (void) (simconnect_calldispatch(hSimConnect, processNextSimconnectEvent, NULL));
 }
 
 #pragma GCC diagnostic ignored "-Wmissing-field-initializers"
@@ -203,25 +186,37 @@ bool FTNoIR_Protocol::correct()
 	return true;
 }
 
-void CALLBACK FTNoIR_Protocol::processNextSimconnectEvent(SIMCONNECT_RECV* pData, DWORD, void *)
+void FTNoIR_Protocol::handle()
+{
+    if (prevSCPosX != virtSCPosX ||
+        prevSCPosY != virtSCPosY ||
+        prevSCPosZ != virtSCPosZ ||
+        prevSCRotX != virtSCRotX ||
+        prevSCRotY != virtSCRotY ||
+        prevSCRotZ != virtSCRotZ)
+    {
+        (void) simconnect_set6DOF(hSimConnect, virtSCPosX, virtSCPosY, virtSCPosZ, virtSCRotX, virtSCRotZ, virtSCRotY);
+    }
+    
+    prevSCPosX = virtSCPosX;
+    prevSCPosY = virtSCPosY;
+    prevSCPosZ = virtSCPosZ;
+    prevSCRotX = virtSCRotX;
+    prevSCRotY = virtSCRotY;
+    prevSCRotZ = virtSCRotZ;
+}
+
+void CALLBACK FTNoIR_Protocol::processNextSimconnectEvent(SIMCONNECT_RECV* pData, DWORD, void *self_)
 {
+    FTNoIR_Protocol& self = *reinterpret_cast<FTNoIR_Protocol*>(self_);
+    
     switch(pData->dwID)
     {
     default:
         break;
     case SIMCONNECT_RECV_ID_EVENT_FRAME:
-    {
-        if ((prevSCPosX != virtSCPosX) || (prevSCPosY != virtSCPosY) || (prevSCPosZ != virtSCPosZ) ||
-                (prevSCRotX != virtSCRotX) || (prevSCRotY != virtSCRotY) || (prevSCRotZ != virtSCRotZ)) {
-            (void) simconnect_set6DOF(hSimConnect, virtSCPosX, virtSCPosY, virtSCPosZ, virtSCRotX, virtSCRotZ, virtSCRotY);
-        }
-        prevSCPosX = virtSCPosX;
-        prevSCPosY = virtSCPosY;
-        prevSCPosZ = virtSCPosZ;
-        prevSCRotX = virtSCRotX;
-        prevSCRotY = virtSCRotY;
-        prevSCRotZ = virtSCRotZ;
-    }
+        self.handle();
+        break;
     case SIMCONNECT_RECV_ID_EXCEPTION:
     {
         SIMCONNECT_RECV_EXCEPTION *except = (SIMCONNECT_RECV_EXCEPTION*)pData;
@@ -240,10 +235,8 @@ void CALLBACK FTNoIR_Protocol::processNextSimconnectEvent(SIMCONNECT_RECV* pData
     }
 
     case SIMCONNECT_RECV_ID_QUIT:
-    {
         break;
     }
-    }
 }
 
 extern "C" OPENTRACK_EXPORT IProtocol* GetConstructor()
diff --git a/ftnoir_protocol_sc/ftnoir_protocol_sc.h b/ftnoir_protocol_sc/ftnoir_protocol_sc.h
index f8db519b..b65bac85 100644
--- a/ftnoir_protocol_sc/ftnoir_protocol_sc.h
+++ b/ftnoir_protocol_sc/ftnoir_protocol_sc.h
@@ -39,6 +39,7 @@
 #include <SimConnect.h>
 
 #include "ui_ftnoir_sccontrols.h"
+#include <QThread>
 #include <QMessageBox>
 #include <QSettings>
 #include <QLibrary>
@@ -83,45 +84,46 @@ struct settings : opts {
     {}
 };
 
-class FTNoIR_Protocol : public IProtocol
+class FTNoIR_Protocol : public IProtocol, private QThread
 {
 public:
 	FTNoIR_Protocol();
     ~FTNoIR_Protocol() override;
     bool correct();
     void pose(const double* headpose);
+    void handle();
     QString game_name() {
         return "FS2004/FSX";
     }
 private:
-	static float virtSCPosX;
-	static float virtSCPosY;
-	static float virtSCPosZ;
-	
-	static float virtSCRotX;
-	static float virtSCRotY;
-	static float virtSCRotZ;
+    void run() override;
+    volatile bool should_stop;
+    
+	volatile float virtSCPosX;
+	volatile float virtSCPosY;
+	volatile float virtSCPosZ;
+	volatile float virtSCRotX;
+	volatile float virtSCRotY;
+	volatile float virtSCRotZ;
 
-	static float prevSCPosX;
-	static float prevSCPosY;
-	static float prevSCPosZ;
+	float prevSCPosX;
+	float prevSCPosY;
+	float prevSCPosZ;
 	
-	static float prevSCRotX;
-	static float prevSCRotY;
-	static float prevSCRotZ;
-
-    bool blnSimConnectActive;
+	float prevSCRotX;
+	float prevSCRotY;
+	float prevSCRotZ;
 
     importSimConnect_Open simconnect_open;							// SimConnect function(s) in DLL
 	importSimConnect_Close simconnect_close;
-	static importSimConnect_CameraSetRelative6DOF simconnect_set6DOF;
+	importSimConnect_CameraSetRelative6DOF simconnect_set6DOF;
 	importSimConnect_CallDispatch simconnect_calldispatch;
 	importSimConnect_SubscribeToSystemEvent simconnect_subscribetosystemevent;
 	importSimConnect_MapClientEventToSimEvent simconnect_mapclienteventtosimevent;
 	importSimConnect_AddClientEventToNotificationGroup simconnect_addclienteventtonotificationgroup;
 	importSimConnect_SetNotificationGroupPriority simconnect_setnotificationgrouppriority;
 
-	static HANDLE hSimConnect;						// Handle to SimConnect
+	HANDLE hSimConnect;						// Handle to SimConnect
 	static void CALLBACK processNextSimconnectEvent(SIMCONNECT_RECV* pData, DWORD cbData, void *pContext);
     settings s;
 };