fixed gyroscope and accelerometer orientations (X, Y, Z)

Valkirie · Nov 13, 2021 · a44d428 · a44d428
1 parent 20e6b93
commit a44d428
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 20 deletions.
diff --git a/ControllerService/DSUServer.cs b/ControllerService/DSUServer.cs
@@ -586,13 +586,13 @@ private bool ReportToBuffer(XInputController hidReport, byte[] outputData, long
                 if (hidReport.Acceleration != null)
                 {
                     // accelXG
-                    Array.Copy(BitConverter.GetBytes(hidReport.Acceleration.X), 0, outputData, outIdx, 4);
+                    Array.Copy(BitConverter.GetBytes(-hidReport.Acceleration.X), 0, outputData, outIdx, 4);
                     outIdx += 4;
                     // accelYG
-                    Array.Copy(BitConverter.GetBytes(hidReport.Acceleration.Z), 0, outputData, outIdx, 4);
+                    Array.Copy(BitConverter.GetBytes(hidReport.Acceleration.Y), 0, outputData, outIdx, 4);
                     outIdx += 4;
                     // accelZG
-                    Array.Copy(BitConverter.GetBytes(-hidReport.Acceleration.Y), 0, outputData, outIdx, 4);
+                    Array.Copy(BitConverter.GetBytes(-hidReport.Acceleration.Z), 0, outputData, outIdx, 4);
                     outIdx += 4;
                 }
                 else
@@ -605,13 +605,13 @@ private bool ReportToBuffer(XInputController hidReport, byte[] outputData, long
                 if (hidReport.AngularVelocity != null)
                 {
                     // angVelPitch
-                    Array.Copy(BitConverter.GetBytes(hidReport.AngularVelocity.X), 0, outputData, outIdx, 4);
+                    Array.Copy(BitConverter.GetBytes(-hidReport.AngularVelocity.X), 0, outputData, outIdx, 4);
                     outIdx += 4;
                     // angVelYaw
-                    Array.Copy(BitConverter.GetBytes(hidReport.AngularVelocity.Z), 0, outputData, outIdx, 4);
+                    Array.Copy(BitConverter.GetBytes(hidReport.AngularVelocity.Y), 0, outputData, outIdx, 4);
                     outIdx += 4;
                     // angVelRoll
-                    Array.Copy(BitConverter.GetBytes(-hidReport.AngularVelocity.Y), 0, outputData, outIdx, 4);
+                    Array.Copy(BitConverter.GetBytes(-hidReport.AngularVelocity.Z), 0, outputData, outIdx, 4);
                     outIdx += 4;
                 }
                 else

diff --git a/ControllerService/XInputController.cs b/ControllerService/XInputController.cs
@@ -109,16 +109,16 @@ public void SetDSUServer(DSUServer _server)
 
         private void Accelerometer_ReadingChanged(object sender, XInputAccelerometerReadingChangedEventArgs e)
         {
-            Acceleration.X = -e.AccelerationY;
-            Acceleration.Y = e.AccelerationX;
-            Acceleration.Z = e.AccelerationZ;
+            Acceleration.X = e.AccelerationY;
+            Acceleration.Y = e.AccelerationZ;
+            Acceleration.Z = e.AccelerationX;
         }
 
         private void Girometer_ReadingChanged(object sender, XInputGirometerReadingChangedEventArgs e)
         {
-            AngularVelocity.X = -e.AngularVelocityY;
-            AngularVelocity.Y = e.AngularVelocityX;
-            AngularVelocity.Z = e.AngularVelocityZ;
+            AngularVelocity.X = e.AngularVelocityY;
+            AngularVelocity.Y = e.AngularVelocityZ;
+            AngularVelocity.Z = e.AngularVelocityX;
         }
 
         private unsafe void UpdateController(object sender, ElapsedEventArgs e)
@@ -229,15 +229,14 @@ private unsafe void UpdateController(object sender, ElapsedEventArgs e)
                     outDS4Report.sCurrentTouch.bTouchData2[2] = (byte)(touch.TrackPadTouch1.Y >> 4);
                 }
 
-                outDS4Report.wGyroX = (short)(AngularVelocity.X * F_GYRO_RES_IN_DEG_SEC); // gyroPitchFull
-                outDS4Report.wGyroY = (short)(-AngularVelocity.Z * F_GYRO_RES_IN_DEG_SEC); // gyroYawFull
-                outDS4Report.wGyroZ = (short)(AngularVelocity.Y * F_GYRO_RES_IN_DEG_SEC); // gyroRollFull
+                outDS4Report.wGyroX = (short)(-AngularVelocity.X * F_GYRO_RES_IN_DEG_SEC); // gyroPitchFull
+                outDS4Report.wGyroY = (short)(-AngularVelocity.Y * F_GYRO_RES_IN_DEG_SEC); // gyroYawFull
+                outDS4Report.wGyroZ = (short)(AngularVelocity.Z * F_GYRO_RES_IN_DEG_SEC); // gyroRollFull
+                outDS4Report.wAccelX = (short)(Acceleration.X * F_ACC_RES_PER_G); // accelXFull
+                outDS4Report.wAccelY = (short)(-Acceleration.Y * F_ACC_RES_PER_G); // accelYFull
+                outDS4Report.wAccelZ = (short)(Acceleration.Z * F_ACC_RES_PER_G); // accelZFull
 
-                outDS4Report.wAccelX = (short)(-Acceleration.X * F_ACC_RES_PER_G); // accelXFull
-                outDS4Report.wAccelY = (short)(-Acceleration.Z * F_ACC_RES_PER_G); // accelYFull
-                outDS4Report.wAccelZ = (short)(Acceleration.Y * F_ACC_RES_PER_G); // accelZFull
-
-                outDS4Report.bBatteryLvlSpecial = 10;
+                outDS4Report.bBatteryLvlSpecial = 11;
 
                 outDS4Report.wTimestamp = (ushort)(microseconds / 5.33f);