/* * 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