motion axis remap and config

r57zone · Aug 28, 2022 · 4f7126c · 4f7126c
1 parent 9e00645
commit 4f7126c
Showing 1 changed file with 25 additions and 8 deletions.
diff --git a/Source/DS4Emulator.cpp b/Source/DS4Emulator.cpp
@@ -46,20 +46,36 @@ float bytesToFloat(unsigned char b3, unsigned char b2, unsigned char b1, unsigne
 }
 
 int SleepTimeOutMotion = 1;
+bool MotionOrientation = true; // true = landscape
 void MotionReceiver()
 {
 	while (SocketActivated) {
 		memset(&freePieIMU, 0, sizeof(freePieIMU));
 		bytes_read = recvfrom(socketS, (char*)(&freePieIMU), sizeof(freePieIMU), 0, (sockaddr*)&from, &fromlen);
 		if (bytes_read > 0) {
-			AccelX = bytesToFloat(freePieIMU[2], freePieIMU[3], freePieIMU[4], freePieIMU[5]);
-			AccelY = bytesToFloat(freePieIMU[6], freePieIMU[7], freePieIMU[8], freePieIMU[9]);
-			AccelZ = bytesToFloat(freePieIMU[10], freePieIMU[11], freePieIMU[12], freePieIMU[13]);
-
-			GyroX = bytesToFloat(freePieIMU[14], freePieIMU[15], freePieIMU[16], freePieIMU[17]);
-			GyroY = bytesToFloat(freePieIMU[18], freePieIMU[19], freePieIMU[20], freePieIMU[21]);
-			GyroZ = bytesToFloat(freePieIMU[22], freePieIMU[23], freePieIMU[24], freePieIMU[25]);
-		} else
+
+			if (MotionOrientation) {
+				// landscape mapping
+				AccelZ = bytesToFloat(freePieIMU[2], freePieIMU[3], freePieIMU[4], freePieIMU[5]);
+				AccelX = bytesToFloat(freePieIMU[6], freePieIMU[7], freePieIMU[8], freePieIMU[9]);
+				AccelY = bytesToFloat(freePieIMU[10], freePieIMU[11], freePieIMU[12], freePieIMU[13]);
+
+				GyroZ = bytesToFloat(freePieIMU[14], freePieIMU[15], freePieIMU[16], freePieIMU[17]);
+				GyroX = bytesToFloat(freePieIMU[18], freePieIMU[19], freePieIMU[20], freePieIMU[21]);
+				GyroY = bytesToFloat(freePieIMU[22], freePieIMU[23], freePieIMU[24], freePieIMU[25]);
+			}
+			else {
+				// portrait mapping
+				AccelX = bytesToFloat(freePieIMU[2], freePieIMU[3], freePieIMU[4], freePieIMU[5]);
+				AccelZ = bytesToFloat(freePieIMU[6], freePieIMU[7], freePieIMU[8], freePieIMU[9]);
+				AccelY = bytesToFloat(freePieIMU[10], freePieIMU[11], freePieIMU[12], freePieIMU[13]);
+
+				GyroX = bytesToFloat(freePieIMU[14], freePieIMU[15], freePieIMU[16], freePieIMU[17]);
+				GyroZ = bytesToFloat(freePieIMU[18], freePieIMU[19], freePieIMU[20], freePieIMU[21]);
+				GyroY = bytesToFloat(freePieIMU[22], freePieIMU[23], freePieIMU[24], freePieIMU[25]);
+			}
+		}
+		else
 			Sleep(SleepTimeOutMotion); // Don't overload the CPU with reading
 	}
 }
@@ -166,6 +182,7 @@ int main(int argc, char **argv)
 	int InverseXStatus = IniFile.ReadBoolean("Motion", "InverseX", false) == false ? 1 : -1 ;
 	int InverseYStatus = IniFile.ReadBoolean("Motion", "InverseY", false) == false ? 1 : -1 ;
 	int InverseZStatus = IniFile.ReadBoolean("Motion", "InverseZ", false) == false ? 1 : -1 ;
+	MotionOrientation = IniFile.ReadBoolean("Motion", "Orientation", true);
 
 	SleepTimeOutMotion = IniFile.ReadInteger("Motion", "SleepTimeOut", 1);