From 10ad073003cdb8cad4bff5ae829aa6b02ca529d1 Mon Sep 17 00:00:00 2001
From: jusax23 <jusax23@gmail.com>
Date: Sun, 21 Aug 2022 15:13:33 +0200
Subject: [PATCH] mpu offsets

---
 MPU.h | 102 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 100 insertions(+), 2 deletions(-)

diff --git a/MPU.h b/MPU.h
index 37ab0e6..15b0bdd 100644
--- a/MPU.h
+++ b/MPU.h
@@ -2,6 +2,21 @@
 #define jCommand_MPU_Acc 0
 #define jCommand_MPU_Gyro 1
 #define jCommand_MPU_Angle 2
+
+#define jCommand_MPU_reset_offset_calc 0x10
+
+#define jCommand_MPU_set_calc_offsets_acc 0x11
+#define jCommand_MPU_set_calc_offsets_gyro 0x12
+
+#define jCommand_MPU_get_calc_offsets_acc 0x13
+#define jCommand_MPU_get_calc_offsets_gyro 0x14
+
+#define jCommand_MPU_set_offsets_acc 0x15
+#define jCommand_MPU_set_offsets_gyro 0x16
+
+#define jCommand_MPU_get_offsets_acc 0x17
+#define jCommand_MPU_get_offsets_gyro 0x18
+
 #define jCommand_MPU_fail 0xff
 
 typedef struct{
@@ -20,8 +35,9 @@ MPU6050 mpu6050(Wire, 0.001f, 0.999f);
 
 void initMPU(){
   mpu6050.begin();
-  mpu6050.setGyroOffsets(-7.8,2.32,-0.22);
-  mpu6050.setAccXoffset(0.03,0.0,0.3);
+
+  mpu6050.setGyroOffsets(preferences.getFloat("mpuOffGyrX", -7.8), preferences.getFloat("mpuOffGyrY",2.32), preferences.getFloat("mpuOffGyrZ",-0.22));
+  mpu6050.setAccXoffset(preferences.getFloat("mpuOffAccX",0.03), preferences.getFloat("mpuOffAccY",0.0), preferences.getFloat("mpuOffAccZ",0.3));
   //mpu6050.calcGyroOffsets(false,50,50);
 }
 
@@ -55,6 +71,88 @@ void MPUradioTask(){
       data.Z = mpu6050.getAngleZ();
       jSendNRF(&data, sizeof(data));
     }break;
+
+    case jCommand_MPU_reset_offset_calc: {
+      mpu6050.restGyroOffsetGlide();
+      mpu6050.resetAccOffsetWatch();
+    }
+    break;
+    case jCommand_MPU_set_calc_offsets_acc:{
+      float offX = mpu6050.calcAccXoffset();
+      float offY = mpu6050.calcAccYoffset();
+      float offZ = mpu6050.calcAccZoffset();
+      preferences.putFloat("mpuOffAccX",offX);
+      preferences.putFloat("mpuOffAccY",offY);
+      preferences.putFloat("mpuOffAccZ",offZ);
+      mpu6050.setAccXoffset(offX,offY,offZ);
+    }
+    break;
+    case jCommand_MPU_set_calc_offsets_gyro:{
+      float offX = mpu6050.getGyroXoffsetGlide();
+      float offY = mpu6050.getGyroYoffsetGlide();
+      float offZ = mpu6050.getGyroZoffsetGlide();
+      preferences.putFloat("mpuOffGyrX",offX);
+      preferences.putFloat("mpuOffGyrY",offY);
+      preferences.putFloat("mpuOffGyrZ",offZ);
+      mpu6050.setGyroOffsets(offX,offY,offZ);
+    }
+    break;
+    case jCommand_MPU_get_calc_offsets_acc:{
+      mpuSendData data;
+      data.command = jCommand_MPU_get_calc_offsets_acc;
+      data.X = mpu6050.getGyroXoffsetGlide();
+      data.Y = mpu6050.getGyroYoffsetGlide();
+      data.Z = mpu6050.getGyroZoffsetGlide();
+      jSendNRF(&data, sizeof(data));
+    }
+    break;
+    case jCommand_MPU_get_calc_offsets_gyro:{
+      mpuSendData data;
+      data.command = jCommand_MPU_get_calc_offsets_gyro;
+      data.X = mpu6050.calcAccXoffset();
+      data.Y = mpu6050.calcAccYoffset();
+      data.Z = mpu6050.calcAccZoffset();
+      jSendNRF(&data, sizeof(data));
+    }
+    break;
+    case jCommand_MPU_set_offsets_acc:
+    {
+      mpuSendData data;
+      memmove(&data, &jRCbuff, sizeof(data));
+      preferences.putFloat("mpuOffGyrX", data.X);
+      preferences.putFloat("mpuOffGyrY", data.Y);
+      preferences.putFloat("mpuOffGyrZ", data.Z);
+      mpu6050.setGyroOffsets(data.X, data.Y, data.Z);
+    }
+    break;
+    case jCommand_MPU_set_offsets_gyro:
+    {
+      mpuSendData data;
+      memmove(&data, &jRCbuff, sizeof(data));
+      preferences.putFloat("mpuOffAccX", data.X);
+      preferences.putFloat("mpuOffAccY", data.Y);
+      preferences.putFloat("mpuOffAccZ", data.Z);
+      mpu6050.setAccXoffset(data.X, data.Y, data.Z);
+    }
+    break;
+
+    case jCommand_MPU_get_offsets_acc:{
+      mpuSendData data;
+      data.command = jCommand_MPU_get_offsets_acc;
+      data.X = preferences.getFloat("mpuOffGyrX", -7.8);
+      data.Y = preferences.getFloat("mpuOffGyrY", 2.32);
+      data.Z = preferences.getFloat("mpuOffGyrZ", -0.22);
+      jSendNRF(&data, sizeof(data));
+    }
+    break;
+    case jCommand_MPU_get_offsets_gyro:{
+      mpuSendData data;
+      data.command = jCommand_MPU_get_offsets_gyro;
+      data.X = preferences.getFloat("mpuOffAccX", 0.03);
+      data.Y = preferences.getFloat("mpuOffAccY", 0.0);
+      data.Z = preferences.getFloat("mpuOffAccZ", 0.3);
+      jSendNRF(&data, sizeof(data));
+    }
   }
 }