Mateusz Jaskula
/
KL25Z_TangoIO
KL25 driver for Tango Control System
Diff: GY80/GY80.cpp
- Revision:
- 2:9fe6f1e273b4
- Parent:
- 0:5d27c333afa6
diff -r f1ee978773d3 -r 9fe6f1e273b4 GY80/GY80.cpp --- a/GY80/GY80.cpp Tue Aug 26 06:46:55 2014 +0000 +++ b/GY80/GY80.cpp Thu Aug 28 07:50:06 2014 +0000 @@ -2,6 +2,7 @@ Serial pc2(USBTX, USBRX); + GY80::GY80() : Wire( SDA,SCL) { Wire.frequency(I2C_FREQ); @@ -73,16 +74,19 @@ buff[0] = 0x32; // Send address to read from Wire.write(ACCEL_ADDRESS, buff, 1); - int accel[3]; + //int accel[3]; if (Wire.read(ACCEL_ADDRESS, buff,6) == 0) // All bytes received? { - accel[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]); - accel[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]); - accel[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]); + accel_v[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]); + accel_v[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]); + accel_v[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]); } - accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE; + /*accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE; accel_v[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE; - accel_v[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE; + accel_v[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;*/ + /*accel_v[0] = accel[0]*0.5 + accel_v[0]*0.5; + accel_v[1] = accel[0]*0.5 + accel_v[0]*0.5; + accel_v[2] = accel[0]*0.5 + accel_v[0]*0.5;*/ } @@ -93,16 +97,16 @@ buff[0] = 0xA8; // 0x28 | (1 << 7) Send address to read from Wire.write(GYRO_ADDRESS, buff, 1); // Request 6 bytes - int gyro[3]; + // int gyro[3]; if (Wire.read(GYRO_ADDRESS, buff,6) == 0) // All bytes received? { - gyro[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]); - gyro[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]); - gyro[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]); + gyro_v[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]); + gyro_v[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]); + gyro_v[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]); } - gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X); + /*gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X); gyro_v[1] = DEG2RAD((gyro[1] - GYRO_Y_OFFSET) * GYRO_GAIN_Y); - gyro_v[2] = DEG2RAD((gyro[2] - GYRO_Z_OFFSET) * GYRO_GAIN_Z); + gyro_v[2] = DEG2RAD((gyro[2] - GYRO_Z_OFFSET) * GYRO_GAIN_Z);*/ } void GY80::Read_Magn(float* magn_v) @@ -113,15 +117,15 @@ Wire.write(MAGN_ADDRESS, buff, 1); // Request 6 bytes - int mag[3]; + //int mag[3]; if (Wire.read(MAGN_ADDRESS, buff,6) == 0) // All bytes received? { - mag[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]); - mag[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]); - mag[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]); + magn_v[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]); + magn_v[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]); + magn_v[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]); } - magn_v[0] = (mag[0] - MAGN_X_OFFSET) * MAGN_X_SCALE; + /*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; + magn_v[2] = (mag[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE; */ }