Mateusz Jaskula / Mbed 2 deprecated KL25Z_TangoIO

KL25 driver for Tango Control System

Dependencies:   mbed

GY80/GY80.cpp@2:9fe6f1e273b4, 2014-08-28 (annotated)

Committer:
jskl
Date:
Thu Aug 28 07:50:06 2014 +0000
Revision:
2:9fe6f1e273b4
Parent:
0:5d27c333afa6 
Fixed bugs in JSON format replies

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