specchioriflesso/USB_Host_Shield_2.0
1
Fork 0
mirror of https://github.com/felis/USB_Host_Shield_2.0.git synced 2024-03-22 11:31:26 +01:00
Code Issues Packages Projects Releases Wiki Activity

Updated getAngle()

Browse source
This commit is contained in:
Kristian Lauszus 2012-05-09 19:46:26 +02:00
parent e60ddbac5a
commit 95a797e735
1 changed files with 24 additions and 30 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

54
PS3BT.cpp
View file

@ -507,45 +507,39 @@ double PS3BT::getAngle(Angle a) {
if(PS3BTConnected) { if(PS3BTConnected) {
// Data for the Kionix KXPC4 used in the DualShock 3 // Data for the Kionix KXPC4 used in the DualShock 3
double sensivity = 204.6; // 0.66/3.3*1023 (660mV/g) const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V)
double zeroG = 511.5; // 1.65/3.3*1023 (1,65V) accXval = -((double)getSensor(aX)-zeroG);
accXval = ((double)getSensor(aX)-zeroG) / sensivity; // Convert to g's accYval = -((double)getSensor(aY)-zeroG);
accXval *= 2; accZval = -((double)getSensor(aZ)-zeroG);
accYval = ((double)getSensor(aY)-zeroG) / sensivity; // Convert to g's } else if(PS3MoveBTConnected) {
accYval *= 2; // It's a Kionix KXSC4 inside the Motion controller
accZval = ((double)getSensor(aZ)-zeroG) / sensivity; // Convert to g's const uint16_t zeroG = 0x8000;
accZval *= 2; accXval = getSensor(aXmove);
} else if(PS3MoveBTConnected) { accYval = getSensor(aYmove);
// It's a Kionix KXSC4 inside the Motion controller accZval = getSensor(aZmove);
const uint16_t sensivity = 28285; // Find by experimenting
accXval = (double)getSensor(aXmove)/sensivity;
accYval = (double)getSensor(aYmove)/sensivity;
accZval = (double)getSensor(aZmove)/sensivity;
if(accXval < -1) // Convert to g's if(accXval < 0)
accXval = ((1+accXval)-(1-1.15))*(-1/0.15); accXval += zeroG;
else if(accXval > 1) else
accXval = ((1+accXval)-(1+1.15))*(-1/0.15); accXval -= zeroG;
if(accYval < 0)
if(accYval < -1) // Convert to g's accYval += zeroG;
accYval = ((1+accYval)-(1-1.15))*(-1/0.15); else
else if(accYval > 1) accYval -= zeroG;
accYval = ((1+accYval)-(1+1.15))*(-1/0.15); if(accZval < 0)
accZval += zeroG;
if(accZval < -1) // Convert to g's else
accZval = ((1+accZval)-(1-1.15))*(-1/0.15); accZval -= zeroG;
else if(accZval > 1)
accZval = ((1+accZval)-(1+1.15))*(-1/0.15);
} }
// Convert to 360 degrees resolution // Convert to 360 degrees resolution
// atan2 outputs the value of -π to π (radians) // atan2 outputs the value of -π to π (radians)
// We are then converting it to 0 to 2π and then to degrees // We are then converting it to 0 to 2π and then to degrees
if (a == Pitch) { if (a == Pitch) {
double angle = (atan2(-accYval,-accZval)+PI)*RAD_TO_DEG; double angle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG;
return angle; return angle;
} else { } else {
double angle = (atan2(-accXval,-accZval)+PI)*RAD_TO_DEG; double angle = (atan2(accXval,accZval)+PI)*RAD_TO_DEG;
return angle; return angle;
} }
} }