From b0a517439aa763a31cc0c902964942d51d78f75b Mon Sep 17 00:00:00 2001 From: Rob Tillaart Date: Mon, 12 Jun 2023 17:30:43 +0200 Subject: [PATCH] fix #42, pitch and roll (#43) - fix #42 roll + pitch "jumps" after full rotation. - fixed normalization code. - made normalization conditional - add **void setNormalize(bool normalize = true)** - add **bool getNormalize()** - update readme.md - minor edits --- CHANGELOG.md | 12 ++++++++- GY521.cpp | 65 ++++++++++++++++++++++++++++++---------------- GY521.h | 27 ++++++++++++------- README.md | 45 +++++++++++++++++++------------- keywords.txt | 2 ++ library.json | 2 +- library.properties | 2 +- 7 files changed, 102 insertions(+), 53 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3dea7e5..067ffa3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,17 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/) and this project adheres to [Semantic Versioning](http://semver.org/). +## [0.4.0] - 2023-06-11 +- fix #42 roll + pitch "jumps" after full rotation. +- fixed normalization code. +- made normalization conditional + - add **void setNormalize(bool normalize = true)** + - add **bool getNormalize()** +- update readme.md +- minor edits + +---- + ## [0.3.9] - 2023-01-27 - update GitHub actions - update license 2023 @@ -13,7 +24,6 @@ and this project adheres to [Semantic Versioning](http://semver.org/). - update readme.md - minor edits - ## [0.3.8] - 2022-10-07 - added CHANGELOG.md - fix #36 - limit values of pitch() roll() yaw() to 0..360 range. diff --git a/GY521.cpp b/GY521.cpp index 989deaf..2e3fd54 100644 --- a/GY521.cpp +++ b/GY521.cpp @@ -1,7 +1,7 @@ // // FILE: GY521.cpp // AUTHOR: Rob Tillaart -// VERSION: 0.3.9 +// VERSION: 0.4.0 // PURPOSE: Arduino library for I2C GY521 accelerometer-gyroscope sensor // URL: https://github.com/RobTillaart/GY521 @@ -128,7 +128,7 @@ int16_t GY521::read() _lastMicros = now; - // next lines might be merged per axis. + // next lines might be merged per axis. (performance) // Convert raw acceleration to g's _ax *= _raw2g; @@ -170,23 +170,37 @@ int16_t GY521::read() _gay += _gy * duration; _gaz += _gz * duration; - // normalize internal vars - if (_gax < 0) _gax += 360.0; - else if (_gax >= 360.0) _gax -= 360.0; - if (_gay < 0) _gay += 360.0; - else if (_gay >= 360.0) _gay -= 360.0; - if (_gaz < 0) _gaz += 360.0; - else if (_gaz >= 360.0) _gaz -= 360.0; + // normalize + // _gax etc might loose precision after many iterations #36 + if (_normalize) + { + // correction at 375 due to the factor 0.96 in roll + if (_gax >= 375) _gax -= 375; + else if (_gax < 0) _gax += 375; + // correction at 375 due to the factor 0.96 in pitch + if (_gay >= 375) _gay -= 375; + else if (_gay < 0) _gay += 375; + // correction at 360 + if (_gaz >= 360) _gaz -= 360; + else if (_gaz < 0) _gaz += 360; + } + + // Calculate Pitch Roll Yaw _yaw = _gaz; - _pitch = 0.96 * _gay + 0.04 * _aay; _roll = 0.96 * _gax + 0.04 * _aax; + _pitch = 0.96 * _gay + 0.04 * _aay; - // normalize external vars. - if (_pitch < 0) _pitch += 360; - else if (_pitch >= 360) _pitch -= 360; - if (_roll < 0) _roll += 360; - else if (_roll >= 360) _roll -= 360; + + if (_normalize) + { + if (_pitch >= 360) _pitch -= 360; + else if (_pitch < 0) _pitch += 360; + if (_roll >= 360) _roll -= 360; + else if (_roll < 0) _roll += 360; + if (_yaw >= 360) _yaw -= 360; + else if (_yaw < 0) _yaw += 360; + } return GY521_OK; } @@ -310,14 +324,21 @@ int16_t GY521::readGyro() _gay += _gy * duration; _gaz += _gz * duration; - // normalize internal vars - if (_gax < 0) _gax += 360.0; - else if (_gax >= 360.0) _gax -= 360.0; - if (_gay < 0) _gay += 360.0; - else if (_gay >= 360.0) _gay -= 360.0; - if (_gaz < 0) _gaz += 360.0; - else if (_gaz >= 360.0) _gaz -= 360.0; + // normalize + // _gax etc might loose precision after many iterations #36 + if (_normalize) + { + // correction at 375 due to the factor 0.96 in roll + if (_gax >= 375) _gax -= 375; + else if (_gax < 0) _gax += 375; + // correction at 375 due to the factor 0.96 in pitch + if (_gay >= 375) _gay -= 375; + else if (_gay < 0) _gay += 375; + // correction at 360 + if (_gaz >= 360) _gaz -= 360; + else if (_gaz < 0) _gaz += 360; + } return GY521_OK; } diff --git a/GY521.h b/GY521.h index 6306228..6b2d7fa 100644 --- a/GY521.h +++ b/GY521.h @@ -2,7 +2,7 @@ // // FILE: GY521.h // AUTHOR: Rob Tillaart -// VERSION: 0.3.9 +// VERSION: 0.4.0 // PURPOSE: Arduino library for I2C GY521 accelerometer-gyroscope sensor // URL: https://github.com/RobTillaart/GY521 // @@ -12,7 +12,7 @@ #include "Wire.h" -#define GY521_LIB_VERSION (F("0.3.9")) +#define GY521_LIB_VERSION (F("0.4.0")) // THROTTLE TIMING @@ -58,6 +58,18 @@ class GY521 uint16_t getThrottleTime() { return _throttleTime; }; + // SET BEFORE READ + // as = 0,1,2,3 ==> 2g 4g 8g 16g + bool setAccelSensitivity(uint8_t as); + uint8_t getAccelSensitivity(); // returns 0,1,2,3 + // gs = 0,1,2,3 ==> 250, 500, 1000, 2000 degrees/second + bool setGyroSensitivity(uint8_t gs); + uint8_t getGyroSensitivity(); // returns 0,1,2,3 + // normalizes Pitch Roll and Yaw. + void setNormalize(bool normalize = true) { _normalize = normalize; }; + bool getNormalize() { return _normalize; }; + + // READ THE SENSOR // returns GY521_OK or one of the error codes above. int16_t read(); @@ -70,13 +82,6 @@ class GY521 // read temperature only, does not affect throttle. int16_t readTemperature(); - // SET BEFORE READ - // as = 0,1,2,3 ==> 2g 4g 8g 16g - bool setAccelSensitivity(uint8_t as); - uint8_t getAccelSensitivity(); // returns 0,1,2,3 - // gs = 0,1,2,3 ==> 250, 500, 1000, 2000 degrees/second - bool setGyroSensitivity(uint8_t gs); - uint8_t getGyroSensitivity(); // returns 0,1,2,3 // CALL AFTER READ float getAccelX() { return _ax; }; @@ -128,11 +133,13 @@ class GY521 uint8_t _gfs = 0; float _raw2dps = GY521_RAW2DPS; float _gx, _gy, _gz; // gyro raw - float _gax, _gay, _gaz; // gyro processed. + float _gax, _gay, _gaz; // gyro processed float _pitch, _roll, _yaw; // used by user float _temperature = 0; + bool _normalize = true; // default true. + // to read register of 2 bytes. int16_t _WireRead2(); diff --git a/README.md b/README.md index 8358577..4d1f12e 100644 --- a/README.md +++ b/README.md @@ -18,6 +18,8 @@ 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. + #### Examples @@ -93,6 +95,8 @@ AD0 connected to VCC => 0x69 - **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 @@ -108,28 +112,33 @@ 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..360 degrees. (Issue #36). -Problem is that with continuous rotation in a same direction internal variables will overflow and new + **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). + -#### Call after read +#### 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 -- **float getPitch()** idem. Returns 0.00 - 359.99. -- **float getRoll()** idem. Returns 0.00 - 359.99. -- **float getYaw()** idem. Returns 0.00 - 359.99. +- **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. +- **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 @@ -155,10 +164,10 @@ See examples, use with care #### Must - improve documentation +- test test and test ...(ESP too) -#### Should -- test test and test ...(ESP too) +#### Should #### Could diff --git a/keywords.txt b/keywords.txt index e366774..1231524 100644 --- a/keywords.txt +++ b/keywords.txt @@ -20,6 +20,8 @@ setThrottle KEYWORD2 getThrottle KEYWORD2 setThrottleTime KEYWORD2 getThrottleTime KEYWORD2 +setNormalize KEYWORD2 +getNormalize KEYWORD2 setAccelSensitivity KEYWORD2 getAccelSensitivity KEYWORD2 diff --git a/library.json b/library.json index a894b4e..b73e2b9 100644 --- a/library.json +++ b/library.json @@ -15,7 +15,7 @@ "type": "git", "url": "https://github.com/RobTillaart/GY521.git" }, - "version": "0.3.9", + "version": "0.4.0", "license": "MIT", "frameworks": "arduino", "platforms": "*", diff --git a/library.properties b/library.properties index bc82993..fc81b01 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=GY521 -version=0.3.9 +version=0.4.0 author=Rob Tillaart maintainer=Rob Tillaart sentence=Arduino library for GY521 angle measurement