From 8024ad8cfd1a60779daeeeb62753e9362b1aec37 Mon Sep 17 00:00:00 2001
From: jusax23 <jusax23@gmail.com>
Date: Wed, 3 Aug 2022 15:19:06 +0200
Subject: [PATCH] offsets

---
 library.properties      |  2 +-
 src/MPU6050_jusax23.cpp | 22 ++++++++++++++++++++--
 src/MPU6050_jusax23.h   | 20 ++++++++++++++++++++
 3 files changed, 41 insertions(+), 3 deletions(-)

diff --git a/library.properties b/library.properties
index a3fc9a9..7db2ad5 100644
--- a/library.properties
+++ b/library.properties
@@ -1,6 +1,6 @@
 name=mpu6050
 version=1.0.0
-author=tockn
+author=jusax23 (tockn)
 maintainer=jusax23
 sentence=Arduino library for easy communicating with the MPU6050.
 paragraph=It acces get accel, gyro, and angle data.
diff --git a/src/MPU6050_jusax23.cpp b/src/MPU6050_jusax23.cpp
index 3f6c7ad..292b2ec 100644
--- a/src/MPU6050_jusax23.cpp
+++ b/src/MPU6050_jusax23.cpp
@@ -81,6 +81,10 @@ void MPU6050::calcGyroOffsets(bool console, uint16_t delayBefore, uint16_t delay
   gyroYoffset = y / 3000;
   gyroZoffset = z / 3000;
 
+  gyroXoffsetGlide = gyroXoffset;
+  gyroYoffsetGlide = gyroYoffset;
+  gyroZoffsetGlide = gyroZoffset;
+
   if(console){
     Serial.println();
     Serial.println("Done!");
@@ -113,8 +117,18 @@ void MPU6050::update(){
   accY = ((float)rawAccY) / 16384.0;
   accZ = ((float)rawAccZ) / 16384.0;
 
-  /*accX -= 0.04f;
-  accZ -= 0.3f;*/
+  if(accX>maxAccX) maxAccX = accX;
+  if(accX<minAccX) minAccX = accX;
+
+  if(accY>maxAccY) maxAccY = accY;
+  if(accY<minAccY) minAccY = accY;
+  
+  if(accZ>maxAccZ) maxAccZ = accZ;
+  if(accZ<minAccZ) minAccZ = accZ;
+
+  accX -= accXoffset;
+  accY -= accYoffset;
+  accZ -= accZoffset;
 
   /*angleAccX = atan2(accY, accZ + abs(accX)) * 360 / 2.0 / PI;
   angleAccY = atan2(accX, accZ + abs(accY)) * 360 / -2.0 / PI;*/
@@ -125,6 +139,10 @@ void MPU6050::update(){
   gyroY = ((float)rawGyroY) / 65.5;
   gyroZ = ((float)rawGyroZ) / 65.5;
 
+  gyroXoffsetGlide = gyroXoffsetGlide * 0.99 + gyroX * 0.01;
+  gyroYoffsetGlide = gyroYoffsetGlide * 0.99 + gyroY * 0.01;
+  gyroZoffsetGlide = gyroZoffsetGlide * 0.99 + gyroZ * 0.01;
+
   gyroX -= gyroXoffset;
   gyroY -= gyroYoffset;
   gyroZ -= gyroZoffset;
diff --git a/src/MPU6050_jusax23.h b/src/MPU6050_jusax23.h
index b6acdb3..c1b9cf6 100644
--- a/src/MPU6050_jusax23.h
+++ b/src/MPU6050_jusax23.h
@@ -43,6 +43,14 @@ class MPU6050{
   float getAccY(){ return accY; };
   float getAccZ(){ return accZ; };
 
+  void resetAccOffsetWatch() { maxAccX = 0; maxAccY = 0; maxAccZ = 0; minAccX = 0; minAccY = 0; minAccZ = 0; };
+
+  float calcAccXoffset() { return (maxAccX + minAccX)/2; };
+  float calcAccYoffset() { return (maxAccY + minAccY)/2; };
+  float calcAccZoffset() { return (maxAccZ + minAccZ)/2; };
+
+  float setAccXoffset(float x, float y, float z){accXoffset = x; accYoffset = y; accZoffset = z; };
+
   float getGyroX(){ return gyroX; };
   float getGyroY(){ return gyroY; };
   float getGyroZ(){ return gyroZ; };
@@ -53,6 +61,12 @@ class MPU6050{
   float getGyroYoffset(){ return gyroYoffset; };
   float getGyroZoffset(){ return gyroZoffset; };
 
+  void restGyroOffsetGlide(){gyroXoffsetGlide = 0; gyroYoffsetGlide = 0;gyroZoffsetGlide = 0;};
+
+  float getGyroXoffsetGlide() { return gyroXoffsetGlide; };
+  float getGyroYoffsetGlide() { return gyroYoffsetGlide; };
+  float getGyroZoffsetGlide() { return gyroZoffsetGlide; };
+
   void update();
 
   float getAccAngleX(){ return angleAccX; };
@@ -75,8 +89,14 @@ class MPU6050{
 
   float gyroXoffset, gyroYoffset, gyroZoffset;
 
+  float gyroXoffsetGlide, gyroYoffsetGlide, gyroZoffsetGlide;
+
   float temp, accX, accY, accZ, gyroX, gyroY, gyroZ;
 
+  float maxAccX, maxAccY, maxAccZ, minAccX, minAccY, minAccZ;
+
+  float accXoffset = 0.0, accYoffset = 0.0, accZoffset = 0.0;
+
   float angleGyroX, angleGyroY, angleGyroZ,
   angleAccX, angleAccY, angleAccZ;