c
Diff: GY80.cpp
- Revision:
- 2:2553ca95fba0
- Parent:
- 1:6909d797972f
- Child:
- 3:981c93d62e16
diff -r 6909d797972f -r 2553ca95fba0 GY80.cpp --- a/GY80.cpp Sat Oct 05 02:22:15 2013 +0000 +++ b/GY80.cpp Sun Oct 06 07:09:50 2013 +0000 @@ -1,13 +1,17 @@ #include "GY80.h" -GY80::GY80(PinName sda, PinName scl): Wire( sda, scl) +Serial pc2(USBTX, USBRX); + +GY80::GY80() : Wire( SDA,SCL) { Wire.frequency(I2C_FREQ); Accel_Init(); Gyro_Init(); Magn_Init(); } - +GY80::~GY80() +{ +} void GY80::Accel_Init() { byte data[2]; @@ -33,7 +37,7 @@ byte data[2]; data[0] = 0x20; //L3G4200D_CTRL_REG1 - data[1] = 0x0; // normal power mode, all axes enable, 100Hz + data[1] = 0x0F; // normal power mode, all axes enable, 100Hz Wire.write(GYRO_ADDRESS, data, 2); wait_ms(1); @@ -51,7 +55,6 @@ data[0] = 0x24; // L3G4200D_CTRL_REG5 data[1] = 0x02; //Low Pass Filter Wire.write(GYRO_ADDRESS, data, 2); - wait_ms(1); } void GY80::Magn_Init() @@ -76,7 +79,7 @@ if (Wire.read(ACCEL_ADDRESS, buff,6) == 0) // All bytes received? { - Convert(accel_v, buff); + Convert(accel, buff); } // Why aren't we scaling accelerometer? I think the DCM paper talks a little about this... ?? accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE; @@ -94,7 +97,7 @@ // Request 6 bytes if (Wire.read(GYRO_ADDRESS, buff,6) == 0) // All bytes received? { - Convert(gyro_v, buff); + Convert(gyro, buff); } gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X); gyro_v[1] = DEG2RAD((gyro[1] - GYRO_Y_OFFSET) * GYRO_GAIN_Y); @@ -111,16 +114,16 @@ // Request 6 bytes if (Wire.read(MAGN_ADDRESS, buff,6) == 0) // All bytes received? { - Convert(magn_v, buff); + Convert(mag, buff); } magn_v[0] = (mag[0] - MAGN_X_OFFSET) * MAGN_X_SCALE; magn_v[1] = (mag[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE; magn_v[2] = (mag[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE; } -void GY80::Convert(float* value, byte* raw) +void GY80::Convert(short* value, byte* raw) { - value[0] = ((signed short) raw[1] << 8) | raw[0]; // Y axis (internal sensor x axis) - value[1] = ((signed short) raw[3] << 8) | raw[2]; // X axis (internal sensor y axis) - value[2] = ((signed short) raw[5] << 8) | raw[4]; // Z axis (internal sensor z axis) + value[0] = (short) ((uint16_t) raw[1] << 8 | raw[0]); + value[1] = (short) ((uint16_t) raw[3] << 8 | raw[2]); + value[2] = (short) ((uint16_t) raw[5] << 8 | raw[4]); } \ No newline at end of file