diff --git a/PS3BT.cpp b/PS3BT.cpp index 235092e0..ab5a8ed5 100644 --- a/PS3BT.cpp +++ b/PS3BT.cpp @@ -82,15 +82,15 @@ int16_t PS3BT::getSensor(SensorEnum a) { return 0; } -double PS3BT::getAngle(AngleEnum a) { - double accXval, accYval, accZval; +float PS3BT::getAngle(AngleEnum a) { + float accXval, accYval, accZval; if(PS3Connected) { // Data for the Kionix KXPC4 used in the DualShock 3 - const double zeroG = 511.5; // 1.65/3.3*1023 (1.65V) - accXval = -((double)getSensor(aX) - zeroG); - accYval = -((double)getSensor(aY) - zeroG); - accZval = -((double)getSensor(aZ) - zeroG); + const float zeroG = 511.5f; // 1.65/3.3*1023 (1.65V) + accXval = -((float)getSensor(aX) - zeroG); + accYval = -((float)getSensor(aY) - zeroG); + accZval = -((float)getSensor(aZ) - zeroG); } else if(PS3MoveConnected) { // It's a Kionix KXSC4 inside the Motion controller const uint16_t zeroG = 0x8000; @@ -104,36 +104,36 @@ double PS3BT::getAngle(AngleEnum a) { // atan2 outputs the value of -π to π (radians) // We are then converting it to 0 to 2π and then to degrees if(a == Pitch) - return (atan2(accYval, accZval) + PI) * RAD_TO_DEG; + return (atan2f(accYval, accZval) + PI) * RAD_TO_DEG; else - return (atan2(accXval, accZval) + PI) * RAD_TO_DEG; + return (atan2f(accXval, accZval) + PI) * RAD_TO_DEG; } -double PS3BT::get9DOFValues(SensorEnum a) { // Thanks to Manfred Piendl +float PS3BT::get9DOFValues(SensorEnum a) { // Thanks to Manfred Piendl if(!PS3MoveConnected) return 0; int16_t value = getSensor(a); if(a == mXmove || a == mYmove || a == mZmove) { if(value > 2047) value -= 0x1000; - return (double)value / 3.2; // unit: muT = 10^(-6) Tesla + return (float)value / 3.2f; // unit: muT = 10^(-6) Tesla } else if(a == aXmove || a == aYmove || a == aZmove) { if(value < 0) value += 0x8000; else value -= 0x8000; - return (double)value / 442.0; // unit: m/(s^2) + return (float)value / 442.0f; // unit: m/(s^2) } else if(a == gXmove || a == gYmove || a == gZmove) { if(value < 0) value += 0x8000; else value -= 0x8000; if(a == gXmove) - return (double)value / 11.6; // unit: deg/s + return (float)value / 11.6f; // unit: deg/s else if(a == gYmove) - return (double)value / 11.2; // unit: deg/s + return (float)value / 11.2f; // unit: deg/s else // gZmove - return (double)value / 9.6; // unit: deg/s + return (float)value / 9.6f; // unit: deg/s } else return 0; } diff --git a/PS3BT.h b/PS3BT.h index c25ac5e5..5fd04c32 100644 --- a/PS3BT.h +++ b/PS3BT.h @@ -89,13 +89,13 @@ public: * @param a Either ::Pitch or ::Roll. * @return Return the angle in the range of 0-360. */ - double getAngle(AngleEnum a); + float getAngle(AngleEnum a); /** * Read the sensors inside the Move controller. * @param a ::aXmove, ::aYmove, ::aZmove, ::gXmove, ::gYmove, ::gZmove, ::mXmove, ::mYmove, and ::mXmove. * @return The value in SI units. */ - double get9DOFValues(SensorEnum a); + float get9DOFValues(SensorEnum a); /** * Get the status from the controller. * @param c The ::StatusEnum you want to read. diff --git a/PS3USB.cpp b/PS3USB.cpp index c3217538..43b12648 100755 --- a/PS3USB.cpp +++ b/PS3USB.cpp @@ -335,25 +335,23 @@ uint16_t PS3USB::getSensor(SensorEnum a) { return ((readBuf[((uint16_t)a) - 9] << 8) | readBuf[((uint16_t)a + 1) - 9]); } -double PS3USB::getAngle(AngleEnum a) { +float PS3USB::getAngle(AngleEnum a) { if(PS3Connected) { - double accXval; - double accYval; - double accZval; + float accXval, accYval, accZval; // Data for the Kionix KXPC4 used in the DualShock 3 - const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V) - accXval = -((double)getSensor(aX) - zeroG); - accYval = -((double)getSensor(aY) - zeroG); - accZval = -((double)getSensor(aZ) - zeroG); + const float zeroG = 511.5f; // 1.65/3.3*1023 (1,65V) + accXval = -((float)getSensor(aX) - zeroG); + accYval = -((float)getSensor(aY) - zeroG); + accZval = -((float)getSensor(aZ) - zeroG); // Convert to 360 degrees resolution // atan2 outputs the value of -π to π (radians) // We are then converting it to 0 to 2π and then to degrees if(a == Pitch) - return (atan2(accYval, accZval) + PI) * RAD_TO_DEG; + return (atan2f(accYval, accZval) + PI) * RAD_TO_DEG; else - return (atan2(accXval, accZval) + PI) * RAD_TO_DEG; + return (atan2f(accXval, accZval) + PI) * RAD_TO_DEG; } else return 0; } diff --git a/PS3USB.h b/PS3USB.h index 2eba9258..c10faaea 100644 --- a/PS3USB.h +++ b/PS3USB.h @@ -177,7 +177,7 @@ public: * @param a Either ::Pitch or ::Roll. * @return Return the angle in the range of 0-360. */ - double getAngle(AngleEnum a); + float getAngle(AngleEnum a); /** * Get the ::StatusEnum from the controller. * @param c The ::StatusEnum you want to read. diff --git a/PS4Parser.h b/PS4Parser.h index 51f08063..5af13e80 100644 --- a/PS4Parser.h +++ b/PS4Parser.h @@ -225,11 +225,11 @@ public: * @param a Either ::Pitch or ::Roll. * @return Return the angle in the range of 0-360. */ - double getAngle(AngleEnum a) { + float getAngle(AngleEnum a) { if (a == Pitch) - return (atan2(ps4Data.accY, ps4Data.accZ) + PI) * RAD_TO_DEG; + return (atan2f(ps4Data.accY, ps4Data.accZ) + PI) * RAD_TO_DEG; else - return (atan2(ps4Data.accX, ps4Data.accZ) + PI) * RAD_TO_DEG; + return (atan2f(ps4Data.accX, ps4Data.accZ) + PI) * RAD_TO_DEG; }; /** diff --git a/Wii.cpp b/Wii.cpp index 4bbf4c91..20975a81 100755 --- a/Wii.cpp +++ b/Wii.cpp @@ -503,9 +503,9 @@ void WII::ACLData(uint8_t* l2capinbuf) { gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)) - gyroRollZero); gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)) - gyroPitchZero); - yawGyroSpeed = (double)gyroYawRaw / ((double)gyroYawZero / yawGyroScale); - rollGyroSpeed = -(double)gyroRollRaw / ((double)gyroRollZero / rollGyroScale); // We invert these values so they will fit the acc values - pitchGyroSpeed = (double)gyroPitchRaw / ((double)gyroPitchZero / pitchGyroScale); + yawGyroSpeed = (float)gyroYawRaw / ((float)gyroYawZero / yawGyroScale); + rollGyroSpeed = -(float)gyroRollRaw / ((float)gyroRollZero / rollGyroScale); // We invert these values so they will fit the acc values + pitchGyroSpeed = (float)gyroPitchRaw / ((float)gyroPitchZero / pitchGyroScale); /* The onboard gyro has two ranges for slow and fast mode */ if(!(l2capinbuf[18] & 0x02)) // Check if fast mode is used @@ -515,12 +515,12 @@ void WII::ACLData(uint8_t* l2capinbuf) { if(!(l2capinbuf[19] & 0x02)) // Check if fast mode is used rollGyroSpeed *= 4.545; - compPitch = (0.93 * (compPitch + (pitchGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * getWiimotePitch()); // Use a complimentary filter to calculate the angle - compRoll = (0.93 * (compRoll + (rollGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * getWiimoteRoll()); + compPitch = (0.93f * (compPitch + (pitchGyroSpeed * (float)(micros() - timer) / 1000000.0f)))+(0.07f * getWiimotePitch()); // Use a complimentary filter to calculate the angle + compRoll = (0.93f * (compRoll + (rollGyroSpeed * (float)(micros() - timer) / 1000000.0f)))+(0.07f * getWiimoteRoll()); - gyroYaw += (yawGyroSpeed * ((double)(micros() - timer) / 1000000)); - gyroRoll += (rollGyroSpeed * ((double)(micros() - timer) / 1000000)); - gyroPitch += (pitchGyroSpeed * ((double)(micros() - timer) / 1000000)); + gyroYaw += (yawGyroSpeed * ((float)(micros() - timer) / 1000000.0f)); + gyroRoll += (rollGyroSpeed * ((float)(micros() - timer) / 1000000.0f)); + gyroPitch += (pitchGyroSpeed * ((float)(micros() - timer) / 1000000.0f)); timer = micros(); /* // Uncomment these lines to tune the gyro scale variabels diff --git a/Wii.h b/Wii.h index 960f2273..efb64b2f 100755 --- a/Wii.h +++ b/Wii.h @@ -107,7 +107,7 @@ public: * Pitch calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected. * @return Pitch in the range from 0-360. */ - double getPitch() { + float getPitch() { if(motionPlusConnected) return compPitch; return getWiimotePitch(); @@ -117,7 +117,7 @@ public: * Roll calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected. * @return Roll in the range from 0-360. */ - double getRoll() { + float getRoll() { if(motionPlusConnected) return compRoll; return getWiimoteRoll(); @@ -129,7 +129,7 @@ public: * NOTE: This angle will drift a lot and is only available if the Motion Plus extension is connected. * @return The angle calculated using the gyro. */ - double getYaw() { + float getYaw() { return gyroYaw; }; @@ -209,24 +209,24 @@ public: /**@{*/ /** Pitch and roll calculated from the accelerometer inside the Wiimote. */ - double getWiimotePitch() { - return (atan2(accYwiimote, accZwiimote) + PI) * RAD_TO_DEG; + float getWiimotePitch() { + return (atan2f(accYwiimote, accZwiimote) + PI) * RAD_TO_DEG; }; - double getWiimoteRoll() { - return (atan2(accXwiimote, accZwiimote) + PI) * RAD_TO_DEG; + float getWiimoteRoll() { + return (atan2f(accXwiimote, accZwiimote) + PI) * RAD_TO_DEG; }; /**@}*/ /**@{*/ /** Pitch and roll calculated from the accelerometer inside the Nunchuck. */ - double getNunchuckPitch() { - return (atan2(accYnunchuck, accZnunchuck) + PI) * RAD_TO_DEG; + float getNunchuckPitch() { + return (atan2f(accYnunchuck, accZnunchuck) + PI) * RAD_TO_DEG; }; - double getNunchuckRoll() { - return (atan2(accXnunchuck, accZnunchuck) + PI) * RAD_TO_DEG; + float getNunchuckRoll() { + return (atan2f(accXnunchuck, accZnunchuck) + PI) * RAD_TO_DEG; }; /**@}*/ @@ -238,17 +238,17 @@ public: /* Variables for the gyro inside the Motion Plus */ /** This is the pitch calculated by the gyro - use this to tune WII#pitchGyroScale. */ - double gyroPitch; + float gyroPitch; /** This is the roll calculated by the gyro - use this to tune WII#rollGyroScale. */ - double gyroRoll; + float gyroRoll; /** This is the yaw calculated by the gyro - use this to tune WII#yawGyroScale. */ - double gyroYaw; + float gyroYaw; /**@{*/ /** The speed in deg/s from the gyro. */ - double pitchGyroSpeed; - double rollGyroSpeed; - double yawGyroSpeed; + float pitchGyroSpeed; + float rollGyroSpeed; + float yawGyroSpeed; /**@}*/ /**@{*/ @@ -482,8 +482,8 @@ private: uint16_t wiiBalanceBoardRaw[4]; // Wii Balance Board raw values uint16_t wiiBalanceBoardCal[3][4]; // Wii Balance Board calibration values - double compPitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected - double compRoll; // Fusioned angle using a complimentary filter if the Motion Plus is connected + float compPitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected + float compRoll; // Fusioned angle using a complimentary filter if the Motion Plus is connected bool activateNunchuck; bool motionValuesReset; // This bool is true when the gyro values has been reset