IMU.cpp

00001 /* Atmel Inertial One IMU Library
00002  * File: IMU.H
00003  * 
00004  * Copyright (c) 2012 Dan Kouba
00005  *
00006  * Permission is hereby granted, free of charge, to any person obtaining a copy
00007  * of this software and associated documentation files (the "Software"), to deal
00008  * in the Software without restriction, including without limitation the rights
00009  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00010  * copies of the Software, and to permit persons to whom the Software is
00011  * furnished to do so, subject to the following conditions:
00012  *
00013  * The above copyright notice and this permission notice shall be included in
00014  * all copies or substantial portions of the Software.
00015  *
00016  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00017  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00018  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00019  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00020  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00021  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00022  * THE SOFTWARE.
00023  *
00024  */
00025 
00026  #include "IMU.h"
00027  
00028  /**************
00029  * Constructor *
00030  **************/
00031  
00032  IMU::IMU(PinName sda, PinName scl) : _i2c(sda, scl)
00033  {
00034  
00035     _i2c.frequency(400000);     //400kHz bus speed
00036     
00037     //Initial gyro config - must set DLPF[4:3] to 0x03 for proper operation
00038     char poke[2];
00039     
00040     poke[0] = GYRO_DLPF_REG;
00041     poke[1] = FS_SEL_INIT;
00042     
00043     _i2c.write(GYRO_ADR, poke, 2, false);
00044     
00045  }
00046  
00047  /***************
00048  * Gyro Methods *
00049  ***************/
00050     
00051  IMU::data3d IMU::getGyroData(void) 
00052  {
00053  
00054     char poke = GYRO_XOUT_H_REG;
00055     char peek[6];
00056  
00057     _i2c.write(GYRO_ADR, &poke, 1, false);
00058     _i2c.read(GYRO_ADR, peek, 6, false);
00059     
00060     int16_t result[3] = 
00061     {
00062         
00063         ( ( peek[0] << 8 ) | peek[1] ),   // X
00064         ( ( peek[2] << 8 ) | peek[3] ),   // Y
00065         ( ( peek[4] << 8 ) | peek[5] )    // Z
00066         
00067     };
00068     
00069     gyro_data.x = int(result[0]);
00070     gyro_data.y = int(result[1]);
00071     gyro_data.z = int(result[2]);
00072        
00073     return gyro_data;
00074     
00075  }
00076 
00077  void IMU::setGyroLPF(char bw) 
00078  {
00079  
00080     char poke[2] = { 
00081     
00082         GYRO_DLPF_REG, 
00083         ( bw | ( FS_SEL_INIT << 3 ) )      //Bandwidth value with FS_SEL bits set properly
00084         
00085         };
00086     
00087     _i2c.write(GYRO_ADR, poke, 2, false);
00088     
00089  }
00090  
00091  /************************
00092  * Accelerometer Methods *
00093  ************************/
00094  
00095  int IMU::accX(void) 
00096  {
00097  
00098     char poke = ACC_XOUT_L_REG;
00099     char peek[2];
00100     
00101     _i2c.write(ACC_ADR, &poke, 1, false);
00102     _i2c.read(ACC_ADR, peek, 2, false);
00103     
00104     //Turning a 10 bit signed number into a signed 16 bit int
00105     return ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) );
00106 
00107  }
00108 
00109  int IMU::accY(void) 
00110  {
00111 
00112     char poke = ACC_YOUT_L_REG;
00113     char peek[2];
00114     
00115     _i2c.write(ACC_ADR, &poke, 1, false);
00116     _i2c.read(ACC_ADR, peek, 2, false);
00117     
00118     //Turning a 10 bit signed number into a signed 16 bit int
00119     return ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) );
00120 
00121  }
00122 
00123  int IMU::accZ(void) 
00124  {
00125 
00126     char poke = ACC_ZOUT_L_REG;
00127     char peek[2];
00128     
00129     _i2c.write(ACC_ADR, &poke, 1, false);
00130     _i2c.read(ACC_ADR, peek, 2, false);
00131     
00132     //Turning a 10 bit signed number into a signed 16 bit int
00133     return ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) );
00134 
00135  }
00136  
00137   IMU::data3d IMU::getAccData(void) 
00138  {
00139  
00140     char poke = ACC_XOUT_L_REG;
00141     char peek[6];
00142     
00143     _i2c.write(ACC_ADR, &poke, 1, false);
00144     _i2c.read(ACC_ADR, peek, 6, false);
00145     
00146     //Turning a 10 bit signed number into a signed 16 bit int    
00147     
00148     acc_data.x = ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) );
00149     acc_data.y = ( ( (0x80 & peek[3]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[3] & 0x7F ) << 2 ) | ( peek[2] >> 6 ) );
00150     acc_data.z = ( ( (0x80 & peek[5]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[5] & 0x7F ) << 2 ) | ( peek[4] >> 6 ) );
00151           
00152     return acc_data;
00153     
00154  }
00155 
00156  void IMU::setAccLPF(char bw) 
00157  {
00158 
00159     char poke[2] = { 
00160         
00161         ACC_OPER_REG,
00162         0x00
00163         
00164         };
00165         
00166     char peek;
00167     
00168     _i2c.write(ACC_ADR, poke, 1, false);
00169     _i2c.read(ACC_ADR, &peek, 1, false);       //Get current register value
00170     
00171     poke[1] = ( peek | bw );                  //Insert bandwidth bits at [2:0]
00172     _i2c.write(ACC_ADR, poke, 2, false);       //Write register
00173 
00174  }
00175 
00176  void IMU::setAccRange(char range) 
00177  {
00178 
00179     char poke[2] = { 
00180         
00181         ACC_OPER_REG,
00182         0x00
00183         
00184         };
00185         
00186     char peek;
00187     
00188     _i2c.write(ACC_ADR, poke, 1, false);
00189     _i2c.read(ACC_ADR, &peek, 1, false);       //Get current register value
00190     
00191     poke[1] = ( peek | range << 3 );          //Insert bandwidth bits at [4:3]
00192     _i2c.write(ACC_ADR, poke, 2, false);       //Write register
00193 
00194  }
00195 
00196  void IMU::accUpdateImage(void) 
00197  {
00198 
00199  }
00200 
00201  void IMU::accEEWriteEn(bool we) 
00202  {
00203 
00204  }
00205  
00206  /***********************
00207  * Magnetometer Methods *
00208  ***********************/
00209  
00210