MPU9250.cpp

00001 /*
00002  * MPU9250.cpp
00003  *
00004  *  Created on: 19.05.2016
00005  *      Author: Adrian
00006  */
00007 
00008 #include "MPU9250.h "
00009 
00010 MPU9250::MPU9250(I2C_RT* i2c){
00011     setI2c(i2c);
00012     this->config = new MPU9250Config();
00013 }
00014 
00015 MPU9250::~MPU9250() {
00016     // TODO Auto-generated destructor stub
00017 }
00018 
00019 void MPU9250::setI2c(I2C_RT* i2c){
00020     this->i2c = i2c;
00021 }
00022 
00023 void MPU9250::init(MPU9250_MODE desiredMode){
00024         config->build(desiredMode);
00025         enableAxisAccelerationMeasurement();
00026         enableAxisGyroscopeMeasurement();
00027         enableAxisTeslaMeasurement();
00028         configureInterrupts();
00029         setWakeOnReceiveThreshold();
00030 }
00031 
00032 float MPU9250::getXAxisAcceleration(){
00033     uint8_t xAccelarationHighByte;
00034     uint8_t xAccelarationLowByte;
00035 
00036     i2c->read_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_ACCEL_XOUT_H,false,
00037             &xAccelarationHighByte,1);
00038     i2c->read_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_ACCEL_XOUT_L,false,
00039             &xAccelarationLowByte,1);
00040 
00041     int16_t acceleration = ((int16_t)xAccelarationHighByte<<8)|((int16_t)xAccelarationLowByte);
00042 
00043     return ((float) acceleration)/config->getAccelerationDivider();
00044 
00045 }
00046 
00047 float MPU9250::getYAxisAcceleration(){
00048     uint8_t yAccelarationHighByte;
00049     uint8_t yAccelarationLowByte;
00050 
00051     i2c->read_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_ACCEL_YOUT_H,false,
00052             &yAccelarationHighByte,1);
00053     i2c->read_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_ACCEL_YOUT_L,false,
00054             &yAccelarationLowByte,1);
00055 
00056     int16_t acceleration = yAccelarationHighByte<<8|yAccelarationLowByte;
00057 
00058     return ((float) acceleration)/config->getAccelerationDivider();
00059 
00060 }
00061 
00062 float MPU9250::getZAxisAcceleration(){
00063     uint8_t zAccelarationHighByte;
00064     uint8_t zAccelarationLowByte;
00065 
00066     i2c->read_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_ACCEL_ZOUT_H,false,
00067             &zAccelarationHighByte,1);
00068     i2c->read_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_ACCEL_ZOUT_L,false,
00069             &zAccelarationLowByte,1);
00070 
00071     int16_t acceleration = zAccelarationHighByte<<8|zAccelarationLowByte;
00072 
00073     return ((float) acceleration)/config->getAccelerationDivider();
00074 
00075 }
00076 
00077 float MPU9250::getXAxisGyro(){
00078     uint8_t xGyroscopeHighByte;
00079     uint8_t xGyroscopeLowByte;
00080 
00081     i2c->read_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_GYRO_XOUT_H,false,
00082             &xGyroscopeHighByte,1);
00083     i2c->read_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_GYRO_XOUT_L,false,
00084             &xGyroscopeLowByte,1);
00085 
00086     int16_t gyroscope = xGyroscopeHighByte<<8|xGyroscopeLowByte;
00087 
00088     return ((float) gyroscope)/config->getGyroDivider();
00089 }
00090 
00091 float MPU9250::getYAxisGyro(){
00092     uint8_t yGyroscopeHighByte;
00093     uint8_t yGyroscopeLowByte;
00094 
00095     i2c->read_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_GYRO_YOUT_H,false,
00096             &yGyroscopeHighByte,1);
00097     i2c->read_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_GYRO_YOUT_L,false,
00098             &yGyroscopeLowByte,1);
00099 
00100     int16_t gyroscope = yGyroscopeHighByte<<8|yGyroscopeLowByte;
00101 
00102     return ((float) gyroscope)/config->getGyroDivider();
00103 }
00104 
00105 float MPU9250::getZAxisGyro(){
00106     uint8_t zGyroscopeHighByte;
00107     uint8_t zGyroscopeLowByte;
00108 
00109     i2c->read_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_GYRO_ZOUT_H,0,
00110             &zGyroscopeHighByte,1);
00111     i2c->read_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_GYRO_ZOUT_L,0,
00112             &zGyroscopeLowByte,1);
00113 
00114     int16_t gyroscope = zGyroscopeHighByte<<8|zGyroscopeLowByte;
00115 
00116     return ((float) gyroscope)/config->getGyroDivider();
00117 }
00118 
00119 float MPU9250::getXAxisTesla(){
00120     uint8_t xTelsaHighByte;
00121     uint8_t xTelsaLowByte;
00122 
00123     i2c->read_RT(MPU9250_MAG_ADDRESS,MPU9250_MAG_XOUT_H,false,
00124             &xTelsaHighByte,1);
00125     i2c->read_RT(MPU9250_MAG_ADDRESS,MPU9250_MAG_XOUT_L,false,
00126             &xTelsaLowByte,1);
00127 
00128     uint8_t status;
00129 
00130     i2c->read_RT(MPU9250_MAG_ADDRESS,MPU9250_MAG_ST2,false,
00131             &status,1);
00132 
00133     int16_t tesla = xTelsaHighByte<<8|xTelsaLowByte;
00134 
00135     return (float) (tesla) / config->getTeslaDivider();
00136 }
00137 
00138 float MPU9250::getYAxisTesla(){
00139     uint8_t yTelsaHighByte;
00140     uint8_t yTelsaLowByte;
00141 
00142     i2c->read_RT(MPU9250_MAG_ADDRESS,MPU9250_MAG_YOUT_H,false,
00143             &yTelsaHighByte,1);
00144     i2c->read_RT(MPU9250_MAG_ADDRESS,MPU9250_MAG_YOUT_L,false,
00145             &yTelsaLowByte,1);
00146 
00147     uint8_t status;
00148 
00149     i2c->read_RT(MPU9250_MAG_ADDRESS,MPU9250_MAG_ST2,false,
00150             &status,1);
00151 
00152     int16_t tesla = yTelsaHighByte<<8|yTelsaLowByte;
00153 
00154     return (float) (tesla) / config->getTeslaDivider();
00155 }
00156 
00157 float MPU9250::getZAxisTesla(){
00158     uint8_t zTelsaHighByte;
00159     uint8_t zTelsaLowByte;
00160 
00161     i2c->read_RT(MPU9250_MAG_ADDRESS,MPU9250_MAG_ZOUT_H,false,
00162             &zTelsaHighByte,1);
00163     i2c->read_RT(MPU9250_MAG_ADDRESS,MPU9250_MAG_ZOUT_L,false,
00164             &zTelsaLowByte,1);
00165 
00166     uint8_t status;
00167 
00168     i2c->read_RT(MPU9250_MAG_ADDRESS,MPU9250_MAG_ST2,false,
00169             &status,1);
00170 
00171     int16_t tesla = zTelsaHighByte<<8|zTelsaLowByte;
00172 
00173     return (float) (tesla) / config->getTeslaDivider();
00174 }
00175 
00176 void MPU9250::enableAxisAccelerationMeasurement(){
00177     uint8_t configValueAcceleration=config->getAccelerometerScale()<<3;
00178 
00179     i2c->write_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_ACCEL_CONFIG,false,
00180             &configValueAcceleration,1);
00181 }
00182 
00183 void MPU9250::enableAxisGyroscopeMeasurement(){
00184     uint8_t configValueGyroscope = config->getGyroscopeScale()<<3;
00185 
00186     i2c->write_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_GYRO_CONFIG,false,
00187             &configValueGyroscope,1);
00188 }
00189 
00190 void MPU9250::enableAxisTeslaMeasurement(){
00191     uint8_t configValueMagnetometer = 0x02;
00192 
00193     i2c->write_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_INT_PIN_CFG,false,
00194             &configValueMagnetometer,1);
00195 
00196     configValueMagnetometer = 0;
00197     i2c->write_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_USER_CTRL,false,
00198                 &configValueMagnetometer,1);
00199 
00200     configValueMagnetometer = 0;
00201     i2c->write_RT(MPU9250_MAG_ADDRESS,MPU9250_MAG_CNTL1,false,
00202             &configValueMagnetometer,1);
00203 
00204     configValueMagnetometer = config->getMagnetometerBitResolution()<<4|
00205             config->getMagnetometerMeasureMode();
00206     i2c->write_RT(MPU9250_MAG_ADDRESS,MPU9250_MAG_CNTL1,false,
00207             &configValueMagnetometer,1);
00208 
00209 }
00210 
00211 void MPU9250::configureInterrupts(){
00212     uint8_t configValueInterruptPin = config->getInterruptPinConfiguration()|0x02;
00213 
00214     i2c->write_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_INT_PIN_CFG,false,
00215                 &configValueInterruptPin,1);
00216 
00217     uint8_t configValueInterruptEnable = config->getInterruptEnableConfiguration();
00218 
00219     i2c->write_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_INT_ENABLE,false,
00220                 &configValueInterruptEnable,1);
00221 }
00222 
00223 void MPU9250::setWakeOnReceiveThreshold(){
00224     uint8_t wakeOnReceiveThreshold = config->getWakeOnMotionThreshold();
00225 
00226     i2c->write_RT(MPU9250_DEFAULT_ADDRESS,MPU9250_WOM_THR,false,
00227                     &wakeOnReceiveThreshold,1);
00228 }
00229 
00230