diff options
Diffstat (limited to 'peripheral/libupm/src/lsm9ds0/lsm9ds0.cxx')
| -rw-r--r-- | peripheral/libupm/src/lsm9ds0/lsm9ds0.cxx | 804 |
1 files changed, 0 insertions, 804 deletions
diff --git a/peripheral/libupm/src/lsm9ds0/lsm9ds0.cxx b/peripheral/libupm/src/lsm9ds0/lsm9ds0.cxx deleted file mode 100644 index 3253a62..0000000 --- a/peripheral/libupm/src/lsm9ds0/lsm9ds0.cxx +++ /dev/null @@ -1,804 +0,0 @@ -/* - * 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 <stdexcept> -#include <string.h> - -#include "lsm9ds0.h" - -using namespace upm; -using namespace std; - - -LSM9DS0::LSM9DS0(int bus, uint8_t gAddress, uint8_t xmAddress) : - m_i2cG(bus), m_i2cXM(bus), m_gpioG_INT(0), m_gpioG_DRDY(0), - m_gpioXM_GEN1(0), m_gpioXM_GEN2(0) -{ - m_gAddr = gAddress; - m_xmAddr = xmAddress; - - 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_magX = 0.0; - m_magY = 0.0; - m_magZ = 0.0; - - m_temp = 0.0; - - m_accelScale = 0.0; - m_gyroScale = 0.0; - m_magScale = 0.0; - - mraa::Result rv; - if ( (rv = m_i2cG.address(m_gAddr)) != mraa::SUCCESS) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Could not initialize Gyro i2c address"); - return; - } - - if ( (rv = m_i2cXM.address(m_xmAddr)) != mraa::SUCCESS) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Could not initialize XM i2c address"); - return; - } -} - -LSM9DS0::~LSM9DS0() -{ - uninstallISR(INTERRUPT_G_INT); - uninstallISR(INTERRUPT_G_DRDY); - uninstallISR(INTERRUPT_XM_GEN1); - uninstallISR(INTERRUPT_XM_GEN2); -} - -bool LSM9DS0::init() -{ - // Init the gyroscope - - // power up - if (!setGyroscopePowerDown(false)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to wake up gyro"); - return false; - } - - // enable all axes - if (!setGyroscopeEnableAxes(CTRL_REG1_G_YEN |CTRL_REG1_G_XEN | - CTRL_REG1_G_ZEN)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to enable gyro axes"); - return false; - } - - // set gyro ODR - if (!setGyroscopeODR(G_ODR_95_25)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to set gyro ODR"); - return false; - } - - // set gyro scale - if (!setGyroscopeScale(G_FS_245)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to set gyro scale"); - return false; - } - - // Init the accelerometer - - // power up and set ODR - if (!setAccelerometerODR(XM_AODR_100)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to set accel ODR"); - return false; - } - - // enable all axes - if (!setAccelerometerEnableAxes(CTRL_REG1_XM_AXEN |CTRL_REG1_XM_AYEN | - CTRL_REG1_XM_AZEN)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to enable accel axes"); - return false; - } - - // set scaling rate - if (!setAccelerometerScale(XM_AFS_2)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to set accel scale"); - return false; - } - - // temperature sensor - - // enable the temperature sensor - if (!enableTemperatureSensor(true)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to enable temp sensor"); - return false; - } - - // Init the magnetometer - - // set mode (this also powers it up if not XM_MD_POWERDOWN) - if (!setMagnetometerMode(XM_MD_CONTINUOUS)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to set mag scale"); - return false; - } - - // turn LPM off - if (!setMagnetometerLPM(false)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to disable mag LPM"); - return false; - } - - // set resolution - if (!setMagnetometerResolution(XM_RES_LOW)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to set mag res"); - return false; - } - - // set ODR - if (!setMagnetometerODR(XM_ODR_12_5)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to set mag ODR"); - return false; - } - - // set scale - if (!setMagnetometerScale(XM_MFS_2)) - { - throw std::runtime_error(string(__FUNCTION__) + - ": Unable to set mag scale"); - return false; - } - - return true; -} - - -void LSM9DS0::update() -{ - updateGyroscope(); - updateAccelerometer(); - updateMagnetometer(); - updateTemperature(); -} - -void LSM9DS0::updateGyroscope() -{ - uint8_t buffer[6]; - - memset(buffer, 0, 6); - readRegs(DEV_GYRO, REG_OUT_X_L_G, buffer, 6); - - int16_t x, y, z; - - x = ( (buffer[1] << 8) | buffer[0] ); - y = ( (buffer[3] << 8) | buffer[2] ); - z = ( (buffer[5] << 8) | buffer[4] ); - - m_gyroX = float(x); - m_gyroY = float(y); - m_gyroZ = float(z); -} - -void LSM9DS0::updateAccelerometer() -{ - uint8_t buffer[6]; - - memset(buffer, 0, 6); - readRegs(DEV_XM, REG_OUT_X_L_A, buffer, 6); - - int16_t x, y, z; - - x = ( (buffer[1] << 8) | buffer[0] ); - y = ( (buffer[3] << 8) | buffer[2] ); - z = ( (buffer[5] << 8) | buffer[4] ); - - m_accelX = float(x); - m_accelY = float(y); - m_accelZ = float(z); -} - -void LSM9DS0::updateMagnetometer() -{ - uint8_t buffer[6]; - - memset(buffer, 0, 6); - readRegs(DEV_XM, REG_OUT_X_L_M, buffer, 6); - - int16_t x, y, z; - - x = ( (buffer[1] << 8) | buffer[0] ); - y = ( (buffer[3] << 8) | buffer[2] ); - z = ( (buffer[5] << 8) | buffer[4] ); - - m_magX = float(x); - m_magY = float(y); - m_magZ = float(z); -} - -void LSM9DS0::updateTemperature() -{ - uint8_t buffer[2]; - - memset(buffer, 0, 2); - readRegs(DEV_XM, REG_OUT_TEMP_L_XM, buffer, 2); - - // cerr << "HIGH: " << int(buffer[1]) << " LOW: " << int(buffer[0]) << endl; - - // 12b signed - int16_t temp = ( (buffer[1] << 8) | (buffer[0] ) ); - if (temp & 0x0800) - { - temp &= ~0x0800; - temp *= -1; - } - - m_temp = float(temp); -} - -uint8_t LSM9DS0::readReg(DEVICE_T dev, uint8_t reg) -{ - mraa::I2c *device; - - switch(dev) - { - case DEV_GYRO: device = &m_i2cG; break; - case DEV_XM: device = &m_i2cXM; break; - default: - throw std::logic_error(string(__FUNCTION__) + - ": Internal error, invalid device specified"); - return 0; - } - - return device->readReg(reg); -} - -void LSM9DS0::readRegs(DEVICE_T dev, uint8_t reg, uint8_t *buffer, int len) -{ - mraa::I2c *device; - - switch(dev) - { - case DEV_GYRO: device = &m_i2cG; break; - case DEV_XM: device = &m_i2cXM; break; - default: - throw std::logic_error(string(__FUNCTION__) + - ": Internal error, invalid device specified"); - return; - } - - // We need to set the high bit of the register to enable - // auto-increment mode for reading multiple registers in one go. - device->readBytesReg(reg | m_autoIncrementMode, buffer, len); -} - -bool LSM9DS0::writeReg(DEVICE_T dev, uint8_t reg, uint8_t val) -{ - mraa::I2c *device; - - switch(dev) - { - case DEV_GYRO: device = &m_i2cG; break; - case DEV_XM: device = &m_i2cXM; break; - default: - throw std::logic_error(string(__FUNCTION__) + - ": Internal error, invalid device specified"); - return false; - } - - mraa::Result rv; - if ((rv = device->writeReg(reg, val)) != mraa::SUCCESS) - { - throw std::runtime_error(std::string(__FUNCTION__) + - ": I2c.writeReg() failed"); - return false; - } - - return true; -} - -bool LSM9DS0::setGyroscopePowerDown(bool enable) -{ - uint8_t reg = readReg(DEV_GYRO, REG_CTRL_REG1_G); - - if (enable) - reg &= ~CTRL_REG1_G_PD; - else - reg |= CTRL_REG1_G_PD; - - return writeReg(DEV_GYRO, REG_CTRL_REG1_G, reg); -} - -bool LSM9DS0::setGyroscopeEnableAxes(uint8_t axes) -{ - uint8_t reg = readReg(DEV_GYRO, REG_CTRL_REG1_G); - - // filter out any non-axis related data from arg - axes &= (CTRL_REG1_G_YEN | CTRL_REG1_G_XEN | CTRL_REG1_G_ZEN); - - // clear them in the register - reg &= ~(CTRL_REG1_G_YEN | CTRL_REG1_G_XEN | CTRL_REG1_G_ZEN); - - // now add them - reg |= axes; - - return writeReg(DEV_GYRO, REG_CTRL_REG1_G, reg); -} - -bool LSM9DS0::setGyroscopeODR(G_ODR_T odr) -{ - uint8_t reg = readReg(DEV_GYRO, REG_CTRL_REG1_G); - - reg &= ~(_CTRL_REG1_G_ODR_MASK << _CTRL_REG1_G_ODR_SHIFT); - - reg |= (odr << _CTRL_REG1_G_ODR_SHIFT); - - return writeReg(DEV_GYRO, REG_CTRL_REG1_G, reg); -} - -bool LSM9DS0::setGyroscopeScale(G_FS_T scale) -{ - uint8_t reg = readReg(DEV_GYRO, REG_CTRL_REG4_G); - - reg &= ~(_CTRL_REG4_G_FS_MASK << _CTRL_REG4_G_FS_SHIFT); - - reg |= (scale << _CTRL_REG4_G_FS_SHIFT); - - if (!writeReg(DEV_GYRO, REG_CTRL_REG4_G, reg)) - { - return false; - } - - // store scaling factor (mDeg/s/LSB) - - switch (scale) - { - case G_FS_245: - m_gyroScale = 8.75; - break; - - case G_FS_500: - m_gyroScale = 17.5; - break; - - case G_FS_2000: - m_gyroScale = 70.0; - break; - - default: // should never occur, but... - m_gyroScale = 0.0; // set a safe, though incorrect value - throw std::logic_error(string(__FUNCTION__) + - ": internal error, unsupported scale"); - break; - } - - return true; -} - -bool LSM9DS0::setAccelerometerEnableAxes(uint8_t axes) -{ - uint8_t reg = readReg(DEV_XM, REG_CTRL_REG1_XM); - - // filter out any non-axis related data from arg - axes &= (CTRL_REG1_XM_AXEN | CTRL_REG1_XM_AYEN | CTRL_REG1_XM_AZEN); - - // clear them in the register - reg &= ~(CTRL_REG1_XM_AXEN | CTRL_REG1_XM_AYEN | CTRL_REG1_XM_AZEN); - - // now add them - reg |= axes; - - return writeReg(DEV_XM, REG_CTRL_REG1_XM, reg); -} - -bool LSM9DS0::setAccelerometerODR(XM_AODR_T odr) -{ - uint8_t reg = readReg(DEV_XM, REG_CTRL_REG1_XM); - - reg &= ~(_CTRL_REG1_XM_AODR_MASK << _CTRL_REG1_XM_AODR_SHIFT); - - reg |= (odr << _CTRL_REG1_XM_AODR_SHIFT); - - return writeReg(DEV_XM, REG_CTRL_REG1_XM, reg); -} - -bool LSM9DS0::setAccelerometerScale(XM_AFS_T scale) -{ - uint8_t reg = readReg(DEV_XM, REG_CTRL_REG2_XM); - - reg &= ~(_CTRL_REG2_XM_AFS_MASK << _CTRL_REG2_XM_AFS_SHIFT); - - reg |= (scale << _CTRL_REG2_XM_AFS_SHIFT); - - if (!writeReg(DEV_XM, REG_CTRL_REG2_XM, reg)) - { - return false; - } - - // store scaling factor - - switch (scale) - { - case XM_AFS_2: - m_accelScale = 0.061; - break; - - case XM_AFS_4: - m_accelScale = 0.122 ; - break; - - case XM_AFS_6: - m_accelScale = 0.183 ; - break; - - case XM_AFS_8: - m_accelScale = 0.244 ; - break; - - case XM_AFS_16: - m_accelScale = 0.732 ; - break; - - default: // should never occur, but... - m_accelScale = 0.0; // set a safe, though incorrect value - throw std::logic_error(string(__FUNCTION__) + - ": internal error, unsupported scale"); - break; - } - - return true; -} - -bool LSM9DS0::setMagnetometerResolution(XM_RES_T res) -{ - uint8_t reg = readReg(DEV_XM, REG_CTRL_REG5_XM); - - reg &= ~(_CTRL_REG5_XM_RES_MASK << _CTRL_REG5_XM_RES_SHIFT); - - reg |= (res << _CTRL_REG5_XM_RES_SHIFT); - - return writeReg(DEV_XM, REG_CTRL_REG5_XM, reg); -} - -bool LSM9DS0::setMagnetometerODR(XM_ODR_T odr) -{ - uint8_t reg = readReg(DEV_XM, REG_CTRL_REG5_XM); - - reg &= ~(_CTRL_REG5_XM_ODR_MASK << _CTRL_REG5_XM_ODR_SHIFT); - - reg |= (odr << _CTRL_REG5_XM_ODR_SHIFT); - - return writeReg(DEV_XM, REG_CTRL_REG5_XM, reg); -} - -bool LSM9DS0::setMagnetometerMode(XM_MD_T mode) -{ - uint8_t reg = readReg(DEV_XM, REG_CTRL_REG7_XM); - - reg &= ~(_CTRL_REG7_XM_MD_MASK << _CTRL_REG7_XM_MD_SHIFT); - - reg |= (mode << _CTRL_REG7_XM_MD_SHIFT); - - return writeReg(DEV_XM, REG_CTRL_REG7_XM, reg); -} - -bool LSM9DS0::setMagnetometerLPM(bool enable) -{ - uint8_t reg = readReg(DEV_XM, REG_CTRL_REG7_XM); - - if (enable) - reg |= CTRL_REG7_XM_MLP; - else - reg &= ~CTRL_REG7_XM_MLP; - - return writeReg(DEV_XM, REG_CTRL_REG7_XM, reg); -} - -bool LSM9DS0::setMagnetometerScale(XM_MFS_T scale) -{ - uint8_t reg = readReg(DEV_XM, REG_CTRL_REG6_XM); - - reg &= ~(_CTRL_REG6_XM_MFS_MASK << _CTRL_REG6_XM_MFS_SHIFT); - - reg |= (scale << _CTRL_REG6_XM_MFS_SHIFT); - - if (!writeReg(DEV_XM, REG_CTRL_REG6_XM, reg)) - { - return false; - } - - // store scaling factor - - switch (scale) - { - case XM_MFS_2: - m_magScale = 0.08; - break; - - case XM_MFS_4: - m_magScale = 0.16; - break; - - case XM_MFS_8: - m_magScale = 0.32; - break; - - case XM_MFS_12: - m_magScale = 0.48; - break; - - default: // should never occur, but... - m_magScale = 0.0; // set a safe, though incorrect value - throw std::logic_error(string(__FUNCTION__) + - ": internal error, unsupported scale"); - break; - } - - return true; -} - -void LSM9DS0::getAccelerometer(float *x, float *y, float *z) -{ - if (x) - *x = (m_accelX * m_accelScale) / 1000.0; - - if (y) - *y = (m_accelY * m_accelScale) / 1000.0; - - if (z) - *z = (m_accelZ * m_accelScale) / 1000.0; -} - -void LSM9DS0::getGyroscope(float *x, float *y, float *z) -{ - if (x) - *x = (m_gyroX * m_gyroScale) / 1000.0; - - if (y) - *y = (m_gyroY * m_gyroScale) / 1000.0; - - if (z) - *z = (m_gyroZ * m_gyroScale) / 1000.0; -} - -void LSM9DS0::getMagnetometer(float *x, float *y, float *z) -{ - if (x) - *x = (m_magX * m_magScale) / 1000.0; - - if (y) - *y = (m_magY * m_magScale) / 1000.0; - - if (z) - *z = (m_magZ * m_magScale) / 1000.0; -} - -#ifdef JAVACALLBACK -float *LSM9DS0::getAccelerometer() -{ - float *v = new float[3]; - getAccelerometer(&v[0], &v[1], &v[2]); - return v; -} - -float *LSM9DS0::getGyroscope() -{ - float *v = new float[3]; - getGyroscope(&v[0], &v[1], &v[2]); - return v; -} - -float *LSM9DS0::getMagnetometer() -{ - float *v = new float[3]; - getMagnetometer(&v[0], &v[1], &v[2]); - return v; -} -#endif - -float LSM9DS0::getTemperature() -{ - // This might be wrong... The datasheet does not provide enough info - // to calculate the temperature given a specific sensor reading. So - // - with 12b resolution, signed, and 8 degrees/per LSB, we come up - // with the following. Then scale up and we get a number that seems - // pretty close. - return (((m_temp / 2048.0) * 8.0) * 100.0); -} - -bool LSM9DS0::enableTemperatureSensor(bool enable) -{ - uint8_t reg = readReg(DEV_XM, REG_CTRL_REG5_XM); - - if (enable) - reg |= CTRL_REG5_XM_TEMP_EN; - else - reg &= ~CTRL_REG5_XM_TEMP_EN; - - return writeReg(DEV_XM, REG_CTRL_REG5_XM, reg); -} - -uint8_t LSM9DS0::getGyroscopeStatus() -{ - return readReg(DEV_GYRO, REG_STATUS_REG_G); -} - -uint8_t LSM9DS0::getMagnetometerStatus() -{ - return readReg(DEV_XM, REG_STATUS_REG_M); -} - -uint8_t LSM9DS0::getAccelerometerStatus() -{ - return readReg(DEV_XM, REG_STATUS_REG_A); -} - -uint8_t LSM9DS0::getGyroscopeInterruptConfig() -{ - return readReg(DEV_GYRO, REG_INT1_CFG_G); -} - -bool LSM9DS0::setGyroscopeInterruptConfig(uint8_t enables) -{ - return writeReg(DEV_GYRO, REG_INT1_CFG_G, enables); -} - -uint8_t LSM9DS0::getGyroscopeInterruptSrc() -{ - return readReg(DEV_GYRO, REG_INT1_SRC_G); -} - -uint8_t LSM9DS0::getMagnetometerInterruptControl() -{ - return readReg(DEV_XM, REG_INT_CTRL_REG_M); -} - -bool LSM9DS0::setMagnetometerInterruptControl(uint8_t enables) -{ - return writeReg(DEV_XM, REG_INT_CTRL_REG_M, enables); -} - -uint8_t LSM9DS0::getMagnetometerInterruptSrc() -{ - return readReg(DEV_XM, REG_INT_SRC_REG_M); -} - -uint8_t LSM9DS0::getInterruptGen1() -{ - return readReg(DEV_XM, REG_INT_GEN_1_REG); -} - -bool LSM9DS0::setInterruptGen1(uint8_t enables) -{ - return writeReg(DEV_XM, REG_INT_GEN_1_REG, enables); -} - -uint8_t LSM9DS0::getInterruptGen1Src() -{ - return readReg(DEV_XM, REG_INT_GEN_1_SRC); -} - -uint8_t LSM9DS0::getInterruptGen2() -{ - return readReg(DEV_XM, REG_INT_GEN_2_REG); -} - -bool LSM9DS0::setInterruptGen2(uint8_t enables) -{ - return writeReg(DEV_XM, REG_INT_GEN_2_REG, enables); -} - -uint8_t LSM9DS0::getInterruptGen2Src() -{ - return readReg(DEV_XM, REG_INT_GEN_2_SRC); -} - -#if defined(SWIGJAVA) || defined (JAVACALLBACK) -void LSM9DS0::installISR(INTERRUPT_PINS_T intr, int gpio, mraa::Edge level, - jobject runnable) -{ - // delete any existing ISR and GPIO context - uninstallISR(intr); - - // greate gpio context - getPin(intr) = new mraa::Gpio(gpio); - - getPin(intr)->dir(mraa::DIR_IN); - getPin(intr)->isr(level, runnable); - -} -#else -void LSM9DS0::installISR(INTERRUPT_PINS_T intr, int gpio, mraa::Edge level, - void (*isr)(void *), void *arg) -{ - // delete any existing ISR and GPIO context - uninstallISR(intr); - - // greate gpio context - getPin(intr) = new mraa::Gpio(gpio); - - getPin(intr)->dir(mraa::DIR_IN); - getPin(intr)->isr(level, isr, arg); -} -#endif - -void LSM9DS0::uninstallISR(INTERRUPT_PINS_T intr) -{ - if (getPin(intr)) - { - getPin(intr)->isrExit(); - delete getPin(intr); - - getPin(intr) = 0; - } -} - -mraa::Gpio*& LSM9DS0::getPin(INTERRUPT_PINS_T intr) -{ - switch(intr) - { - case INTERRUPT_G_INT: - return m_gpioG_INT; - break; - case INTERRUPT_G_DRDY: - return m_gpioG_DRDY; - break; - case INTERRUPT_XM_GEN1: - return m_gpioXM_GEN1; - break; - case INTERRUPT_XM_GEN2: - return m_gpioXM_GEN2; - break; - default: - throw std::out_of_range(string(__FUNCTION__) + - ": Invalid interrupt enum passed"); - } -} |