390 lines
7.5 KiB
C++
390 lines
7.5 KiB
C++
/*
|
|
* MMA8653 library
|
|
* (C) 2012 Akafugu Corporation
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify it under the
|
|
* terms of the GNU General Public License as published by the Free Software
|
|
* Foundation; either version 2 of the License, or (at your option) any later
|
|
* version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but WITHOUT ANY
|
|
* WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
|
|
* PARTICULAR PURPOSE. See the GNU General Public License for more details.
|
|
*
|
|
*/
|
|
|
|
|
|
#include "MMA8653.h"
|
|
|
|
|
|
MMA8653::MMA8653(uint8_t addr)
|
|
{
|
|
_addr = addr;
|
|
_rad2deg = 180.0 / M_PI;
|
|
}
|
|
|
|
|
|
//begin private methods
|
|
|
|
|
|
#define MMA_8653_CTRL_REG1 0x2A
|
|
#define MMA_8653_CTRL_REG1_VALUE_ACTIVE 0x01
|
|
#define MMA_8653_CTRL_REG1_VALUE_F_READ 0x02
|
|
|
|
|
|
#define MMA_8653_CTRL_REG2 0x2B
|
|
#define MMA_8653_CTRL_REG2_RESET 0x40
|
|
|
|
|
|
#define MMA_8653_PL_STATUS 0x10
|
|
#define MMA_8653_PL_CFG 0x11
|
|
#define MMA_8653_PL_EN 0x40
|
|
|
|
|
|
#define MMA_8653_XYZ_DATA_CFG 0x0E
|
|
#define MMA_8653_2G_MODE 0x00 //Set Sensitivity to 2g
|
|
#define MMA_8653_4G_MODE 0x01 //Set Sensitivity to 4g
|
|
#define MMA_8653_8G_MODE 0x02 //Set Sensitivity to 8g
|
|
|
|
|
|
#define MMA_8653_FF_MT_CFG 0x15
|
|
#define MMA_8653_FF_MT_CFG_ELE 0x80
|
|
#define MMA_8653_FF_MT_CFG_OAE 0x40
|
|
|
|
|
|
#define MMA_8653_FF_MT_SRC 0x16
|
|
#define MMA_8653_FF_MT_SRC_EA 0x80
|
|
|
|
|
|
#define MMA_8653_PULSE_CFG 0x21
|
|
#define MMA_8653_PULSE_CFG_ELE 0x80
|
|
|
|
|
|
#define MMA_8653_PULSE_SRC 0x22
|
|
#define MMA_8653_PULSE_SRC_EA 0x80
|
|
|
|
// Sample rate
|
|
#define MMA_8653_ODR_800 0x00
|
|
#define MMA_8653_ODR_400 0x08
|
|
#define MMA_8653_ODR_200 0x10
|
|
#define MMA_8653_ODR_100 0x18 // default ratio 100 samples per second
|
|
#define MMA_8653_ODR_50 0x20
|
|
#define MMA_8653_ODR_12_5 0x28
|
|
#define MMA_8653_ODR_6_25 0x30
|
|
#define MMA_8653_ODR_1_56 0x38
|
|
|
|
uint8_t MMA8653::_read_register(uint8_t offset)
|
|
{
|
|
Wire.beginTransmission(_addr);
|
|
Wire.write(offset);
|
|
Wire.endTransmission(false);
|
|
|
|
|
|
|
|
Wire.requestFrom(_addr, (uint8_t)1);
|
|
|
|
|
|
if (Wire.available()) return Wire.read();
|
|
return 0;
|
|
}
|
|
|
|
|
|
void MMA8653::_write_register(uint8_t b, uint8_t offset)
|
|
{
|
|
Wire.beginTransmission(_addr);
|
|
Wire.write(offset);
|
|
Wire.write(b);
|
|
Wire.endTransmission();
|
|
}
|
|
|
|
|
|
|
|
|
|
void MMA8653::_standby()
|
|
{
|
|
uint8_t reg1 = 0x00;
|
|
Wire.beginTransmission(_addr); // Set to status reg
|
|
Wire.write((uint8_t)MMA_8653_CTRL_REG1);
|
|
Wire.endTransmission();
|
|
|
|
Wire.requestFrom((uint8_t)_addr, (uint8_t)1);
|
|
if (Wire.available())
|
|
{
|
|
reg1 = Wire.read();
|
|
}
|
|
Wire.beginTransmission(_addr); // Reset
|
|
Wire.write((uint8_t)MMA_8653_CTRL_REG1);
|
|
Wire.write(reg1 & ~MMA_8653_CTRL_REG1_VALUE_ACTIVE);
|
|
Wire.endTransmission();
|
|
}
|
|
|
|
|
|
void MMA8653::_active()
|
|
{
|
|
uint8_t reg1 = 0x00;
|
|
Wire.beginTransmission(_addr); // Set to status reg
|
|
Wire.write((uint8_t)MMA_8653_CTRL_REG1);
|
|
Wire.endTransmission();
|
|
|
|
Wire.requestFrom((uint8_t)_addr, (uint8_t)1);
|
|
if (Wire.available())
|
|
{
|
|
reg1 = Wire.read();
|
|
}
|
|
Wire.beginTransmission(_addr); // Reset
|
|
Wire.write((uint8_t)MMA_8653_CTRL_REG1);
|
|
Wire.write(reg1 | MMA_8653_CTRL_REG1_VALUE_ACTIVE | (_highres ? 0 : MMA_8653_CTRL_REG1_VALUE_F_READ) | MMA_8653_ODR_100);
|
|
Wire.endTransmission();
|
|
}
|
|
|
|
|
|
float MMA8653::geta2d(float gx, float gy)
|
|
{
|
|
float a;
|
|
|
|
a = gx * gx;
|
|
a = fma(gy,gy,a);
|
|
|
|
return sqrt(a);
|
|
}
|
|
|
|
|
|
//gets the magnitude of the 3d vector
|
|
//the formula is a^2 = x^2 + y^2 + z^2
|
|
float MMA8653::geta3d(float gx, float gy, float gz)
|
|
{
|
|
float a;
|
|
|
|
//use floating point multiply-add cpu func
|
|
//sometimes we get better precision
|
|
a = gx * gx;
|
|
a = fma(gy,gy,a);
|
|
a = fma(gz,gz,a);
|
|
|
|
return sqrt(a);
|
|
}
|
|
|
|
|
|
float MMA8653::_getRho(float ax, float ay, float az)
|
|
{
|
|
return geta3d(_xg,_yg,_zg);
|
|
}
|
|
|
|
|
|
float MMA8653::_getPhi(float ax, float ay, float az)
|
|
{
|
|
return atan2(ay, ax) * _rad2deg;
|
|
}
|
|
|
|
|
|
float MMA8653::_getTheta(float ax, float ay, float az)
|
|
{
|
|
float rho = _getRho(ax, ay, az);
|
|
|
|
if (rho == 0.0)
|
|
return NAN;
|
|
else
|
|
return acos(az / rho) * _rad2deg;
|
|
}
|
|
|
|
|
|
//end private methods
|
|
|
|
|
|
//begin public methods
|
|
bool MMA8653::testConnection()
|
|
{
|
|
uint8_t wai = _read_register(0x0D); // Get Who Am I from the device.
|
|
// return value for MMA8653 is 0x5A
|
|
return wai == 0x5A;
|
|
}
|
|
|
|
|
|
void MMA8653::begin(bool highres, uint8_t scale)
|
|
{
|
|
_highres = highres;
|
|
|
|
_scale = scale;
|
|
_step_factor = (_highres ? 0.0039 : 0.0156); // Base value at 2g setting
|
|
if( _scale == 4 )
|
|
_step_factor *= 2;
|
|
else if (_scale == 8)
|
|
_step_factor *= 4;
|
|
uint8_t wai = _read_register(0x0D); // Get Who Am I from the device.
|
|
// return value for MMA8543Q is 0x3A
|
|
|
|
Wire.beginTransmission(_addr); // Reset
|
|
Wire.write(MMA_8653_CTRL_REG2);
|
|
Wire.write(MMA_8653_CTRL_REG2_RESET);
|
|
Wire.endTransmission();
|
|
delay(10); // Give it time to do the reset
|
|
_standby();
|
|
Wire.beginTransmission(_addr); // Set Portrait/Landscape mode
|
|
Wire.write(MMA_8653_PL_CFG);
|
|
Wire.write(0x80 | MMA_8653_PL_EN);
|
|
Wire.endTransmission();
|
|
Wire.beginTransmission(_addr);
|
|
Wire.write(MMA_8653_XYZ_DATA_CFG);
|
|
if (_scale == 4 || _scale == 8)
|
|
Wire.write((_scale == 4) ? MMA_8653_4G_MODE : MMA_8653_8G_MODE);
|
|
else // Default to 2g mode
|
|
Wire.write((uint8_t)MMA_8653_2G_MODE);
|
|
Wire.endTransmission();
|
|
_active();
|
|
}
|
|
|
|
|
|
uint8_t MMA8653::getPLStatus()
|
|
{
|
|
return _read_register(MMA_8653_PL_STATUS);
|
|
}
|
|
|
|
|
|
uint8_t MMA8653::getPulse()
|
|
{
|
|
_write_register(MMA_8653_PULSE_CFG, MMA_8653_PULSE_CFG_ELE);
|
|
return (_read_register(MMA_8653_PULSE_SRC) & MMA_8653_PULSE_SRC_EA);
|
|
}
|
|
|
|
|
|
float MMA8653::getXG()
|
|
{
|
|
return _xg;
|
|
}
|
|
|
|
|
|
float MMA8653::getYG()
|
|
{
|
|
return _yg;
|
|
}
|
|
|
|
|
|
float MMA8653::getZG()
|
|
{
|
|
return _zg;
|
|
}
|
|
|
|
int16_t MMA8653::getX()
|
|
{
|
|
return _x;
|
|
}
|
|
|
|
int16_t MMA8653::getY()
|
|
{
|
|
return _y;
|
|
}
|
|
|
|
int16_t MMA8653::getZ()
|
|
{
|
|
return _z;
|
|
}
|
|
|
|
float MMA8653::getRho()
|
|
{
|
|
return _getRho(_xg,_yg,_zg);
|
|
}
|
|
|
|
|
|
float MMA8653::getPhi()
|
|
{
|
|
return _getPhi(_xg,_yg,_zg);
|
|
}
|
|
|
|
|
|
float MMA8653::getTheta()
|
|
{
|
|
return _getTheta(_xg,_yg,_zg);
|
|
}
|
|
|
|
|
|
int16_t rx, ry, rz;
|
|
|
|
|
|
void MMA8653::update()
|
|
{
|
|
Wire.beginTransmission(_addr); // Set to status reg
|
|
Wire.write((uint8_t)0x00);
|
|
Wire.endTransmission(false);
|
|
|
|
Wire.requestFrom((uint8_t)_addr, (uint8_t)(_highres ? 7 : 4));
|
|
if (Wire.available())
|
|
{
|
|
_stat = Wire.read();
|
|
if(_highres)
|
|
{
|
|
//rx = (int16_t)((Wire.read() << 8) + Wire.read());
|
|
_x = (int16_t)((Wire.read() << 8) + Wire.read());
|
|
_xg = (_x / 64) * _step_factor;
|
|
//ry = (int16_t)((Wire.read() << 8) + Wire.read());
|
|
_y = (int16_t)((Wire.read() << 8) + Wire.read());
|
|
_yg = (_y / 64) * _step_factor;
|
|
//rz = (int16_t)((Wire.read() << 8) + Wire.read());
|
|
_z = (int16_t)((Wire.read() << 8) + Wire.read());
|
|
_zg = (_z / 64) * _step_factor;
|
|
}
|
|
else
|
|
{
|
|
/*
|
|
_xg = (int8_t)Wire.read() * _step_factor;
|
|
_yg = (int8_t)Wire.read() * _step_factor;
|
|
_zg = (int8_t)Wire.read() * _step_factor;
|
|
_x = 0;
|
|
_y = 0;
|
|
_z = 0;
|
|
*/
|
|
_x = (int16_t)(Wire.read() << 8);
|
|
_x = _x/256;
|
|
_y = (int16_t)(Wire.read() << 8);
|
|
_y = _y/256;
|
|
_z = (int16_t)(Wire.read() << 8);
|
|
_z = _z/256;
|
|
_xg = _x*_step_factor;
|
|
_yg = _y*_step_factor;
|
|
_zg = _z*_step_factor;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
bool MMA8653::setInterrupt(uint8_t type, uint8_t pin, bool on)
|
|
{
|
|
uint8_t current_value = _read_register(0x2D);
|
|
|
|
|
|
if(on)
|
|
current_value |= type;
|
|
else
|
|
current_value &= ~(type);
|
|
|
|
|
|
_write_register(0x2D, current_value);
|
|
|
|
|
|
uint8_t current_routing_value = _read_register(0x2E);
|
|
|
|
|
|
if (pin == 1) {
|
|
current_routing_value &= ~(type);
|
|
}
|
|
else if (pin == 2) {
|
|
current_routing_value |= type;
|
|
}
|
|
|
|
|
|
_write_register(0x2E, current_routing_value);
|
|
}
|
|
|
|
|
|
bool MMA8653::disableAllInterrupts()
|
|
{
|
|
_write_register(0x2D, 0);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//end public methods
|
|
|