GY80.cpp

00001 #include "GY80.h"
00002 
00003 Serial pc2(USBTX, USBRX);
00004 
00005 GY80::GY80() : Wire( SDA,SCL)
00006 {
00007     Wire.frequency(I2C_FREQ);
00008     Accel_Init();
00009     Gyro_Init();
00010     Magn_Init();
00011 }
00012 GY80::~GY80()
00013 {
00014 }
00015 void GY80::Accel_Init()
00016 {    
00017     byte data[2];
00018     data[0] = 0x2D; // Power register
00019     data[1] = 0x08; //Measurement mode
00020     Wire.write(ACCEL_ADDRESS, data, 2);
00021     wait_ms(1);
00022 
00023     data[0] = 0x31; // Data format register
00024     data[1] = 0x08; //Set to full resolution
00025     Wire.write(ACCEL_ADDRESS, data, 2);
00026     wait_ms(1);
00027 
00028     // 
00029     data[0] = 0x2C; // Rate
00030     data[1] = 0x0D; //Set to 800Hz, normal operation, 0x0A 100hz 
00031     Wire.write(ACCEL_ADDRESS, data, 2);
00032     wait_ms(1);
00033 }
00034 
00035 void GY80::Gyro_Init()
00036 {
00037     byte data[2];
00038 
00039     data[0] = 0x20; //L3G4200D_CTRL_REG1
00040     data[1] = 0xCF; // normal power mode, all axes enable, 8:20 9:25 A:50 B:110 
00041     Wire.write(GYRO_ADDRESS, data, 2);
00042     wait_ms(1);
00043 
00044 
00045     data[0] = 0x23; // L3G4200D_CTRL_REG4
00046     data[1] = 0x20; //2000 dps full scale 
00047     Wire.write(GYRO_ADDRESS, data, 2);
00048     wait_ms(1);
00049 
00050 
00051     data[0] = 0x24; // L3G4200D_CTRL_REG5
00052     data[1] = 0x02; //Low Pass Filter
00053     Wire.write(GYRO_ADDRESS, data, 2);
00054 }
00055 
00056 void GY80::Magn_Init()
00057 {   
00058     byte data[2];
00059     data[0] = 0x02;
00060     data[1] = 0x00; // 00000000 Set continuous mode (default 10Hz)
00061     Wire.write(MAGN_ADDRESS, data, 2);
00062     wait_ms(1);
00063 
00064     data[0] = 0x00;
00065     data[1] = 0x50; // 01010000
00066     Wire.write(MAGN_ADDRESS, data, 2);
00067     wait_ms(1);
00068 }
00069 
00070 void GY80::Read_Accel(float* accel_v)
00071 {
00072     byte buff[6];
00073     buff[0] = 0x32; // Send address to read from
00074     Wire.write(ACCEL_ADDRESS, buff, 1);
00075 
00076     int accel[3];
00077     if (Wire.read(ACCEL_ADDRESS, buff,6) == 0)  // All bytes received?
00078     {
00079         accel[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
00080         accel[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
00081         accel[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
00082     }
00083     accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
00084     accel_v[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
00085     accel_v[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
00086     
00087     accel_v[0] = (accel_v[0]/255.0)*1000;
00088     accel_v[1] = (accel[1]/255.0)*1000;
00089     accel_v[2] = (accel[2]/255.0)*1000;
00090     
00091 }
00092 
00093 
00094 void GY80::Read_Gyro(float* gyro_v)
00095 {
00096     byte buff[6];
00097 
00098     buff[0] = 0xA8; // 0x28 | (1 << 7) Send address to read from 
00099     Wire.write(GYRO_ADDRESS, buff, 1);
00100     // Request 6 bytes
00101     int gyro[3];
00102     if (Wire.read(GYRO_ADDRESS, buff,6) == 0)  // All bytes received?
00103     {
00104         gyro[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
00105         gyro[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
00106         gyro[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
00107     }
00108     
00109     gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X); 
00110     gyro_v[1] = DEG2RAD((gyro[1] - GYRO_Y_OFFSET) * GYRO_GAIN_Y);
00111     gyro_v[2] = DEG2RAD((gyro[2] - GYRO_Z_OFFSET) * GYRO_GAIN_Z);
00112     
00113 }
00114 
00115 void GY80::Read_Magn(float* magn_v)
00116 {
00117     byte buff[6];
00118 
00119     buff[0] = 0x03; // Send address to read from
00120     Wire.write(MAGN_ADDRESS, buff, 1);
00121 
00122     // Request 6 bytes
00123     int mag[3];
00124     if (Wire.read(MAGN_ADDRESS, buff,6) == 0)  // All bytes received?
00125     {
00126         mag[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
00127         mag[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
00128         mag[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
00129     }
00130     
00131     magn_v[0] = (mag[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
00132     magn_v[1] = (mag[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
00133     magn_v[2] = (mag[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
00134 }
00135