proto/fsuipc: fix float <-> double promote

1 files changed, 101 insertions, 85 deletions

diff --git a/proto-fsuipc/ftnoir_protocol_fsuipc.cpp b/proto-fsuipc/ftnoir_protocol_fsuipc.cpp
index 91a61693..82cb44e9 100644
--- a/proto-fsuipc/ftnoir_protocol_fsuipc.cpp
+++ b/proto-fsuipc/ftnoir_protocol_fsuipc.cpp
@@ -13,12 +13,12 @@
 
 FTNoIR_Protocol::FTNoIR_Protocol()
 {
-	prevPosX = 0.0f;
-	prevPosY = 0.0f;
-	prevPosZ = 0.0f;
-	prevRotX = 0.0f;
-	prevRotY = 0.0f;
-	prevRotZ = 0.0f;
+    prevPosX = 0.0f;
+    prevPosY = 0.0f;
+    prevPosZ = 0.0f;
+    prevRotX = 0.0f;
+    prevRotY = 0.0f;
+    prevRotZ = 0.0f;
 }
 
 FTNoIR_Protocol::~FTNoIR_Protocol()
@@ -27,19 +27,30 @@ FTNoIR_Protocol::~FTNoIR_Protocol()
     FSUIPCLib.unload();
 }
 
