From 148ceac296dc679070f0516253ae03eaef92bed2 Mon Sep 17 00:00:00 2001
From: Dan Welch <danwelch3@gmail.com>
Date: Sat, 14 Dec 2024 09:46:26 -0700
Subject: [PATCH] Improve compass calibration method

- initiate calibration with first read values to account for min values
  > 0
---
 src/motion/BMX160Sensor.cpp | 23 +++++++++++++++++------
 src/motion/MotionSensor.h   |  1 +
 2 files changed, 18 insertions(+), 6 deletions(-)

diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp
index 06cea3229..14a2571d6 100755
--- a/src/motion/BMX160Sensor.cpp
+++ b/src/motion/BMX160Sensor.cpp
@@ -40,18 +40,28 @@ int32_t BMX160Sensor::runOnce()
             screen->startAlert((FrameCallback)drawFrameCalibration);
         }
 
-        if (magAccel.x > highestX)
+        if (firstCalibrationRead) {
             highestX = magAccel.x;
-        if (magAccel.x < lowestX)
             lowestX = magAccel.x;
-        if (magAccel.y > highestY)
             highestY = magAccel.y;
-        if (magAccel.y < lowestY)
             lowestY = magAccel.y;
-        if (magAccel.z > highestZ)
             highestZ = magAccel.z;
-        if (magAccel.z < lowestZ)
             lowestZ = magAccel.z;
+            firstCalibrationRead = false;
+        } else {
+            if (magAccel.x > highestX)
+                highestX = magAccel.x;
+            if (magAccel.x < lowestX)
+                lowestX = magAccel.x;
+            if (magAccel.y > highestY)
+                highestY = magAccel.y;
+            if (magAccel.y < lowestY)
+                lowestY = magAccel.y;
+            if (magAccel.z > highestZ)
+                highestZ = magAccel.z;
+            if (magAccel.z < lowestZ)
+                lowestZ = magAccel.z;
+        }
 
         uint32_t now = millis();
         if (now > endCalibrationAt) {
@@ -116,6 +126,7 @@ void BMX160Sensor::calibrate(uint16_t forSeconds)
     LOG_DEBUG("BMX160 calibration started for %is", forSeconds);
 
     doCalibration = true;
+    firstCalibrationRead = true;
     uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided
     endCalibrationAt = millis() + calibrateFor;
     screen->setEndCalibration(endCalibrationAt);
diff --git a/src/motion/MotionSensor.h b/src/motion/MotionSensor.h
index 1f4d093bf..6026033bc 100755
--- a/src/motion/MotionSensor.h
+++ b/src/motion/MotionSensor.h
@@ -58,6 +58,7 @@ class MotionSensor
 
     // Do calibration if true
     bool doCalibration = false;
+    bool firstCalibrationRead = false;
     uint32_t endCalibrationAt = 0;
 };