Thiago Lima / GY80

c

GY80.cpp@3:981c93d62e16, 2013-10-15 (annotated)

Committer:
oprospero
Date:
Tue Oct 15 16:31:41 2013 +0000
Revision:
3:981c93d62e16
Parent:
2:2553ca95fba0
Child:
4:cc026f60357a
Set to 400hz

Who changed what in which revision?

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