From 579dea8fec93e33e429b338ff175b13eaea10fbf Mon Sep 17 00:00:00 2001
From: Maik Menz <myco@gmx.net>
Date: Wed, 14 Aug 2024 10:29:08 +0200
Subject: [PATCH] Added orientation arguments to calibrate function (#59)

* Fix calibration error scaling
* Added orientation arguments to calibrate function

---------

Co-authored-by: Maik Menz <mycosd@hotmail.de>
---
 GY521.cpp | 35 +++++++++++++++++++++--------------
 GY521.h   |  3 ++-
 2 files changed, 23 insertions(+), 15 deletions(-)

diff --git a/GY521.cpp b/GY521.cpp
index 9fad638..6e2b147 100644
--- a/GY521.cpp
+++ b/GY521.cpp
@@ -26,6 +26,10 @@ GY521::GY521(uint8_t address, TwoWire *wire)
   _address = address;
   _wire    = wire;
 
+  //  initialize errors
+  //  don't do it in reset, as users might want to keep them
+  axe = aye = aze = 0;
+
   reset();
 }
 
@@ -68,12 +72,8 @@ void GY521::reset()
 }
 
 
-void GY521::calibrate(uint16_t times)
+void GY521::calibrate(uint16_t times, float angleX, float angleY, bool inverted)
 {
-  //  disable throttling / caching of read values.
-  bool oldThrottle = _throttle;
-  _throttle = false;
-
   //  set all errors to zero, to get the raw reads.
   axe = aye = aze = 0;
   gxe = gye = gze = 0;
@@ -97,20 +97,27 @@ void GY521::calibrate(uint16_t times)
     _gze -= getGyroZ();
   }
 
-  //  scale accelerometer calibration errors so read() should get all zero's on average.
-  float factor = _raw2g / times;
-  axe = _axe * factor;
-  aye = _aye * factor;
-  aze = _aze * factor;
-
   //  scale gyro calibration errors so read() should get all zero's on average.
-  factor = _raw2dps / times;
+  float factor = _raw2dps / times;
   gxe = _gxe * factor;
   gye = _gye * factor;
   gze = _gze * factor;
 
-  //  restore throttle state.
-  _throttle = oldThrottle;
+  //  scale accelerometer calibration errors so read() should get all zero's on average.
+  factor = _raw2g / times;
+  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;
 }
 
 
diff --git a/GY521.h b/GY521.h
index 483b1bd..768078f 100644
--- a/GY521.h
+++ b/GY521.h
@@ -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);
+  void     calibrate(uint16_t times, float angleX = 0, float angleY = 0, bool inverted = false);
 
   bool     wakeup();
   //  throttle to force delay between reads.