Add Kalman filter for library
- float get_pitch_kalman() reads pitch angle after using kalman filter.
- float get_roll_kalman() reads roll angle after using kalman filter.
Change the value calibration to get (0,0,1) in readCalibration2.ino, hence we can use formula to calculate angle from accelerator properly
Add examples GY521_test_main.ino to get output raw and compare complementary filter and Kalman filter.
Arduino library for I2C GY521 accelerometer-gyroscope sensor a.k.a. MCU-6050.
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.
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().
- 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.
- 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
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 |
AD0 connected to GND => 0x68 AD0 connected to VCC => 0x69
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.
- load and run calibration example
it shows a header containing 6 numbers and 10 lines of 8 numbers - wait until the middle 6 of the longer lines stabilize (should all be 0)
- copy the 6 numbers above the axe aye aze as these are the numbers needed.
#include "GY521.h"- 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.
- 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.
- 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.
- 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.
- 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).
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.
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
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)
- check details registers - MPU-6000-Register-Map1.pdf
| 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 |
unit g = gravity == 9.81 m/s^2
| Acceleration | value | notes |
|---|---|---|
| 2 g | 0 | default |
| 4 g | 1 | |
| 8 g | 2 | |
| 16 g | 3 |
unit dps = degrees per second.
| Gyroscope | value | notes |
|---|---|---|
| 250 dps | 0 | default |
| 500 dps | 1 | |
| 1000 dps | 2 | |
| 2000 dps | 3 |
See examples, use with care.
- time
- improve documentation
- investigate Pitch Roll and Yaw math in detail.
- investigate math needed.
- implementation.
- when?
- test test and test ...(ESP too)
- test calibrate() function for different sensitivities.
- 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?
- 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
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,