From b5fb77fe350a7f51751a061108ec3d28066cc09d Mon Sep 17 00:00:00 2001
From: jusax23 <jusax23@gmail.com>
Date: Thu, 21 Sep 2023 11:51:54 +0200
Subject: [PATCH] blocking callibrate, fix

---
 system_sensors/compass.h | 49 ++++++++++++++++++++++++++++++++--------
 system_sensors/mpu.h     |  6 ++---
 2 files changed, 42 insertions(+), 13 deletions(-)

diff --git a/system_sensors/compass.h b/system_sensors/compass.h
index a33d5a7..88603e7 100644
--- a/system_sensors/compass.h
+++ b/system_sensors/compass.h
@@ -3,7 +3,8 @@
 
 #define jType_S_COMP 0x22
 #define jCommand_COMP_fail 0xff
-// #define jCommand_Compass_Raw 0
+#define jCommand_Compass_Data 0
+#define jCommand_Compass_Calibrate 1
 
 typedef struct {
     byte type = jType_S_COMP;
@@ -19,9 +20,21 @@ typedef struct {
 #include <QMC5883LCompass.h>
 QMC5883LCompass compass;
 
-void initCOMP() { compass.init(); }
+bool compass_is_calibrating = false;
 
-void COMPLoop() { compass.read(); }
+void initCOMP() {
+    compass.init();
+    compass.setCalibrationOffsets(preferences.getFloat("compOffX", 0),
+                                  preferences.getFloat("compOffY", 0),
+                                  preferences.getFloat("compOffZ", 0));
+    compass.setCalibrationScales(preferences.getFloat("compScaleX", 1),
+                                 preferences.getFloat("compScaleY", 1),
+                                 preferences.getFloat("compScaleZ", 1));
+}
+
+void COMPLoop() {
+    if (!compass_is_calibrating) compass.read();
+}
 
 /*float Altitude = mpu6050.getAngleX()/180*PI;
 float Roll = mpu6050.getAngleY()/180*PI;
@@ -31,12 +44,30 @@ yh=compass.getX()*sin(Roll)*sin(Altitude)+compass.getY()*cos(Roll)-compass.getZ(
 float heading = atan2(yh, xh);*/
 
 void COMPradioTask() {
-    compData data;
-    data.X = compass.getX();
-    data.Y = compass.getY();
-    data.Z = compass.getZ();
-    data.angle = compass.getAzimuth();
-    jSendNRF(&data, sizeof(data));
+    switch (jRCbuff[1]) {
+        case jCommand_Compass_Data: {
+            compData data;
+            data.X = compass.getX();
+            data.Y = compass.getY();
+            data.Z = compass.getZ();
+            data.angle = compass.getAzimuth();
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_Compass_Calibrate: {
+            compass_is_calibrating = true;
+            delay(10);
+            compass.calibrate();
+
+            preferences.putFloat("compOffX", compass.getCalibrationOffset(0));
+            preferences.putFloat("compOffY", compass.getCalibrationOffset(1));
+            preferences.putFloat("compOffZ", compass.getCalibrationOffset(2));
+
+            preferences.putFloat("compScaleX", compass.getCalibrationScale(0));
+            preferences.putFloat("compScaleY", compass.getCalibrationScale(1));
+            preferences.putFloat("compScaleZ", compass.getCalibrationScale(2));
+            compass_is_calibrating = false;
+        } break;
+    }
 }
 
 #else
diff --git a/system_sensors/mpu.h b/system_sensors/mpu.h
index 6f8d70a..fd1f9eb 100644
--- a/system_sensors/mpu.h
+++ b/system_sensors/mpu.h
@@ -42,14 +42,12 @@ void initMPU() {
     Serial.println("began mpu6050");
 #endif
 
-    /*mpu6050.setGyroOffsets(preferences.getFloat("mpuOffGyrX", -7.8),
+    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.setGyroOffsets(-7.8, 2.32, -0.22);
+                          preferences.getFloat("mpuOffAccZ", 0.3));
 #if debug
     Serial.println("set mpu6050 gyro");
 #endif