1765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang/* 2765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Author: Jon Trulson <jtrulson@ics.com> 3765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Copyright (c) 2015 Intel Corporation. 4765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 5765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Permission is hereby granted, free of charge, to any person obtaining 6765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * a copy of this software and associated documentation files (the 7765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * "Software"), to deal in the Software without restriction, including 8765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * without limitation the rights to use, copy, modify, merge, publish, 9765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * distribute, sublicense, and/or sell copies of the Software, and to 10765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * permit persons to whom the Software is furnished to do so, subject to 11765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * the following conditions: 12765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 13765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * The above copyright notice and this permission notice shall be 14765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * included in all copies or substantial portions of the Software. 15765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 16765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 20765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 22765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang */ 24765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 25765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <unistd.h> 26765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <iostream> 27765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <string.h> 28765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 29765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include "mpu60x0.h" 30765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 31765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangusing namespace upm; 32765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangusing namespace std; 33765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 34765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 35765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangMPU60X0::MPU60X0(int bus, uint8_t address) : 36765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_i2c(bus), m_gpioIRQ(0) 37765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 38765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_addr = address; 39765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 40765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_accelX = 0.0; 41765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_accelY = 0.0; 42765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_accelZ = 0.0; 43765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 44765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gyroX = 0.0; 45765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gyroY = 0.0; 46765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gyroZ = 0.0; 47765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 48765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_temp = 0.0; 49765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 50765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_accelScale = 1.0; 51765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gyroScale = 1.0; 52765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 53765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa::Result rv; 54765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if ( (rv = m_i2c.address(m_addr)) != mraa::SUCCESS) 55765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 56765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::runtime_error(std::string(__FUNCTION__) + 57765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": I2c.address() failed"); 58765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 59765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 60765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 61765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 62765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangMPU60X0::~MPU60X0() 63765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 64765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uninstallISR(); 65765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 66765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 67765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::init() 68765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 69765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // first, take the device out of sleep mode 70765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (!setSleep(false)) 71765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 72765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::runtime_error(std::string(__FUNCTION__) + 73765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": Unable to wake up device"); 74765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return false; 75765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 76765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 77765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // set the clock source to use the gyroscope PLL rather than the 78765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // internal clock for stability 79765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (!setClockSource(PLL_XG)) 80765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 81765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::runtime_error(std::string(__FUNCTION__) + 82765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": Unable to set clock source"); 83765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return false; 84765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 85765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 86765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang usleep(5000); 87765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 88765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // enable temperature measurement (default on power up, but let's be sure) 89765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang enableTemperatureSensor(true); 90765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 91765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // set the gyro and accel scale bits to reasonable values 92765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang setGyroscopeScale(FS_500); 93765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang setAccelerometerScale(AFS_2); 94765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 95765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // enable the DLPF 96765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang setDigitalLowPassFilter(DLPF_94_98); 97765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 98765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // let things stabilize... 99765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang usleep(100000); 100765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 101765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return true; 102765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 103765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 104765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 105765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid MPU60X0::update() 106765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 107765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // read all of the sensor registers - accel, gyro, and temp 108765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint8_t buffer[14]; 109765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 110765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang memset(buffer, 0, 14); 111765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang readRegs(REG_ACCEL_XOUT_H, buffer, 14); 112765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 113765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang int16_t ax, ay, az; 114765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang int16_t temp; 115765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang int16_t gx, gy, gz; 116765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 117765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ax = ( (buffer[0] << 8) | buffer[1] ); 118765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ay = ( (buffer[2] << 8) | buffer[3] ); 119765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang az = ( (buffer[4] << 8) | buffer[5] ); 120765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 121765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang temp = ( (buffer[6] << 8) | buffer[7] ); 122765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 123765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang gx = ( (buffer[8] << 8) | buffer[9] ); 124765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang gy = ( (buffer[10] << 8) | buffer[11] ); 125765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang gz = ( (buffer[12] << 8) | buffer[13] ); 126765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 127765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // now stash them 128765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_accelX = float(ax); 129765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_accelY = float(ay); 130765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_accelZ = float(az); 131765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 132765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_temp = float(temp); 133765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 134765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gyroX = float(gx); 135765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gyroY = float(gy); 136765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gyroZ = float(gz); 137765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 138765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 139765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhanguint8_t MPU60X0::readReg(uint8_t reg) 140765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 141765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return m_i2c.readReg(reg); 142765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 143765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 144765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid MPU60X0::readRegs(uint8_t reg, uint8_t *buffer, int len) 145765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 146765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_i2c.readBytesReg(reg, buffer, len); 147765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 148765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 149765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::writeReg(uint8_t reg, uint8_t val) 150765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 151765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa::Result rv; 152765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if ((rv = m_i2c.writeReg(reg, val)) != mraa::SUCCESS) 153765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 154765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::runtime_error(std::string(__FUNCTION__) + 155765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": I2c.writeReg() failed"); 156765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return false; 157765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 158765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 159765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return true; 160765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 161765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 162765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::setSleep(bool enable) 163765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 164765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint8_t reg = readReg(REG_PWR_MGMT_1); 165765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 166765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (enable) 167765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg |= PWR_SLEEP; 168765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 169765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg &= ~PWR_SLEEP; 170765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 171765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writeReg(REG_PWR_MGMT_1, reg); 172765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 173765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 174765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::setClockSource(CLKSEL_T clk) 175765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 176765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint8_t reg = readReg(REG_PWR_MGMT_1); 177765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 178765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg &= ~(_CLKSEL_MASK << _CLKSEL_SHIFT); 179765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 180765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg |= (clk << _CLKSEL_SHIFT); 181765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 182765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writeReg(REG_PWR_MGMT_1, reg); 183765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 184765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 185765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::setGyroscopeScale(FS_SEL_T scale) 186765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 187765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint8_t reg = readReg(REG_GYRO_CONFIG); 188765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 189765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg &= ~(_FS_SEL_MASK << _FS_SEL_SHIFT); 190765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 191765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg |= (scale << _FS_SEL_SHIFT); 192765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 193765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (!writeReg(REG_GYRO_CONFIG, reg)) 194765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 195765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return false; 196765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 197765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 198765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // store scaling factor 199765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 200765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang switch (scale) 201765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 202765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case FS_250: 203765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gyroScale = 131.0; 204765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 205765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 206765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case FS_500: 207765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gyroScale = 65.5; 208765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 209765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 210765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case FS_1000: 211765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gyroScale = 32.8; 212765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 213765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 214765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case FS_2000: 215765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gyroScale = 16.4; 216765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 217765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 218765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang default: // should never occur, but... 219765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gyroScale = 1.0; // set a safe, though incorrect value 220765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::logic_error(string(__FUNCTION__) + 221765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": internal error, unsupported scale"); 222765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 223765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 224765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 225765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return true; 226765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 227765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 228765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::setAccelerometerScale(AFS_SEL_T scale) 229765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 230765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint8_t reg = readReg(REG_ACCEL_CONFIG); 231765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 232765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg &= ~(_AFS_SEL_MASK << _AFS_SEL_SHIFT); 233765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 234765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg |= (scale << _AFS_SEL_SHIFT); 235765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 236765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (!writeReg(REG_ACCEL_CONFIG, reg)) 237765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 238765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return false; 239765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 240765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 241765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // store scaling factor 242765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 243765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang switch (scale) 244765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 245765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case AFS_2: 246765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_accelScale = 16384.0; 247765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 248765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 249765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case AFS_4: 250765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_accelScale = 8192.0; 251765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 252765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 253765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case AFS_8: 254765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_accelScale = 4096.0; 255765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 256765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 257765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case AFS_16: 258765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_accelScale = 2048.0; 259765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 260765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 261765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang default: // should never occur, but... 262765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_accelScale = 1.0; // set a safe, though incorrect value 263765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::logic_error(string(__FUNCTION__) + 264765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": internal error, unsupported scale"); 265765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 266765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 267765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 268765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return true; 269765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 270765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 271765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::setDigitalLowPassFilter(DLPF_CFG_T dlp) 272765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 273765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint8_t reg = readReg(REG_CONFIG); 274765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 275765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg &= ~(_CONFIG_DLPF_MASK << _CONFIG_DLPF_SHIFT); 276765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 277765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg |= (dlp << _CONFIG_DLPF_SHIFT); 278765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 279765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writeReg(REG_CONFIG, reg); 280765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 281765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 282765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::setSampleRateDivider(uint8_t div) 283765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 284765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writeReg(REG_SMPLRT_DIV, div); 285765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 286765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 287765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhanguint8_t MPU60X0::getSampleRateDivider() 288765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 289765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return readReg(REG_SMPLRT_DIV); 290765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 291765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 292765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid MPU60X0::getAccelerometer(float *x, float *y, float *z) 293765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 294765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (x) 295765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang *x = m_accelX / m_accelScale; 296765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 297765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (y) 298765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang *y = m_accelY / m_accelScale; 299765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 300765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (z) 301765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang *z = m_accelZ / m_accelScale; 302765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 303765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 304765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid MPU60X0::getGyroscope(float *x, float *y, float *z) 305765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 306765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (x) 307765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang *x = m_gyroX / m_gyroScale; 308765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 309765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (y) 310765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang *y = m_gyroY / m_gyroScale; 311765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 312765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (z) 313765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang *z = m_gyroZ / m_gyroScale; 314765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 315765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 316765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangfloat MPU60X0::getTemperature() 317765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 318765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // this equation is taken from the datasheet 319765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return (m_temp / 340.0) + 36.53; 320765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 321765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 322765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::enableTemperatureSensor(bool enable) 323765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 324765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint8_t reg = readReg(REG_PWR_MGMT_1); 325765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 326765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (enable) 327765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg &= ~TEMP_DIS; 328765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 329765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg |= TEMP_DIS; 330765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 331765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writeReg(REG_PWR_MGMT_1, reg); 332765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 333765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 334765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::setExternalSync(EXT_SYNC_SET_T val) 335765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 336765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint8_t reg = readReg(REG_CONFIG); 337765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 338765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg &= ~(_CONFIG_EXT_SYNC_SET_MASK << _CONFIG_EXT_SYNC_SET_SHIFT); 339765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 340765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg |= (val << _CONFIG_EXT_SYNC_SET_SHIFT); 341765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 342765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writeReg(REG_CONFIG, reg); 343765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 344765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 345765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::enableI2CBypass(bool enable) 346765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 347765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint8_t reg = readReg(REG_INT_PIN_CFG); 348765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 349765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (enable) 350765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg |= I2C_BYPASS_ENABLE; 351765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 352765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang reg &= ~I2C_BYPASS_ENABLE; 353765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 354765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writeReg(REG_INT_PIN_CFG, reg); 355765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 356765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 357765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::setMotionDetectionThreshold(uint8_t thr) 358765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 359765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writeReg(REG_MOT_THR, thr); 360765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 361765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 362765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhanguint8_t MPU60X0::getInterruptStatus() 363765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 364765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return readReg(REG_INT_STATUS); 365765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 366765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 367765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::setInterruptEnables(uint8_t enables) 368765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 369765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writeReg(REG_INT_ENABLE, enables); 370765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 371765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 372765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhanguint8_t MPU60X0::getInterruptEnables() 373765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 374765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return readReg(REG_INT_ENABLE); 375765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 376765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 377765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool MPU60X0::setInterruptPinConfig(uint8_t cfg) 378765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 379765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writeReg(REG_INT_PIN_CFG, cfg); 380765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 381765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 382765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhanguint8_t MPU60X0::getInterruptPinConfig() 383765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 384765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return readReg(REG_INT_PIN_CFG); 385765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 386765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 387765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#ifdef JAVACALLBACK 388765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid MPU60X0::installISR(int gpio, mraa::Edge level, 389765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang IsrCallback *cb) 390765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 391765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang installISR(gpio, level, generic_callback_isr, cb); 392765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 393765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#endif 394765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 395765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid MPU60X0::installISR(int gpio, mraa::Edge level, 396765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang void (*isr)(void *), void *arg) 397765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 398765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // delete any existing ISR and GPIO context 399765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uninstallISR(); 400765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 401765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // greate gpio context 402765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gpioIRQ = new mraa::Gpio(gpio); 403765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 404765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gpioIRQ->dir(mraa::DIR_IN); 405765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gpioIRQ->isr(level, isr, arg); 406765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 407765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 408765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid MPU60X0::uninstallISR() 409765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 410765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_gpioIRQ) 411765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 412765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gpioIRQ->isrExit(); 413765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang delete m_gpioIRQ; 414765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 415765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_gpioIRQ = 0; 416765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 417765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 418