initial commit
This commit is contained in:
@@ -0,0 +1,389 @@
|
||||
/*
|
||||
* 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
|
||||
|
||||
@@ -0,0 +1,100 @@
|
||||
/*
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
#ifndef MMA8653_H
|
||||
#define MMA8653_H
|
||||
|
||||
|
||||
#define MMA_8653_DEFAULT_ADDRESS 0x1D
|
||||
|
||||
|
||||
#include "Arduino.h"
|
||||
#include <Wire.h>
|
||||
|
||||
|
||||
////////////////////////////////////////////
|
||||
// Interrupts
|
||||
|
||||
|
||||
// Auto SLEEP/WAKE interrupt
|
||||
#define INT_ASLP (1<<7)
|
||||
// Transient interrupt
|
||||
#define INT_TRANS (1<<5)
|
||||
// Orientation (landscape/portrait) interrupt
|
||||
#define INT_LNDPRT (1<<4)
|
||||
// Pulse detection interrupt
|
||||
#define INT_PULSE (1<<3)
|
||||
// Freefall/Motion interrupt
|
||||
#define INT_FF_MT (1<<2)
|
||||
// Data ready interrupt
|
||||
#define INT_DRDY (1<<0)
|
||||
|
||||
|
||||
|
||||
|
||||
class MMA8653
|
||||
{
|
||||
public:
|
||||
MMA8653(uint8_t addr = MMA_8653_DEFAULT_ADDRESS);
|
||||
void begin(bool highres = true, uint8_t scale = 2);
|
||||
// Added and modified by kagayama@ArTec 2014/7/23
|
||||
// Original get[XYZ] (float) were converted to get[XYZ]G (float).
|
||||
// int16_t get[XYZ] were newly defined.
|
||||
float getXG();
|
||||
float getYG();
|
||||
float getZG();
|
||||
int16_t getX();
|
||||
int16_t getY();
|
||||
int16_t getZ();
|
||||
float getRho();
|
||||
float getPhi();
|
||||
float getTheta();
|
||||
void update();
|
||||
// Added by kagayama@ArTec 2014/7/23
|
||||
bool testConnection();
|
||||
|
||||
uint8_t getPLStatus();
|
||||
uint8_t getPulse();
|
||||
|
||||
// Interrupts
|
||||
bool setInterrupt(uint8_t type, uint8_t pin, bool on);
|
||||
bool disableAllInterrupts();
|
||||
private:
|
||||
uint8_t _read_register(uint8_t offset);
|
||||
void _write_register(uint8_t b, uint8_t offset);
|
||||
|
||||
float geta2d(float gx, float gy);
|
||||
float geta3d(float gx, float gy, float gz);
|
||||
float _getRho(float ax, float ay, float az);
|
||||
float _getPhi(float ax, float ay, float az);
|
||||
float _getTheta(float ax, float ay, float az);
|
||||
|
||||
void _standby();
|
||||
void _active();
|
||||
|
||||
uint8_t _addr;
|
||||
uint8_t _stat;
|
||||
uint8_t _scale;
|
||||
int16_t _x;
|
||||
int16_t _y;
|
||||
int16_t _z;
|
||||
float _step_factor;
|
||||
bool _highres;
|
||||
float _xg;
|
||||
float _yg;
|
||||
float _zg;
|
||||
float _rad2deg;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,53 @@
|
||||
/*
|
||||
* MMA845XQ test code
|
||||
* (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 "Wire.h"
|
||||
#include "MMA8653.h"
|
||||
|
||||
MMA8653 accel;
|
||||
|
||||
void setup() {
|
||||
Serial.begin(38400);
|
||||
Wire.begin();
|
||||
accel.begin(false, 2);
|
||||
}
|
||||
|
||||
|
||||
uint8_t rdata;
|
||||
uint8_t i;
|
||||
int x, x1, y, y1, z, z1;
|
||||
uint8_t stat;
|
||||
String stringOutput;
|
||||
|
||||
|
||||
void loop() {
|
||||
accel.update();
|
||||
Serial.println("X,\tY,\tZ,\tRho,\tPhi,\tTheta");
|
||||
Serial.print(accel.getX());
|
||||
Serial.print(" , ");
|
||||
Serial.print(accel.getY());
|
||||
Serial.print(", ");
|
||||
Serial.print(accel.getZ());
|
||||
Serial.print(", ");
|
||||
Serial.print(accel.getRho());
|
||||
Serial.print(", ");
|
||||
Serial.print(accel.getPhi());
|
||||
Serial.print(", ");
|
||||
Serial.print(accel.getTheta());
|
||||
Serial.println();
|
||||
delay(500);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,58 @@
|
||||
#######################################
|
||||
# Syntax Coloring Map DigitalAccelerometer_MMA8653FC
|
||||
#######################################
|
||||
|
||||
#######################################
|
||||
# Datatypes (KEYWORD1)
|
||||
#######################################
|
||||
|
||||
MMA8653 KEYWORD1
|
||||
|
||||
#######################################
|
||||
# Methods and Functions (KEYWORD2)
|
||||
#######################################
|
||||
begin KEYWORD2
|
||||
update KEYWORD2
|
||||
getXG KEYWORD2
|
||||
getYG KEYWORD2
|
||||
getZG KEYWORD2
|
||||
getX KEYWORD2
|
||||
getY KEYWORD2
|
||||
getZ KEYWORD2
|
||||
getRho KEYWORD2
|
||||
getPhi KEYWORD2
|
||||
getTheta KEYWORD2
|
||||
testConnection KEYWORD2
|
||||
|
||||
#######################################
|
||||
# Constants (LITERAL1)
|
||||
#######################################
|
||||
MMA_8653_CTRL_REG1 LITERAL1
|
||||
MMA_8653_CTRL_REG1_VALUE_ACTIVE LITERAL1
|
||||
MMA_8653_CTRL_REG1_VALUE_F_READ LITERAL1
|
||||
MMA_8653_CTRL_REG2 LITERAL1
|
||||
MMA_8653_CTRL_REG2_RESET LITERAL1
|
||||
MMA_8653_PL_STATUS LITERAL1
|
||||
MMA_8653_PL_CFG LITERAL1
|
||||
MMA_8653_PL_EN LITERAL1
|
||||
MMA_8653_XYZ_DATA_CFG LITERAL1
|
||||
MMA_8653_2G_MODE LITERAL1
|
||||
MMA_8653_4G_MODE LITERAL1
|
||||
MMA_8653_8G_MODE LITERAL1
|
||||
MMA_8653_FF_MT_CFG LITERAL1
|
||||
MMA_8653_FF_MT_CFG_ELE LITERAL1
|
||||
MMA_8653_FF_MT_CFG_OAE LITERAL1
|
||||
MMA_8653_FF_MT_SRC LITERAL1
|
||||
MMA_8653_FF_MT_SRC_EA LITERAL1
|
||||
MMA_8653_PULSE_CFG LITERAL1
|
||||
MMA_8653_PULSE_CFG_ELE LITERAL1
|
||||
MMA_8653_PULSE_SRC LITERAL1
|
||||
MMA_8653_PULSE_SRC_EA LITERAL1
|
||||
MMA_8653_ODR_800 LITERAL1
|
||||
MMA_8653_ODR_400 LITERAL1
|
||||
MMA_8653_ODR_200 LITERAL1
|
||||
MMA_8653_ODR_100 LITERAL1
|
||||
MMA_8653_ODR_50 LITERAL1
|
||||
MMA_8653_ODR_12_5 LITERAL1
|
||||
MMA_8653_ODR_6_25 LITERAL1
|
||||
MMA_8653_ODR_1_56 LITERAL1
|
||||
Reference in New Issue
Block a user