initial commit

2026-09-25 10:48:51 +10:00
commit b90beea8ae
1020 changed files with 131665 additions and 0 deletions
@@ -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
+