| /* |
| * 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.h" |
| |
| 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; |
| } |
| } |