robtillaart / gy521

Arduino library for GY521 accelerometer- gyroscope a.k.a. MCU-6050

GY521

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 it is extracted and extended from an old project. It needs to be tested a lot more.

See changelog.md for latest updates.

0.5.0 Breaking change

Version 0.5.0 introduced a breaking change. You cannot set the pins in begin() any more. This reduces the dependency of processor dependent Wire implementations. The user has to call Wire.begin() and can optionally set the Wire pins before calling begin().

Examples

• GY521_angle read angleX, angleY, angleZ.
• GY521_performance_calibrate.ino determine calibration performance.
• GY521_performance measure performance.
• GY521_pitch_roll_yaw to get pitch roll yaw.
• GY521_raw_cooked make a table of raw measurements and derived data for analysis e.g. in a spreadsheet.
• GY521_readCalibration_1 read calibration values / errors for a "flat" sensor.
• GY521_readCalibration_2 generate calibration code snippet.
• GY521_test_1 test working of the sensor.
• GY521_test_2 test set/get functions (sort of unit test).
• GY521_two_sensors demo for two sensors.

Related

• https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf
• https://cdn.sparkfun.com/datasheets/Sensors/Accelerometers/RM-MPU-6000A.pdf register map.
• https://github.com/RobTillaart/Angle
• https://github.com/RobTillaart/AngleConverter

Breakout board

From left to right

pin	pinName	description	notes
0	VCC	+5V
1	GND	ground
2	SCL	I2C clock
3	SDA	I2C data
4	XDA	auxiliary data	see datasheet
5	XCL	auxiliary clock	see datasheet
6	AD0	address
7	INT	interrupt

Address

AD0 connected to GND => 0x68 AD0 connected to VCC => 0x69

Calibration (short version for now)

Since version 0.5.2 the library has a build in void calibrate(times) function which can be called instead of manual calibration. This function overwrites the values of axe aye aze gxe gye gze. calibrate() must be called after setAccelSensitivity(as) and setGyroSensitivity(gs).

Note the calibrate() function takes time, depending on the number of times.

Manual calibration

1. load and run calibration example
   it shows a header containing 6 numbers and 10 lines of 8 numbers
2. wait until the middle 6 of the longer lines stabilize (should all be 0)
3. copy the 6 numbers above the axe aye aze as these are the numbers needed.

Interface

#include "GY521.h"

Constructor

• GY521(uint8_t address = 0x69, , TwoWire *wire = &Wire) Constructor with default address. 0x68 is also a valid address. The wire argument is optional to select Wire1 Wire2 etc. on some boards.
• bool begin() Returns true if address can be found on I2C bus. Note call Wire.begin() before begin().
• bool isConnected() returns true if device can be found on I2C bus.
• void reset() set all internal values to 0 and throttle time to 10 ms.
• bool wakeUp() idem.

Calibrate

• void calibrate(uint16_t times) This function overwrites the values of axe aye aze gxe gye gze. To get "quality error" offsets, the GY521 sensor should not move during the calibration. The parameter times determines the number of measurements made. Typical values are 100 or more. Please note this is a time consuming function.

Ideal the function calibrate() should continue until it is stable (how to define) for n reads. Drawback is that this would make the duration unpredictable.

Throttle

• void setThrottle(bool throttle = true) throttle to force "delay" between reads.
• bool getThrottle() returns true if throttle mode is set.
• void setThrottleTime(uint16_t ti ) throttle time in milliseconds, max = 65535 = 1++ minute
• uint16_t getThrottleTime() returns throttle time set.

Read

Set before read

• bool setAccelSensitivity(uint8_t as) as = 0, 1, 2, 3 ==> 2g 4g 8g 16g
• uint8_t getAccelSensitivity() returns 0, 1, 2, 3
• bool setGyroSensitivity(uint8_t gs) gs = 0,1,2,3 ==> 250, 500, 1000, 2000 degrees/second
• uint8_t getGyroSensitivity() returns 0, 1, 2, 3
  = void setNormalize(bool normalize = true) normalizes pitch roll yaw or not. Default true. = bool getNormalize() returns flag.

