0.1.1 experimental version

GY521.cpp

@@ -0,0 +1,163 @@
+//
+//    FILE: GY521.cpp
+//  AUTHOR: Rob Tillaart
+// VERSION: 0.1.1
+// PURPOSE: Arduino library for I2C GY521 accelerometer-gyroscope sensor
+//     URL: https://github.com/RobTillaart/GY521
+//
+// HISTORY:
+// 0.1.0    2017-11-20 initial version
+// 0.1.1    2020-07-09 refactor + initial release
+
+#include "Wire.h"
+#include "GY521.h"
+
+// keep names in sync with BIG MPU6050 lib
+#define GY521_GYRO_CONFIG     0x1B
+#define GY521_ACCEL_CONFIG    0x1C
+
+#define GY521_ACCEL_XOUT_H    0x3B
+#define GY521_TEMP_OUT_H      0x41
+#define GY521_GYRO_XOUT_H     0x43
+
+#define GY521_PWR_MGMT_1      0x6B
+#define GY521_WAKEUP          0x00
+
+#define RAD2DEGREES           (180.0 / PI)
+
+
+/////////////////////////////////////////////////////
+//
+// PUBLIC
+//
+GY521::GY521(uint8_t address)
+{
+  _address = 0x69;
+  if (address == 0x68) _address = 0x68;
+}
+
+bool GY521::wakeup()
+{
+  Wire.beginTransmission(_address);
+  Wire.write(GY521_PWR_MGMT_1);
+  Wire.write(GY521_WAKEUP);
+  return Wire.endTransmission() == 0;
+}
+
+
+int GY521::read()
+{
+  if (_throttle)
+  {
+    if ((millis() - _lastTime) < GY521_THROTTLE_TIME) return GY521_THROTTLED;
+  }
+
+  // Connected ?
+  Wire.beginTransmission(_address);
+  Wire.write(GY521_ACCEL_XOUT_H);
+  if (Wire.endTransmission() != 0) return GY521_ERROR_READ;
+
+  // Get the data
+  Wire.requestFrom(_address, (uint8_t)14);
+  // ACCELEROMETER
+  _ax = ( ( ((int)Wire.read()) << 8) | Wire.read() );  // ACCEL_XOUT_H  ACCEL_XOUT_L
+  _ay = ( ( ((int)Wire.read()) << 8) | Wire.read() );  // ACCEL_YOUT_H  ACCEL_YOUT_L
+  _az = ( ( ((int)Wire.read()) << 8) | Wire.read() );  // ACCEL_ZOUT_H  ACCEL_ZOUT_L
+  // TEMPERATURE
+  _temperature = ( ((int)Wire.read()) << 8) | Wire.read();  // TEMP_OUT_H    TEMP_OUT_L
+  // GYROSCOPE
+  _gx = (( ((int)Wire.read()) << 8) | Wire.read());  // GYRO_XOUT_H   GYRO_XOUT_L
+  _gy = (( ((int)Wire.read()) << 8) | Wire.read());  // GYRO_YOUT_H   GYRO_YOUT_L
+  _gz = (( ((int)Wire.read()) << 8) | Wire.read());  // GYRO_ZOUT_H   GYRO_ZOUT_L
+
+  // time interval
+  uint32_t now = millis();
+  float duration = (now - _lastTime) * 0.001;   // time in seconds.
+  _lastTime = now;
+
+  // Convert raw acceleration to g's
+  _ax *= _raw2g;
+  _ay *= _raw2g;
+  _az *= _raw2g;
+
+  // prepare for Pitch Roll Yaw
+  _aax = atan(_ay / hypot(_ax, _az)) * RAD2DEGREES;
+  _aay = atan(-1.0 * _ax / hypot(_ay, _az)) * RAD2DEGREES;
+  _aaz = atan(_az / hypot(_ax, _ay)) * RAD2DEGREES;
+
+  // Error correct the angles
+  _aax += axe;
+  _aay += aye;
+  _aaz += aze;
+
+  // Convert to Celsius
+  _temperature = (_temperature + 12412.0) * 0.00294117647;  //  == /340.0  + 36.53;
+
+  // Convert raw Gyro to degrees/seconds
+  _gx *= _raw2dps;
+  _gy *= _raw2dps;
+  _gz *= _raw2dps;
+
+  // Error correct raw gyro measurements.
+  _gx += gxe;
+  _gy += gye;
+  _gz += gze;
+
+  _gax += _gx * duration;
+  _gay += _gy * duration;
+  _gaz += _gz * duration;
+
+  _yaw = _gaz;
+  _pitch = 0.96 * _gay + 0.04 * _aay;
+  _roll = 0.96 * _gax + 0.04 * _aax;
+
+  return GY521_OK;
+}
+
+void GY521::setAccelSensitivity(uint8_t as)
+{
+  if (as > 3) as = 3;
+  uint8_t val = getRegister(GY521_ACCEL_CONFIG);
+  val &= 0xE7;
+  val |= (as << 3);
+  setRegister(GY521_ACCEL_CONFIG, val);
+  // calculate conversion factor.
+  _raw2g = 16384.0;
+  for (uint8_t i = 0; i < _afs; i++) _raw2g *= 0.5;
+  _raw2g = 1.0 / _raw2g;
+}
+
+void  GY521::setGyroSensitivity(uint8_t gs)
+{
+  _gfs = gs;
+  if (_gfs > 3) _gfs = 3;
+  uint8_t val = getRegister(GY521_GYRO_CONFIG);
+  val &= 0xE7;
+  val |= (_gfs << 3);
+  setRegister(GY521_GYRO_CONFIG, val);
+    _raw2dps = 131.0;
+  for (uint8_t i = 0; i < _gfs; i++) _raw2dps *= 0.5;
+  _raw2dps = 1.0 / _raw2dps;
+}
+
+uint8_t GY521::setRegister(uint8_t reg, uint8_t value)
+{
+  Wire.beginTransmission(_address);
+  Wire.write(reg);
+  Wire.write(value);
+  // no need to do anything if not connected.
+  if (Wire.endTransmission() != 0) return GY521_ERROR_WRITE;
+  return GY521_OK;
+}
+
+uint8_t GY521::getRegister(uint8_t reg)
+{
+  Wire.beginTransmission(_address);
+  Wire.write(reg);
+  if (Wire.endTransmission() != 0) return GY521_ERROR_WRITE;
+  Wire.requestFrom(_address, (uint8_t) 1);
+  uint8_t val = Wire.read();
+  return val;
+}
+
+// END OF FILE

