Thiago Lima / GY80

c

GY80.cpp@5:9dc0f2d15783, 2019-08-08 (annotated)

Committer:
TMRL123
Date:
Thu Aug 08 16:06:48 2019 +0000
Revision:
5:9dc0f2d15783
Parent:
4:cc026f60357a 
cc

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 4:cc026f60357a 30 data[1] = 0x0D; //Set to 800Hz, 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 4:cc026f60357a 40 data[1] = 0xCF; // 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 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 }
oprospero 0:336ec4d70363 55
oprospero 0:336ec4d70363 56 void GY80::Magn_Init()
oprospero 0:336ec4d70363 57 {
oprospero 0:336ec4d70363 58 byte data[2];
oprospero 0:336ec4d70363 59 data[0] = 0x02;
oprospero 3:981c93d62e16 60 data[1] = 0x00; // 00000000 Set continuous mode (default 10Hz)
oprospero 0:336ec4d70363 61 Wire.write(MAGN_ADDRESS, data, 2);
oprospero 0:336ec4d70363 62 wait_ms(1);
oprospero 1:6909d797972f 63
oprospero 0:336ec4d70363 64 data[0] = 0x00;
oprospero 3:981c93d62e16 65 data[1] = 0x50; // 01010000
oprospero 0:336ec4d70363 66 Wire.write(MAGN_ADDRESS, data, 2);
oprospero 0:336ec4d70363 67 wait_ms(1);
oprospero 0:336ec4d70363 68 }
oprospero 0:336ec4d70363 69
oprospero 0:336ec4d70363 70 void GY80::Read_Accel(float* accel_v)
oprospero 0:336ec4d70363 71 {
oprospero 0:336ec4d70363 72 byte buff[6];
oprospero 0:336ec4d70363 73 buff[0] = 0x32; // Send address to read from
oprospero 0:336ec4d70363 74 Wire.write(ACCEL_ADDRESS, buff, 1);
oprospero 1:6909d797972f 75
oprospero 4:cc026f60357a 76 int accel[3];
oprospero 0:336ec4d70363 77 if (Wire.read(ACCEL_ADDRESS, buff,6) == 0) // All bytes received?
oprospero 0:336ec4d70363 78 {
oprospero 4:cc026f60357a 79 accel[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
oprospero 4:cc026f60357a 80 accel[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
oprospero 4:cc026f60357a 81 accel[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
oprospero 0:336ec4d70363 82 }
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;
TMRL123 5:9dc0f2d15783 86
TMRL123 5:9dc0f2d15783 87 accel_v[0] = (accel_v[0]/255.0)*1000;
TMRL123 5:9dc0f2d15783 88 accel_v[1] = (accel[1]/255.0)*1000;
TMRL123 5:9dc0f2d15783 89 accel_v[2] = (accel[2]/255.0)*1000;
TMRL123 5:9dc0f2d15783 90
oprospero 0:336ec4d70363 91 }
oprospero 0:336ec4d70363 92
oprospero 0:336ec4d70363 93
oprospero 0:336ec4d70363 94 void GY80::Read_Gyro(float* gyro_v)
oprospero 0:336ec4d70363 95 {
oprospero 0:336ec4d70363 96 byte buff[6];
oprospero 0:336ec4d70363 97
oprospero 0:336ec4d70363 98 buff[0] = 0xA8; // 0x28 | (1 << 7) Send address to read from
oprospero 0:336ec4d70363 99 Wire.write(GYRO_ADDRESS, buff, 1);
oprospero 0:336ec4d70363 100 // Request 6 bytes
oprospero 4:cc026f60357a 101 int gyro[3];
oprospero 0:336ec4d70363 102 if (Wire.read(GYRO_ADDRESS, buff,6) == 0) // All bytes received?
oprospero 0:336ec4d70363 103 {
oprospero 4:cc026f60357a 104 gyro[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
oprospero 4:cc026f60357a 105 gyro[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
oprospero 4:cc026f60357a 106 gyro[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
oprospero 0:336ec4d70363 107 }
TMRL123 5:9dc0f2d15783 108
oprospero 0:336ec4d70363 109 gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X);
oprospero 0:336ec4d70363 110 gyro_v[1] = DEG2RAD((gyro[1] - GYRO_Y_OFFSET) * GYRO_GAIN_Y);
oprospero 0:336ec4d70363 111 gyro_v[2] = DEG2RAD((gyro[2] - GYRO_Z_OFFSET) * GYRO_GAIN_Z);
TMRL123 5:9dc0f2d15783 112
oprospero 0:336ec4d70363 113 }
oprospero 0:336ec4d70363 114
oprospero 0:336ec4d70363 115 void GY80::Read_Magn(float* magn_v)
oprospero 0:336ec4d70363 116 {
oprospero 0:336ec4d70363 117 byte buff[6];
oprospero 0:336ec4d70363 118
oprospero 0:336ec4d70363 119 buff[0] = 0x03; // Send address to read from
oprospero 0:336ec4d70363 120 Wire.write(MAGN_ADDRESS, buff, 1);
oprospero 1:6909d797972f 121
oprospero 0:336ec4d70363 122 // Request 6 bytes
oprospero 4:cc026f60357a 123 int mag[3];
oprospero 0:336ec4d70363 124 if (Wire.read(MAGN_ADDRESS, buff,6) == 0) // All bytes received?
oprospero 0:336ec4d70363 125 {
oprospero 4:cc026f60357a 126 mag[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
oprospero 4:cc026f60357a 127 mag[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
oprospero 4:cc026f60357a 128 mag[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
oprospero 0:336ec4d70363 129 }
TMRL123 5:9dc0f2d15783 130
oprospero 0:336ec4d70363 131 magn_v[0] = (mag[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
oprospero 0:336ec4d70363 132 magn_v[1] = (mag[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
oprospero 0:336ec4d70363 133 magn_v[2] = (mag[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
oprospero 0:336ec4d70363 134 }
oprospero 0:336ec4d70363 135