From 3f2d339eac37cd1a37c57fc7bfa116a2332bbe05 Mon Sep 17 00:00:00 2001
From: net234 <51489394+net234@users.noreply.github.com>
Date: Fri, 29 Jan 2021 21:22:28 +0100
Subject: [PATCH] Acces method to get and set actives offsets

void MPU6050::getActiveOffsets(int16_t Data[6]);
void MPU6050::setActiveOffsets(int16_t offsets[6]);
---
 src/MPU6050.cpp | 23 +++++++++++++++++++++++
 src/MPU6050.h   |  3 ++-
 2 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/src/MPU6050.cpp b/src/MPU6050.cpp
index ae7a555..f8cdf32 100755
--- a/src/MPU6050.cpp
+++ b/src/MPU6050.cpp
@@ -3327,4 +3327,27 @@ void MPU6050::PrintActiveOffsets() {
 	printfloatx("", Data[0], 5, 0, ",  ");
 	printfloatx("", Data[1], 5, 0, ",  ");
 	printfloatx("", Data[2], 5, 0, "\n");
+
+}
+
+void MPU6050::getActiveOffsets(int16_t Data[6]) {
+  uint8_t AOffsetRegister = (getDeviceID() < 0x38 ) ? MPU6050_RA_XA_OFFS_H : 0x77;
+  //  A_OFFSET_H_READ_A_OFFS(Data);
+  if (AOffsetRegister == 0x06) I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)Data);
+  else {
+    I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)Data);
+    I2Cdev::readWords(devAddr, AOffsetRegister + 3, 1, (uint16_t *)Data + 1);
+    I2Cdev::readWords(devAddr, AOffsetRegister + 6, 1, (uint16_t *)Data + 2);
+  }
+  //  XG_OFFSET_H_READ_OFFS_USR(Data);
+  I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)Data + 3);
+}
+
+void MPU6050::setActiveOffsets(int16_t offsets[6]) {
+  this->setXAccelOffset(offsets[0]);
+  this->setYAccelOffset(offsets[1]);
+  this->setZAccelOffset(offsets[2]);
+  this->setXGyroOffset(offsets[3]);
+  this->setYGyroOffset(offsets[4]);
+  this->setZGyroOffset(offsets[5]);
 }
diff --git a/src/MPU6050.h b/src/MPU6050.h
index 871f423..1275594 100755
--- a/src/MPU6050.h
+++ b/src/MPU6050.h
@@ -827,7 +827,8 @@ class MPU6050 {
 		void CalibrateAccel(uint8_t Loops = 15);// Fine tune after setting offsets with less Loops.
 		void PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops);  // Does the math
 		void PrintActiveOffsets(); // See the results of the Calibration
-
+		void getActiveOffsets(int16_t offsets[6]); // Get the results of the Calibration  
+		void setActiveOffsets(int16_t offsets[6]); // Set offsets
 
 
         // special methods for MotionApps 2.0 implementation