Improve calibration (#60)

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

Author: RobTillaart

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

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

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

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
 

library.json

@@ -8,14 +8,17 @@
       "name": "Rob Tillaart",
       "email": "[email protected]",
       "maintainer": true
+    },
+    {
+      "name": "Maik Menz"
     }
   ],
   "repository":
   {
     "type": "git",
     "url": "https://github.com/RobTillaart/GY521.git"
   },
-  "version": "0.6.0",
+  "version": "0.6.1",
   "license": "MIT",
   "frameworks": "*",
   "platforms": "*",

library.properties

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