Compass improvements/refactoring (#10166)

* Infinite calibration loop fix

* Save calibration

* Screen refresh

* reduce repeated code

* reduce repeated code to reduce flash

* fix Waypoint compass size and no fix no heading labels

* Don't show compass unless we have a heading and location

* If no calculated heading from moving, we should have no heading

* Slow walking calculated heading and auto stale heading when not moving

* Triming flash space

* cleanup

* show "?" when no location or heading for distance and heading screen

* cleanup

* Stale heading logic

* final trim

* Compass Calibration screen redesign

* Trunk Fix

* Compile fix

* patch

* Update src/motion/MotionSensor.cpp

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* Update WaypointModule.cpp

---------

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

src/motion/BMM150Sensor.cpp

@@ -7,9 +7,6 @@
 extern graphics::Screen *screen;
 #endif
 
-// Flag when an interrupt has been detected
-volatile static bool BMM150_IRQ = false;
-
 BMM150Sensor::BMM150Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {}
 
 bool BMM150Sensor::init()
@@ -23,24 +20,7 @@ int32_t BMM150Sensor::runOnce()
 {
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
     float heading = sensor->getCompassDegree();
-
-    switch (config.display.compass_orientation) {
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0_INVERTED:
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0:
-        break;
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90:
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED:
-        heading += 90;
-        break;
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180:
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED:
-        heading += 180;
-        break;
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270:
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED:
-        heading += 270;
-        break;
-    }
+    heading = applyCompassOrientation(heading);
     if (screen)
         screen->setHeading(heading);
 #endif
@@ -90,4 +70,4 @@ bool BMM150Singleton::init(ScanI2C::FoundDevice device)
     return true;
 }
 
-#endif
+#endif

src/motion/BMX160Sensor.cpp

@@ -16,6 +16,7 @@ bool BMX160Sensor::init()
     if (sensor.begin()) {
         // set output data rate
         sensor.ODR_Config(BMX160_ACCEL_ODR_100HZ, BMX160_GYRO_ODR_100HZ);
+        loadMagnetometerCalibration(compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
         LOG_DEBUG("BMX160 init ok");
         return true;
     }
@@ -33,42 +34,12 @@ int32_t BMX160Sensor::runOnce()
     sensor.getAllData(&magAccel, NULL, &gAccel);
 
     if (doCalibration) {
-
-        if (!showingScreen) {
-            powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration
-            showingScreen = true;
-            if (screen)
-                screen->startAlert((FrameCallback)drawFrameCalibration);
-        }
-
-        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) {
-            doCalibration = false;
-            endCalibrationAt = 0;
-            showingScreen = false;
-            if (screen)
-                screen->endAlert();
-        }
-
-        // LOG_DEBUG("BMX160 min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", lowestX, highestX,
-        // lowestY, highestY, lowestZ, highestZ);
+        beginCalibrationDisplay(showingScreen);
+        updateCalibrationExtrema(magAccel.x, magAccel.y, magAccel.z, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+        finishCalibrationIfExpired(showingScreen, compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ,
+                                   lowestZ);
     }
 
-    int highestRealX = highestX - (highestX + lowestX) / 2;
-
     magAccel.x -= (highestX + lowestX) / 2;
     magAccel.y -= (highestY + lowestY) / 2;
     magAccel.z -= (highestZ + lowestZ) / 2;
@@ -88,23 +59,7 @@ int32_t BMX160Sensor::runOnce()
 
     float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
 
-    switch (config.display.compass_orientation) {
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0_INVERTED:
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0:
-        break;
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90:
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED:
-        heading += 90;
-        break;
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180:
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED:
-        heading += 180;
-        break;
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270:
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED:
-        heading += 270;
-        break;
-    }
+    heading = applyCompassOrientation(heading);
     if (screen)
         screen->setHeading(heading);
 #endif
@@ -119,15 +74,8 @@ void BMX160Sensor::calibrate(uint16_t forSeconds)
     sBmx160SensorData_t gAccel;
     LOG_DEBUG("BMX160 calibration started for %is", forSeconds);
     sensor.getAllData(&magAccel, NULL, &gAccel);
-    highestX = magAccel.x, lowestX = magAccel.x;
-    highestY = magAccel.y, lowestY = magAccel.y;
-    highestZ = magAccel.z, lowestZ = magAccel.z;
-
-    doCalibration = true;
-    uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided
-    endCalibrationAt = millis() + calibrateFor;
-    if (screen)
-        screen->setEndCalibration(endCalibrationAt);
+    seedCalibrationExtrema(magAccel.x, magAccel.y, magAccel.z, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+    startCalibrationWindow(forSeconds);
 #endif
 }
 

src/motion/BMX160Sensor.h

@@ -17,6 +17,7 @@ class BMX160Sensor : public MotionSensor
   private:
     RAK_BMX160 sensor;
     bool showingScreen = false;
+    static constexpr const char *compassCalibrationFileName = "/prefs/compass_bmx160.dat";
     float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0;
 
   public:
@@ -39,4 +40,4 @@ class BMX160Sensor : public MotionSensor
 
 #endif
 
-#endif
+#endif

src/motion/ICM20948Sensor.cpp

@@ -26,7 +26,11 @@ bool ICM20948Sensor::init()
         return false;
 
     // Enable simple Wake on Motion
-    return sensor->setWakeOnMotion();
+    bool wakeOnMotionOk = sensor->setWakeOnMotion();
+    if (wakeOnMotionOk) {
+        loadMagnetometerCalibration(compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+    }
+    return wakeOnMotionOk;
 }
 
 #ifdef ICM_20948_INT_PIN
@@ -47,7 +51,8 @@ int32_t ICM20948Sensor::runOnce()
 int32_t ICM20948Sensor::runOnce()
 {
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
-    if (screen && !screen->isScreenOn() && !config.display.wake_on_tap_or_motion && !config.device.double_tap_as_button_press) {
+    if (screen && !doCalibration && !screen->isScreenOn() && !config.display.wake_on_tap_or_motion &&
+        !config.device.double_tap_as_button_press) {
         if (!isAsleep) {
             LOG_DEBUG("sleeping IMU");
             sensor->sleep(true);
@@ -69,38 +74,10 @@ int32_t ICM20948Sensor::runOnce()
     }
 
     if (doCalibration) {
-
-        if (!showingScreen) {
-            powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration
-            showingScreen = true;
-            if (screen)
-                screen->startAlert((FrameCallback)drawFrameCalibration);
-        }
-
-        if (magX > highestX)
-            highestX = magX;
-        if (magX < lowestX)
-            lowestX = magX;
-        if (magY > highestY)
-            highestY = magY;
-        if (magY < lowestY)
-            lowestY = magY;
-        if (magZ > highestZ)
-            highestZ = magZ;
-        if (magZ < lowestZ)
-            lowestZ = magZ;
-
-        uint32_t now = millis();
-        if (now > endCalibrationAt) {
-            doCalibration = false;
-            endCalibrationAt = 0;
-            showingScreen = false;
-            if (screen)
-                screen->endAlert();
-        }
-
-        // LOG_DEBUG("ICM20948 min_x: %.4f, max_X: %.4f, min_Y: %.4f, max_Y: %.4f, min_Z: %.4f, max_Z: %.4f", lowestX, highestX,
-        //           lowestY, highestY, lowestZ, highestZ);
+        beginCalibrationDisplay(showingScreen);
+        updateCalibrationExtrema(magX, magY, magZ, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+        finishCalibrationIfExpired(showingScreen, compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ,
+                                   lowestZ);
     }
 
     magX -= (highestX + lowestX) / 2;
@@ -122,23 +99,7 @@ int32_t ICM20948Sensor::runOnce()
 
     float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
 
-    switch (config.display.compass_orientation) {
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0_INVERTED:
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0:
-        break;
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90:
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED:
-        heading += 90;
-        break;
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180:
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED:
-        heading += 180;
-        break;
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270:
-    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED:
-        heading += 270;
-        break;
-    }
+    heading = applyCompassOrientation(heading);
     if (screen)
         screen->setHeading(heading);
 #endif
@@ -169,26 +130,16 @@ int32_t ICM20948Sensor::runOnce()
 void ICM20948Sensor::calibrate(uint16_t forSeconds)
 {
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
-    LOG_DEBUG("Old calibration data: highestX = %f, lowestX = %f, highestY = %f, lowestY = %f, highestZ = %f, lowestZ = %f",
-              highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
-    LOG_DEBUG("BMX160 calibration started for %is", forSeconds);
+    LOG_DEBUG("ICM20948 cal start %is", forSeconds);
     if (sensor->dataReady()) {
         sensor->getAGMT();
-        highestX = sensor->agmt.mag.axes.x;
-        lowestX = sensor->agmt.mag.axes.x;
-        highestY = sensor->agmt.mag.axes.y;
-        lowestY = sensor->agmt.mag.axes.y;
-        highestZ = sensor->agmt.mag.axes.z;
-        lowestZ = sensor->agmt.mag.axes.z;
+        seedCalibrationExtrema(sensor->agmt.mag.axes.x, sensor->agmt.mag.axes.y, sensor->agmt.mag.axes.z, highestX, lowestX,
+                               highestY, lowestY, highestZ, lowestZ);
     } else {
-        highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0;
+        seedCalibrationExtrema(0.0f, 0.0f, 0.0f, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
     }
 
-    doCalibration = true;
-    uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided
-    endCalibrationAt = millis() + calibrateFor;
-    if (screen)
-        screen->setEndCalibration(endCalibrationAt);
+    startCalibrationWindow(forSeconds);
 #endif
 }
 // ----------------------------------------------------------------------
@@ -314,11 +265,6 @@ bool ICM20948Singleton::setWakeOnMotion()
     status = intEnableWOM(true);
     LOG_DEBUG("ICM20948 init set intEnableWOM - %s", statusString());
     return status == ICM_20948_Stat_Ok;
-
-    // Clear any current interrupts
-    ICM20948_IRQ = false;
-    clearInterrupts();
-    return true;
 }
 
 #endif

src/motion/ICM20948Sensor.h

@@ -83,6 +83,7 @@ class ICM20948Sensor : public MotionSensor
     ICM20948Singleton *sensor = nullptr;
     bool showingScreen = false;
     bool isAsleep = false;
+    static constexpr const char *compassCalibrationFileName = "/prefs/compass_icm20948.dat";
 #ifdef MUZI_BASE
     float highestX = 449.000000, lowestX = -140.000000, highestY = 422.000000, lowestY = -232.000000, highestZ = 749.000000,
           lowestZ = 98.000000;
@@ -103,4 +104,4 @@ class ICM20948Sensor : public MotionSensor
 
 #endif
 
-#endif
+#endif

src/motion/MotionSensor.cpp

@@ -1,10 +1,37 @@
 #include "MotionSensor.h"
+#include "FSCommon.h"
+#include "SPILock.h"
+#include "SafeFile.h"
 #include "graphics/draw/CompassRenderer.h"
 
 #if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C
 
 char timeRemainingBuffer[12];
 
+namespace
+{
+constexpr uint32_t COMPASS_CALIBRATION_MAGIC = 0x4D43414CL; // "MCAL"
+constexpr uint16_t COMPASS_CALIBRATION_VERSION = 1;
+
+struct CompassCalibrationRecord {
+    uint32_t magic;
+    uint16_t version;
+    uint16_t reserved;
+    float highestX;
+    float lowestX;
+    float highestY;
+    float lowestY;
+    float highestZ;
+    float lowestZ;
+};
+
+bool isRangeValid(float highest, float lowest)
+{
+    // NaN/Inf guard without pulling in extra math helpers.
+    return (highest == highest) && (lowest == lowest) && (highest > lowest);
+}
+} // namespace
+
 // screen is defined in main.cpp
 extern graphics::Screen *screen;
 
@@ -32,33 +59,237 @@ ScanI2C::I2CPort MotionSensor::devicePort()
     return device.address.port;
 }
 
+bool MotionSensor::saveMagnetometerCalibration(const char *filePath, float highestX, float lowestX, float highestY, float lowestY,
+                                               float highestZ, float lowestZ)
+{
+#ifdef FSCom
+    if (!isRangeValid(highestX, lowestX) || !isRangeValid(highestY, lowestY) || !isRangeValid(highestZ, lowestZ)) {
+        return false;
+    }
+
+    FSCom.mkdir("/prefs");
+    CompassCalibrationRecord record = {
+        COMPASS_CALIBRATION_MAGIC, COMPASS_CALIBRATION_VERSION, 0, highestX, lowestX, highestY, lowestY, highestZ, lowestZ};
+
+    auto file = SafeFile(filePath, true);
+    const size_t written = file.write(reinterpret_cast<const uint8_t *>(&record), sizeof(record));
+    return (written == sizeof(record)) && file.close();
+#else
+    return false;
+#endif
+}
+
+bool MotionSensor::loadMagnetometerCalibration(const char *filePath, float &highestX, float &lowestX, float &highestY,
+                                               float &lowestY, float &highestZ, float &lowestZ)
+{
+#ifdef FSCom
+    CompassCalibrationRecord record = {};
+    size_t bytesRead = 0;
+
+    spiLock->lock();
+    auto file = FSCom.open(filePath, FILE_O_READ);
+    if (!file) {
+        spiLock->unlock();
+        return false;
+    }
+    bytesRead = file.read(reinterpret_cast<uint8_t *>(&record), sizeof(record));
+    file.close();
+    spiLock->unlock();
+
+    const bool headerValid = (bytesRead == sizeof(record)) && (record.magic == COMPASS_CALIBRATION_MAGIC) &&
+                             (record.version == COMPASS_CALIBRATION_VERSION) && (record.reserved == 0U);
+    const bool rangeValid = isRangeValid(record.highestX, record.lowestX) && isRangeValid(record.highestY, record.lowestY) &&
+                            isRangeValid(record.highestZ, record.lowestZ);
+    if (!headerValid || !rangeValid) {
+        return false;
+    }
+
+    highestX = record.highestX;
+    lowestX = record.lowestX;
+    highestY = record.highestY;
+    lowestY = record.lowestY;
+    highestZ = record.highestZ;
+    lowestZ = record.lowestZ;
+
+    return true;
+#else
+    return false;
+#endif
+}
+
+void MotionSensor::beginCalibrationDisplay(bool &showingScreen)
+{
+#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
+    if (!showingScreen) {
+        powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration
+        showingScreen = true;
+        if (screen)
+            screen->startAlert((FrameCallback)drawFrameCalibration);
+    }
+#else
+    (void)showingScreen;
+#endif
+}
+
+void MotionSensor::finishCalibrationIfExpired(bool &showingScreen, const char *filePath, float highestX, float lowestX,
+                                              float highestY, float lowestY, float highestZ, float lowestZ)
+{
+    const uint32_t now = millis();
+    if ((int32_t)(now - endCalibrationAt) < 0)
+        return;
+
+    doCalibration = false;
+    endCalibrationAt = 0;
+    showingScreen = false;
+    saveMagnetometerCalibration(filePath, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+
+#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
+    if (screen) {
+        screen->setEndCalibration(0);
+        screen->endAlert();
+    }
+#endif
+}
+
+void MotionSensor::startCalibrationWindow(uint16_t forSeconds)
+{
+    doCalibration = true;
+    const uint32_t calibrateFor = static_cast<uint32_t>(forSeconds) * 1000U;
+    endCalibrationAt = millis() + calibrateFor;
+#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
+    if (screen)
+        screen->setEndCalibration(endCalibrationAt);
+#endif
+}
+
+void MotionSensor::seedCalibrationExtrema(float x, float y, float z, float &highestX, float &lowestX, float &highestY,
+                                          float &lowestY, float &highestZ, float &lowestZ)
+{
+    highestX = lowestX = x;
+    highestY = lowestY = y;
+    highestZ = lowestZ = z;
+}
+
+void MotionSensor::updateCalibrationExtrema(float x, float y, float z, float &highestX, float &lowestX, float &highestY,
+                                            float &lowestY, float &highestZ, float &lowestZ)
+{
+    if (x > highestX)
+        highestX = x;
+    if (x < lowestX)
+        lowestX = x;
+    if (y > highestY)
+        highestY = y;
+    if (y < lowestY)
+        lowestY = y;
+    if (z > highestZ)
+        highestZ = z;
+    if (z < lowestZ)
+        lowestZ = z;
+}
+
+float MotionSensor::applyCompassOrientation(float heading)
+{
+    switch (config.display.compass_orientation) {
+    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90:
+    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED:
+        return heading + 90;
+    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180:
+    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED:
+        return heading + 180;
+    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270:
+    case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED:
+        return heading + 270;
+    default:
+        return heading;
+    }
+}
+
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
 void MotionSensor::drawFrameCalibration(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
 {
     if (screen == nullptr)
         return;
-    // int x_offset = display->width() / 2;
-    // int y_offset = display->height() <= 80 ? 0 : 32;
-    display->setTextAlignment(TEXT_ALIGN_LEFT);
-    display->setFont(FONT_MEDIUM);
-    display->drawString(x, y, "Calibrating\nCompass");
 
-    uint8_t timeRemaining = (screen->getEndCalibration() - millis()) / 1000;
-    sprintf(timeRemainingBuffer, "( %02d )", timeRemaining);
-    display->setFont(FONT_SMALL);
-    display->drawString(x, y + 40, timeRemainingBuffer);
+    const int16_t width = display->getWidth();
+    const int16_t height = display->getHeight();
+    const bool compactLayout = (height <= 80);
+    const int16_t margin = 4;
+
+    const uint32_t now = millis();
+    const uint32_t endCalibrationAt = screen->getEndCalibration();
+    uint32_t timeRemaining = 0;
+    if (endCalibrationAt > now) {
+        timeRemaining = (endCalibrationAt - now + 999) / 1000;
+    }
 
     int16_t compassX = 0, compassY = 0;
-    uint16_t compassDiam = graphics::CompassRenderer::getCompassDiam(display->getWidth(), display->getHeight());
+    uint16_t compassDiam = graphics::CompassRenderer::getCompassDiam(width, height);
+    const int16_t compassRadius = compassDiam / 2;
 
     // coordinates for the center of the compass/circle
     if (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT) {
-        compassX = x + display->getWidth() - compassDiam / 2 - 5;
-        compassY = y + display->getHeight() / 2;
+        compassX = x + width - compassRadius - margin;
+        compassY = y + height / 2;
     } else {
-        compassX = x + display->getWidth() - compassDiam / 2 - 5;
-        compassY = y + FONT_HEIGHT_SMALL + (display->getHeight() - FONT_HEIGHT_SMALL) / 2;
+        compassX = x + width - compassRadius - margin;
+        compassY = y + FONT_HEIGHT_SMALL + (height - FONT_HEIGHT_SMALL) / 2;
     }
+
+    const int16_t textLeft = x + 1;
+    const int16_t textRight = compassX - compassRadius - margin;
+    const int16_t textWidth = textRight - textLeft;
+    int16_t lineY = y;
+
+    display->setTextAlignment(TEXT_ALIGN_LEFT);
+    if (textWidth > 12) {
+        const char *title = "Cal";
+        const char *line1 = "Figure-8";
+        const char *line2 = "Rotate axes";
+        const char *line3 = "Away from metal";
+
+        display->setFont(FONT_SMALL);
+        if (!compactLayout && display->getStringWidth("Compass Calibration") <= textWidth) {
+            display->setFont(FONT_MEDIUM);
+            title = "Compass Calibration";
+            line1 = "Move in figure-8";
+            line2 = "Rotate all axes";
+            line3 = "Keep from metal";
+            display->drawString(textLeft, lineY, title);
+            lineY += FONT_HEIGHT_MEDIUM;
+            display->setFont(FONT_SMALL);
+        } else if (display->getStringWidth("Compass Cal") <= textWidth) {
+            title = "Compass Cal";
+            if (textWidth >= display->getStringWidth("Move in figure-8")) {
+                line1 = "Move in figure-8";
+                line2 = "Rotate all axes";
+                line3 = "Keep from metal";
+            }
+            display->drawString(textLeft, lineY, title);
+            lineY += FONT_HEIGHT_SMALL;
+        } else {
+            display->drawString(textLeft, lineY, title);
+            lineY += FONT_HEIGHT_SMALL;
+        }
+
+        display->drawString(textLeft, lineY, line1);
+        lineY += FONT_HEIGHT_SMALL;
+        display->drawString(textLeft, lineY, line2);
+        lineY += FONT_HEIGHT_SMALL;
+        if (!compactLayout || textWidth >= display->getStringWidth(line3)) {
+            display->drawString(textLeft, lineY, line3);
+        }
+    }
+
+    if (textWidth >= display->getStringWidth("000s left")) {
+        snprintf(timeRemainingBuffer, sizeof(timeRemainingBuffer), "%lus left", (unsigned long)timeRemaining);
+    } else {
+        snprintf(timeRemainingBuffer, sizeof(timeRemainingBuffer), "%lus", (unsigned long)timeRemaining);
+    }
+    display->setFont(FONT_SMALL);
+    if (textWidth > 12) {
+        display->drawString(textLeft, y + height - FONT_HEIGHT_SMALL - 1, timeRemainingBuffer);
+    }
+
     display->drawCircle(compassX, compassY, compassDiam / 2);
     graphics::CompassRenderer::drawCompassNorth(display, compassX, compassY, screen->getHeading() * PI / 180, (compassDiam / 2));
 }

src/motion/MotionSensor.h

@@ -2,7 +2,7 @@
 #ifndef _MOTION_SENSOR_H_
 #define _MOTION_SENSOR_H_
 
-#define MOTION_SENSOR_CHECK_INTERVAL_MS 100
+#define MOTION_SENSOR_CHECK_INTERVAL_MS 50
 #define MOTION_SENSOR_CLICK_THRESHOLD 40
 
 #include "../configuration.h"
@@ -54,6 +54,20 @@ class MotionSensor
     static void drawFrameCalibration(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y);
 #endif
 
+    bool saveMagnetometerCalibration(const char *filePath, float highestX, float lowestX, float highestY, float lowestY,
+                                     float highestZ, float lowestZ);
+    bool loadMagnetometerCalibration(const char *filePath, float &highestX, float &lowestX, float &highestY, float &lowestY,
+                                     float &highestZ, float &lowestZ);
+    void beginCalibrationDisplay(bool &showingScreen);
+    void finishCalibrationIfExpired(bool &showingScreen, const char *filePath, float highestX, float lowestX, float highestY,
+                                    float lowestY, float highestZ, float lowestZ);
+    void startCalibrationWindow(uint16_t forSeconds);
+    static void seedCalibrationExtrema(float x, float y, float z, float &highestX, float &lowestX, float &highestY,
+                                       float &lowestY, float &highestZ, float &lowestZ);
+    static void updateCalibrationExtrema(float x, float y, float z, float &highestX, float &lowestX, float &highestY,
+                                         float &lowestY, float &highestZ, float &lowestZ);
+    static float applyCompassOrientation(float heading);
+
     ScanI2C::FoundDevice device;
 
     // Do calibration if true
@@ -63,4 +77,4 @@ class MotionSensor
 
 #endif
 
-#endif
+#endif