Prosper Van / GY80

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

GY80.cpp@2:2553ca95fba0, 2013-10-06 (annotated)

Committer:
oprospero
Date:
Sun Oct 06 07:09:50 2013 +0000
Revision:
2:2553ca95fba0
Parent:
1:6909d797972f
Child:
3:981c93d62e16
fixed

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 0:336ec4d70363 28 // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth)
oprospero 0:336ec4d70363 29 data[0] = 0x2C; // Rate
oprospero 0:336ec4d70363 30 data[1] = 0x0A; //Set to 50Hz, 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 2:2553ca95fba0 40 data[1] = 0x0F; // normal power mode, all axes enable, 100Hz
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 0:336ec4d70363 64 data[1] = 0x00; // 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 0:336ec4d70363 69 data[1] = 0x18;
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 1:6909d797972f 84 // Why aren't we scaling accelerometer? I think the DCM paper talks a little about this... ??
oprospero 0:336ec4d70363 85 accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
oprospero 0:336ec4d70363 86 accel_v[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
oprospero 0:336ec4d70363 87 accel_v[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
oprospero 0:336ec4d70363 88 }
oprospero 0:336ec4d70363 89
oprospero 0:336ec4d70363 90
oprospero 0:336ec4d70363 91 void GY80::Read_Gyro(float* gyro_v)
oprospero 0:336ec4d70363 92 {
oprospero 0:336ec4d70363 93 byte buff[6];
oprospero 0:336ec4d70363 94
oprospero 0:336ec4d70363 95 buff[0] = 0xA8; // 0x28 | (1 << 7) Send address to read from
oprospero 0:336ec4d70363 96 Wire.write(GYRO_ADDRESS, buff, 1);
oprospero 0:336ec4d70363 97 // Request 6 bytes
oprospero 0:336ec4d70363 98 if (Wire.read(GYRO_ADDRESS, buff,6) == 0) // All bytes received?
oprospero 0:336ec4d70363 99 {
oprospero 2:2553ca95fba0 100 Convert(gyro, buff);
oprospero 0:336ec4d70363 101 }
oprospero 0:336ec4d70363 102 gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X);
oprospero 0:336ec4d70363 103 gyro_v[1] = DEG2RAD((gyro[1] - GYRO_Y_OFFSET) * GYRO_GAIN_Y);
oprospero 0:336ec4d70363 104 gyro_v[2] = DEG2RAD((gyro[2] - GYRO_Z_OFFSET) * GYRO_GAIN_Z);
oprospero 0:336ec4d70363 105 }
oprospero 0:336ec4d70363 106
oprospero 0:336ec4d70363 107 void GY80::Read_Magn(float* magn_v)
oprospero 0:336ec4d70363 108 {
oprospero 0:336ec4d70363 109 byte buff[6];
oprospero 0:336ec4d70363 110
oprospero 0:336ec4d70363 111 buff[0] = 0x03; // Send address to read from
oprospero 0:336ec4d70363 112 Wire.write(MAGN_ADDRESS, buff, 1);
oprospero 1:6909d797972f 113
oprospero 0:336ec4d70363 114 // Request 6 bytes
oprospero 0:336ec4d70363 115 if (Wire.read(MAGN_ADDRESS, buff,6) == 0) // All bytes received?
oprospero 0:336ec4d70363 116 {
oprospero 2:2553ca95fba0 117 Convert(mag, buff);
oprospero 0:336ec4d70363 118 }
oprospero 0:336ec4d70363 119 magn_v[0] = (mag[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
oprospero 0:336ec4d70363 120 magn_v[1] = (mag[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
oprospero 0:336ec4d70363 121 magn_v[2] = (mag[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
oprospero 0:336ec4d70363 122 }
oprospero 0:336ec4d70363 123
oprospero 2:2553ca95fba0 124 void GY80::Convert(short* value, byte* raw)
oprospero 0:336ec4d70363 125 {
oprospero 2:2553ca95fba0 126 value[0] = (short) ((uint16_t) raw[1] << 8 | raw[0]);
oprospero 2:2553ca95fba0 127 value[1] = (short) ((uint16_t) raw[3] << 8 | raw[2]);
oprospero 2:2553ca95fba0 128 value[2] = (short) ((uint16_t) raw[5] << 8 | raw[4]);
oprospero 0:336ec4d70363 129 }