Accelerometer fixes

JoyShockLibrary/InputHelpers.cpp

@@ -56,6 +56,8 @@ bool handle_input(JoyShock *jc, uint8_t *packet, int len, bool &hasIMU) {
 			jc->imu_state.accelZ = (float)(accelSampleY) / -8192.0;
 			jc->imu_state.accelY = (float)(accelSampleZ) / 8192.0;
 
+			//printf("DS4 accel: %.4f, %.4f, %.4f\n", jc->imu_state.accelX, jc->imu_state.accelY, jc->imu_state.accelZ);
+
 			//printf("%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d\n",
 			//	jc->gyro.yaw, jc->gyro.pitch, jc->gyro.roll, jc->accel.x, jc->accel.y, jc->accel.z, universal_counter++);
 
@@ -240,8 +242,8 @@ bool handle_input(JoyShock *jc, uint8_t *packet, int len, bool &hasIMU) {
 		// Accelerometer data is absolute
 		{
 			// get accelerometer X:
-			int16_t accelSampleX = uint16_to_int16(packet[13] | (packet[14] << 8) & 0xFF00);
-			int16_t accelSampleY = uint16_to_int16(packet[15] | (packet[16] << 8) & 0xFF00);
+			int16_t accelSampleY = uint16_to_int16(packet[13] | (packet[14] << 8) & 0xFF00);
+			int16_t accelSampleX = uint16_to_int16(packet[15] | (packet[16] << 8) & 0xFF00);
 			int16_t accelSampleZ = uint16_to_int16(packet[17] | (packet[18] << 8) & 0xFF00);
 			int16_t gyroSampleX = uint16_to_int16(packet[19] | (packet[20] << 8) & 0xFF00);
 			int16_t gyroSampleY = uint16_to_int16(packet[21] | (packet[22] << 8) & 0xFF00);
@@ -261,8 +263,8 @@ bool handle_input(JoyShock *jc, uint8_t *packet, int len, bool &hasIMU) {
 			float pitch = gyroSampleY - jc->sensor_cal[1][1];
 			float yaw = gyroSampleZ - jc->sensor_cal[1][2];
 			// each packet actually has 3 samples worth of data, so collect sample 2
-			accelSampleX = uint16_to_int16(packet[25] | (packet[26] << 8) & 0xFF00);
-			accelSampleY = uint16_to_int16(packet[27] | (packet[28] << 8) & 0xFF00);
+			accelSampleY = uint16_to_int16(packet[25] | (packet[26] << 8) & 0xFF00);
+			accelSampleX = uint16_to_int16(packet[27] | (packet[28] << 8) & 0xFF00);
 			accelSampleZ = uint16_to_int16(packet[29] | (packet[30] << 8) & 0xFF00);
 			gyroSampleX = uint16_to_int16(packet[31] | (packet[32] << 8) & 0xFF00);
 			gyroSampleY = uint16_to_int16(packet[33] | (packet[34] << 8) & 0xFF00);
@@ -275,8 +277,8 @@ bool handle_input(JoyShock *jc, uint8_t *packet, int len, bool &hasIMU) {
 			pitch += gyroSampleY - jc->sensor_cal[1][1];
 			yaw += gyroSampleZ - jc->sensor_cal[1][2];
 			// ... and sample 3
-			accelSampleX = uint16_to_int16(packet[37] | (packet[38] << 8) & 0xFF00);
-			accelSampleY = uint16_to_int16(packet[39] | (packet[40] << 8) & 0xFF00);
+			accelSampleY = uint16_to_int16(packet[37] | (packet[38] << 8) & 0xFF00);
+			accelSampleX = uint16_to_int16(packet[39] | (packet[40] << 8) & 0xFF00);
 			accelSampleZ = uint16_to_int16(packet[41] | (packet[42] << 8) & 0xFF00);
 			gyroSampleX = uint16_to_int16(packet[43] | (packet[44] << 8) & 0xFF00);
 			gyroSampleY = uint16_to_int16(packet[45] | (packet[46] << 8) & 0xFF00);
@@ -295,13 +297,15 @@ bool handle_input(JoyShock *jc, uint8_t *packet, int len, bool &hasIMU) {
 			roll /= 3;
 			pitch /= 3;
 			yaw /= 3;
-			jc->imu_state.accelX = (float)(accelX) / 4096.0;
-			jc->imu_state.accelY = (float)(accelY) / 4096.0;
-			jc->imu_state.accelZ = (float)(accelZ) / 4096.0;
+			jc->imu_state.accelX = (float)(accelX) / -4096.0;
+			jc->imu_state.accelY = (float)(accelY) / -4096.0;
+			jc->imu_state.accelZ = (float)(accelZ) / -4096.0;
 			jc->imu_state.gyroZ = (float)(roll) * (2294.0 / 32767.0);
 			jc->imu_state.gyroX = (float)(pitch) * (2294.0 / 32767.0);
 			jc->imu_state.gyroY = (float)(yaw) * (2294.0 / 32767.0);
 
+			//printf("Switch accel: %.4f, %.4f, %.4f\n", jc->imu_state.accelX, jc->imu_state.accelY, jc->imu_state.accelZ);
+
 			if (jc->use_continuous_calibration) {
 				jc->push_sensor_samples(jc->imu_state.gyroX, jc->imu_state.gyroY, jc->imu_state.gyroZ);
 				jc->get_average_gyro(jc->offset_x, jc->offset_y, jc->offset_z);