alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-12-16 22:05:18 +03:00
Code Issues Actions Packages Projects Releases Wiki Activity

mpu9150: rewrite from scratch

Browse Source 
This driver has been rewritten from scratch.  It is implemented as 3
seperate drivers now (but all included as part of the mpu9150 UPM
library):

AK8975 (Magnetometer)
MPU60X0 (Accelerometer, Gyroscope, and Temperature sensor)
MPU9150 (composed of AK8975 and MPU60X0)

Each driver can be used independently and includes examples in
C++/JS/Python.

Commonly used capabilities are supported, and methods/register
definitions exist to easily implement any desired functionality that
is missing.  Interrupt support has also been added.

Scaling support has also been properly implemented for both the
Accelerometer and Gyroscope.

Signed-off-by: Jon Trulson <jtrulson@ics.com>
Signed-off-by: Mihai Tudor Panu <mihai.tudor.panu@intel.com>
This commit is contained in:
Jon Trulson
2015-07-16 16:56:23 -06:00
committed by Mihai Tudor Panu
parent 6613dea552
commit 03e72e02f8
19 changed files with 2590 additions and 390 deletions
Download Patch File Download Diff File Expand all files Collapse all files

246
src/mpu9150/mpu9150.cxx
View File

@@ -1,9 +1,6 @@
/*
* 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>
* 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
@@ -32,195 +29,74 @@
#include "mpu9150.h"
using namespace upm;
using namespace std;
MPU9150::MPU9150 (int bus, int devAddr) {
m_name = "MPU9150";
MPU9150::MPU9150 (int bus, int address, int magAddress) :
m_mag(0), MPU60X0(bus, address)
{
m_magAddress = magAddress;
m_i2cBus = bus;
}
m_i2cAddr = devAddr;
m_bus = bus;
MPU9150::~MPU9150()
{
if (m_mag)
delete m_mag;
}
m_i2Ctx = mraa_i2c_init(m_bus);
mraa_result_t ret = mraa_i2c_address(m_i2Ctx, m_i2cAddr);
if (ret != MRAA_SUCCESS) {
fprintf(stderr, "Messed up i2c bus\n");
bool MPU9150::init()
{
// init the gyro/accel component
if (!MPU60X0::init())
{
cerr << __FUNCTION__ << ": Unable to init MPU60X0" << endl;
return false;
}
initSensor ();
}
MPU9150::~MPU9150() {
mraa_i2c_stop(m_i2Ctx);
}
mraa_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 MRAA_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;
}
void
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;
// Now, we need to enable I2C bypass on the MPU60X0 component. This
// will allow us to access the AK8975 Magnetometer on I2C addr 0x0c.
if (!enableI2CBypass(true))
{
cerr << __FUNCTION__ << ": Unable to enable I2C bypass" << endl;
return false;
}
axisAcceleromter.data.axisX = axisAcceleromter.sumData.axisX / SMOOTH_TIMES;
axisAcceleromter.data.axisY = axisAcceleromter.sumData.axisY / SMOOTH_TIMES;
axisAcceleromter.data.axisZ = axisAcceleromter.sumData.axisZ / SMOOTH_TIMES;
// Now that we've done that, create an AK8975 instance and
// initialize it.
m_mag = new AK8975(m_i2cBus, m_magAddress);
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;
}
mraa_result_t
MPU9150::getAcceleromter (Vector3D * data) {
data->axisX = axisAcceleromter.data.axisX;
data->axisY = axisAcceleromter.data.axisY;
data->axisZ = axisAcceleromter.data.axisZ;
return MRAA_SUCCESS;
}
mraa_result_t
MPU9150::getGyro (Vector3D * data) {
data->axisX = axisGyroscope.data.axisX;
data->axisY = axisGyroscope.data.axisY;
data->axisZ = axisGyroscope.data.axisZ;
return MRAA_SUCCESS;
}
mraa_result_t
MPU9150::getMagnometer (Vector3D * data) {
data->axisX = axisMagnetomer.data.axisX;
data->axisY = axisMagnetomer.data.axisY;
data->axisZ = axisMagnetomer.data.axisZ;
return MRAA_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;
mraa_i2c_address(m_i2Ctx, m_i2cAddr);
mraa_i2c_write_byte(m_i2Ctx, reg);
mraa_i2c_address(m_i2Ctx, m_i2cAddr);
readByte = mraa_i2c_read(m_i2Ctx, buffer, len);
return readByte;
}
mraa_result_t
MPU9150::i2cWriteReg (uint8_t reg, uint8_t value) {
mraa_result_t error = MRAA_SUCCESS;
uint8_t data[2] = { reg, value };
error = mraa_i2c_address (m_i2Ctx, m_i2cAddr);
error = mraa_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;
if (!m_mag->init())
{
cerr << __FUNCTION__ << ": Unable to init magnetometer" << endl;
delete m_mag;
m_mag = 0;
return false;
}
return true;
}
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;
void MPU9150::update()
{
MPU60X0::update();
if (m_mag)
m_mag->update();
}
void MPU9150::getMagnetometer(float *x, float *y, float *z)
{
float mx, my, mz;
if (!m_mag)
mx = my = mz = 0.0;
else
m_mag->getMagnetometer(&mx, &my, &mz);
if (x)
*x = mx;
if (y)
*y = my;
if (z)
*z = mz;
}