-int FTNoIR_Protocol::scale2AnalogLimits( float x, float min_x, float max_x ) {
-    float y, local_x;
-	
-	local_x = x;
-	if (local_x > max_x) {
-		local_x = max_x;
-	}
-	if (local_x < min_x) {
-		local_x = min_x;
-	}
-	y = ( 16383 * local_x ) / max_x;
-
-	return (int) y;
+template<typename t>
+int FTNoIR_Protocol::scale2AnalogLimits(t x, t min_x, t max_x)
+{
+    t local_x = x;
+
+    if (local_x > max_x)
+    {
+        local_x = max_x;
+    }
+    if (local_x < min_x)
+    {
+        local_x = min_x;
+    }
+
+    t y = (16383 * local_x) / max_x;
+
+    return (int) y;
+}
+
+template<typename t>
+static inline bool check_float_fresh(t x, t y)
+{
+    static constexpr t eps = t(1e-4);
+    return std::fabs(x - y) >= eps;
 }
 
 void FTNoIR_Protocol::pose(const double *headpose ) {
@@ -49,90 +60,95 @@ void FTNoIR_Protocol::pose(const double *headpose ) {
     TFSState roll;
     WORD FSZoom;
 
-    float virtPosX;
-    float virtPosY;
-    float virtPosZ;
+    double virtPosX;
+    double virtPosY;
+    double virtPosZ;
 
-    float virtRotX;
-    float virtRotY;
-    float virtRotZ;
+    double virtRotX;
+    double virtRotY;
+    double virtRotZ;
 
-//	qDebug() << "FSUIPCServer::run() says: started!";
+    //	qDebug() << "FSUIPCServer::run() says: started!";
 
     virtRotX = -headpose[Pitch];				// degrees
     virtRotY = headpose[Yaw];
     virtRotZ = headpose[Roll];
 
-	virtPosX = 0.0f;											// cm, X and Y are not working for FS2002/2004!
-	virtPosY = 0.0f;
+    virtPosX = 0.0f;											// cm, X and Y are not working for FS2002/2004!
+    virtPosY = 0.0f;
     virtPosZ = headpose[TZ];
 
-	//
-	// Init. the FSUIPC offsets (derived from Free-track...)
-	//
-	pitch.Control = 66503;
-	yaw.Control = 66504;
-	roll.Control = 66505;
-
-	//
-	// Only do this when the data has changed. This way, the HAT-switch can be used when tracking is OFF.
-	//
-	if ((prevPosX != virtPosX) || (prevPosY != virtPosY) || (prevPosZ != virtPosZ) ||
-		(prevRotX != virtRotX) || (prevRotY != virtRotY) || (prevRotZ != virtRotZ)) {
-		//
-		// Open the connection
-		//
-		FSUIPC_Open(SIM_ANY, &result);
-
-		//
-		// Check the FS-version
-		//
-		if  (((result == FSUIPC_ERR_OK) || (result == FSUIPC_ERR_OPEN)) && 
-			 ((FSUIPC_FS_Version == SIM_FS2K2) || (FSUIPC_FS_Version == SIM_FS2K4))) {
-//			qDebug() << "FSUIPCServer::run() says: FSUIPC opened succesfully";
-			//
-			// Write the 4! DOF-data to FS. Only rotations and zoom are possible.
-			//
-			pitch.Value = scale2AnalogLimits(virtRotX, -180, 180);
-			FSUIPC_Write(0x3110, 8, &pitch, &result);
-
-			yaw.Value = scale2AnalogLimits(virtRotY, -180, 180);
-			FSUIPC_Write(0x3110, 8, &yaw, &result);
-
-			roll.Value = scale2AnalogLimits(virtRotZ, -180, 180);
-			FSUIPC_Write(0x3110, 8, &roll, &result);
+    //
+    // Init. the FSUIPC offsets (derived from Free-track...)
+    //
+    pitch.Control = 66503;
+    yaw.Control = 66504;
+    roll.Control = 66505;
+
+    //
+    // Only do this when the data has changed. This way, the HAT-switch can be used when tracking is OFF.
+    //
+    if (check_float_fresh(prevRotX, virtRotX) ||
+        check_float_fresh(prevRotY, virtRotY) ||
+        check_float_fresh(prevRotZ, virtRotZ) ||
+        check_float_fresh(prevPosX, virtPosX) ||
+        check_float_fresh(prevPosY, virtPosY) ||
+        check_float_fresh(prevPosZ, virtPosZ))
+    {
+        //
+        // Open the connection
+        //
+        FSUIPC_Open(SIM_ANY, &result);
+
+        //
+        // Check the FS-version
+        //
+        if  (((result == FSUIPC_ERR_OK) || (result == FSUIPC_ERR_OPEN)) &&
+             ((FSUIPC_FS_Version == SIM_FS2K2) || (FSUIPC_FS_Version == SIM_FS2K4)))
+        {
+            // Write the 4! DOF-data to FS. Only rotations and zoom are possible.
+
+            pitch.Value = scale2AnalogLimits(virtRotX, -180., 180.);
+            FSUIPC_Write(0x3110, 8, &pitch, &result);
+
+            yaw.Value = scale2AnalogLimits(virtRotY, -180., 180.);
+            FSUIPC_Write(0x3110, 8, &yaw, &result);
 
 			FSZoom = (WORD) (64/50) * virtPosZ + 64;
-			FSUIPC_Write(0x832E, 2, &FSZoom, &result);
+            roll.Value = scale2AnalogLimits(virtRotZ, -180., 180.);
+            FSUIPC_Write(0x3110, 8, &roll, &result);
+
+            FSUIPC_Write(0x832E, 2, &FSZoom, &result);
 
-			//
-			// Write the data, in one go!
-			//
-			FSUIPC_Process(&result);
-			if (result == FSUIPC_ERR_SENDMSG) {
+            //
+            // Write the data, in one go!
+            //
+            FSUIPC_Process(&result);
+            if (result == FSUIPC_ERR_SENDMSG)
+            {
                 // FSUIPC checks for already open connections and returns FSUIPC_ERR_OPEN in that case
                 // the connection scope is global for the process. this is why above code doesn't
                 // leak resources or have logic errors. see: http://www.purebasic.fr/english/viewtopic.php?t=31112
-				FSUIPC_Close();
-			}
-		}
-	}
-
-	prevPosX = virtPosX;
-	prevPosY = virtPosY;
-	prevPosZ = virtPosZ;
-	prevRotX = virtRotX;
-	prevRotY = virtRotY;
-	prevRotZ = virtRotZ;
+                FSUIPC_Close();
+            }
+        }
+    }
+
+    prevPosX = virtPosX;
+    prevPosY = virtPosY;
+    prevPosZ = virtPosZ;
+    prevRotX = virtRotX;
+    prevRotY = virtRotY;
+    prevRotZ = virtRotZ;
 }
 
 bool FTNoIR_Protocol::correct()
-{   
-	qDebug() << "correct says: Starting Function";
+{
+    qDebug() << "correct says: Starting Function";
 
-	//
-	// Load the DLL.
-	//
+    //
+    // Load the DLL.
+    //
     FSUIPCLib.setFileName( s.LocationOfDLL );
     if (FSUIPCLib.load() != true) {
         qDebug() << "correct says: Error loading FSUIPC DLL";
@@ -142,7 +158,7 @@ bool FTNoIR_Protocol::correct()
         qDebug() << "correct says: FSUIPC DLL loaded.";
     }
 
-	return true;
+    return true;
 }
 
 OPENTRACK_DECLARE_PROTOCOL(FTNoIR_Protocol, FSUIPCControls, FTNoIR_ProtocolDll)