Actual read

• int16_t read() reads all core measurements. returns status = GY521_OK on success.
• int16_t readAccel() read accelerometer only to speed up interaction. This call does update the throttle timer. returns status = GY521_OK on success.
• int16_t readGyro() read gyroscope only to speed up interaction. This call does update the throttle timer. returns status = GY521_OK on success. Note: for pitch roll yaw you need to call read().
• int16_t readTemperature() read temperature only, does not update the throttle timer. returns status = GY521_OK on success.
• uint32_t lastTime() last time sensor is actually read. In milliseconds. Not updated by readTemperature().

Since version 0.3.8 the read() and readGyro() function is updated to keep the range of getPitch(), getRoll() and getYaw() in the range 0..359.999 degrees. (Issue #36). Problem is that with continuous rotation in a same direction internal variables will overflow and new movements (angles) will get lost as insignificant digits.

In version 0.4.0 the normalization of pitch, roll and yaw is fixed and made conditional. (Issue #42).

Calls after read

Note that multiple calls will return the same value. One must explicitly call read() to get new values.

• float getAccelX() idem.
• float getAccelY() idem.
• float getAccelZ() idem.
• float getAngleX() idem.
• float getAngleY() idem.
• float getAngleZ() idem.
• float getTemperature() idem.
• float getGyroX() idem.
• float getGyroY() idem.
• float getGyroZ() idem.

Experimental Pitch Roll and Yaw

Pitch Roll and Yaw is work in progress and should not be used for projects yet.

• float getPitch() idem. May return any number. If setNormalize(true) return value will be 0-359.999
• float getRoll() idem. May return any number. If setNormalize(true) return value will be 0-359.999
• float getYaw() idem. May return any number. If setNormalize(true) return value will be 0-359.999

Register access

Read the register PDF for the specific value and meaning of registers.

• uint8_t setRegister(uint8_t reg, uint8_t value)
• uint8_t getRegister(uint8_t reg)

Documents

• check details registers - MPU-6000-Register-Map1.pdf

Error codes

Error code	value	notes
GY521_OK	0	not an error
GY521_THROTTLED	1	not an error
GY521_ERROR_READ	-1
GY521_ERROR_WRITE	-2
GY521_ERROR_NOT_CONNECTED	-3

Sensitivity Acceleration

unit g = gravity == 9.81 m/s^2

Acceleration	value	notes
2 g	0	default
4 g	1
8 g	2
16 g	3

Sensitivity Gyroscope

unit dps = degrees per second.

Gyroscope	value	notes
250 dps	0	default
500 dps	1
1000 dps	2
2000 dps	3

Operation

See examples, use with care.

Future

Must

• time
• improve documentation
• investigate Pitch Roll and Yaw math in detail.
  • investigate math needed.
  • implementation.
  • when?
• test test and test ...(ESP too)

Should

• test calibrate() function for different sensitivities.

Could

• add examples
• improve unit tests?
• reorder code in read(), would that save some micros.?
  • first all ax, then ay etc
  • footprint / performance gain?
• make enum for sensitivity Accel?
• make enum for sensitivity Gyro?

Wont

• look for maths optimizations (atan, hypot, performance)
  • ==> hypot optimized (fastTrig?)
  • other ideas affect accuracy, so unless new ideas arise.
• calibrate function in the lib
  • not as lib will grow too large.
• defaults value for functions?
  • user must set function parameters explicit

Support

If you appreciate my libraries, you can support the development and maintenance. Improve the quality of the libraries by providing issues and Pull Requests, or donate through PayPal or GitHub sponsors.

Thank you,

gy521's Issues

improve performance + precision

There are 3 hypot() calls in read(). These can be replaced by the equivalent

    float a2 = a * a;
    float b2 = b * b;
    float c2 = c * c;
    x = sqrt(a2 + b2);
    y = sqrt(b2 + c2);
    z = sqrt(c2 + a2);

advantage is a slightly 26 us faster read() on an UNO (163 us -> 137 us) as squares are only calculated once, and smaller size ~258 bytes ~ (number based upon test sketch).

---

use micros() for the duration to correct pitch yaw etc.
NB with a throttle of 10 millis the accuracy of millis is typically in 2 decimals.
By using micros() one would expect one or two additional digits accuracy.

---

Elaborate unit tests

• wakeup()
• read()
  • throttle / not
  • all xyz accel gyro angle pitch roll yaw
  • • lastTime?

Add multiWire support

allow to set Wire Wire1 Wire2 etc

• see PCF8574

swire support

https://github.com/netpipe/Arduino-Projects/tree/master/libs/GY521

other than version information it should be the same with swire support. thanks.

version number incorrect in .h and .cpp (minor)

low prio, just comment.

use float variables in example GY521_test_1.ino

  int ax = sensor.getAccelX();
  int ay = sensor.getAccelY();
  int az = sensor.getAccelZ();
  int gx = sensor.getGyroX();
  int gy = sensor.getGyroY();
  int gz = sensor.getGyroZ();
  int t = sensor.getTemperature();

change to float would give more info about the noise.
print it with 3 decimals.

void GY521 :: setAccelSensitivity (uint8_t as) _afs problem in function.

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;
}

_afs is zero-defined.
the corrected version should be like this.

void GY521::setAccelSensitivity(uint8_t as)
{
  _afs = as;
  if (_afs > 3) _afs = 3;
  uint8_t val = getRegister(GY521_ACCEL_CONFIG);
  val &= 0xE7;
  val |= (_afs << 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;
}

am I right?

why when I run the GY521_pitch_roll_yaw example code, all the numbers slowly increase 0 -> 359 -> 0?

I didn't move my chip, but:

...
CNT	PITCH	ROLL	YAW
97940	68.228	70.984	113.046
97941	68.247	71.046	113.051
97942	68.247	71.046	113.051
97943	68.247	71.046	113.051
97944	68.247	71.046	113.051
97945	68.263	71.107	113.057
97946	68.263	71.107	113.057
97947	68.263	71.107	113.057
97948	68.263	71.107	113.057
97949	68.279	71.166	113.061

CNT	PITCH	ROLL	YAW
97950	68.279	71.166	113.061

CNT	PITCH	ROLL	YAW
130150	215.906	233.981	152.385
130151	215.906	233.981	152.385
130152	215.924	234.042	152.390
130153	215.924	234.042	152.390
130154	215.924	234.042	152.390
130155	215.924	234.042	152.390
130156	215.942	234.107	152.395
130157	215.942	234.107	152.395
130158	215.942	234.107	152.395
130159	215.955	234.159	152.399

it goes on and on.

Raw data

hi,
I thought my board was broken, but if you read out the data manually, I get values back.
Unfortunately, with the test1 example, I only get zeros and short deflections when I move it strongly. only the temperature value works properly.
until I found out that you don't get the raw data but a calculated value.
I then built myself functions that give me the raw value, I think it would be good to add that.
the variables also have the name raw but do not contain the raw data.
I think that would help many and also mention it in the explanation.

otherwise a very good library and i hope this is a good suggestion.
Greetings orichienal

refactor GY521_readCalibration_2 (generate calibration code snippet) into a proper calibrate() function, as part of GY521 init

Right now:

GY521_readCalibration_2 generate calibration code snippet.

User need to manually copy & paste the data numbers, e.g. into application code, e.g. GY521_pitch_roll_yaw.

It's better to move GY521_readCalibration_2 (generate calibration code snippet) into a proper calibrate() function, as part of GY521 init, so the user can just call:

void setup()
  ...
  sensor.calibrate(optional_n_times_counter=100);  // easy init
}

Invalid data without Reset while initializing the begin

whith out the code bellow i am getting invalid data when starting up the sensor
Wire.write(0x6B); //make the reset (place a 0 into the 6B register)
Wire.write(0x00);
Wire.endTransmission(true); //end the transmission

we need to reset the MPU6050 before we set any other register

this is coded in your function bool GY521::wakeup()
we need to call this after begin() before we read() any data.

Please verify and include this Readme if needed !!

How can I get data from 2 sensors?

I like this library. But... I need to get angles from 2 GY521.. How can I do it?
0x68 & 0x69

Enabling the interruptions

The datasheet explained that we can write into the Interrupt Configuration Register to enable interrupts. And then read into the Interrupt Status Register.
Is that possible to make that from your library ?

Examples use GetAngle instead of GetAccel

Examples use GetAngle instead of GetAccel leading to the calibration giving NaN for all accelerometer values

Calibration never zeros out

13:04:18.394 -> 	ACCELEROMETER		GYROSCOPE		TEMPERATURE
13:04:18.394 -> 	-8.040	2.777	-500.143	0.568	-2.084	1.113
13:04:18.394 -> 	axe	aye	aze	gxe	gye	gze	T
13:04:18.394 -> 480	-0.018	0.005	-1.042	0.019	0.041	0.013	39.77
13:04:18.584 -> 481	-0.016	0.006	-1.042	-0.029	-0.003	-0.050	39.84
13:04:18.725 -> 482	-0.017	0.006	-1.042	0.005	0.020	0.072	39.79
13:04:18.863 -> 483	-0.017	0.006	-1.042	0.042	-0.053	-0.061	39.83
13:04:19.049 -> 484	-0.017	0.006	-1.043	-0.041	0.048	0.060	39.77
13:04:19.187 -> 485	-0.017	0.007	-1.042	0.013	0.001	-0.018	39.82
13:04:19.378 -> 486	-0.017	0.006	-1.042	0.034	-0.045	-0.005	39.82
13:04:19.556 -> 487	-0.017	0.007	-1.042	-0.049	0.002	0.036	39.80
13:04:19.704 -> 488	-0.016	0.006	-1.043	0.017	0.013	-0.014	39.82
13:04:19.844 -> 489	-0.016	0.006	-1.044	0.035	-0.029	-0.034	39.77

...

13:11:07.122 -> 	ACCELEROMETER		GYROSCOPE		TEMPERATURE
13:11:07.122 -> 	-53.020	16.354	-3115.312	0.550	-2.084	1.194
13:11:07.122 -> 	axe	aye	aze	gxe	gye	gze	T
13:11:07.122 -> 2990	-0.022	0.004	-1.042	0.023	0.038	-0.060	39.82
13:11:07.259 -> 2991	-0.020	0.004	-1.043	0.005	0.027	-0.027	39.84
13:11:07.446 -> 2992	-0.021	0.003	-1.043	-0.006	-0.040	0.068	39.80
13:11:07.586 -> 2993	-0.020	0.004	-1.042	0.021	-0.018	-0.088	39.81
13:11:07.773 -> 2994	-0.021	0.005	-1.042	0.008	-0.003	0.047	39.85
13:11:07.915 -> 2995	-0.020	0.005	-1.043	-0.038	0.004	-0.016	39.82
13:11:08.102 -> 2996	-0.021	0.004	-1.043	0.026	0.019	0.023	39.78
13:11:08.242 -> 2997	-0.020	0.003	-1.043	0.000	-0.017	-0.040	39.82
13:11:08.430 -> 2998	-0.021	0.004	-1.042	-0.025	0.039	0.042	39.80
13:11:08.572 -> 2999	-0.021	0.003	-1.041	0.037	-0.021	-0.018	39.83

In my case, the values for the accel and gyro switched.

Bad results with Teensy 3.2

Using the library with and Arduino NANO works fine. I'm plotting getAccelX, getAccelY, getAccelZ on a serial plotter and values are as expected (+1 / -1 for gradually changing position.

However, using a teensy 3.2 the same code produces very wrong results. The issue is with how the ints are treated on a 32 bit MCU, easy fix, here's how....

replace any int with int16_t and add the uint16_t to the entire value

_ax = ( ( ((int)Wire.read()) << 8) | Wire.read() ); // ACCEL_XOUT_H ACCEL_XOUT_L

final code
_ax = (int16_t)(((int16_t)Wire.read() << 8) | Wire.read()) ; // ACCEL_XOUT_H ACCEL_XOUT_L
_ay = (int16_t)(((int16_t)Wire.read() << 8) | Wire.read()) ; // ACCEL_YOUT_H ACCEL_YOUT_L
_az = (int16_t)(((int16_t)Wire.read() << 8) | Wire.read()) ; // ACCEL_ZOUT_H ACCEL_ZOUT_L
// TEMPERATURE
_temperature = (int16_t)(((int16_t)Wire.read() << 8) | Wire.read()) ; // TEMP_OUT_H TEMP_OUT_L
// GYROSCOPE
_gx = (int16_t)(((int16_t)Wire.read() << 8) | Wire.read()) ; // GYRO_XOUT_H GYRO_XOUT_L
_gy = (int16_t)(((int16_t)Wire.read() << 8) | Wire.read()) ; // GYRO_YOUT_H GYRO_YOUT_L
_gz = (int16_t)(((int16_t)Wire.read() << 8) | Wire.read()) ; // GYRO_ZOUT_H GYRO_ZOUT_L

This code works on Arduino NANO and Teensy 3.2

Add support for ESP32 Wire

Need a begin(sda, scl) function for ESP32 and ESP8266

Shifted pitch and yaw measurement

It's difficult to describe the issue in words, so I made an illustration of it.

The following output is from the GY521_pitch_roll_yaw example after calibration. The actual pitch and roll movement was gradual and slow. From line 34 to 35, the measured pitch shifts by around -17. A similar shift occurs in the roll from line 36 to 37.

CNT	PITCH	ROLL	YAW
30	0.772	345.005	358.791
31	0.669	344.142	358.607
32	0.797	342.071	358.438
33	0.244	338.645	358.423
34	0.171	335.321	358.668
35	343.608	341.118	357.288
36	341.322	342.920	355.539
37	342.271	2.811	354.572
38	342.297	1.450	354.143
39	342.342	2.843	353.974

I'm not sure why this behavior happens, but I assume that the problem is caused by this library, since it does the computation of pitch/roll/yaw from the raw sensor output. I'm using version 0.3.9.

---

added the image for future reference

missing ; in .h file

See line 38

bool isConnected()

missing ;

getYaw, getPitch and getRoll go past 360 degrees

Hello!

Great work on the library, very useful for getting prototypes done, however, I would like to know if there is any way to make the return of the functions getYaw, getPitch and getRoll to clamp to 360 degrees? I have a device that continuously rotates in one direction (quickly), I don't know if waiting for a float overflow to reset the counter is a good idea.

Feature request: API for low-pass filter

Nice library! I just woul like to have a dedicated function to set/get the digital low pass filter mode. Something like setDLPFmode and getDLPFmode.

In the mean time, I will directly get/set the register, but a new function looks nicer.

Thanks!

improve precision

issue split of from #22

use micros() for the duration to correct pitch yaw etc.
NB with a throttle of 10 millis the accuracy of millis is typically in 2 decimals.
By using micros() one would expect one or two additional digits accuracy.

The gain will be that the duration of a force will become accurate in 0.1 or 0.01 of a millisecond.

Need to investigate how this affects the math.

• all math is done in seconds ==> conversion factors from 1.000 => 1.000.000 (or 1e-3 to 1e-6 )
• function lastTime() will be in micros() ==> interface break;
• throttle time, should it be in micros() too?
• initial values may be affected more as initial duration might be large?
• need extra memory ?

getRegister can return an error which is not handled properly

The getRegister() function can fail and should set an error flag.
This should be handled better in the library.

Although chances are pretty small I2C errors can occur due to noise or whatever.

need to add

• int16_t _error;
• int16_t lastError();
• patch code
• function signature void -> bool?

small bugfix in readCalibration

In line 90 is says "aze" and should be "gze".

Serial.print("sensor.aze = ");
to
Serial.print("sensor.gze = ");

Best regards

getting position from distance = acceleration * time

was trying to get some mouse position movement data for the screen trying to get acceleration over time maybe someone knows a trick for this with multiple sensors perhaps ?

VR_Gloves.zip