Replaced all double variables with float

This commit is contained in:
Kristian Sloth Lauszus 2015-10-12 13:20:48 +02:00
parent 523e66e827
commit 7e449d2d3a
7 changed files with 55 additions and 57 deletions

View file

@ -82,15 +82,15 @@ int16_t PS3BT::getSensor(SensorEnum a) {
return 0; return 0;
} }
double PS3BT::getAngle(AngleEnum a) { float PS3BT::getAngle(AngleEnum a) {
double accXval, accYval, accZval; float accXval, accYval, accZval;
if(PS3Connected) { if(PS3Connected) {
// Data for the Kionix KXPC4 used in the DualShock 3 // Data for the Kionix KXPC4 used in the DualShock 3
const double zeroG = 511.5; // 1.65/3.3*1023 (1.65V) const float zeroG = 511.5f; // 1.65/3.3*1023 (1.65V)
accXval = -((double)getSensor(aX) - zeroG); accXval = -((float)getSensor(aX) - zeroG);
accYval = -((double)getSensor(aY) - zeroG); accYval = -((float)getSensor(aY) - zeroG);
accZval = -((double)getSensor(aZ) - zeroG); accZval = -((float)getSensor(aZ) - zeroG);
} else if(PS3MoveConnected) { } else if(PS3MoveConnected) {
// It's a Kionix KXSC4 inside the Motion controller // It's a Kionix KXSC4 inside the Motion controller
const uint16_t zeroG = 0x8000; const uint16_t zeroG = 0x8000;
@ -104,36 +104,36 @@ double PS3BT::getAngle(AngleEnum a) {
// 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)
return (atan2(accYval, accZval) + PI) * RAD_TO_DEG; return (atan2f(accYval, accZval) + PI) * RAD_TO_DEG;
else 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) if(!PS3MoveConnected)
return 0; return 0;
int16_t value = getSensor(a); int16_t value = getSensor(a);
if(a == mXmove || a == mYmove || a == mZmove) { if(a == mXmove || a == mYmove || a == mZmove) {
if(value > 2047) if(value > 2047)
value -= 0x1000; 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) { } else if(a == aXmove || a == aYmove || a == aZmove) {
if(value < 0) if(value < 0)
value += 0x8000; value += 0x8000;
else else
value -= 0x8000; 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) { } else if(a == gXmove || a == gYmove || a == gZmove) {
if(value < 0) if(value < 0)
value += 0x8000; value += 0x8000;
else else
value -= 0x8000; value -= 0x8000;
if(a == gXmove) if(a == gXmove)
return (double)value / 11.6; // unit: deg/s return (float)value / 11.6f; // unit: deg/s
else if(a == gYmove) else if(a == gYmove)
return (double)value / 11.2; // unit: deg/s return (float)value / 11.2f; // unit: deg/s
else // gZmove else // gZmove
return (double)value / 9.6; // unit: deg/s return (float)value / 9.6f; // unit: deg/s
} else } else
return 0; return 0;
} }

View file

@ -89,13 +89,13 @@ public:
* @param a Either ::Pitch or ::Roll. * @param a Either ::Pitch or ::Roll.
* @return Return the angle in the range of 0-360. * @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. * Read the sensors inside the Move controller.
* @param a ::aXmove, ::aYmove, ::aZmove, ::gXmove, ::gYmove, ::gZmove, ::mXmove, ::mYmove, and ::mXmove. * @param a ::aXmove, ::aYmove, ::aZmove, ::gXmove, ::gYmove, ::gZmove, ::mXmove, ::mYmove, and ::mXmove.
* @return The value in SI units. * @return The value in SI units.
*/ */
double get9DOFValues(SensorEnum a); float get9DOFValues(SensorEnum a);
/** /**
* Get the status from the controller. * Get the status from the controller.
* @param c The ::StatusEnum you want to read. * @param c The ::StatusEnum you want to read.

View file

@ -335,25 +335,23 @@ uint16_t PS3USB::getSensor(SensorEnum a) {
return ((readBuf[((uint16_t)a) - 9] << 8) | readBuf[((uint16_t)a + 1) - 9]); 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) { if(PS3Connected) {
double accXval; float accXval, accYval, accZval;
double accYval;
double accZval;
// Data for the Kionix KXPC4 used in the DualShock 3 // Data for the Kionix KXPC4 used in the DualShock 3
const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V) const float zeroG = 511.5f; // 1.65/3.3*1023 (1,65V)
accXval = -((double)getSensor(aX) - zeroG); accXval = -((float)getSensor(aX) - zeroG);
accYval = -((double)getSensor(aY) - zeroG); accYval = -((float)getSensor(aY) - zeroG);
accZval = -((double)getSensor(aZ) - zeroG); accZval = -((float)getSensor(aZ) - zeroG);
// 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)
return (atan2(accYval, accZval) + PI) * RAD_TO_DEG; return (atan2f(accYval, accZval) + PI) * RAD_TO_DEG;
else else
return (atan2(accXval, accZval) + PI) * RAD_TO_DEG; return (atan2f(accXval, accZval) + PI) * RAD_TO_DEG;
} else } else
return 0; return 0;
} }

View file

@ -177,7 +177,7 @@ public:
* @param a Either ::Pitch or ::Roll. * @param a Either ::Pitch or ::Roll.
* @return Return the angle in the range of 0-360. * @return Return the angle in the range of 0-360.
*/ */
double getAngle(AngleEnum a); float getAngle(AngleEnum a);
/** /**
* Get the ::StatusEnum from the controller. * Get the ::StatusEnum from the controller.
* @param c The ::StatusEnum you want to read. * @param c The ::StatusEnum you want to read.

View file

@ -225,11 +225,11 @@ public:
* @param a Either ::Pitch or ::Roll. * @param a Either ::Pitch or ::Roll.
* @return Return the angle in the range of 0-360. * @return Return the angle in the range of 0-360.
*/ */
double getAngle(AngleEnum a) { float getAngle(AngleEnum a) {
if (a == Pitch) if (a == Pitch)
return (atan2(ps4Data.accY, ps4Data.accZ) + PI) * RAD_TO_DEG; return (atan2f(ps4Data.accY, ps4Data.accZ) + PI) * RAD_TO_DEG;
else else
return (atan2(ps4Data.accX, ps4Data.accZ) + PI) * RAD_TO_DEG; return (atan2f(ps4Data.accX, ps4Data.accZ) + PI) * RAD_TO_DEG;
}; };
/** /**

16
Wii.cpp
View file

@ -503,9 +503,9 @@ void WII::ACLData(uint8_t* l2capinbuf) {
gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)) - gyroRollZero); gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)) - gyroRollZero);
gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)) - gyroPitchZero); gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)) - gyroPitchZero);
yawGyroSpeed = (double)gyroYawRaw / ((double)gyroYawZero / yawGyroScale); yawGyroSpeed = (float)gyroYawRaw / ((float)gyroYawZero / yawGyroScale);
rollGyroSpeed = -(double)gyroRollRaw / ((double)gyroRollZero / rollGyroScale); // We invert these values so they will fit the acc values rollGyroSpeed = -(float)gyroRollRaw / ((float)gyroRollZero / rollGyroScale); // We invert these values so they will fit the acc values
pitchGyroSpeed = (double)gyroPitchRaw / ((double)gyroPitchZero / pitchGyroScale); pitchGyroSpeed = (float)gyroPitchRaw / ((float)gyroPitchZero / pitchGyroScale);
/* The onboard gyro has two ranges for slow and fast mode */ /* The onboard gyro has two ranges for slow and fast mode */
if(!(l2capinbuf[18] & 0x02)) // Check if fast mode is used 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 if(!(l2capinbuf[19] & 0x02)) // Check if fast mode is used
rollGyroSpeed *= 4.545; rollGyroSpeed *= 4.545;
compPitch = (0.93 * (compPitch + (pitchGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * getWiimotePitch()); // Use a complimentary filter to calculate the angle compPitch = (0.93f * (compPitch + (pitchGyroSpeed * (float)(micros() - timer) / 1000000.0f)))+(0.07f * getWiimotePitch()); // Use a complimentary filter to calculate the angle
compRoll = (0.93 * (compRoll + (rollGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * getWiimoteRoll()); compRoll = (0.93f * (compRoll + (rollGyroSpeed * (float)(micros() - timer) / 1000000.0f)))+(0.07f * getWiimoteRoll());
gyroYaw += (yawGyroSpeed * ((double)(micros() - timer) / 1000000)); gyroYaw += (yawGyroSpeed * ((float)(micros() - timer) / 1000000.0f));
gyroRoll += (rollGyroSpeed * ((double)(micros() - timer) / 1000000)); gyroRoll += (rollGyroSpeed * ((float)(micros() - timer) / 1000000.0f));
gyroPitch += (pitchGyroSpeed * ((double)(micros() - timer) / 1000000)); gyroPitch += (pitchGyroSpeed * ((float)(micros() - timer) / 1000000.0f));
timer = micros(); timer = micros();
/* /*
// Uncomment these lines to tune the gyro scale variabels // Uncomment these lines to tune the gyro scale variabels

38
Wii.h
View file

@ -107,7 +107,7 @@ public:
* Pitch calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected. * Pitch calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected.
* @return Pitch in the range from 0-360. * @return Pitch in the range from 0-360.
*/ */
double getPitch() { float getPitch() {
if(motionPlusConnected) if(motionPlusConnected)
return compPitch; return compPitch;
return getWiimotePitch(); return getWiimotePitch();
@ -117,7 +117,7 @@ public:
* Roll calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected. * Roll calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected.
* @return Roll in the range from 0-360. * @return Roll in the range from 0-360.
*/ */
double getRoll() { float getRoll() {
if(motionPlusConnected) if(motionPlusConnected)
return compRoll; return compRoll;
return getWiimoteRoll(); return getWiimoteRoll();
@ -129,7 +129,7 @@ public:
* <B>NOTE:</B> This angle will drift a lot and is only available if the Motion Plus extension is connected. * <B>NOTE:</B> This angle will drift a lot and is only available if the Motion Plus extension is connected.
* @return The angle calculated using the gyro. * @return The angle calculated using the gyro.
*/ */
double getYaw() { float getYaw() {
return gyroYaw; return gyroYaw;
}; };
@ -209,24 +209,24 @@ public:
/**@{*/ /**@{*/
/** Pitch and roll calculated from the accelerometer inside the Wiimote. */ /** Pitch and roll calculated from the accelerometer inside the Wiimote. */
double getWiimotePitch() { float getWiimotePitch() {
return (atan2(accYwiimote, accZwiimote) + PI) * RAD_TO_DEG; return (atan2f(accYwiimote, accZwiimote) + PI) * RAD_TO_DEG;
}; };
double getWiimoteRoll() { float getWiimoteRoll() {
return (atan2(accXwiimote, accZwiimote) + PI) * RAD_TO_DEG; return (atan2f(accXwiimote, accZwiimote) + PI) * RAD_TO_DEG;
}; };
/**@}*/ /**@}*/
/**@{*/ /**@{*/
/** Pitch and roll calculated from the accelerometer inside the Nunchuck. */ /** Pitch and roll calculated from the accelerometer inside the Nunchuck. */
double getNunchuckPitch() { float getNunchuckPitch() {
return (atan2(accYnunchuck, accZnunchuck) + PI) * RAD_TO_DEG; return (atan2f(accYnunchuck, accZnunchuck) + PI) * RAD_TO_DEG;
}; };
double getNunchuckRoll() { float getNunchuckRoll() {
return (atan2(accXnunchuck, accZnunchuck) + PI) * RAD_TO_DEG; return (atan2f(accXnunchuck, accZnunchuck) + PI) * RAD_TO_DEG;
}; };
/**@}*/ /**@}*/
@ -238,17 +238,17 @@ public:
/* Variables for the gyro inside the Motion Plus */ /* Variables for the gyro inside the Motion Plus */
/** This is the pitch calculated by the gyro - use this to tune WII#pitchGyroScale. */ /** 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. */ /** 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. */ /** 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. */ /** The speed in deg/s from the gyro. */
double pitchGyroSpeed; float pitchGyroSpeed;
double rollGyroSpeed; float rollGyroSpeed;
double yawGyroSpeed; float yawGyroSpeed;
/**@}*/ /**@}*/
/**@{*/ /**@{*/
@ -482,8 +482,8 @@ private:
uint16_t wiiBalanceBoardRaw[4]; // Wii Balance Board raw values uint16_t wiiBalanceBoardRaw[4]; // Wii Balance Board raw values
uint16_t wiiBalanceBoardCal[3][4]; // Wii Balance Board calibration 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 float 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 compRoll; // Fusioned angle using a complimentary filter if the Motion Plus is connected
bool activateNunchuck; bool activateNunchuck;
bool motionValuesReset; // This bool is true when the gyro values has been reset bool motionValuesReset; // This bool is true when the gyro values has been reset