c
Diff: GY80.cpp
- Revision:
- 1:6909d797972f
- Parent:
- 0:336ec4d70363
- Child:
- 2:2553ca95fba0
--- a/GY80.cpp Fri Oct 04 05:27:10 2013 +0000 +++ b/GY80.cpp Sat Oct 05 02:22:15 2013 +0000 @@ -1,12 +1,11 @@ #include "GY80.h" - -GY80::GY80(PinName sda, PinName scl):Wire(sda,scl) +GY80::GY80(PinName sda, PinName scl): Wire( sda, scl) { + Wire.frequency(I2C_FREQ); Accel_Init(); Gyro_Init(); Magn_Init(); - Wire.frequency(I2C_FREQ); } void GY80::Accel_Init() @@ -16,12 +15,12 @@ data[1] = 0x08; //Measurement mode Wire.write(ACCEL_ADDRESS, data, 2); wait_ms(1); - + data[0] = 0x31; // Data format register data[1] = 0x08; //Set to full resolution Wire.write(ACCEL_ADDRESS, data, 2); wait_ms(1); - + // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth) data[0] = 0x2C; // Rate data[1] = 0x0A; //Set to 50Hz, normal operation, 0x0A 100hz @@ -37,18 +36,18 @@ data[1] = 0x0; // normal power mode, all axes enable, 100Hz Wire.write(GYRO_ADDRESS, data, 2); wait_ms(1); - - + + data[0] = 0x23; // L3G4200D_CTRL_REG4 data[1] = 0x20; //2000 dps full scale Wire.write(GYRO_ADDRESS, data, 2); wait_ms(1); - + data[0] = 0x21; // L3G4200D_CTRL_REG2 data[1] = 0x20; //High pass cutoff freq Wire.write(GYRO_ADDRESS, data, 2); wait_ms(1); - + data[0] = 0x24; // L3G4200D_CTRL_REG5 data[1] = 0x02; //Low Pass Filter Wire.write(GYRO_ADDRESS, data, 2); @@ -62,7 +61,7 @@ data[1] = 0x00; // Set continuous mode (default 10Hz) Wire.write(MAGN_ADDRESS, data, 2); wait_ms(1); - + data[0] = 0x00; data[1] = 0x18; Wire.write(MAGN_ADDRESS, data, 2); @@ -74,12 +73,12 @@ byte buff[6]; buff[0] = 0x32; // Send address to read from Wire.write(ACCEL_ADDRESS, buff, 1); - + if (Wire.read(ACCEL_ADDRESS, buff,6) == 0) // All bytes received? { Convert(accel_v, buff); } - // Why aren't we scaling accelerometer? I think the DCM paper talks a little about this... ?? + // 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; accel_v[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE; accel_v[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE; @@ -108,7 +107,7 @@ buff[0] = 0x03; // Send address to read from Wire.write(MAGN_ADDRESS, buff, 1); - + // Request 6 bytes if (Wire.read(MAGN_ADDRESS, buff,6) == 0) // All bytes received? { @@ -121,7 +120,7 @@ void GY80::Convert(float* 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] = ((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) } \ No newline at end of file