Home | History | Annotate | Download | only in mpu9150 
      1 /*
      2  * Author: Jon Trulson <jtrulson (at) ics.com>
      3  * Copyright (c) 2015 Intel Corporation.
      4  *
      5  * Permission is hereby granted, free of charge, to any person obtaining
      6  * a copy of this software and associated documentation files (the
      7  * "Software"), to deal in the Software without restriction, including
      8  * without limitation the rights to use, copy, modify, merge, publish,
      9  * distribute, sublicense, and/or sell copies of the Software, and to
     10  * permit persons to whom the Software is furnished to do so, subject to
     11  * the following conditions:
     12  *
     13  * The above copyright notice and this permission notice shall be
     14  * included in all copies or substantial portions of the Software.
     15  *
     16  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
     17  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
     18  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
     19  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
     20  * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
     21  * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
     22  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
     23  */
     24 
     25 #include <unistd.h>
     26 #include <iostream>
     27 #include <string.h>
     28 
     29 #include "mpu60x0.h"
     30 
     31 using namespace upm;
     32 using namespace std;
     33 
     34 
     35 MPU60X0::MPU60X0(int bus, uint8_t address) :
     36   m_i2c(bus), m_gpioIRQ(0)
     37 {
     38   m_addr = address;
     39 
     40   m_accelX = 0.0;
     41   m_accelY = 0.0;
     42   m_accelZ = 0.0;
     43 
     44   m_gyroX = 0.0;
     45   m_gyroY = 0.0;
     46   m_gyroZ = 0.0;
     47 
     48   m_temp = 0.0;
     49 
     50   m_accelScale = 1.0;
     51   m_gyroScale = 1.0;
     52 
     53   mraa::Result rv;
     54   if ( (rv = m_i2c.address(m_addr)) != mraa::SUCCESS)
     55     {
     56       throw std::runtime_error(std::string(__FUNCTION__) +
     57                                ": I2c.address() failed");
     58       return;
     59     }
     60 }
     61 
     62 MPU60X0::~MPU60X0()
     63 {
     64   uninstallISR();
     65 }
     66 
     67 bool MPU60X0::init()
     68 {
     69   // first, take the device out of sleep mode
     70   if (!setSleep(false))
     71     {
     72       throw std::runtime_error(std::string(__FUNCTION__) +
     73                                ": Unable to wake up device");
     74       return false;
     75     }
     76 
     77   // set the clock source to use the gyroscope PLL rather than the
     78   // internal clock for stability
     79   if (!setClockSource(PLL_XG))
     80     {
     81       throw std::runtime_error(std::string(__FUNCTION__) +
     82                                ": Unable to set clock source");
     83       return false;
     84     }
     85 
     86   usleep(5000);
     87 
     88   // enable temperature measurement (default on power up, but let's be sure)
     89   enableTemperatureSensor(true);
     90 
     91   // set the gyro and accel scale bits to reasonable values
     92   setGyroscopeScale(FS_500);
     93   setAccelerometerScale(AFS_2);
     94 
     95   // enable the DLPF
     96   setDigitalLowPassFilter(DLPF_94_98);
     97 
     98   // let things stabilize...
     99   usleep(100000);
    100 
    101   return true;
    102 }
    103 
    104 
    105 void MPU60X0::update()
    106 {
    107   // read all of the sensor registers - accel, gyro, and temp
    108   uint8_t buffer[14];
    109 
    110   memset(buffer, 0, 14);
    111   readRegs(REG_ACCEL_XOUT_H, buffer, 14);
    112 
    113   int16_t ax, ay, az;
    114   int16_t temp;
    115   int16_t gx, gy, gz;
    116 
    117   ax =  ( (buffer[0] << 8) | buffer[1] );
    118   ay =  ( (buffer[2] << 8) | buffer[3] );
    119   az =  ( (buffer[4] << 8) | buffer[5] );
    120 
    121   temp = ( (buffer[6] << 8) | buffer[7] );
    122 
    123   gx =  ( (buffer[8] << 8) | buffer[9] );
    124   gy =  ( (buffer[10] << 8) | buffer[11] );
    125   gz =  ( (buffer[12] << 8) | buffer[13] );
    126 
    127   // now stash them
    128   m_accelX = float(ax);
    129   m_accelY = float(ay);
    130   m_accelZ = float(az);
    131 
    132   m_temp = float(temp);
    133 
    134   m_gyroX = float(gx);
    135   m_gyroY = float(gy);
    136   m_gyroZ = float(gz);
    137 }
    138 
    139 uint8_t MPU60X0::readReg(uint8_t reg)
    140 {
    141   return m_i2c.readReg(reg);
    142 }
    143 
    144 void MPU60X0::readRegs(uint8_t reg, uint8_t *buffer, int len)
    145 {
    146   m_i2c.readBytesReg(reg, buffer, len);
    147 }
    148 
    149 bool MPU60X0::writeReg(uint8_t reg, uint8_t val)
    150 {
    151   mraa::Result rv;
    152   if ((rv = m_i2c.writeReg(reg, val)) != mraa::SUCCESS)
    153     {
    154       throw std::runtime_error(std::string(__FUNCTION__) +
    155                                ": I2c.writeReg() failed");
    156       return false;
    157     }
    158 
    159   return true;
    160 }
    161 
    162 bool MPU60X0::setSleep(bool enable)
    163 {
    164   uint8_t reg = readReg(REG_PWR_MGMT_1);
    165 
    166   if (enable)
    167     reg |= PWR_SLEEP;
    168   else
    169     reg &= ~PWR_SLEEP;
    170 
    171   return writeReg(REG_PWR_MGMT_1, reg);
    172 }
    173 
    174 bool MPU60X0::setClockSource(CLKSEL_T clk)
    175 {
    176   uint8_t reg = readReg(REG_PWR_MGMT_1);
    177 
    178   reg &= ~(_CLKSEL_MASK << _CLKSEL_SHIFT);
    179 
    180   reg |= (clk << _CLKSEL_SHIFT);
    181 
    182   return writeReg(REG_PWR_MGMT_1, reg);
    183 }
    184 
    185 bool MPU60X0::setGyroscopeScale(FS_SEL_T scale)
    186 {
    187   uint8_t reg = readReg(REG_GYRO_CONFIG);
    188 
    189   reg &= ~(_FS_SEL_MASK << _FS_SEL_SHIFT);
    190 
    191   reg |= (scale << _FS_SEL_SHIFT);
    192 
    193   if (!writeReg(REG_GYRO_CONFIG, reg))
    194     {
    195       return false;
    196     }
    197 
    198   // store scaling factor
    199 
    200   switch (scale)
    201     {
    202     case FS_250:
    203       m_gyroScale = 131.0;
    204       break;
    205 
    206     case FS_500:
    207       m_gyroScale = 65.5;
    208       break;
    209 
    210     case FS_1000:
    211       m_gyroScale = 32.8;
    212       break;
    213 
    214     case FS_2000:
    215       m_gyroScale = 16.4;
    216       break;
    217 
    218     default: // should never occur, but...
    219       m_gyroScale = 1.0;        // set a safe, though incorrect value
    220       throw std::logic_error(string(__FUNCTION__) +
    221                              ": internal error, unsupported scale");
    222       break;
    223     }
    224 
    225   return true;
    226 }
    227 
    228 bool MPU60X0::setAccelerometerScale(AFS_SEL_T scale)
    229 {
    230   uint8_t reg = readReg(REG_ACCEL_CONFIG);
    231 
    232   reg &= ~(_AFS_SEL_MASK << _AFS_SEL_SHIFT);
    233 
    234   reg |= (scale << _AFS_SEL_SHIFT);
    235 
    236   if (!writeReg(REG_ACCEL_CONFIG, reg))
    237     {
    238       return false;
    239     }
    240 
    241   // store scaling factor
    242 
    243   switch (scale)
    244     {
    245     case AFS_2:
    246       m_accelScale = 16384.0;
    247       break;
    248 
    249     case AFS_4:
    250       m_accelScale = 8192.0;
    251       break;
    252 
    253     case AFS_8:
    254       m_accelScale = 4096.0;
    255       break;
    256 
    257     case AFS_16:
    258       m_accelScale = 2048.0;
    259       break;
    260 
    261     default: // should never occur, but...
    262       m_accelScale = 1.0;        // set a safe, though incorrect value
    263       throw std::logic_error(string(__FUNCTION__) +
    264                              ": internal error, unsupported scale");
    265       break;
    266     }
    267 
    268   return true;
    269 }
    270 
    271 bool MPU60X0::setDigitalLowPassFilter(DLPF_CFG_T dlp)
    272 {
    273   uint8_t reg = readReg(REG_CONFIG);
    274 
    275   reg &= ~(_CONFIG_DLPF_MASK << _CONFIG_DLPF_SHIFT);
    276 
    277   reg |= (dlp << _CONFIG_DLPF_SHIFT);
    278 
    279   return writeReg(REG_CONFIG, reg);
    280 }
    281 
    282 bool MPU60X0::setSampleRateDivider(uint8_t div)
    283 {
    284   return writeReg(REG_SMPLRT_DIV, div);
    285 }
    286 
    287 uint8_t MPU60X0::getSampleRateDivider()
    288 {
    289   return readReg(REG_SMPLRT_DIV);
    290 }
    291 
    292 void MPU60X0::getAccelerometer(float *x, float *y, float *z)
    293 {
    294   if (x)
    295     *x = m_accelX / m_accelScale;
    296 
    297   if (y)
    298     *y = m_accelY / m_accelScale;
    299 
    300   if (z)
    301     *z = m_accelZ / m_accelScale;
    302 }
    303 
    304 void MPU60X0::getGyroscope(float *x, float *y, float *z)
    305 {
    306   if (x)
    307     *x = m_gyroX / m_gyroScale;
    308 
    309   if (y)
    310     *y = m_gyroY / m_gyroScale;
    311 
    312   if (z)
    313     *z = m_gyroZ / m_gyroScale;
    314 }
    315 
    316 float MPU60X0::getTemperature()
    317 {
    318   // this equation is taken from the datasheet
    319   return (m_temp / 340.0) + 36.53;
    320 }
    321 
    322 bool MPU60X0::enableTemperatureSensor(bool enable)
    323 {
    324   uint8_t reg = readReg(REG_PWR_MGMT_1);
    325 
    326   if (enable)
    327     reg &= ~TEMP_DIS;
    328   else
    329     reg |= TEMP_DIS;
    330 
    331   return writeReg(REG_PWR_MGMT_1, reg);
    332 }
    333 
    334 bool MPU60X0::setExternalSync(EXT_SYNC_SET_T val)
    335 {
    336   uint8_t reg = readReg(REG_CONFIG);
    337 
    338   reg &= ~(_CONFIG_EXT_SYNC_SET_MASK << _CONFIG_EXT_SYNC_SET_SHIFT);
    339 
    340   reg |= (val << _CONFIG_EXT_SYNC_SET_SHIFT);
    341 
    342   return writeReg(REG_CONFIG, reg);
    343 }
    344 
    345 bool MPU60X0::enableI2CBypass(bool enable)
    346 {
    347   uint8_t reg = readReg(REG_INT_PIN_CFG);
    348 
    349   if (enable)
    350     reg |= I2C_BYPASS_ENABLE;
    351   else
    352     reg &= ~I2C_BYPASS_ENABLE;
    353 
    354   return writeReg(REG_INT_PIN_CFG, reg);
    355 }
    356 
    357 bool MPU60X0::setMotionDetectionThreshold(uint8_t thr)
    358 {
    359   return writeReg(REG_MOT_THR, thr);
    360 }
    361 
    362 uint8_t MPU60X0::getInterruptStatus()
    363 {
    364   return readReg(REG_INT_STATUS);
    365 }
    366 
    367 bool MPU60X0::setInterruptEnables(uint8_t enables)
    368 {
    369   return writeReg(REG_INT_ENABLE, enables);
    370 }
    371 
    372 uint8_t MPU60X0::getInterruptEnables()
    373 {
    374   return readReg(REG_INT_ENABLE);
    375 }
    376 
    377 bool MPU60X0::setInterruptPinConfig(uint8_t cfg)
    378 {
    379   return writeReg(REG_INT_PIN_CFG, cfg);
    380 }
    381 
    382 uint8_t MPU60X0::getInterruptPinConfig()
    383 {
    384   return readReg(REG_INT_PIN_CFG);
    385 }
    386 
    387 #ifdef JAVACALLBACK
    388 void MPU60X0::installISR(int gpio, mraa::Edge level,
    389                          IsrCallback *cb)
    390 {
    391         installISR(gpio, level, generic_callback_isr, cb);
    392 }
    393 #endif
    394 
    395 void MPU60X0::installISR(int gpio, mraa::Edge level,
    396                          void (*isr)(void *), void *arg)
    397 {
    398   // delete any existing ISR and GPIO context
    399   uninstallISR();
    400 
    401   // greate gpio context
    402   m_gpioIRQ = new mraa::Gpio(gpio);
    403 
    404   m_gpioIRQ->dir(mraa::DIR_IN);
    405   m_gpioIRQ->isr(level, isr, arg);
    406 }
    407 
    408 void MPU60X0::uninstallISR()
    409 {
    410   if (m_gpioIRQ)
    411     {
    412       m_gpioIRQ->isrExit();
    413       delete m_gpioIRQ;
    414 
    415       m_gpioIRQ = 0;
    416     }
    417 }
    418