mpu9150: added new sensor (acceleromter, gyro and compass)

Signed-off-by: Kiveisha Yevgeniy <yevgeniy.kiveisha@intel.com>

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})

examples/mpu9150-example.cxx

@@ -0,0 +1,59 @@
+/*
+ * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com>
+ * 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 <unistd.h>
+#include <iostream>
+#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;
+}

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()

src/mpu9150/jsupm_mpu9150.i

@@ -0,0 +1,7 @@
+%module jsupm_mpu9150
+
+%{
+    #include "mpu9150.h"
+%}
+
+%include "mpu9150.h"

src/mpu9150/mpu9150.cxx

@@ -0,0 +1,226 @@
+/*
+ * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com>
+ * 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 <jeff@rowberg.net>
+ *
+ * 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 <iostream>
+#include <unistd.h>
+#include <stdlib.h>
+
+#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, &regData);
+    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, &regData);
+    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, &regData) != 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, &regData) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        regData &= mask;
+        regData >>= (bitStart - length + 1);
+        *data = regData;
+    }
+    return count;
+}

src/mpu9150/mpu9150.h

@@ -0,0 +1,196 @@
+/*
+ * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com>
+ * 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 <jeff@rowberg.net>
+ *
+ * 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 <string>
+#include <maa/i2c.h>
+
+#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);
+};
+
+}

src/mpu9150/pyupm_mpu9150.i

@@ -0,0 +1,10 @@
+%module pyupm_mpu9150
+
+%include "stdint.i"
+
+%feature("autodoc", "3");
+
+%include "mpu9150.h"
+%{
+    #include "mpu9150.h"
+%}