From e0593b104199736aa49f528f289d40cee8ae83d2 Mon Sep 17 00:00:00 2001 From: John Peterson Date: Fri, 20 Feb 2009 13:14:23 +0000 Subject: [PATCH] Emulated Wiimote: Fixed the Nunchuck calibration, for recording playback for example git-svn-id: https://dolphin-emu.googlecode.com/svn/trunk@2317 8ced0084-cf51-0410-be5f-012b33b47a6e --- .../Plugin_Wiimote/Src/EmuDynamics.cpp | 176 +++++++++--------- Source/Plugins/Plugin_Wiimote/Src/EmuMain.cpp | 17 +- .../Plugins/Plugin_Wiimote/Src/FillReport.cpp | 83 ++++++--- Source/Plugins/Plugin_Wiimote/Src/main.cpp | 8 +- Source/Plugins/Plugin_Wiimote/Src/main.h | 2 +- .../Plugins/Plugin_Wiimote/Src/wiimote_hid.h | 1 + 6 files changed, 163 insertions(+), 124 deletions(-) diff --git a/Source/Plugins/Plugin_Wiimote/Src/EmuDynamics.cpp b/Source/Plugins/Plugin_Wiimote/Src/EmuDynamics.cpp index 352e83b214..569e147b33 100644 --- a/Source/Plugins/Plugin_Wiimote/Src/EmuDynamics.cpp +++ b/Source/Plugins/Plugin_Wiimote/Src/EmuDynamics.cpp @@ -142,12 +142,12 @@ void PitchDegreeToAccelerometer(float _Roll, float _Pitch, u8 &_x, u8 &_y, u8 &_ } // Multiply with the neutral of z and its g - float xg = g_accel.cal_g.x; - float yg = g_accel.cal_g.y; - float zg = g_accel.cal_g.z; - float x_zero = g_accel.cal_zero.x; - float y_zero = g_accel.cal_zero.y; - float z_zero = g_accel.cal_zero.z; + float xg = g_wm.cal_g.x; + float yg = g_wm.cal_g.y; + float zg = g_wm.cal_g.z; + float x_zero = g_wm.cal_zero.x; + float y_zero = g_wm.cal_zero.y; + float z_zero = g_wm.cal_zero.z; int ix = (int) (x_zero + xg * x); int iy = (int) (y_zero + yg * y); int iz = (int) (z_zero + zg * z); @@ -176,9 +176,9 @@ void PitchAccelerometerToDegree(u8 _x, u8 _y, u8 _z, int &_Roll, int &_Pitch, in float Roll = 0, Pitch = 0; // Calculate how many g we are from the neutral - float x = AccelerometerToG((float)_x, (float)g_accel.cal_zero.x, (float)g_accel.cal_g.x); - float y = AccelerometerToG((float)_y, (float)g_accel.cal_zero.y, (float)g_accel.cal_g.y); - float z = AccelerometerToG((float)_z, (float)g_accel.cal_zero.z, (float)g_accel.cal_g.z); + float x = AccelerometerToG((float)_x, (float)g_wm.cal_zero.x, (float)g_wm.cal_g.x); + float y = AccelerometerToG((float)_y, (float)g_wm.cal_zero.y, (float)g_wm.cal_g.y); + float z = AccelerometerToG((float)_z, (float)g_wm.cal_zero.z, (float)g_wm.cal_g.z); // If it is over 1g then it is probably accelerating and may not reliable //if (abs(accel->x - ac->cal_zero.x) <= ac->cal_g.x) @@ -187,7 +187,7 @@ void PitchAccelerometerToDegree(u8 _x, u8 _y, u8 _z, int &_Roll, int &_Pitch, in Roll = InputCommon::Rad2Deg(atan2(x, z)); } - //if (abs(_y - g_accel.cal_zero.y) <= g_accel.cal_g.y) + //if (abs(_y - g_wm.cal_zero.y) <= g_wm.cal_g.y) { // Calculate the degree Pitch = InputCommon::Rad2Deg(atan2(y, z)); @@ -217,43 +217,43 @@ void PitchAccelerometerToDegree(u8 _x, u8 _y, u8 _z, int &_Roll, int &_Pitch, in ////////////////////////////////////////////////////////////////////////////////////////// // Calculate dot positions from the basic 10 byte IR data -// ŻŻŻŻŻŻŻŻŻŻŻŻŻ -void IRData2DotsBasic(u8 *Data) -{ - struct SDot* Dot = g_Wm.IR.Dot; - - Dot[0].Rx = 1023 - (Data[0] | ((Data[2] & 0x30) << 4)); - Dot[0].Ry = Data[1] | ((Data[2] & 0xc0) << 2); - - Dot[1].Rx = 1023 - (Data[3] | ((Data[2] & 0x03) << 8)); - Dot[1].Ry = Data[4] | ((Data[2] & 0x0c) << 6); - - Dot[2].Rx = 1023 - (Data[5] | ((Data[7] & 0x30) << 4)); - Dot[2].Ry = Data[6] | ((Data[7] & 0xc0) << 2); - - Dot[3].Rx = 1023 - (Data[8] | ((Data[7] & 0x03) << 8)); - Dot[3].Ry = Data[9] | ((Data[7] & 0x0c) << 6); - - /* set each IR spot to visible if spot is in range */ - for (int i = 0; i < 4; ++i) - { - if (Dot[i].Ry == 1023) - { - Dot[i].Visible = 0; - } - else - { - Dot[i].Visible = 1; - Dot[i].Size = 0; /* since we don't know the size, set it as 0 */ - } - +// ŻŻŻŻŻŻŻŻŻŻŻŻŻ +void IRData2DotsBasic(u8 *Data) +{ + struct SDot* Dot = g_Wm.IR.Dot; + + Dot[0].Rx = 1023 - (Data[0] | ((Data[2] & 0x30) << 4)); + Dot[0].Ry = Data[1] | ((Data[2] & 0xc0) << 2); + + Dot[1].Rx = 1023 - (Data[3] | ((Data[2] & 0x03) << 8)); + Dot[1].Ry = Data[4] | ((Data[2] & 0x0c) << 6); + + Dot[2].Rx = 1023 - (Data[5] | ((Data[7] & 0x30) << 4)); + Dot[2].Ry = Data[6] | ((Data[7] & 0xc0) << 2); + + Dot[3].Rx = 1023 - (Data[8] | ((Data[7] & 0x03) << 8)); + Dot[3].Ry = Data[9] | ((Data[7] & 0x0c) << 6); + + /* set each IR spot to visible if spot is in range */ + for (int i = 0; i < 4; ++i) + { + if (Dot[i].Ry == 1023) + { + Dot[i].Visible = 0; + } + else + { + Dot[i].Visible = 1; + Dot[i].Size = 0; /* since we don't know the size, set it as 0 */ + } + // For now we let our virtual resolution be the same as the default one - Dot[i].X = Dot[i].Rx; Dot[i].Y = Dot[i].Ry; - } - + Dot[i].X = Dot[i].Rx; Dot[i].Y = Dot[i].Ry; + } + // Calculate the other values ReorderIRDots(); - IRData2Distance(); + IRData2Distance(); } @@ -294,36 +294,36 @@ void IRData2Dots(u8 *Data) ////////////////////////////////////////////////////////////////////////////////////////// // Reorder the IR dots according to their x-axis value -// ŻŻŻŻŻŻŻŻŻŻŻŻŻ -void ReorderIRDots() -{ +// ŻŻŻŻŻŻŻŻŻŻŻŻŻ +void ReorderIRDots() +{ // Create a shortcut - struct SDot* Dot = g_Wm.IR.Dot; - - // Variables - int i, j, order; - - // Reset the dot ordering to zero - for (i = 0; i < 4; ++i) - Dot[i].Order = 0; - - for (order = 1; order < 5; ++order) - { - i = 0; - - // - for (; !Dot[i].Visible || Dot[i].Order; ++i) - if (i > 4) return; - - // - for (j = 0; j < 4; ++j) - { - if (Dot[j].Visible && !Dot[j].Order && (Dot[j].X < Dot[i].X)) - i = j; - } - - Dot[i].Order = order; - } + struct SDot* Dot = g_Wm.IR.Dot; + + // Variables + int i, j, order; + + // Reset the dot ordering to zero + for (i = 0; i < 4; ++i) + Dot[i].Order = 0; + + for (order = 1; order < 5; ++order) + { + i = 0; + + // + for (; !Dot[i].Visible || Dot[i].Order; ++i) + if (i > 4) return; + + // + for (j = 0; j < 4; ++j) + { + if (Dot[j].Visible && !Dot[j].Order && (Dot[j].X < Dot[i].X)) + i = j; + } + + Dot[i].Order = order; + } } //////////////////////////////// @@ -339,25 +339,25 @@ void IRData2Distance() // Make these ones global int i1, i2; - for (i1 = 0; i1 < 4; ++i1) - if (Dot[i1].Visible) break; - - // Only one dot was visible, we can not calculate the distance - if (i1 == 4) { g_Wm.IR.Distance = 0; return; } - - // Look at the next dot - for (i2 = i1 + 1; i2 < 4; ++i2) - if (Dot[i2].Visible) break; - - // Only one dot was visible, we can not calculate the distance + for (i1 = 0; i1 < 4; ++i1) + if (Dot[i1].Visible) break; + + // Only one dot was visible, we can not calculate the distance + if (i1 == 4) { g_Wm.IR.Distance = 0; return; } + + // Look at the next dot + for (i2 = i1 + 1; i2 < 4; ++i2) + if (Dot[i2].Visible) break; + + // Only one dot was visible, we can not calculate the distance if (i2 == 4) { g_Wm.IR.Distance = 0; return; } /* For the emulated Wiimote the y distance is always zero so then the distance is the simple distance between the x dots, i.e. the sensor bar width */ - int xd = Dot[i2].X - Dot[i1].X; - int yd = Dot[i2].Y - Dot[i1].Y; - - // Save the distance + int xd = Dot[i2].X - Dot[i1].X; + int yd = Dot[i2].Y - Dot[i1].Y; + + // Save the distance g_Wm.IR.Distance = (int)sqrt((float)(xd*xd) + (float)(yd*yd)); } //////////////////////////////// diff --git a/Source/Plugins/Plugin_Wiimote/Src/EmuMain.cpp b/Source/Plugins/Plugin_Wiimote/Src/EmuMain.cpp index d3ff34afa5..b16f49bc90 100644 --- a/Source/Plugins/Plugin_Wiimote/Src/EmuMain.cpp +++ b/Source/Plugins/Plugin_Wiimote/Src/EmuMain.cpp @@ -241,16 +241,19 @@ void LoadRecordedMovements() // Update the accelerometer neutral values void UpdateEeprom() { - g_accel.cal_zero.x = g_Eeprom[22]; - g_accel.cal_zero.y = g_Eeprom[23]; - g_accel.cal_zero.z = g_Eeprom[24]; - g_accel.cal_g.x = g_Eeprom[26] - g_Eeprom[22]; - g_accel.cal_g.y = g_Eeprom[27] - g_Eeprom[24]; - g_accel.cal_g.z = g_Eeprom[28] - g_Eeprom[24]; + g_wm.cal_zero.x = g_Eeprom[22]; + g_wm.cal_zero.y = g_Eeprom[23]; + g_wm.cal_zero.z = g_Eeprom[24]; + g_wm.cal_g.x = g_Eeprom[26] - g_Eeprom[22]; + g_wm.cal_g.y = g_Eeprom[27] - g_Eeprom[24]; + g_wm.cal_g.z = g_Eeprom[28] - g_Eeprom[24]; g_nu.cal_zero.x = g_RegExt[0x20]; g_nu.cal_zero.y = g_RegExt[0x21]; - g_nu.cal_zero.z = g_RegExt[0x26]; // Including the g-force + g_nu.cal_zero.z = g_RegExt[0x22]; + g_nu.cal_g.x = g_RegExt[0x24] - g_RegExt[0x20]; + g_nu.cal_g.y = g_RegExt[0x25] - g_RegExt[0x21]; + g_nu.cal_g.z = g_RegExt[0x26] - g_RegExt[0x22]; g_nu.jx.max = g_RegExt[0x28]; g_nu.jx.min = g_RegExt[0x29]; g_nu.jx.center = g_RegExt[0x2a]; diff --git a/Source/Plugins/Plugin_Wiimote/Src/FillReport.cpp b/Source/Plugins/Plugin_Wiimote/Src/FillReport.cpp index abaf0a46d8..536f810ee5 100644 --- a/Source/Plugins/Plugin_Wiimote/Src/FillReport.cpp +++ b/Source/Plugins/Plugin_Wiimote/Src/FillReport.cpp @@ -57,31 +57,67 @@ double g_RecordingStart[3]; //g_RecordingStart[0] = 0; g_RecordingStart[1] = 0; double g_RecordingCurrentTime[3]; //g_RecordingCurrentTime[0] = 0; g_RecordingCurrentTime[1] = 0; // -------------------------- -// Convert from -350 to -3.5 g -int G2Accelerometer(int _G, int XYZ) +///////////////////////////////////////////////////////////////////////// +/* Convert from -350 to -3.5 g. The Nunchuck gravity size is 51 compared to the 26 to 28 for the Wiimote. + So the maximum g values are higher for the Wiimote. */ +// --------------- +int G2Accelerometer(int _G, int XYZ, int Wm) { float G = (float)_G / 100.0; float Neutral, OneG, Accelerometer; switch(XYZ) { - case 0: - OneG = (float)g_accel.cal_g.x; - Neutral = (float)g_accel.cal_zero.x; + case 0: // X + if(Wm == WM_RECORDING_WIIMOTE) + { + OneG = (float)g_wm.cal_g.x; + Neutral = (float)g_wm.cal_zero.x; + } + else + { + OneG = (float)g_nu.cal_g.x; + Neutral = (float)g_nu.cal_zero.x; + } break; - case 1: - OneG = (float)g_accel.cal_g.y; - Neutral = (float)g_accel.cal_zero.y; + case 1: // Y + if(Wm == WM_RECORDING_WIIMOTE) + { + OneG = (float)g_wm.cal_g.y; + Neutral = (float)g_wm.cal_zero.y; + } + else + { + OneG = (float)g_nu.cal_g.y; + Neutral = (float)g_nu.cal_zero.y; + } break; - case 2: - OneG = (float)g_accel.cal_g.z; - Neutral = (float)g_accel.cal_zero.z; + case 2: // Z + if(Wm == WM_RECORDING_WIIMOTE) + { + OneG = (float)g_wm.cal_g.z; + Neutral = (float)g_wm.cal_zero.z; + } + else + { + OneG = (float)g_nu.cal_g.z; + Neutral = (float)g_nu.cal_zero.z; + } break; default: PanicAlert("There is a syntax error in a function that is calling G2Accelerometer(%i, %i)", _G, XYZ); } Accelerometer = Neutral + (OneG * G); - return (int)Accelerometer; + int Return = (int)Accelerometer; + + // Logging + //Console::Print("G2Accelerometer():%f %f %f %f\n", Neutral, OneG, G, Accelerometer); + + // Boundaries + if (Return > 255) Return = 255; + if (Return < 0) Return = 0; + + return Return; } template @@ -159,9 +195,9 @@ bool RecordingPlayAccIR(u8 &_x, u8 &_y, u8 &_z, IRReportType &_IR, int Wm) } // Update accelerometer values - _x = G2Accelerometer(VRecording.at(g_RecordingPlaying[Wm]).Recording.at(g_RecordingPoint[Wm]).x, 0); - _y = G2Accelerometer(VRecording.at(g_RecordingPlaying[Wm]).Recording.at(g_RecordingPoint[Wm]).y, 1); - _z = G2Accelerometer(VRecording.at(g_RecordingPlaying[Wm]).Recording.at(g_RecordingPoint[Wm]).z, 2); + _x = G2Accelerometer(VRecording.at(g_RecordingPlaying[Wm]).Recording.at(g_RecordingPoint[Wm]).x, 0, Wm); + _y = G2Accelerometer(VRecording.at(g_RecordingPlaying[Wm]).Recording.at(g_RecordingPoint[Wm]).y, 1, Wm); + _z = G2Accelerometer(VRecording.at(g_RecordingPlaying[Wm]).Recording.at(g_RecordingPoint[Wm]).z, 2, Wm); // Update IR values if(Wm == WM_RECORDING_IR) memcpy(&_IR, VRecording.at(g_RecordingPlaying[Wm]).Recording.at(g_RecordingPoint[Wm]).IR, IRBytes); @@ -440,7 +476,7 @@ void SingleShake(u8 &_y, u8 &_z, int i) else if(Shake[i] == 2) { // This works regardless of calibration, in Wario Land - _z = g_accel.cal_zero.z - 2; + _z = g_wm.cal_zero.z - 2; _y = 0; Shake[i] = 1; } @@ -631,9 +667,9 @@ void FillReportAcc(wm_accel& _acc) // --------------------- // The default values can change so we need to update them all the time - g_X = g_accel.cal_zero.x; - g_Y = g_accel.cal_zero.y; - g_Z = g_accel.cal_zero.z + g_accel.cal_g.z; + g_X = g_wm.cal_zero.x; + g_Y = g_wm.cal_zero.y; + g_Z = g_wm.cal_zero.z + g_wm.cal_g.z; // Check that Dolphin is in focus @@ -958,11 +994,10 @@ void FillReportExtension(wm_extension& _ext) // We should not play back the accelerometer values if (!(g_RecordingPlaying[1] >= 0 && RecordingPlay(_ext.ax, _ext.ay, _ext.az, 1))) { - /* These are the default neutral values for the nunchuck accelerometer according to the calibration - data we have in nunchuck_calibration[] */ - _ext.ax = 0x80; - _ext.ay = 0x80; - _ext.az = 0xb3; + // Use the neutral values + _ext.ax = g_nu.cal_zero.x; + _ext.ay = g_nu.cal_zero.y; + _ext.az = g_nu.cal_zero.z + g_nu.cal_g.z; } // --------------------- diff --git a/Source/Plugins/Plugin_Wiimote/Src/main.cpp b/Source/Plugins/Plugin_Wiimote/Src/main.cpp index 954797067c..032ca96a58 100644 --- a/Source/Plugins/Plugin_Wiimote/Src/main.cpp +++ b/Source/Plugins/Plugin_Wiimote/Src/main.cpp @@ -72,7 +72,7 @@ bool g_EmulatedWiiMoteInitialized = false; bool g_WiimoteUnexpectedDisconnect = false; // Settings -accel_cal g_accel; +accel_cal g_wm; nu_cal g_nu; // Debugging @@ -755,9 +755,9 @@ void ReadDebugging(bool Emu, const void* _pData, int Size) // ------------------------- // Show the number of g forces on the axes - float Gx = WiiMoteEmu::AccelerometerToG((float)data[4], (float)g_accel.cal_zero.x, (float)g_accel.cal_g.x); - float Gy = WiiMoteEmu::AccelerometerToG((float)data[5], (float)g_accel.cal_zero.y, (float)g_accel.cal_g.y); - float Gz = WiiMoteEmu::AccelerometerToG((float)data[6], (float)g_accel.cal_zero.z, (float)g_accel.cal_g.z); + float Gx = WiiMoteEmu::AccelerometerToG((float)data[4], (float)g_wm.cal_zero.x, (float)g_wm.cal_g.x); + float Gy = WiiMoteEmu::AccelerometerToG((float)data[5], (float)g_wm.cal_zero.y, (float)g_wm.cal_g.y); + float Gz = WiiMoteEmu::AccelerometerToG((float)data[6], (float)g_wm.cal_zero.z, (float)g_wm.cal_g.z); std::string GForce = StringFromFormat("%s %s %s", ((int)Gx >= 0) ? StringFromFormat(" %i", (int)Gx).c_str() : StringFromFormat("%i", (int)Gx).c_str(), ((int)Gy >= 0) ? StringFromFormat(" %i", (int)Gy).c_str() : StringFromFormat("%i", (int)Gy).c_str(), diff --git a/Source/Plugins/Plugin_Wiimote/Src/main.h b/Source/Plugins/Plugin_Wiimote/Src/main.h index e96c55ece8..ca7e264579 100644 --- a/Source/Plugins/Plugin_Wiimote/Src/main.h +++ b/Source/Plugins/Plugin_Wiimote/Src/main.h @@ -79,7 +79,7 @@ struct SRecordingAll #endif // Settings - extern accel_cal g_accel; + extern accel_cal g_wm; extern nu_cal g_nu; // Debugging diff --git a/Source/Plugins/Plugin_Wiimote/Src/wiimote_hid.h b/Source/Plugins/Plugin_Wiimote/Src/wiimote_hid.h index d34f65a9fa..f62a6e0d26 100644 --- a/Source/Plugins/Plugin_Wiimote/Src/wiimote_hid.h +++ b/Source/Plugins/Plugin_Wiimote/Src/wiimote_hid.h @@ -308,6 +308,7 @@ struct nu_js { struct nu_cal { wm_accel cal_zero; // zero calibratio + wm_accel cal_g; // g size nu_js jx; // nu_js jy; // };