upm/src/mpu9150/mpu60x0.cxx

/*
 * Author: Jon Trulson <jtrulson@ics.com>
 * Copyright (c) 2015 Intel Corporation.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include <unistd.h>
#include <iostream>
#include <string.h>

#include "mpu60x0.hpp"

using namespace upm;
using namespace std;


MPU60X0::MPU60X0(int bus, uint8_t address) :
  m_i2c(bus), m_gpioIRQ(0)
{
  m_addr = address;

  m_accelX = 0.0;
  m_accelY = 0.0;
  m_accelZ = 0.0;
  
  m_gyroX = 0.0;
  m_gyroY = 0.0;
  m_gyroZ = 0.0;
  
  m_temp = 0.0;

  m_accelScale = 1.0;
  m_gyroScale = 1.0;

  mraa::Result rv;
  if ( (rv = m_i2c.address(m_addr)) != mraa::SUCCESS)
    {
      throw std::runtime_error(std::string(__FUNCTION__) +
                               ": I2c.address() failed");
      return;
    }
}

MPU60X0::~MPU60X0()
{
  uninstallISR();
}

bool MPU60X0::init()
{
  // first, take the device out of sleep mode
  if (!setSleep(false))
    {
      throw std::runtime_error(std::string(__FUNCTION__) +
                               ": Unable to wake up device");
      return false;
    }

  // set the clock source to use the gyroscope PLL rather than the
  // internal clock for stability
  if (!setClockSource(PLL_XG))
    {
      throw std::runtime_error(std::string(__FUNCTION__) +
                               ": Unable to set clock source");
      return false;
    }

  usleep(5000);

  // enable temperature measurement (default on power up, but let's be sure)
  enableTemperatureSensor(true);

  // set the gyro and accel scale bits to reasonable values
  setGyroscopeScale(FS_500);
  setAccelerometerScale(AFS_2);

  // enable the DLPF
  setDigitalLowPassFilter(DLPF_94_98);

  // let things stabilize...
  usleep(100000);

  return true;
}


void MPU60X0::update()
{
  // read all of the sensor registers - accel, gyro, and temp
  uint8_t buffer[14];

  memset(buffer, 0, 14);
  readRegs(REG_ACCEL_XOUT_H, buffer, 14);

  int16_t ax, ay, az;
  int16_t temp;
  int16_t gx, gy, gz;

  ax =  ( (buffer[0] << 8) | buffer[1] );
  ay =  ( (buffer[2] << 8) | buffer[3] );
  az =  ( (buffer[4] << 8) | buffer[5] );

  temp = ( (buffer[6] << 8) | buffer[7] );

  gx =  ( (buffer[8] << 8) | buffer[9] );
  gy =  ( (buffer[10] << 8) | buffer[11] );
  gz =  ( (buffer[12] << 8) | buffer[13] );

  // now stash them
  m_accelX = float(ax);
  m_accelY = float(ay);
  m_accelZ = float(az);

  m_temp = float(temp);

  m_gyroX = float(gx);
  m_gyroY = float(gy);
  m_gyroZ = float(gz);
}

uint8_t MPU60X0::readReg(uint8_t reg)
{
  return m_i2c.readReg(reg);
}

void MPU60X0::readRegs(uint8_t reg, uint8_t *buffer, int len)
{
  m_i2c.readBytesReg(reg, buffer, len);
}

bool MPU60X0::writeReg(uint8_t reg, uint8_t val)
{
  mraa::Result rv;
  if ((rv = m_i2c.writeReg(reg, val)) != mraa::SUCCESS)
    {
      throw std::runtime_error(std::string(__FUNCTION__) +
                               ": I2c.writeReg() failed");
      return false;
    } 
  
  return true;
}

bool MPU60X0::setSleep(bool enable)
{
  uint8_t reg = readReg(REG_PWR_MGMT_1);

  if (enable)
    reg |= PWR_SLEEP;
  else
    reg &= ~PWR_SLEEP;

  return writeReg(REG_PWR_MGMT_1, reg);
}

bool MPU60X0::setClockSource(CLKSEL_T clk)
{
  uint8_t reg = readReg(REG_PWR_MGMT_1);

  reg &= ~(_CLKSEL_MASK << _CLKSEL_SHIFT);

  reg |= (clk << _CLKSEL_SHIFT);

  return writeReg(REG_PWR_MGMT_1, reg);
}

bool MPU60X0::setGyroscopeScale(FS_SEL_T scale)
{
  uint8_t reg = readReg(REG_GYRO_CONFIG);

  reg &= ~(_FS_SEL_MASK << _FS_SEL_SHIFT);

  reg |= (scale << _FS_SEL_SHIFT);

  if (!writeReg(REG_GYRO_CONFIG, reg))
    {
      return false;
    }

  // store scaling factor

  switch (scale)
    {
    case FS_250:
      m_gyroScale = 131.0;
      break;

    case FS_500:
      m_gyroScale = 65.5;
      break;

    case FS_1000:
      m_gyroScale = 32.8;
      break;

    case FS_2000:
      m_gyroScale = 16.4;
      break;

    default: // should never occur, but...
      m_gyroScale = 1.0;        // set a safe, though incorrect value
      throw std::logic_error(string(__FUNCTION__) + 
                             ": internal error, unsupported scale");
      break;
    }

  return true;
}

bool MPU60X0::setAccelerometerScale(AFS_SEL_T scale)
{
  uint8_t reg = readReg(REG_ACCEL_CONFIG);

  reg &= ~(_AFS_SEL_MASK << _AFS_SEL_SHIFT);

  reg |= (scale << _AFS_SEL_SHIFT);

  if (!writeReg(REG_ACCEL_CONFIG, reg))
    {
      return false;
    }

  // store scaling factor
  
  switch (scale)
    {
    case AFS_2:
      m_accelScale = 16384.0;
      break;

    case AFS_4:
      m_accelScale = 8192.0;
      break;

    case AFS_8:
      m_accelScale = 4096.0;
      break;

    case AFS_16:
      m_accelScale = 2048.0;
      break;

    default: // should never occur, but...
      m_accelScale = 1.0;        // set a safe, though incorrect value
      throw std::logic_error(string(__FUNCTION__) + 
                             ": internal error, unsupported scale");
      break;
    }

  return true;
}

bool MPU60X0::setDigitalLowPassFilter(DLPF_CFG_T dlp)
{
  uint8_t reg = readReg(REG_CONFIG);

  reg &= ~(_CONFIG_DLPF_MASK << _CONFIG_DLPF_SHIFT);

  reg |= (dlp << _CONFIG_DLPF_SHIFT);

  return writeReg(REG_CONFIG, reg);
}

bool MPU60X0::setSampleRateDivider(uint8_t div)
{
  return writeReg(REG_SMPLRT_DIV, div);
}

uint8_t MPU60X0::getSampleRateDivider()
{
  return readReg(REG_SMPLRT_DIV);
}

void MPU60X0::getAccelerometer(float *x, float *y, float *z)
{
  if (x)
    *x = m_accelX / m_accelScale;

  if (y)
    *y = m_accelY / m_accelScale;

  if (z)
    *z = m_accelZ / m_accelScale;
}

void MPU60X0::getGyroscope(float *x, float *y, float *z)
{
  if (x)
    *x = m_gyroX / m_gyroScale;

  if (y)
    *y = m_gyroY / m_gyroScale;

  if (z)
    *z = m_gyroZ / m_gyroScale;
}

float MPU60X0::getTemperature()
{
  // this equation is taken from the datasheet
  return (m_temp / 340.0) + 36.53;
}

bool MPU60X0::enableTemperatureSensor(bool enable)
{
  uint8_t reg = readReg(REG_PWR_MGMT_1);

  if (enable)
    reg &= ~TEMP_DIS;
  else
    reg |= TEMP_DIS;

  return writeReg(REG_PWR_MGMT_1, reg);
}

bool MPU60X0::setExternalSync(EXT_SYNC_SET_T val)
{
  uint8_t reg = readReg(REG_CONFIG);

  reg &= ~(_CONFIG_EXT_SYNC_SET_MASK << _CONFIG_EXT_SYNC_SET_SHIFT);

  reg |= (val << _CONFIG_EXT_SYNC_SET_SHIFT);

  return writeReg(REG_CONFIG, reg);
}

bool MPU60X0::enableI2CBypass(bool enable)
{
  uint8_t reg = readReg(REG_INT_PIN_CFG);

  if (enable)
    reg |= I2C_BYPASS_ENABLE;
  else
    reg &= ~I2C_BYPASS_ENABLE;

  return writeReg(REG_INT_PIN_CFG, reg);
}

bool MPU60X0::setMotionDetectionThreshold(uint8_t thr)
{
  return writeReg(REG_MOT_THR, thr);
}

uint8_t MPU60X0::getInterruptStatus()
{
  return readReg(REG_INT_STATUS);
}

bool MPU60X0::setInterruptEnables(uint8_t enables)
{
  return writeReg(REG_INT_ENABLE, enables);
}

uint8_t MPU60X0::getInterruptEnables()
{
  return readReg(REG_INT_ENABLE);
}

bool MPU60X0::setInterruptPinConfig(uint8_t cfg)
{
  return writeReg(REG_INT_PIN_CFG, cfg);
}

uint8_t MPU60X0::getInterruptPinConfig()
{
  return readReg(REG_INT_PIN_CFG);
}

#if defined(SWIGJAVA) || defined(JAVACALLBACK)
void MPU60X0::installISR(int gpio, mraa::Edge level,
                         jobject runnable)
{
  // delete any existing ISR and GPIO context
  uninstallISR();

  // greate gpio context
  m_gpioIRQ = new mraa::Gpio(gpio);

  m_gpioIRQ->dir(mraa::DIR_IN);
  m_gpioIRQ->isr(level, runnable);
}
#else
void MPU60X0::installISR(int gpio, mraa::Edge level, 
                         void (*isr)(void *), void *arg)
{
  // delete any existing ISR and GPIO context
  uninstallISR();

  // greate gpio context
  m_gpioIRQ = new mraa::Gpio(gpio);

  m_gpioIRQ->dir(mraa::DIR_IN);
  m_gpioIRQ->isr(level, isr, arg);
}
#endif

void MPU60X0::uninstallISR()
{
  if (m_gpioIRQ)
    {
      m_gpioIRQ->isrExit();
      delete m_gpioIRQ;
      
      m_gpioIRQ = 0;
    }
}