ENGO333_MPU9150.cpp

00001 #include "ENGO333_MPU9150.h"
00002 
00003 const float GRAVITY = 9.8086;
00004 
00005 ENGO333_MPU9150::ENGO333_MPU9150() : i2c(p28, p27)
00006 {
00007     i2c.setSpeed(MPU9150_I2C_FAST_MODE);
00008     setSleepMode(false);
00009     setAccelRange(MPU9150_ACCEL_RANGE_2G);
00010     
00011     measAccel[0] = measAccel[1] = measAccel[2] = 0;
00012 }
00013 
00014 void ENGO333_MPU9150::setSleepMode(bool state)
00015 {
00016     char temp;
00017     temp = i2c.readOneByte(MPU9150_ADDRESS, MPU9150_PWR_MGMT_1_REG);
00018     if (state == true)
00019         temp |= 1 << MPU9150_SLEEP_BIT;
00020     if (state == false)
00021         temp &= ~(1 << MPU9150_SLEEP_BIT);
00022     i2c.writeOneByte(MPU9150_ADDRESS, MPU9150_PWR_MGMT_1_REG, temp);
00023 }
00024 
00025 bool ENGO333_MPU9150::TestConnection()
00026 {
00027     char temp;
00028     temp = i2c.readOneByte(MPU9150_ADDRESS, MPU9150_WHO_AM_I_REG);
00029     return (temp == (0x68 & 0xFE));
00030 }
00031 
00032 void ENGO333_MPU9150::setAccelRange(char range)
00033 {
00034     char temp;
00035     range = range & 0x03;
00036     
00037     temp = i2c.readOneByte(MPU9150_ADDRESS, MPU9150_ACCEL_CONFIG_REG);
00038     temp &= ~(3 << 3);
00039     temp = temp + (range << 3);
00040     i2c.writeOneByte(MPU9150_ADDRESS, MPU9150_ACCEL_CONFIG_REG, temp);
00041 }
00042 
00043 void ENGO333_MPU9150::ReadAccelerometers()
00044 {
00045     char temp[6];
00046     
00047     while ((i2c.readOneByte(MPU9150_ADDRESS, MPU9150_INT_STATUS_REG) & 0x01) != 0x01)
00048     {
00049         // wait data to be ready
00050     }
00051     
00052     i2c.readBytes(MPU9150_ADDRESS, MPU9150_ACCEL_XOUT_H_REG, temp, 6);
00053     measAccel[0] = ((short)((temp[0] << 8) + temp[1])) / 16384.0 * GRAVITY;
00054     measAccel[1] = ((short)((temp[2] << 8) + temp[3])) / 16384.0 * GRAVITY;
00055     measAccel[2] = ((short)((temp[4] << 8) + temp[5])) / 16384.0 * GRAVITY;
00056 }
00057 
00058 float ENGO333_MPU9150::GetAccelX() const
00059 {
00060     return measAccel[0];
00061 }
00062 
00063 float ENGO333_MPU9150::GetAccelY() const
00064 {
00065     return measAccel[1];
00066 }
00067 
00068 float ENGO333_MPU9150::GetAccelZ() const
00069 {
00070     return measAccel[2];
00071 }
00072