GY521.h

@@ -0,0 +1,91 @@
+#pragma once
+//
+//    FILE: GY521.h
+//  AUTHOR: Rob Tillaart
+// VERSION: 0.1.1
+// PURPOSE: Arduino library for I2C GY521 accelerometer-gyroscope sensor
+//     URL: https://github.com/RobTillaart/GY521
+//
+// HISTORY:
+// see GY521.cpp file
+//
+
+#include "Arduino.h"
+#include "Wire.h"
+
+#define GY521_LIB_VERSION (F("0.1.1"))
+
+#ifndef GY521_THROTTLE_TIME
+#define GY521_THROTTLE_TIME   10   // milliseconds
+#endif
+
+// ERRORCODES getRaw()
+#define GY521_OK                0
+#define GY521_THROTTLED         1
+#define GY521_ERROR_READ       -1
+#define GY521_ERROR_WRITE      -2
+
+
+class GY521
+{
+public:
+    GY521(uint8_t address = 0x69); // 0x68 or 0x69
+
+    bool wakeup();
+    // throttle to force delay between reads.
+    void setThrottle(bool throttle = true) { _throttle = throttle; };
+    bool getThrottle() { return _throttle; };
+    int read();
+
+    // as = 0,1,2,3 ==> 2g 4g 8g 16g
+    void  setAccelSensitivity(uint8_t as);
+    float getAccelX()   { return _ax; };
+    float getAccelY()   { return _ay; };
+    float getAccelZ()   { return _az; };
+    float getAngleX()   { return _aax; };
+    float getAngleY()   { return _aay; };
+    float getAngleZ()   { return _aaz; };
+
+    float getTemperature() { return _temperature; };
+
+    // gs = 0,1,2,3  ==>  250, 500, 1000, 2000 degrees/second
+    void  setGyroSensitivity(uint8_t gs);
+    float getGyroX()    { return _gx; };
+    float getGyroY()    { return _gy; };
+    float getGyroZ()    { return _gz; };
+
+    float getPitch()    { return _pitch; };
+    float getRoll()     { return _roll; };
+    float getYaw()      { return _yaw; };
+
+    uint32_t lastTime() { return _lastTime; };
+
+    // generic worker to get access to all fucntionality
+    uint8_t setRegister(uint8_t reg, uint8_t value);
+    uint8_t getRegister(uint8_t reg);
+
+    // callibration errors
+    float    axe = 0, aye = 0, aze = 0;  // accelerometer errors
+    float    gxe = 0, gye = 0, gze = 0;  // gyro errors
+
+private:
+    uint8_t  _address;                // I2C address
+    bool     _throttle = true;        // to prevent reading too fast
+    uint32_t _lastTime = 0;           // to measure duration for math & throttle
+
+    uint8_t  _afs = 0;                // sensitivity factor
+    float    _raw2g = 1/16384.0;      // raw data to gravity g's
+    float    _ax, _ay, _az;           // accelerometer raw
+    float    _aax, _aay, _aaz;        // accelerometer processed
+
+    uint8_t  _gfs = 0;
+    float    _raw2dps = 1/131;
+    float    _gx, _gy, _gz;           // gyro raw
+    float    _gax, _gay, _gaz;        // gyro processed.
+    float    _pitch, _roll, _yaw;     // used by user
+
+    float    _temperature = 0;
+};
+
+
+// END OF FILE

LICENSE

@@ -1,6 +1,6 @@
 MIT License
 
-Copyright (c) 2020 Rob Tillaart
+Copyright (c) 2017-2020 Rob Tillaart
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal

README.md

@@ -1,2 +1,41 @@
 # GY521
-Arduino library for GY521  accelerometer- gyroscope a.k.a. MCU-6050
+
+Arduino library for I2C GY521 accelerometer-gyroscope sensor a.k.a. MCU-6050
+
+## Description
+
+Experimental library for GY521 a.k.a. MCU-6050
+
+Library is work in progress, in fact extracted and extended from an old project.
+It needs to be tested a lot more.
+
+It has two examples
+- calibration example to determine the offsets needed
+- example to read values.
+
+## Calibration (short version for now)
+
+1. load and run calibration example  
+   it shows a header containing 6 numbers and 10 lines og 8 numbers
+1. wait until the middle 6 of the longer lines stabilize (should be all 0)
+1. copy the 6 numbers above the axe aye aze as these are the numbers needed.
+
+
+## Future
+
+- test test and test ...(ESP too)
+- pitch roll yaw example
+- better documentation
+- look for math optimizations  (atan/hypot)
+- calibrate function in the lib ?
+- ESP wire support (other pins)
+- ...
+
+## documents
+
+- check details - MPU-6000-Register-Map1.pdf
+
+## Operation
+
+See examples, use with care 
+