Thiago Lima / GY80

c

GY80.cpp@1:6909d797972f, 2013-10-05 (annotated)

Committer:
oprospero
Date:
Sat Oct 05 02:22:15 2013 +0000
Revision:
1:6909d797972f
Parent:
0:336ec4d70363
Child:
2:2553ca95fba0
clean up;

Who changed what in which revision?

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