Diff: IMU.cpp

Revision:

13:eca05448904d

Parent:

12:cab3f7305522

--- a/IMU.cpp	Thu Feb 02 08:50:58 2012 +0000
+++ b/IMU.cpp	Thu Feb 16 08:56:11 2012 +0000
@@ -25,7 +25,12 @@
 
  #include "IMU.h"
  
- IMU::IMU(PinName sda, PinName scl) : _i2c(sda, scl) {
+ /**************
+ * Constructor *
+ **************/
+ 
+ IMU::IMU(PinName sda, PinName scl) : _i2c(sda, scl)
+ {
  
     _i2c.frequency(400000);     //400kHz bus speed
     
@@ -39,46 +44,12 @@
     
  }
  
- int IMU::gyroX(void) {
- 
-    char poke = GYRO_XOUT_H_REG;
-    char peek[2];
- 
-    _i2c.write(GYRO_ADR, &poke, 1, false);
-    _i2c.read(GYRO_ADR, peek, 2, false);
-    
-    int16_t result = ( peek[0] << 8 | peek[1] );
-    return result;
+ /***************
+ * Gyro Methods *
+ ***************/
     
- }
-   
- int IMU::gyroY(void) {
- 
-    char poke = GYRO_YOUT_H_REG;
-    char peek[2];
- 
-    _i2c.write(GYRO_ADR, &poke, 1, false);
-    _i2c.read(GYRO_ADR, peek, 2, false);
-    
-    int16_t result = ( peek[0] << 8 | peek[1] );
-    return result;
-    
- }
-       
- int IMU::gyroZ(void){
- 
-    char poke = GYRO_ZOUT_H_REG;
-    char peek[2];
- 
-    _i2c.write(GYRO_ADR, &poke, 1, false);
-    _i2c.read(GYRO_ADR, peek, 2, false);
-    
-    int16_t result = ( peek[0] << 8 | peek[1] );
-    return result;
-
- }
-    
- int* IMU::gyroXYZ(void) {
+ IMU::data3d IMU::getGyroData(void) 
+ {
  
     char poke = GYRO_XOUT_H_REG;
     char peek[6];
@@ -86,20 +57,154 @@
     _i2c.write(GYRO_ADR, &poke, 1, false);
     _i2c.read(GYRO_ADR, peek, 6, false);
     
-    int result[] = { 
-        ( peek[0] << 8 | peek[1] ),     // X
-        ( peek[2] << 8 | peek[3] ),     // Y
-        ( peek[4] << 8 | peek[5] )      // Z
-        };
-    return result;
+    int16_t result[3] = 
+    {
+        
+        ( ( peek[0] << 8 ) | peek[1] ),   // X
+        ( ( peek[2] << 8 ) | peek[3] ),   // Y
+        ( ( peek[4] << 8 ) | peek[5] )    // Z
+        
+    };
+    
+    gyro_data.x = int(result[0]);
+    gyro_data.y = int(result[1]);
+    gyro_data.z = int(result[2]);
+       
+    return gyro_data;
     
  }
 
- void IMU::gyroSetLPF(char _BW) {
+ void IMU::setGyroLPF(char bw) 
+ {
  
-    char poke[2] = { GYRO_DLPF_REG, _BW };
+    char poke[2] = { 
+    
+        GYRO_DLPF_REG, 
+        ( bw | ( FS_SEL_INIT << 3 ) )      //Bandwidth value with FS_SEL bits set properly
+        
+        };
     
     _i2c.write(GYRO_ADR, poke, 2, false);
     
  }
+ 
+ /************************
+ * Accelerometer Methods *
+ ************************/
+ 
+ int IMU::accX(void) 
+ {
+ 
+    char poke = ACC_XOUT_L_REG;
+    char peek[2];
+    
+    _i2c.write(ACC_ADR, &poke, 1, false);
+    _i2c.read(ACC_ADR, peek, 2, false);
+    
+    //Turning a 10 bit signed number into a signed 16 bit int
+    return ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) );
+
+ }
+
+ int IMU::accY(void) 
+ {
+
+    char poke = ACC_YOUT_L_REG;
+    char peek[2];
+    
+    _i2c.write(ACC_ADR, &poke, 1, false);
+    _i2c.read(ACC_ADR, peek, 2, false);
+    
+    //Turning a 10 bit signed number into a signed 16 bit int
+    return ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) );
+
+ }
+
+ int IMU::accZ(void) 
+ {
+
+    char poke = ACC_ZOUT_L_REG;
+    char peek[2];
+    
+    _i2c.write(ACC_ADR, &poke, 1, false);
+    _i2c.read(ACC_ADR, peek, 2, false);
+    
+    //Turning a 10 bit signed number into a signed 16 bit int
+    return ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) );
+
+ }
+ 
+  IMU::data3d IMU::getAccData(void) 
+ {
+ 
+    char poke = ACC_XOUT_L_REG;
+    char peek[6];
+    
+    _i2c.write(ACC_ADR, &poke, 1, false);
+    _i2c.read(ACC_ADR, peek, 6, false);
+    
+    //Turning a 10 bit signed number into a signed 16 bit int    
+    
+    acc_data.x = ( ( (0x80 & peek[1]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[1] & 0x7F ) << 2 ) | ( peek[0] >> 6 ) );
+    acc_data.y = ( ( (0x80 & peek[3]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[3] & 0x7F ) << 2 ) | ( peek[2] >> 6 ) );
+    acc_data.z = ( ( (0x80 & peek[5]) ? 0xFFFFFE00 : 0x0 ) | ( ( peek[5] & 0x7F ) << 2 ) | ( peek[4] >> 6 ) );
+          
+    return acc_data;
+    
+ }
+
+ void IMU::setAccLPF(char bw) 
+ {
+
+    char poke[2] = { 
+        
+        ACC_OPER_REG,
+        0x00
+        
+        };
+        
+    char peek;
+    
+    _i2c.write(ACC_ADR, poke, 1, false);
+    _i2c.read(ACC_ADR, &peek, 1, false);       //Get current register value
+    
+    poke[1] = ( peek | bw );                  //Insert bandwidth bits at [2:0]
+    _i2c.write(ACC_ADR, poke, 2, false);       //Write register
+
+ }
+
+ void IMU::setAccRange(char range) 
+ {
+
+    char poke[2] = { 
+        
+        ACC_OPER_REG,
+        0x00
+        
+        };
+        
+    char peek;
+    
+    _i2c.write(ACC_ADR, poke, 1, false);
+    _i2c.read(ACC_ADR, &peek, 1, false);       //Get current register value
+    
+    poke[1] = ( peek | range << 3 );          //Insert bandwidth bits at [4:3]
+    _i2c.write(ACC_ADR, poke, 2, false);       //Write register
+
+ }
+
+ void IMU::accUpdateImage(void) 
+ {
+
+ }
+
+ void IMU::accEEWriteEn(bool we) 
+ {
+
+ }
+ 
+ /***********************
+ * Magnetometer Methods *
+ ***********************/
+ 
  
\ No newline at end of file