Thiago Lima / GY80

c

GY80.cpp@0:336ec4d70363, 2013-10-04 (annotated)

Committer:
oprospero
Date:
Fri Oct 04 05:27:10 2013 +0000
Revision:
0:336ec4d70363
Child:
1:6909d797972f
1st

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oprospero 0:336ec4d70363 1 #include "GY80.h"
oprospero 0:336ec4d70363 2
oprospero 0:336ec4d70363 3
oprospero 0:336ec4d70363 4 GY80::GY80(PinName sda, PinName scl):Wire(sda,scl)
oprospero 0:336ec4d70363 5 {
oprospero 0:336ec4d70363 6 Accel_Init();
oprospero 0:336ec4d70363 7 Gyro_Init();
oprospero 0:336ec4d70363 8 Magn_Init();
oprospero 0:336ec4d70363 9 Wire.frequency(I2C_FREQ);
oprospero 0:336ec4d70363 10 }
oprospero 0:336ec4d70363 11
oprospero 0:336ec4d70363 12 void GY80::Accel_Init()
oprospero 0:336ec4d70363 13 {
oprospero 0:336ec4d70363 14 byte data[2];
oprospero 0:336ec4d70363 15 data[0] = 0x2D; // Power register
oprospero 0:336ec4d70363 16 data[1] = 0x08; //Measurement mode
oprospero 0:336ec4d70363 17 Wire.write(ACCEL_ADDRESS, data, 2);
oprospero 0:336ec4d70363 18 wait_ms(1);
oprospero 0:336ec4d70363 19
oprospero 0:336ec4d70363 20 data[0] = 0x31; // Data format register
oprospero 0:336ec4d70363 21 data[1] = 0x08; //Set to full resolution
oprospero 0:336ec4d70363 22 Wire.write(ACCEL_ADDRESS, data, 2);
oprospero 0:336ec4d70363 23 wait_ms(1);
oprospero 0:336ec4d70363 24
oprospero 0:336ec4d70363 25 // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth)
oprospero 0:336ec4d70363 26 data[0] = 0x2C; // Rate
oprospero 0:336ec4d70363 27 data[1] = 0x0A; //Set to 50Hz, normal operation, 0x0A 100hz
oprospero 0:336ec4d70363 28 Wire.write(ACCEL_ADDRESS, data, 2);
oprospero 0:336ec4d70363 29 wait_ms(1);
oprospero 0:336ec4d70363 30 }
oprospero 0:336ec4d70363 31
oprospero 0:336ec4d70363 32 void GY80::Gyro_Init()
oprospero 0:336ec4d70363 33 {
oprospero 0:336ec4d70363 34 byte data[2];
oprospero 0:336ec4d70363 35
oprospero 0:336ec4d70363 36 data[0] = 0x20; //L3G4200D_CTRL_REG1
oprospero 0:336ec4d70363 37 data[1] = 0x0; // normal power mode, all axes enable, 100Hz
oprospero 0:336ec4d70363 38 Wire.write(GYRO_ADDRESS, data, 2);
oprospero 0:336ec4d70363 39 wait_ms(1);
oprospero 0:336ec4d70363 40
oprospero 0:336ec4d70363 41
oprospero 0:336ec4d70363 42 data[0] = 0x23; // L3G4200D_CTRL_REG4
oprospero 0:336ec4d70363 43 data[1] = 0x20; //2000 dps full scale
oprospero 0:336ec4d70363 44 Wire.write(GYRO_ADDRESS, data, 2);
oprospero 0:336ec4d70363 45 wait_ms(1);
oprospero 0:336ec4d70363 46
oprospero 0:336ec4d70363 47 data[0] = 0x21; // L3G4200D_CTRL_REG2
oprospero 0:336ec4d70363 48 data[1] = 0x20; //High pass cutoff freq
oprospero 0:336ec4d70363 49 Wire.write(GYRO_ADDRESS, data, 2);
oprospero 0:336ec4d70363 50 wait_ms(1);
oprospero 0:336ec4d70363 51
oprospero 0:336ec4d70363 52 data[0] = 0x24; // L3G4200D_CTRL_REG5
oprospero 0:336ec4d70363 53 data[1] = 0x02; //Low Pass Filter
oprospero 0:336ec4d70363 54 Wire.write(GYRO_ADDRESS, data, 2);
oprospero 0:336ec4d70363 55 wait_ms(1);
oprospero 0:336ec4d70363 56 }
oprospero 0:336ec4d70363 57
oprospero 0:336ec4d70363 58 void GY80::Magn_Init()
oprospero 0:336ec4d70363 59 {
oprospero 0:336ec4d70363 60 byte data[2];
oprospero 0:336ec4d70363 61 data[0] = 0x02;
oprospero 0:336ec4d70363 62 data[1] = 0x00; // Set continuous mode (default 10Hz)
oprospero 0:336ec4d70363 63 Wire.write(MAGN_ADDRESS, data, 2);
oprospero 0:336ec4d70363 64 wait_ms(1);
oprospero 0:336ec4d70363 65
oprospero 0:336ec4d70363 66 data[0] = 0x00;
oprospero 0:336ec4d70363 67 data[1] = 0x18;
oprospero 0:336ec4d70363 68 Wire.write(MAGN_ADDRESS, data, 2);
oprospero 0:336ec4d70363 69 wait_ms(1);
oprospero 0:336ec4d70363 70 }
oprospero 0:336ec4d70363 71
oprospero 0:336ec4d70363 72 void GY80::Read_Accel(float* accel_v)
oprospero 0:336ec4d70363 73 {
oprospero 0:336ec4d70363 74 byte buff[6];
oprospero 0:336ec4d70363 75 buff[0] = 0x32; // Send address to read from
oprospero 0:336ec4d70363 76 Wire.write(ACCEL_ADDRESS, buff, 1);
oprospero 0:336ec4d70363 77
oprospero 0:336ec4d70363 78 if (Wire.read(ACCEL_ADDRESS, buff,6) == 0) // All bytes received?
oprospero 0:336ec4d70363 79 {
oprospero 0:336ec4d70363 80 Convert(accel_v, buff);
oprospero 0:336ec4d70363 81 }
oprospero 0:336ec4d70363 82 // Why aren't we scaling accelerometer? I think the DCM paper talks a little about this... ??
oprospero 0:336ec4d70363 83 accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
oprospero 0:336ec4d70363 84 accel_v[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
oprospero 0:336ec4d70363 85 accel_v[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
oprospero 0:336ec4d70363 86 }
oprospero 0:336ec4d70363 87
oprospero 0:336ec4d70363 88
oprospero 0:336ec4d70363 89 void GY80::Read_Gyro(float* gyro_v)
oprospero 0:336ec4d70363 90 {
oprospero 0:336ec4d70363 91 byte buff[6];
oprospero 0:336ec4d70363 92
oprospero 0:336ec4d70363 93 buff[0] = 0xA8; // 0x28 | (1 << 7) Send address to read from
oprospero 0:336ec4d70363 94 Wire.write(GYRO_ADDRESS, buff, 1);
oprospero 0:336ec4d70363 95 // Request 6 bytes
oprospero 0:336ec4d70363 96 if (Wire.read(GYRO_ADDRESS, buff,6) == 0) // All bytes received?
oprospero 0:336ec4d70363 97 {
oprospero 0:336ec4d70363 98 Convert(gyro_v, buff);
oprospero 0:336ec4d70363 99 }
oprospero 0:336ec4d70363 100 gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X);
oprospero 0:336ec4d70363 101 gyro_v[1] = DEG2RAD((gyro[1] - GYRO_Y_OFFSET) * GYRO_GAIN_Y);
oprospero 0:336ec4d70363 102 gyro_v[2] = DEG2RAD((gyro[2] - GYRO_Z_OFFSET) * GYRO_GAIN_Z);
oprospero 0:336ec4d70363 103 }
oprospero 0:336ec4d70363 104
oprospero 0:336ec4d70363 105 void GY80::Read_Magn(float* magn_v)
oprospero 0:336ec4d70363 106 {
oprospero 0:336ec4d70363 107 byte buff[6];
oprospero 0:336ec4d70363 108
oprospero 0:336ec4d70363 109 buff[0] = 0x03; // Send address to read from
oprospero 0:336ec4d70363 110 Wire.write(MAGN_ADDRESS, buff, 1);
oprospero 0:336ec4d70363 111
oprospero 0:336ec4d70363 112 // Request 6 bytes
oprospero 0:336ec4d70363 113 if (Wire.read(MAGN_ADDRESS, buff,6) == 0) // All bytes received?
oprospero 0:336ec4d70363 114 {
oprospero 0:336ec4d70363 115 Convert(magn_v, buff);
oprospero 0:336ec4d70363 116 }
oprospero 0:336ec4d70363 117 magn_v[0] = (mag[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
oprospero 0:336ec4d70363 118 magn_v[1] = (mag[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
oprospero 0:336ec4d70363 119 magn_v[2] = (mag[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
oprospero 0:336ec4d70363 120 }
oprospero 0:336ec4d70363 121
oprospero 0:336ec4d70363 122 void GY80::Convert(float* value, byte* raw)
oprospero 0:336ec4d70363 123 {
oprospero 0:336ec4d70363 124 value[0] = ((signed short) raw[1] << 8) | raw[0]; // Y axis (internal sensor x axis)
oprospero 0:336ec4d70363 125 value[1] = ((signed short) raw[3] << 8) | raw[2]; // X axis (internal sensor y axis)
oprospero 0:336ec4d70363 126 value[2] = ((signed short) raw[5] << 8) | raw[4]; // Z axis (internal sensor z axis)
oprospero 0:336ec4d70363 127 }