Mateusz Jaskula / Mbed 2 deprecated KL25Z_TangoIO

KL25 driver for Tango Control System

Dependencies:   mbed

GY80/GY80.cpp@0:5d27c333afa6, 2014-08-25 (annotated)

Committer:
jskl
Date:
Mon Aug 25 12:00:15 2014 +0000
Revision:
0:5d27c333afa6
Child:
2:9fe6f1e273b4
Initial Release

Who changed what in which revision?

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