From: Kiveisha Yevgeniy Date: Tue, 24 Jun 2014 18:17:46 +0000 (+0000) Subject: mpu9150: added new sensor (acceleromter, gyro and compass) X-Git-Tag: v0.1.5~2 X-Git-Url: http://review.tizen.org/git/?p=contrib%2Fupm.git;a=commitdiff_plain;h=8d25ecacdd0a9c7b2fcc98ceaba81d64461d10df mpu9150: added new sensor (acceleromter, gyro and compass) Signed-off-by: Kiveisha Yevgeniy --- diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 85e2c29..5813d0f 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -20,6 +20,7 @@ add_executable (gy65-example gy65.cxx) add_executable (stepmotor-example stepmotor.cxx) add_executable (pulsensor-example pulsensor.cxx) add_executable (mic-example mic-example.cxx) +add_executable (mpu9150-example mpu9150-example.cxx) include_directories (${PROJECT_SOURCE_DIR}/src/hmc5883l) include_directories (${PROJECT_SOURCE_DIR}/src/grove) @@ -38,6 +39,7 @@ include_directories (${PROJECT_SOURCE_DIR}/src/gy65) include_directories (${PROJECT_SOURCE_DIR}/src/stepmotor) include_directories (${PROJECT_SOURCE_DIR}/src/pulsensor) include_directories (${PROJECT_SOURCE_DIR}/src/mic) +include_directories (${PROJECT_SOURCE_DIR}/src/mpu9150) target_link_libraries (compass hmc5883l ${CMAKE_THREAD_LIBS_INIT}) target_link_libraries (groveled grove ${CMAKE_THREAD_LIBS_INIT}) @@ -61,3 +63,4 @@ target_link_libraries (gy65-example gy65 ${CMAKE_THREAD_LIBS_INIT}) target_link_libraries (stepmotor-example stepmotor ${CMAKE_THREAD_LIBS_INIT}) target_link_libraries (pulsensor-example pulsensor ${CMAKE_THREAD_LIBS_INIT}) target_link_libraries (mic-example mic ${CMAKE_THREAD_LIBS_INIT}) +target_link_libraries (mpu9150-example mpu9150 ${CMAKE_THREAD_LIBS_INIT}) diff --git a/examples/mpu9150-example.cxx b/examples/mpu9150-example.cxx new file mode 100644 index 0000000..84c8e33 --- /dev/null +++ b/examples/mpu9150-example.cxx @@ -0,0 +1,59 @@ +/* + * Author: Yevgeniy Kiveisha + * Copyright (c) 2014 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 +#include +#include "mpu9150.h" + +int +main(int argc, char **argv) +{ + //! [Interesting] + upm::Vector3D data; + upm::MPU9150 *sensor = new upm::MPU9150(0, ADDR); + sensor->getData (); + sensor->getAcceleromter (&data); + std::cout << "*************************************************" << std::endl; + std::cout << "DEVICE ID (" << (int) sensor->getDeviceID () << ")" << std::endl; + std::cout << "*************************************************" << std::endl; + std::cout << "ACCELEROMETER :: X (" << data.axisX << ")" << " Y (" << data.axisY << ")" + << " Z (" << data.axisZ << ")" << std::endl; + + sensor->getGyro (&data); + std::cout << "GYRO :: X (" << data.axisX << ")" << " Y (" << data.axisY << ")" + << " Z (" << data.axisZ << ")" << std::endl; + + sensor->getMagnometer (&data); + std::cout << "MAGNOMETER :: X (" << data.axisX << ")" << " Y (" << data.axisY << ")" + << " Z (" << data.axisZ << ")" << std::endl; + std::cout << "TEMPERATURE (" << sensor->getTemperature () << ")" << std::endl; + std::cout << "*************************************************" << std::endl; + //! [Interesting] + + std::cout << "exiting application" << std::endl; + + delete sensor; + + return 0; +} diff --git a/src/mpu9150/CMakeLists.txt b/src/mpu9150/CMakeLists.txt new file mode 100644 index 0000000..d42f978 --- /dev/null +++ b/src/mpu9150/CMakeLists.txt @@ -0,0 +1,5 @@ +set (libname "mpu9150") +set (libdescription "giro, acceleromter and magnometer sensor based on mpu9150") +set (module_src ${libname}.cxx) +set (module_h ${libname}.h) +upm_module_init() diff --git a/src/mpu9150/jsupm_mpu9150.i b/src/mpu9150/jsupm_mpu9150.i new file mode 100644 index 0000000..3c19e2b --- /dev/null +++ b/src/mpu9150/jsupm_mpu9150.i @@ -0,0 +1,7 @@ +%module jsupm_mpu9150 + +%{ + #include "mpu9150.h" +%} + +%include "mpu9150.h" diff --git a/src/mpu9150/mpu9150.cxx b/src/mpu9150/mpu9150.cxx new file mode 100644 index 0000000..ebb1907 --- /dev/null +++ b/src/mpu9150/mpu9150.cxx @@ -0,0 +1,226 @@ +/* + * Author: Yevgeniy Kiveisha + * Copyright (c) 2014 Intel Corporation. + * + * Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) + * 8/24/2011 by Jeff Rowberg + * + * 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 +#include +#include + +#include "mpu9150.h" + +using namespace upm; + +MPU9150::MPU9150 (int bus, int devAddr) { + m_name = "MPU9150"; + + m_i2cAddr = devAddr; + m_bus = bus; + + m_i2Ctx = maa_i2c_init(m_bus); + + maa_result_t ret = maa_i2c_address(m_i2Ctx, m_i2cAddr); + if (ret != MAA_SUCCESS) { + fprintf(stderr, "Messed up i2c bus\n"); + } + + initSensor (); +} + +MPU9150::~MPU9150() { + maa_i2c_stop(m_i2Ctx); +} + +maa_result_t +MPU9150::initSensor () { + uint8_t regData = 0x0; + + // setClockSource + updateRegBits ( MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, + MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO); + // setFullScaleGyroRange + updateRegBits ( MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, + MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250); + // setFullScaleAccelRange + updateRegBits ( MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, + MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2); + // setSleepEnabled + i2cReadReg_N (MPU6050_RA_PWR_MGMT_1, 0x1, ®Data); + regData &= ~(1 << MPU6050_PWR1_SLEEP_BIT); + i2cWriteReg (MPU6050_RA_PWR_MGMT_1, regData); + + return MAA_SUCCESS; +} + +uint8_t +MPU9150::getDeviceID () { + uint8_t regData = 0x0; + getRegBits (MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, ®Data); + return regData; +} + +maa_result_t +MPU9150::getData () { + uint8_t buffer[14]; + + for (int i = 0; i < SMOOTH_TIMES; i++) { + i2cReadReg_N (MPU6050_RA_ACCEL_XOUT_H, 14, buffer); + axisAcceleromter.rawData.axisX = (((int16_t)buffer[0]) << 8) | buffer[1]; + axisAcceleromter.rawData.axisY = (((int16_t)buffer[2]) << 8) | buffer[3]; + axisAcceleromter.rawData.axisZ = (((int16_t)buffer[4]) << 8) | buffer[5]; + axisAcceleromter.sumData.axisX += (double) axisAcceleromter.rawData.axisX / 16384; + axisAcceleromter.sumData.axisY += (double) axisAcceleromter.rawData.axisY / 16384; + axisAcceleromter.sumData.axisZ += (double) axisAcceleromter.rawData.axisZ / 16384; + + axisGyroscope.rawData.axisX = (((int16_t)buffer[8]) << 8) | buffer[9]; + axisGyroscope.rawData.axisY = (((int16_t)buffer[10]) << 8) | buffer[11]; + axisGyroscope.rawData.axisZ = (((int16_t)buffer[12]) << 8) | buffer[13]; + axisGyroscope.sumData.axisX += (double) axisAcceleromter.rawData.axisX * 250 / 32768; + axisGyroscope.sumData.axisY += (double) axisAcceleromter.rawData.axisY * 250 / 32768; + axisGyroscope.sumData.axisZ += (double) axisAcceleromter.rawData.axisZ * 250 / 32768; + + i2cWriteReg (MPU6050_RA_INT_PIN_CFG, 0x02); + usleep (10000); + m_i2cAddr = MPU9150_RA_MAG_ADDRESS; + i2cWriteReg (0x0A, 0x01); + usleep (10000); + i2cReadReg_N (MPU9150_RA_MAG_XOUT_L, 6, buffer); + m_i2cAddr = ADDR; + + axisMagnetomer.rawData.axisX = (((int16_t)buffer[0]) << 8) | buffer[1]; + axisMagnetomer.rawData.axisY = (((int16_t)buffer[2]) << 8) | buffer[3]; + axisMagnetomer.rawData.axisZ = (((int16_t)buffer[4]) << 8) | buffer[5]; + axisMagnetomer.sumData.axisX += (double) axisMagnetomer.rawData.axisX * 1200 / 4096; + axisMagnetomer.sumData.axisY += (double) axisMagnetomer.rawData.axisY * 1200 / 4096; + axisMagnetomer.sumData.axisZ += (double) axisMagnetomer.rawData.axisZ * 1200 / 4096; + } + + axisAcceleromter.data.axisX = axisAcceleromter.sumData.axisX / SMOOTH_TIMES; + axisAcceleromter.data.axisY = axisAcceleromter.sumData.axisY / SMOOTH_TIMES; + axisAcceleromter.data.axisZ = axisAcceleromter.sumData.axisZ / SMOOTH_TIMES; + + axisGyroscope.data.axisX = axisGyroscope.sumData.axisX / SMOOTH_TIMES; + axisGyroscope.data.axisY = axisGyroscope.sumData.axisY / SMOOTH_TIMES; + axisGyroscope.data.axisZ = axisGyroscope.sumData.axisZ / SMOOTH_TIMES; + + axisMagnetomer.data.axisX = axisMagnetomer.sumData.axisX / SMOOTH_TIMES; + axisMagnetomer.data.axisY = axisMagnetomer.sumData.axisY / SMOOTH_TIMES; + axisMagnetomer.data.axisZ = axisMagnetomer.sumData.axisZ / SMOOTH_TIMES; +} + +maa_result_t +MPU9150::getAcceleromter (Vector3D * data) { + data->axisX = axisAcceleromter.data.axisX; + data->axisY = axisAcceleromter.data.axisY; + data->axisZ = axisAcceleromter.data.axisZ; + + return MAA_SUCCESS; +} + +maa_result_t +MPU9150::getGyro (Vector3D * data) { + data->axisX = axisGyroscope.data.axisX; + data->axisY = axisGyroscope.data.axisY; + data->axisZ = axisGyroscope.data.axisZ; + + return MAA_SUCCESS; +} + +maa_result_t +MPU9150::getMagnometer (Vector3D * data) { + data->axisX = axisMagnetomer.data.axisX; + data->axisY = axisMagnetomer.data.axisY; + data->axisZ = axisMagnetomer.data.axisZ; + + return MAA_SUCCESS; +} + +float +MPU9150::getTemperature () { + uint8_t buffer[2]; + uint16_t tempRaw = 0; + + updateRegBits (MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, 0x1, 0x0); + i2cReadReg_N (MPU6050_RA_TEMP_OUT_H, 2, buffer); + tempRaw = (((int16_t)buffer[0]) << 8) | buffer[1]; + + return (float)tempRaw / 340.0 + 35.0; +} + +/* + * ************** + * private area + * ************** + */ +uint16_t +MPU9150::i2cReadReg_N (int reg, unsigned int len, uint8_t * buffer) { + int readByte = 0; + maa_i2c_address(m_i2Ctx, m_i2cAddr); + maa_i2c_write_byte(m_i2Ctx, reg); + + maa_i2c_address(m_i2Ctx, m_i2cAddr); + readByte = maa_i2c_read(m_i2Ctx, buffer, len); + return readByte; +} + +maa_result_t +MPU9150::i2cWriteReg (uint8_t reg, uint8_t value) { + maa_result_t error = MAA_SUCCESS; + + uint8_t data[2] = { reg, value }; + error = maa_i2c_address (m_i2Ctx, m_i2cAddr); + error = maa_i2c_write (m_i2Ctx, data, 2); + + return error; +} + +int +MPU9150::updateRegBits (uint8_t reg, uint8_t bitStart, uint8_t length, uint16_t data) { + uint8_t regData; + + if (i2cReadReg_N (reg, 0x1, ®Data) != 0) { + uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); + data <<= (bitStart - length + 1); // shift data into correct position + data &= mask; // zero all non-important bits in data + regData &= ~(mask); // zero all important bits in existing byte + regData |= data; // combine data with existing byte + return i2cWriteReg (reg, regData); + } else { + return 0x0; + } +} + +uint8_t +MPU9150::getRegBits (uint8_t reg, uint8_t bitStart, uint8_t length, uint8_t * data) { + uint8_t count = 0; + uint8_t regData; + if (i2cReadReg_N (reg, 0x1, ®Data) != 0) { + uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); + regData &= mask; + regData >>= (bitStart - length + 1); + *data = regData; + } + return count; +} diff --git a/src/mpu9150/mpu9150.h b/src/mpu9150/mpu9150.h new file mode 100644 index 0000000..9f5c77f --- /dev/null +++ b/src/mpu9150/mpu9150.h @@ -0,0 +1,196 @@ +/* + * Author: Yevgeniy Kiveisha + * Copyright (c) 2014 Intel Corporation. + * + * Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) + * 8/24/2011 by Jeff Rowberg + * + * 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. + */ +#pragma once + +#include +#include + +#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board +#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) +#define ADDR MPU6050_ADDRESS_AD0_LOW // device address + +// registers address +#define MPU6050_CLOCK_PLL_XGYRO 0x01 +#define MPU6050_GYRO_FS_250 0x00 +#define MPU6050_ACCEL_FS_2 0x00 +#define MPU6050_RA_INT_PIN_CFG 0x37 + +#define MPU6050_RA_ACCEL_XOUT_H 0x3B +#define MPU6050_RA_ACCEL_XOUT_L 0x3C +#define MPU6050_RA_ACCEL_YOUT_H 0x3D +#define MPU6050_RA_ACCEL_YOUT_L 0x3E +#define MPU6050_RA_ACCEL_ZOUT_H 0x3F +#define MPU6050_RA_ACCEL_ZOUT_L 0x40 +#define MPU6050_RA_TEMP_OUT_H 0x41 +#define MPU6050_RA_TEMP_OUT_L 0x42 +#define MPU6050_RA_GYRO_XOUT_H 0x43 +#define MPU6050_RA_GYRO_XOUT_L 0x44 +#define MPU6050_RA_GYRO_YOUT_H 0x45 +#define MPU6050_RA_GYRO_YOUT_L 0x46 +#define MPU6050_RA_GYRO_ZOUT_H 0x47 +#define MPU6050_RA_GYRO_ZOUT_L 0x48 + +#define MPU6050_RA_CONFIG 0x1A +#define MPU6050_CFG_DLPF_CFG_BIT 2 +#define MPU6050_CFG_DLPF_CFG_LENGTH 3 + +#define MPU6050_RA_GYRO_CONFIG 0x1B +#define MPU6050_GCONFIG_FS_SEL_BIT 4 +#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 + +#define MPU6050_RA_ACCEL_CONFIG 0x1C +#define MPU6050_ACONFIG_AFS_SEL_BIT 4 +#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 + +// magnotometer +#define MPU9150_RA_MAG_ADDRESS 0x0C +#define MPU9150_RA_MAG_XOUT_L 0x03 + +#define MPU6050_RA_PWR_MGMT_1 0x6B +#define MPU6050_PWR1_CLKSEL_BIT 2 +#define MPU6050_PWR1_CLKSEL_LENGTH 3 +#define MPU6050_PWR1_SLEEP_BIT 6 + +#define MPU6050_RA_INT_PIN_CFG 0x37 + +// temperature +#define MPU6050_PWR1_TEMP_DIS_BIT 3 +#define MPU6050_RA_WHO_AM_I 0x75 +#define MPU6050_WHO_AM_I_BIT 6 +#define MPU6050_WHO_AM_I_LENGTH 6 + +#define SMOOTH_TIMES 10.0 + +#define HIGH 1 +#define LOW 0 + +namespace upm { + +struct Vector3DRaw { + uint16_t axisX; + uint16_t axisY; + uint16_t axisZ; +}; + +struct Vector3D { + double axisX; + double axisY; + double axisZ; +}; + +struct AxisData { + Vector3DRaw rawData; + Vector3D sumData; + Vector3D data; +}; + +/** + * @brief C++ API for MPU9150 chip (Accelrometer, Gyro and Magnometer Sensor) + * + * This file defines the MPU9150 C++ interface for libmpu9150 + * + * @snippet mpu9150-example.cxx Interesting + */ +class MPU9150 { + public: + /** + * Instanciates a MPU9150 object + * + * @param bus number of used bus + * @param devAddr addres of used i2c device + */ + MPU9150 (int bus, int devAddr); + + /** + * MPU9150 object destructor, basicaly it close i2c connection. + */ + ~MPU9150 (); + + /** + * Initiate MPU9150 chips + */ + maa_result_t initSensor (); + + /** + * Get identity of the device + */ + uint8_t getDeviceID (); + + /** + * Get the Accelerometer, Gyro and Compass data from the chip and + * save it in private section. + */ + maa_result_t getData (); + + /** + * @param data structure with 3 axis (x,y,z) + */ + maa_result_t getAcceleromter (Vector3D * data); + + /** + * @param data structure with 3 axis (x,y,z) + */ + maa_result_t getGyro (Vector3D * data); + + /** + * @param data structure with 3 axis (x,y,z) + */ + maa_result_t getMagnometer (Vector3D * data); + + /** + * Read on die temperature from the chip + */ + float getTemperature (); + + /** + * Return name of the component + */ + std::string name() + { + return m_name; + } + + private: + std::string m_name; + + int m_i2cAddr; + int m_bus; + maa_i2c_context m_i2Ctx; + + AxisData axisMagnetomer; + AxisData axisAcceleromter; + AxisData axisGyroscope; + + uint16_t i2cReadReg_N (int reg, unsigned int len, uint8_t * buffer); + maa_result_t i2cWriteReg (uint8_t reg, uint8_t value); + int updateRegBits (uint8_t reg, uint8_t bitStart, + uint8_t length, uint16_t data); + uint8_t getRegBits (uint8_t reg, uint8_t bitStart, + uint8_t length, uint8_t * data); +}; + +} diff --git a/src/mpu9150/pyupm_mpu9150.i b/src/mpu9150/pyupm_mpu9150.i new file mode 100644 index 0000000..71c8afe --- /dev/null +++ b/src/mpu9150/pyupm_mpu9150.i @@ -0,0 +1,10 @@ +%module pyupm_mpu9150 + +%include "stdint.i" + +%feature("autodoc", "3"); + +%include "mpu9150.h" +%{ + #include "mpu9150.h" +%}