diff --git a/CHANGELOG.md b/CHANGELOG.md index e96af71..e2c9010 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,18 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/) and this project adheres to [Semantic Versioning](http://semver.org/). +## [0.6.1] - 2024-07-25 +Co-authored-by: Maik Menz +- redo of **calibrate()** function, Kudos to Maik Menz. +- Fix #59 Added orientation arguments to calibrate function (#59) +- Fix #57 calibration error scaling +- update library.json +- improve initialization of gax, gay and gaz +- add **readRaw()** to improve calibrate() +- update readme.md +- improve initialization + + ## [0.6.0] - 2024-06-22 - fix #54, calibrate() function, Kudos to jens-kuerten and MArimont3 - minor edits diff --git a/GY521.cpp b/GY521.cpp index c64ff60..bb80b7f 100644 --- a/GY521.cpp +++ b/GY521.cpp @@ -1,7 +1,7 @@ // // FILE: GY521.cpp // AUTHOR: Rob Tillaart -// VERSION: 0.6.0 +// VERSION: 0.6.1 // PURPOSE: Arduino library for I2C GY521 accelerometer-gyroscope sensor // URL: https://github.com/RobTillaart/GY521 @@ -26,12 +26,11 @@ GY521::GY521(uint8_t address, TwoWire *wire) _address = address; _wire = wire; - _ax = _ay = _az = 0; - _aax = _aay = _aaz = 0; - _gx = _gy = _gz = 0; - _pitch = 0; - _roll = 0; - _yaw = 0; + // initialize errors + // don't do it in reset, as users might want to keep them + axe = aye = aze = 0; + + reset(); } @@ -66,13 +65,14 @@ void GY521::reset() _ax = _ay = _az = 0; _aax = _aay = _aaz = 0; _gx = _gy = _gz = 0; + _gax = _gay = _gaz = 0; _pitch = 0; _roll = 0; _yaw = 0; } -void GY521::calibrate(uint16_t times) +bool GY521::calibrate(uint16_t times, float angleX, float angleY, bool inverted) { // disable throttling / caching of read values. bool oldThrottle = _throttle; @@ -89,29 +89,50 @@ void GY521::calibrate(uint16_t times) // adjust times if zero. if (times == 0) times = 1; - // summarize (6x) the measurements. + // sum (6x) the measurements. + uint16_t samples = 0; for (uint16_t i = 0; i < times; i++) { - read(); - _axe -= getAccelX(); - _aye -= getAccelY(); - _aze -= getAccelZ(); - _gxe -= getGyroX(); - _gye -= getGyroY(); - _gze -= getGyroZ(); + if (_readRaw() == GY521_OK) + { + _axe -= getAccelX(); + _aye -= getAccelY(); + _aze -= getAccelZ(); + _gxe -= getGyroX(); + _gye -= getGyroY(); + _gze -= getGyroZ(); + samples++; + } } - // adjust calibration errors so read() should get all zero's on average. - float factor = 1.0 / times; - axe = _axe * factor; - aye = _aye * factor; - aze = _aze * factor; + if (samples == 0) return false; + + // scale gyro calibration errors so read() should get all zero's on average. + float factor = _raw2dps / samples; gxe = _gxe * factor; gye = _gye * factor; gze = _gze * factor; + // scale accelerometer calibration errors so read() should get all zero's on average. + factor = _raw2g / samples; + axe = _axe * factor; + aye = _aye * factor; + aze = _aze * factor; + + // remove expected gravity from error + angleX *= GY521_DEGREES2RAD; + angleY *= GY521_DEGREES2RAD; + float _gravx = -sin(angleY) * cos(angleX); + float _gravy = sin(angleX); + float _gravz = cos(angleY) * cos(angleX); + axe -= _gravx; + aye -= _gravy; + aze += inverted ? -_gravz : _gravz; + // restore throttle state. _throttle = oldThrottle; + + return true; } @@ -137,32 +158,11 @@ int16_t GY521::read() } _lastTime = now; - // Connected ? - _wire->beginTransmission(_address); - _wire->write(GY521_ACCEL_XOUT_H); - if (_wire->endTransmission() != 0) - { - _error = GY521_ERROR_WRITE; - return _error; - } - - // Get the data - int8_t n = _wire->requestFrom(_address, (uint8_t)14); - if (n != 14) + int16_t rv = _readRaw(); + if (rv != GY521_OK) { - _error = GY521_ERROR_READ; - return _error; + return rv; } - // ACCELEROMETER - _ax = _WireRead2(); // ACCEL_XOUT_H ACCEL_XOUT_L - _ay = _WireRead2(); // ACCEL_YOUT_H ACCEL_YOUT_L - _az = _WireRead2(); // ACCEL_ZOUT_H ACCEL_ZOUT_L - // TEMPERATURE - _temperature = _WireRead2(); // TEMP_OUT_H TEMP_OUT_L - // GYROSCOPE - _gx = _WireRead2(); // GYRO_XOUT_H GYRO_XOUT_L - _gy = _WireRead2(); // GYRO_YOUT_H GYRO_YOUT_L - _gz = _WireRead2(); // GYRO_ZOUT_H GYRO_ZOUT_L // duration interval now = micros(); @@ -549,6 +549,43 @@ uint8_t GY521::getRegister(uint8_t reg) } +/////////////////////////////////////////////////////////////////// +// +// PRIVATE +// +int16_t GY521::_readRaw() +{ + // Connected ? + _wire->beginTransmission(_address); + _wire->write(GY521_ACCEL_XOUT_H); + if (_wire->endTransmission() != 0) + { + _error = GY521_ERROR_WRITE; + return _error; + } + + // Get the data + int8_t n = _wire->requestFrom(_address, (uint8_t)14); + if (n != 14) + { + _error = GY521_ERROR_READ; + return _error; + } + // ACCELEROMETER + _ax = _WireRead2(); // ACCEL_XOUT_H ACCEL_XOUT_L + _ay = _WireRead2(); // ACCEL_YOUT_H ACCEL_YOUT_L + _az = _WireRead2(); // ACCEL_ZOUT_H ACCEL_ZOUT_L + // TEMPERATURE + _temperature = _WireRead2(); // TEMP_OUT_H TEMP_OUT_L + // GYROSCOPE + _gx = _WireRead2(); // GYRO_XOUT_H GYRO_XOUT_L + _gy = _WireRead2(); // GYRO_YOUT_H GYRO_YOUT_L + _gz = _WireRead2(); // GYRO_ZOUT_H GYRO_ZOUT_L + + return GY521_OK; +} + + // to read register of 2 bytes. int16_t GY521::_WireRead2() { diff --git a/GY521.h b/GY521.h index 67f1351..5f6757d 100644 --- a/GY521.h +++ b/GY521.h @@ -2,7 +2,7 @@ // // FILE: GY521.h // AUTHOR: Rob Tillaart -// VERSION: 0.6.0 +// VERSION: 0.6.1 // PURPOSE: Arduino library for I2C GY521 accelerometer-gyroscope sensor // URL: https://github.com/RobTillaart/GY521 @@ -11,7 +11,7 @@ #include "Wire.h" -#define GY521_LIB_VERSION (F("0.6.0")) +#define GY521_LIB_VERSION (F("0.6.1")) const float GRAVITY = 9.80655; @@ -33,6 +33,7 @@ const float GRAVITY = 9.80655; // CONVERSION CONSTANTS #define GY521_RAD2DEGREES (180.0 / PI) +#define GY521_DEGREES2RAD (PI / 180.0) #define GY521_RAW2DPS (1.0 / 131.0) #define GY521_RAW2G (1.0 / 16384.0) @@ -51,7 +52,7 @@ class GY521 // EXPERIMENTAL // calibrate needs to be called to compensate for errors. // must be called after setAccelSensitivity(as); and setGyroSensitivity(gs); - void calibrate(uint16_t times); + bool calibrate(uint16_t times, float angleX = 0, float angleY = 0, bool inverted = false); bool wakeup(); // throttle to force delay between reads. @@ -151,6 +152,7 @@ class GY521 bool _normalize = true; // default true. + int16_t _readRaw(); // to read register of 2 bytes. int16_t _WireRead2(); diff --git a/README.md b/README.md index 25feedf..29e84ce 100644 --- a/README.md +++ b/README.md @@ -24,6 +24,11 @@ It needs to be tested a lot more. See changelog.md for latest updates. +#### 0.6.1 + +Improved **calibrate()** to support any angle. + + #### 0.6.0 Fixed a bug in calibration function, making previous versions obsolete. @@ -92,6 +97,10 @@ This function overwrites the values of axe aye aze gxe gye gze. Note the **calibrate()** function takes time, depending on the number of times. +Since version 0.6.1 the calibrate function is extended with optional parameters so the +sensor can be calibrated in any angle. +**bool calibrate(times, angleX = 0, angleY = 0, inverted = false)** + #### Manual calibration @@ -120,14 +129,24 @@ Note call **Wire.begin()** before **begin()**. ### 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. +- **bool calibrate(uint16_t times, float angleX = 0, float angleY = 0, bool inverted = false)** +This function overwrites the values of axe aye aze and gxe gye gze. +To improve the quality of the error offsets, the GY521 sensor should not move during the calibration. +The parameter times determines the number of measurements the calibration function should make. +Note that the actual number of samples can be less if a read of the sensor fails. +If there is no good read at all the function returns **false**. +Typical values for times are 100 or more. +If times is set to 0, it will be forced to 1. +Please note this call will be very time consuming. 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. +Drawback is that this would make the duration unpredictable. + +New since 0.6.1 (experimental) +The optional parameters **float angleX = 0, float angleY = 0** should be between -90 .. +90. +These can be used if the sensor is not lying flat during calibration. +The optional parameter **bool inverted = false** should be set to true if the sensor is +upside down. ### Throttle @@ -293,7 +312,9 @@ However if one specific is needed, please open an issue. #### Should -- test **calibrate()** function for different sensitivities. +- test **calibrate()** function + - for different sensitivities. + - for different angles. #### Could diff --git a/library.json b/library.json index e2c351c..9fb37e3 100644 --- a/library.json +++ b/library.json @@ -8,6 +8,9 @@ "name": "Rob Tillaart", "email": "Rob.Tillaart@gmail.com", "maintainer": true + }, + { + "name": "Maik Menz" } ], "repository": @@ -15,7 +18,7 @@ "type": "git", "url": "https://github.com/RobTillaart/GY521.git" }, - "version": "0.6.0", + "version": "0.6.1", "license": "MIT", "frameworks": "*", "platforms": "*", diff --git a/library.properties b/library.properties index 330761e..b4c6415 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=GY521 -version=0.6.0 +version=0.6.1 author=Rob Tillaart maintainer=Rob Tillaart sentence=Arduino library for GY521 angle measurement