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

Author: RobTillaart

CHANGELOG.md

@@ -6,14 +6,24 @@ 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
 - edit changelog.md
 - 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.

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

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();
 

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
 

keywords.txt

@@ -20,6 +20,8 @@ setThrottle	KEYWORD2
 getThrottle	KEYWORD2
 setThrottleTime	KEYWORD2
 getThrottleTime	KEYWORD2
+setNormalize	KEYWORD2
+getNormalize	KEYWORD2
 
 setAccelSensitivity	KEYWORD2
 getAccelSensitivity	KEYWORD2

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": "*",

library.properties

@@ -1,5 +1,5 @@
 name=GY521
-version=0.3.9
+version=0.4.0
 author=Rob Tillaart <[email protected]>
 maintainer=Rob Tillaart <[email protected]>
 sentence=Arduino library for GY521 angle measurement