Merge pull request #1985 from CarlKenner/accelerometers

Fix accelerometer wiimote/nunchuk emulation.

2 files changed, 33 insertions, 19 deletions

diff --git a/Source/Core/Core/HW/WiimoteEmu/Attachment/Nunchuk.cpp b/Source/Core/Core/HW/WiimoteEmu/Attachment/Nunchuk.cpp
index a482055fa5..ef5d3fb04f 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Attachment/Nunchuk.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/Attachment/Nunchuk.cpp
@@ -87,23 +87,30 @@ void Nunchuk::GetState(u8* const data)
 	// flip the button bits :/
 	ncdata->bt.hex ^= 0x03;
 
-	u16 accel_x = (u16)(accel.x * ACCEL_RANGE + ACCEL_ZERO_G);
-	u16 accel_y = (u16)(accel.y * ACCEL_RANGE + ACCEL_ZERO_G);
-	u16 accel_z = (u16)(accel.z * ACCEL_RANGE + ACCEL_ZERO_G);
+	// We now use 2 bits more precision, so multiply by 4 before converting to int
+	s16 accel_x = (s16)(4 * (accel.x * ACCEL_RANGE + ACCEL_ZERO_G));
+	s16 accel_y = (s16)(4 * (accel.y * ACCEL_RANGE + ACCEL_ZERO_G));
+	s16 accel_z = (s16)(4 * (accel.z * ACCEL_RANGE + ACCEL_ZERO_G));
 
 	if (accel_x > 1024)
 		accel_x = 1024;
+	else if (accel_x < 0)
+		accel_x = 0;
 	if (accel_y > 1024)
 		accel_y = 1024;
+	else if (accel_y < 0)
+		accel_y = 0;
 	if (accel_z > 1024)
 		accel_z = 1024;
-
-	ncdata->ax = accel_x & 0xFF;
-	ncdata->ay = accel_y & 0xFF;
-	ncdata->az = accel_z & 0xFF;
-	ncdata->passthrough_data.acc_x_lsb = accel_x >> 8 & 0x3;
-	ncdata->passthrough_data.acc_y_lsb = accel_y >> 8 & 0x3;
-	ncdata->passthrough_data.acc_z_lsb = accel_z >> 8 & 0x3;
+	else if (accel_z < 0)
+		accel_z = 0;
+
+	ncdata->ax = (accel_x >> 2) & 0xFF;
+	ncdata->ay = (accel_y >> 2) & 0xFF;
+	ncdata->az = (accel_z >> 2) & 0xFF;
+	ncdata->bt.acc_x_lsb = accel_x & 0x3;
+	ncdata->bt.acc_y_lsb = accel_y & 0x3;
+	ncdata->bt.acc_z_lsb = accel_z & 0x3;
 }
 
 void Nunchuk::LoadDefaults(const ControllerInterface& ciface)
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
index af4de2bffe..47f1944786 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
@@ -396,24 +396,31 @@ void Wiimote::GetAccelData(u8* const data, const ReportFeatures& rptf)
 	wm_accel& accel = *(wm_accel*)(data + rptf.accel);
 	wm_buttons& core = *(wm_buttons*)(data + rptf.core);
 
-	u16 x = (u16)(m_accel.x * ACCEL_RANGE + ACCEL_ZERO_G);
-	u16 y = (u16)(m_accel.y * ACCEL_RANGE + ACCEL_ZERO_G);
-	u16 z = (u16)(m_accel.z * ACCEL_RANGE + ACCEL_ZERO_G);
+	// We now use 2 bits more precision, so multiply by 4 before converting to int
+	s16 x = (s16)(4 * (m_accel.x * ACCEL_RANGE + ACCEL_ZERO_G));
+	s16 y = (s16)(4 * (m_accel.y * ACCEL_RANGE + ACCEL_ZERO_G));
+	s16 z = (s16)(4 * (m_accel.z * ACCEL_RANGE + ACCEL_ZERO_G));
 
 	if (x > 1024)
 		x = 1024;
+	else if (x < 0)
+		x = 0;
 	if (y > 1024)
 		y = 1024;
+	else if (y < 0)
+		y = 0;
 	if (z > 1024)
 		z = 1024;
+	else if (z < 0)
+		z = 0;
 
-	accel.x = x & 0xFF;
-	accel.y = y & 0xFF;
-	accel.z = z & 0xFF;
+	accel.x = (x >> 2) & 0xFF;
+	accel.y = (y >> 2) & 0xFF;
+	accel.z = (z >> 2) & 0xFF;
 
-	core.acc_x_lsb = x >> 8 & 0x3;
-	core.acc_y_lsb = y >> 8 & 0x1;
-	core.acc_z_lsb = z >> 8 & 0x1;
+	core.acc_x_lsb = x & 0x3;
+	core.acc_y_lsb = (y >> 1) & 0x1;
+	core.acc_z_lsb = (z >> 1) & 0x1;
 }
 #define kCutoffFreq 5.0
 inline void LowPassFilter(double & var, double newval, double period)