alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-03-15 04:57:30 +03:00
Code Issues Actions Packages Projects Releases Wiki Activity

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

Browse Source 
Signed-off-by: Kiveisha Yevgeniy <yevgeniy.kiveisha@intel.com>
This commit is contained in:
Kiveisha Yevgeniy 2014-06-24 18:17:46 +00:00
parent 739d4e23e8
commit 8d25ecacdd
7 changed files with 506 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
examples/CMakeLists.txt
View File

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

59
examples/mpu9150-example.cxx Normal file
View File

@ -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;
}

5
src/mpu9150/CMakeLists.txt Normal file
View File

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

7
src/mpu9150/jsupm_mpu9150.i Normal file
View File

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

226
src/mpu9150/mpu9150.cxx Normal file
View File

@ -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;
}

196
src/mpu9150/mpu9150.h Normal file
View File

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

10
src/mpu9150/pyupm_mpu9150.i Normal file
View File

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