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/graphics/Screen.cpp

@@ -60,6 +60,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "main.h"
 #include "mesh-pb-constants.h"
 #include "mesh/Channels.h"
+#include "mesh/Default.h"
 #include "mesh/generated/meshtastic/deviceonly.pb.h"
 #include "modules/ExternalNotificationModule.h"
 #include "modules/TextMessageModule.h"
@@ -98,6 +99,7 @@ namespace graphics
 
 // This means the *visible* area (sh1106 can address 132, but shows 128 for example)
 #define IDLE_FRAMERATE 1 // in fps
+#define COMPASS_ACTIVE_FRAMERATE 20
 
 // DEBUG
 #define NUM_EXTRA_FRAMES 3 // text message and debug frame
@@ -135,6 +137,60 @@ static bool heartbeat = false;
 
 extern bool hasUnreadMessage;
 
+static inline float wrapHeading360(float heading)
+{
+    if (heading < 0.0f) {
+        heading += 360.0f;
+    } else if (heading >= 360.0f) {
+        heading -= 360.0f;
+    }
+    return heading;
+}
+
+void Screen::setHeading(float heading)
+{
+    const float wrappedHeading = wrapHeading360(heading);
+
+    if (!hasCompass) {
+        hasCompass = true;
+        compassHeading = wrappedHeading;
+        return;
+    }
+
+    // Interpolate using shortest-path angular delta to avoid jumps around 0/360.
+    float delta = wrappedHeading - compassHeading;
+    if (delta > 180.0f) {
+        delta -= 360.0f;
+    } else if (delta < -180.0f) {
+        delta += 360.0f;
+    }
+
+    // Adaptive filtering:
+    // - Strong damping for tiny deltas (jitter)
+    // - Faster response for larger turns
+    const float absDelta = (delta >= 0.0f) ? delta : -delta;
+    if (absDelta < 1.0f) {
+        return;
+    }
+
+    float alpha = 0.35f;
+    if (absDelta > 25.0f) {
+        alpha = 0.85f;
+    } else if (absDelta > 10.0f) {
+        alpha = 0.65f;
+    }
+
+    float step = delta * alpha;
+    const float maxStep = 12.0f;
+    if (step > maxStep) {
+        step = maxStep;
+    } else if (step < -maxStep) {
+        step = -maxStep;
+    }
+
+    compassHeading = wrapHeading360(compassHeading + step);
+}
+
 // ==============================
 // Overlay Alert Banner Renderer
 // ==============================
@@ -272,10 +328,25 @@ static void drawModuleFrame(OLEDDisplay *display, OLEDDisplayUiState *state, int
 float Screen::estimatedHeading(double lat, double lon)
 {
     static double oldLat, oldLon;
-    static float b;
+    static float b = -1.0f;
+    static uint32_t lastHeadingAtMs = 0;
+    const uint32_t now = millis();
+    const uint32_t gpsUpdateIntervalSecs =
+        Default::getConfiguredOrDefault(config.position.gps_update_interval, default_gps_update_interval);
+    uint32_t effectiveUpdateIntervalSecs = gpsUpdateIntervalSecs;
+    if (config.position.position_broadcast_smart_enabled) {
+        const uint32_t smartMinIntervalSecs = Default::getConfiguredOrDefault(
+            config.position.broadcast_smart_minimum_interval_secs, default_broadcast_smart_minimum_interval_secs);
+        if (smartMinIntervalSecs > effectiveUpdateIntervalSecs) {
+            effectiveUpdateIntervalSecs = smartMinIntervalSecs;
+        }
+    }
+    // Two expected update windows; keep arithmetic 32-bit to avoid pulling in larger 64-bit helpers.
+    const uint32_t headingStaleMs =
+        (effectiveUpdateIntervalSecs > (UINT32_MAX / 2000U)) ? UINT32_MAX : (effectiveUpdateIntervalSecs * 2000U);
 
     if (oldLat == 0) {
-        // just prepare for next time
+        // Need at least two position points before we can infer heading.
         oldLat = lat;
         oldLon = lon;
 
@@ -283,12 +354,20 @@ float Screen::estimatedHeading(double lat, double lon)
     }
 
     float d = GeoCoord::latLongToMeter(oldLat, oldLon, lat, lon);
-    if (d < 10) // haven't moved enough, just keep current bearing
+    if (d < 10) { // haven't moved enough, keep previous heading (invalid until first real movement)
+        if (lastHeadingAtMs != 0 && (now - lastHeadingAtMs) >= headingStaleMs) {
+            // Heading is stale after prolonged no-movement; force reacquire.
+            b = -1.0f;
+            oldLat = lat;
+            oldLon = lon;
+        }
         return b;
+    }
 
     b = GeoCoord::bearing(oldLat, oldLon, lat, lon) * RAD_TO_DEG;
     oldLat = lat;
     oldLon = lon;
+    lastHeadingAtMs = now;
 
     return b;
 }
@@ -923,9 +1002,22 @@ int32_t Screen::runOnce()
     // but we should only call setTargetFPS when framestate changes, because
     // otherwise that breaks animations.
 
-    if (targetFramerate != IDLE_FRAMERATE && ui->getUiState()->frameState == FIXED) {
+    uint32_t desiredFramerate = IDLE_FRAMERATE;
+#if HAS_GPS && !defined(USE_EINK)
+    if (showingNormalScreen && hasCompass) {
+        const uint8_t currentFrame = ui->getUiState()->currentFrame;
+        if ((framesetInfo.positions.gps != 255 && currentFrame == framesetInfo.positions.gps) ||
+            (framesetInfo.positions.waypoint != 255 && currentFrame == framesetInfo.positions.waypoint) ||
+            (framesetInfo.positions.firstFavorite != 255 && currentFrame >= framesetInfo.positions.firstFavorite &&
+             currentFrame <= framesetInfo.positions.lastFavorite)) {
+            desiredFramerate = COMPASS_ACTIVE_FRAMERATE;
+        }
+    }
+#endif
+
+    if (targetFramerate != desiredFramerate && ui->getUiState()->frameState == FIXED) {
         // oldFrameState = ui->getUiState()->frameState;
-        targetFramerate = IDLE_FRAMERATE;
+        targetFramerate = desiredFramerate;
 
         ui->setTargetFPS(targetFramerate);
         forceDisplay();

src/graphics/Screen.h

@@ -330,15 +330,11 @@ class Screen : public concurrency::OSThread
 
     // Function to allow the AccelerometerThread to set the heading if a sensor provides it
     // Mutex needed?
-    void setHeading(long _heading)
+    void setHeading(float heading);
-    {
-        hasCompass = true;
-        compassHeading = fmod(_heading, 360);
-    }
 
     bool hasHeading() { return hasCompass; }
 
-    long getHeading() { return compassHeading; }
+    float getHeading() { return compassHeading; }
 
     void setEndCalibration(uint32_t _endCalibrationAt) { endCalibrationAt = _endCalibrationAt; }
     uint32_t getEndCalibration() { return endCalibrationAt; }
@@ -782,4 +778,4 @@ extern std::vector<std::string> functionSymbol;
 extern std::string functionSymbolString;
 extern graphics::Screen *screen;
 
-#endif
+#endif

src/graphics/draw/CompassRenderer.cpp

@@ -1,10 +1,6 @@
 #include "configuration.h"
 #if HAS_SCREEN
 #include "CompassRenderer.h"
-#include "NodeDB.h"
-#include "UIRenderer.h"
-#include "configuration.h"
-#include "gps/GeoCoord.h"
 #include "graphics/ScreenFonts.h"
 #include "graphics/SharedUIDisplay.h"
 #include <cmath>
@@ -21,8 +17,8 @@ struct Point {
 
     void rotate(float angle)
     {
-        float cos_a = cos(angle);
+        float cos_a = cosf(angle);
-        float sin_a = sin(angle);
+        float sin_a = sinf(angle);
         float new_x = x * cos_a - y * sin_a;
         float new_y = x * sin_a + y * cos_a;
         x = new_x;
@@ -51,21 +47,30 @@ void drawCompassNorth(OLEDDisplay *display, int16_t compassX, int16_t compassY,
     if (currentResolution == ScreenResolution::High) {
         radius += 4;
     }
-    Point north(0, -radius);
+    float northX = 0.0f;
-    if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING)
+    float northY = -radius;
-        north.rotate(-myHeading);
+    if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING) {
-    north.translate(compassX, compassY);
+        const float c = cosf(-myHeading);
+        const float s = sinf(-myHeading);
+        const float rx = northX * c - northY * s;
+        const float ry = northX * s + northY * c;
+        northX = rx;
+        northY = ry;
+    }
+    northX += compassX;
+    northY += compassY;
 
     display->setFont(FONT_SMALL);
     display->setTextAlignment(TEXT_ALIGN_CENTER);
     display->setColor(BLACK);
+    const int16_t nLabelWidth = display->getStringWidth("N");
     if (currentResolution == ScreenResolution::High) {
-        display->fillRect(north.x - 8, north.y - 1, display->getStringWidth("N") + 3, FONT_HEIGHT_SMALL - 6);
+        display->fillRect(northX - 8, northY - 1, nLabelWidth + 3, FONT_HEIGHT_SMALL - 6);
     } else {
-        display->fillRect(north.x - 4, north.y - 1, display->getStringWidth("N") + 2, FONT_HEIGHT_SMALL - 6);
+        display->fillRect(northX - 4, northY - 1, nLabelWidth + 2, FONT_HEIGHT_SMALL - 6);
     }
     display->setColor(WHITE);
-    display->drawString(north.x, north.y - 3, "N");
+    display->drawString(northX, northY - 3, "N");
 }
 
 void drawNodeHeading(OLEDDisplay *display, int16_t compassX, int16_t compassY, uint16_t compassDiam, float headingRadian)
@@ -113,11 +118,46 @@ void drawArrowToNode(OLEDDisplay *display, int16_t x, int16_t y, int16_t size, f
     display->fillTriangle(tip.x, tip.y, right.x, right.y, tail.x, tail.y);
 }
 
-float estimatedHeading(double lat, double lon)
+bool getHeadingRadians(double lat, double lon, float &headingRadian)
 {
-    // Simple magnetic declination estimation
+    headingRadian = 0.0f;
-    // This is a very basic implementation - the original might be more sophisticated
+
-    return 0.0f; // Return 0 for now, indicating no heading available
+    if (uiconfig.compass_mode == meshtastic_CompassMode_FREEZE_HEADING)
+        return true;
+
+    if (!screen)
+        return false;
+
+    if (screen->hasHeading()) {
+        headingRadian = screen->getHeading() * DEG_TO_RAD;
+        return true;
+    }
+
+    const float estimatedHeadingDeg = screen->estimatedHeading(lat, lon);
+    if (!(estimatedHeadingDeg >= 0.0f))
+        return false;
+
+    headingRadian = estimatedHeadingDeg * DEG_TO_RAD;
+    return true;
+}
+
+float adjustBearingForCompassMode(float bearingRadian, float headingRadian)
+{
+    if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING)
+        return bearingRadian - headingRadian;
+
+    return bearingRadian;
+}
+
+float radiansToDegrees360(float angleRadian)
+{
+    constexpr float fullTurnDeg = 360.0f;
+    float degrees = angleRadian * RAD_TO_DEG;
+    if (degrees < 0.0f)
+        degrees += fullTurnDeg;
+    else if (degrees >= fullTurnDeg)
+        degrees -= fullTurnDeg;
+    return degrees;
 }
 
 uint16_t getCompassDiam(uint32_t displayWidth, uint32_t displayHeight)
@@ -137,4 +177,4 @@ uint16_t getCompassDiam(uint32_t displayWidth, uint32_t displayHeight)
 
 } // namespace CompassRenderer
 } // namespace graphics
-#endif
+#endif

src/graphics/draw/CompassRenderer.h

@@ -1,7 +1,6 @@
 #pragma once
 
 #include "graphics/Screen.h"
-#include "mesh/generated/meshtastic/mesh.pb.h"
 #include <OLEDDisplay.h>
 #include <OLEDDisplayUi.h>
 
@@ -25,7 +24,9 @@ void drawNodeHeading(OLEDDisplay *display, int16_t compassX, int16_t compassY, u
 void drawArrowToNode(OLEDDisplay *display, int16_t x, int16_t y, int16_t size, float bearing);
 
 // Navigation and location functions
-float estimatedHeading(double lat, double lon);
+bool getHeadingRadians(double lat, double lon, float &headingRadian);
+float adjustBearingForCompassMode(float bearingRadian, float headingRadian);
+float radiansToDegrees360(float angleRadian);
 uint16_t getCompassDiam(uint32_t displayWidth, uint32_t displayHeight);
 
 } // namespace CompassRenderer

src/graphics/draw/NodeListRenderer.cpp

@@ -409,14 +409,13 @@ void drawNodeDistance(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16
         }
     }
 
-    if (strlen(distStr) > 0) {
+    const char *distanceLabel = (strlen(distStr) > 0) ? distStr : "?";
-        int offset = (currentResolution == ScreenResolution::High)
+    int offset = (currentResolution == ScreenResolution::High)
-                         ? (isLeftCol ? 7 : 10) // Offset for Wide Screens (Left Column:Right Column)
+                     ? (isLeftCol ? 7 : 10) // Offset for Wide Screens (Left Column:Right Column)
-                         : (isLeftCol ? 4 : 7); // Offset for Narrow Screens (Left Column:Right Column)
+                     : (isLeftCol ? 4 : 7); // Offset for Narrow Screens (Left Column:Right Column)
-        int rightEdge = x + columnWidth - offset;
+    int rightEdge = x + columnWidth - offset;
-        int textWidth = display->getStringWidth(distStr);
+    int textWidth = display->getStringWidth(distanceLabel);
-        display->drawString(rightEdge - textWidth, y, distStr);
+    display->drawString(rightEdge - textWidth, y, distanceLabel);
-    }
 }
 
 void drawEntryDynamic_Nodes(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth)
@@ -467,8 +466,8 @@ void drawEntryCompass(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16
     }
 }
 
-void drawCompassArrow(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth, float myHeading,
+void drawCompassArrow(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth,
-                      double userLat, double userLon)
+                      float myHeadingRadian, double userLat, double userLon)
 {
     if (!nodeDB->hasValidPosition(node))
         return;
@@ -482,11 +481,11 @@ void drawCompassArrow(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16
     double nodeLat = node->position.latitude_i * 1e-7;
     double nodeLon = node->position.longitude_i * 1e-7;
     float bearing = GeoCoord::bearing(userLat, userLon, nodeLat, nodeLon);
-    float bearingToNode = RAD_TO_DEG * bearing;
+    float relativeBearing = CompassRenderer::adjustBearingForCompassMode(bearing, myHeadingRadian);
-    float relativeBearing = fmod((bearingToNode - myHeading + 360), 360);
+    float relativeBearingDeg = CompassRenderer::radiansToDegrees360(relativeBearing);
     // Shrink size by 2px
     int size = FONT_HEIGHT_SMALL - 5;
-    CompassRenderer::drawArrowToNode(display, centerX, centerY, size, relativeBearing);
+    CompassRenderer::drawArrowToNode(display, centerX, centerY, size, relativeBearingDeg);
     /*
     float angle = relativeBearing * DEG_TO_RAD;
     float halfSize = size / 2.0;
@@ -516,12 +515,27 @@ void drawCompassArrow(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16
     */
 }
 
+void drawCompassUnknown(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth, float, double,
+                        double)
+{
+    if (!nodeDB->hasValidPosition(node))
+        return;
+
+    bool isLeftCol = (x < SCREEN_WIDTH / 2);
+    int arrowXOffset = (currentResolution == ScreenResolution::High) ? (isLeftCol ? 22 : 24) : (isLeftCol ? 12 : 18);
+    int centerX = x + columnWidth - arrowXOffset;
+
+    display->setFont(FONT_SMALL);
+    display->setTextAlignment(TEXT_ALIGN_CENTER);
+    display->drawString(centerX, y, "?");
+}
+
 // =============================
 // Main Screen Functions
 // =============================
 
 void drawNodeListScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y, const char *title,
-                        EntryRenderer renderer, NodeExtrasRenderer extras, float heading, double lat, double lon)
+                        EntryRenderer renderer, NodeExtrasRenderer extras, float headingRadian, double lat, double lon)
 {
     const int COMMON_HEADER_HEIGHT = FONT_HEIGHT_SMALL - 1;
     const int rowYOffset = FONT_HEIGHT_SMALL - 3;
@@ -606,7 +620,7 @@ void drawNodeListScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t
         renderer(display, node, xPos, yPos, columnWidth);
 
         if (extras)
-            extras(display, node, xPos, yPos, columnWidth, heading, lat, lon);
+            extras(display, node, xPos, yPos, columnWidth, headingRadian, lat, lon);
 
         lastNodeY = max(lastNodeY, yPos + FONT_HEIGHT_SMALL);
         yOffset += rowYOffset;
@@ -801,9 +815,13 @@ void drawDistanceScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t
 #endif
 void drawNodeListWithCompasses(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
 {
-    float heading = 0;
+    float headingRadian = 0.0f;
-    bool validHeading = false;
     auto ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
+    if (!ourNode || !nodeDB->hasValidPosition(ourNode)) {
+        drawNodeListScreen(display, state, x, y, "Bearings", drawEntryCompass, drawCompassUnknown, headingRadian, 0.0, 0.0);
+        return;
+    }
+
     double lat = DegD(ourNode->position.latitude_i);
     double lon = DegD(ourNode->position.longitude_i);
 
@@ -815,21 +833,12 @@ void drawNodeListWithCompasses(OLEDDisplay *display, OLEDDisplayUiState *state,
         lastSwitchTime = now;
     }
 #endif
-    if (uiconfig.compass_mode != meshtastic_CompassMode_FREEZE_HEADING) {
+    if (!CompassRenderer::getHeadingRadians(lat, lon, headingRadian)) {
-#if HAS_GPS
+        drawNodeListScreen(display, state, x, y, "Bearings", drawEntryCompass, drawCompassUnknown, headingRadian, lat, lon);
-        if (screen->hasHeading()) {
+        return;
-            heading = screen->getHeading(); // degrees
-            validHeading = true;
-        } else {
-            heading = screen->estimatedHeading(lat, lon);
-            validHeading = !isnan(heading);
-        }
-#endif
-
-        if (!validHeading)
-            return;
     }
-    drawNodeListScreen(display, state, x, y, "Bearings", drawEntryCompass, drawCompassArrow, heading, lat, lon);
+
+    drawNodeListScreen(display, state, x, y, "Bearings", drawEntryCompass, drawCompassArrow, headingRadian, lat, lon);
 }
 
 /// Draw a series of fields in a column, wrapping to multiple columns if needed

src/graphics/draw/NodeListRenderer.h

@@ -32,7 +32,7 @@ enum ListMode_Location { MODE_DISTANCE = 0, MODE_BEARING = 1, MODE_COUNT_LOCATIO
 
 // Main node list screen function
 void drawNodeListScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y, const char *title,
-                        EntryRenderer renderer, NodeExtrasRenderer extras = nullptr, float heading = 0, double lat = 0,
+                        EntryRenderer renderer, NodeExtrasRenderer extras = nullptr, float headingRadian = 0, double lat = 0,
                         double lon = 0);
 
 // Entry renderers
@@ -43,8 +43,8 @@ void drawEntryDynamic_Nodes(OLEDDisplay *display, meshtastic_NodeInfoLite *node,
 void drawEntryCompass(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth);
 
 // Extras renderers
-void drawCompassArrow(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth, float myHeading,
+void drawCompassArrow(OLEDDisplay *display, meshtastic_NodeInfoLite *node, int16_t x, int16_t y, int columnWidth,
-                      double userLat, double userLon);
+                      float myHeadingRadian, double userLat, double userLon);
 
 // Screen frame functions
 void drawLastHeardScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y);

src/graphics/draw/UIRenderer.cpp

@@ -41,6 +41,15 @@ static inline void drawSatelliteIcon(OLEDDisplay *display, int16_t x, int16_t y)
     }
 }
 
+static void drawCompassStatusText(OLEDDisplay *display, int16_t compassX, int16_t compassY, const char *statusLine1,
+                                  const char *statusLine2)
+{
+    display->setTextAlignment(TEXT_ALIGN_CENTER);
+    display->drawString(compassX, compassY - FONT_HEIGHT_SMALL, statusLine1);
+    display->drawString(compassX, compassY, statusLine2);
+    display->setTextAlignment(TEXT_ALIGN_LEFT);
+}
+
 void graphics::UIRenderer::rebuildFavoritedNodes()
 {
     favoritedNodes.clear();
@@ -692,51 +701,54 @@ void UIRenderer::drawFavoriteNode(OLEDDisplay *display, OLEDDisplayUiState *stat
         display->drawString(x, getTextPositions(display)[line++], batLine);
     }
 
+    bool showCompass = false;
+    float myHeading = 0.0f;
+    float bearing = 0.0f;
+    const bool hasOwnPositionFix = (ourNode && nodeDB->hasValidPosition(ourNode));
+    const bool hasNodePositionFix = nodeDB->hasValidPosition(node);
+    const char *statusLine1 = nullptr;
+    const char *statusLine2 = nullptr;
+    if (hasOwnPositionFix && hasNodePositionFix) {
+        const auto &op = ourNode->position;
+        showCompass = CompassRenderer::getHeadingRadians(DegD(op.latitude_i), DegD(op.longitude_i), myHeading);
+        if (showCompass) {
+            const auto &p = node->position;
+            bearing = GeoCoord::bearing(DegD(op.latitude_i), DegD(op.longitude_i), DegD(p.latitude_i), DegD(p.longitude_i));
+            bearing = CompassRenderer::adjustBearingForCompassMode(bearing, myHeading);
+        } else {
+            statusLine1 = "No";
+            statusLine2 = "Heading";
+        }
+    } else if (!hasOwnPositionFix || !hasNodePositionFix) {
+        statusLine1 = "No";
+        statusLine2 = "Fix";
+    }
+
     // --- Compass Rendering: landscape (wide) screens use the original side-aligned logic ---
     if (SCREEN_WIDTH > SCREEN_HEIGHT) {
-        bool showCompass = false;
+        if (showCompass || statusLine1) {
-        if (ourNode && (nodeDB->hasValidPosition(ourNode) || screen->hasHeading()) && nodeDB->hasValidPosition(node)) {
-            showCompass = true;
-        }
-        if (showCompass) {
             const int16_t topY = getTextPositions(display)[1];
             const int16_t bottomY = SCREEN_HEIGHT - (FONT_HEIGHT_SMALL - 1);
             const int16_t usableHeight = bottomY - topY - 5;
             int16_t compassRadius = usableHeight / 2;
             if (compassRadius < 8)
                 compassRadius = 8;
-            const int16_t compassDiam = compassRadius * 2;
             const int16_t compassX = x + SCREEN_WIDTH - compassRadius - 8;
             const int16_t compassY = topY + (usableHeight / 2) + ((FONT_HEIGHT_SMALL - 1) / 2) + 2;
-
+            const int16_t compassDiam = compassRadius * 2;
-            const auto &op = ourNode->position;
-            float myHeading = screen->hasHeading() ? screen->getHeading() * PI / 180
-                                                   : screen->estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i));
-
-            const auto &p = node->position;
-            /* unused
-            float d =
-                GeoCoord::latLongToMeter(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
-            */
-            float bearing = GeoCoord::bearing(DegD(op.latitude_i), DegD(op.longitude_i), DegD(p.latitude_i), DegD(p.longitude_i));
-            if (uiconfig.compass_mode == meshtastic_CompassMode_FREEZE_HEADING) {
-                myHeading = 0;
-            } else {
-                bearing -= myHeading;
-            }
 
             display->drawCircle(compassX, compassY, compassRadius);
-            CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, compassRadius);
+            if (showCompass) {
-            CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, bearing);
+                CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, compassRadius);
+                CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, bearing);
+            } else {
+                drawCompassStatusText(display, compassX, compassY, statusLine1, statusLine2);
+            }
         }
         // else show nothing
     } else {
         // Portrait or square: put compass at the bottom and centered, scaled to fit available space
-        bool showCompass = false;
+        if (showCompass || statusLine1) {
-        if (ourNode && (nodeDB->hasValidPosition(ourNode) || screen->hasHeading()) && nodeDB->hasValidPosition(node)) {
-            showCompass = true;
-        }
-        if (showCompass) {
             int yBelowContent = (line > 0 && line <= 5) ? (getTextPositions(display)[line - 1] + FONT_HEIGHT_SMALL + 2)
                                                         : getTextPositions(display)[1];
             const int margin = 4;
@@ -747,8 +759,8 @@ void UIRenderer::drawFavoriteNode(OLEDDisplay *display, OLEDDisplayUiState *stat
 #else
             const int navBarHeight = 0;
 #endif
-            int availableHeight = SCREEN_HEIGHT - yBelowContent - navBarHeight - margin;
             // --------- END PATCH FOR EINK NAV BAR -----------
+            int availableHeight = SCREEN_HEIGHT - yBelowContent - navBarHeight - margin;
 
             if (availableHeight < FONT_HEIGHT_SMALL * 2)
                 return;
@@ -762,25 +774,13 @@ void UIRenderer::drawFavoriteNode(OLEDDisplay *display, OLEDDisplayUiState *stat
             int compassX = x + SCREEN_WIDTH / 2;
             int compassY = yBelowContent + availableHeight / 2;
 
-            const auto &op = ourNode->position;
-            float myHeading = 0;
-            if (uiconfig.compass_mode != meshtastic_CompassMode_FREEZE_HEADING) {
-                myHeading = screen->hasHeading() ? screen->getHeading() * PI / 180
-                                                 : screen->estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i));
-            }
-            graphics::CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, compassRadius);
-
-            const auto &p = node->position;
-            /* unused
-            float d =
-                GeoCoord::latLongToMeter(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
-            */
-            float bearing = GeoCoord::bearing(DegD(op.latitude_i), DegD(op.longitude_i), DegD(p.latitude_i), DegD(p.longitude_i));
-            if (uiconfig.compass_mode != meshtastic_CompassMode_FREEZE_HEADING)
-                bearing -= myHeading;
-            graphics::CompassRenderer::drawNodeHeading(display, compassX, compassY, compassRadius * 2, bearing);
-
             display->drawCircle(compassX, compassY, compassRadius);
+            if (showCompass) {
+                graphics::CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, compassRadius);
+                graphics::CompassRenderer::drawNodeHeading(display, compassX, compassY, compassRadius * 2, bearing);
+            } else {
+                drawCompassStatusText(display, compassX, compassY, statusLine1, statusLine2);
+            }
         }
         // else show nothing
     }
@@ -1216,6 +1216,7 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
 
     // === Header ===
     graphics::drawCommonHeader(display, x, y, titleStr);
+    const int *textPos = getTextPositions(display);
 
     // === First Row: My Location ===
 #if HAS_GPS
@@ -1230,12 +1231,12 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
         } else {
             displayLine = config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_NOT_PRESENT ? "No GPS" : "GPS off";
         }
-        drawSatelliteIcon(display, x, getTextPositions(display)[line]);
+        drawSatelliteIcon(display, x, textPos[line]);
         int xOffset = (currentResolution == ScreenResolution::High) ? 6 : 0;
-        display->drawString(x + 11 + xOffset, getTextPositions(display)[line++], displayLine);
+        display->drawString(x + 11 + xOffset, textPos[line++], displayLine);
     } else {
         // Onboard GPS
-        UIRenderer::drawGps(display, 0, getTextPositions(display)[line++], gpsStatus);
+        UIRenderer::drawGps(display, 0, textPos[line++], gpsStatus);
     }
 
     config.display.heading_bold = origBold;
@@ -1244,18 +1245,36 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
     geoCoord.updateCoords(int32_t(gpsStatus->getLatitude()), int32_t(gpsStatus->getLongitude()),
                           int32_t(gpsStatus->getAltitude()));
 
-    // === Determine Compass Heading ===
+    meshtastic_NodeInfoLite *ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
-    float heading = 0;
+    const bool hasOwnPositionFix = (ourNode && nodeDB->hasValidPosition(ourNode));
+    const bool hasLiveGpsFix =
+        (gpsStatus && gpsStatus->getHasLock() && (gpsStatus->getLatitude() != 0 || gpsStatus->getLongitude() != 0));
+    const bool hasSensorHeading = screen->hasHeading();
+    float heading = 0.0f;
     bool validHeading = false;
-    if (uiconfig.compass_mode == meshtastic_CompassMode_FREEZE_HEADING) {
+    const char *statusLine1 = nullptr;
-        validHeading = true;
+    const char *statusLine2 = nullptr;
-    } else {
+    if (hasSensorHeading || hasLiveGpsFix || hasOwnPositionFix) {
-        if (screen->hasHeading()) {
+        double headingLat = 0.0;
-            heading = radians(screen->getHeading());
+        double headingLon = 0.0;
-            validHeading = true;
+        if (hasLiveGpsFix) {
+            headingLat = DegD(gpsStatus->getLatitude());
+            headingLon = DegD(gpsStatus->getLongitude());
+        } else if (hasOwnPositionFix) {
+            const auto &op = ourNode->position;
+            headingLat = DegD(op.latitude_i);
+            headingLon = DegD(op.longitude_i);
+        }
+        validHeading = CompassRenderer::getHeadingRadians(headingLat, headingLon, heading);
+    }
+
+    if (!validHeading) {
+        if (hasSensorHeading || hasLiveGpsFix || hasOwnPositionFix) {
+            statusLine1 = "No";
+            statusLine2 = "Heading";
         } else {
-            heading = screen->estimatedHeading(geoCoord.getLatitude() * 1e-7, geoCoord.getLongitude() * 1e-7);
+            statusLine1 = "No";
-            validHeading = !isnan(heading);
+            statusLine2 = "Fix";
         }
     }
 
@@ -1273,18 +1292,18 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
             getUptimeStr(delta, "Last: ", uptimeStr, sizeof(uptimeStr), true);
 #endif
 
-            display->drawString(0, getTextPositions(display)[line++], uptimeStr);
+            display->drawString(0, textPos[line++], uptimeStr);
         } else {
-            display->drawString(0, getTextPositions(display)[line++], "Last: ?");
+            display->drawString(0, textPos[line++], "Last: ?");
         }
 
         // === Third Row: Line 1 GPS Info ===
-        UIRenderer::drawGpsCoordinates(display, x, getTextPositions(display)[line++], gpsStatus, "line1");
+        UIRenderer::drawGpsCoordinates(display, x, textPos[line++], gpsStatus, "line1");
 
         if (uiconfig.gps_format != meshtastic_DeviceUIConfig_GpsCoordinateFormat_OLC &&
             uiconfig.gps_format != meshtastic_DeviceUIConfig_GpsCoordinateFormat_MLS) {
             // === Fourth Row: Line 2 GPS Info ===
-            UIRenderer::drawGpsCoordinates(display, x, getTextPositions(display)[line++], gpsStatus, "line2");
+            UIRenderer::drawGpsCoordinates(display, x, textPos[line++], gpsStatus, "line2");
         }
 
         // === Final Row: Altitude ===
@@ -1295,14 +1314,14 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
         } else {
             snprintf(altitudeLine, sizeof(altitudeLine), "Alt: %.0im", alt);
         }
-        display->drawString(x, getTextPositions(display)[line++], altitudeLine);
+        display->drawString(x, textPos[line++], altitudeLine);
     }
 #if !defined(M5STACK_UNITC6L)
-    // === Draw Compass if heading is valid ===
+    // === Draw Compass ===
-    if (validHeading) {
+    if (validHeading || statusLine1) {
         // --- Compass Rendering: landscape (wide) screens use original side-aligned logic ---
         if (SCREEN_WIDTH > SCREEN_HEIGHT) {
-            const int16_t topY = getTextPositions(display)[1];
+            const int16_t topY = textPos[1];
             const int16_t bottomY = SCREEN_HEIGHT - (FONT_HEIGHT_SMALL - 1); // nav row height
             const int16_t usableHeight = bottomY - topY - 5;
 
@@ -1315,29 +1334,33 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
             // Center vertically and nudge down slightly to keep "N" clear of header
             const int16_t compassY = topY + (usableHeight / 2) + ((FONT_HEIGHT_SMALL - 1) / 2) + 2;
 
-            CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, -heading);
             display->drawCircle(compassX, compassY, compassRadius);
+            if (validHeading) {
+                CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, -heading);
 
-            // "N" label
+                // "N" label
-            float northAngle = 0;
+                float northAngle = 0;
-            if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING)
+                if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING)
-                northAngle = -heading;
+                    northAngle = -heading;
-            float radius = compassRadius;
+                float radius = compassRadius;
-            int16_t nX = compassX + (radius - 1) * sin(northAngle);
+                int16_t nX = compassX + (radius - 1) * sin(northAngle);
-            int16_t nY = compassY - (radius - 1) * cos(northAngle);
+                int16_t nY = compassY - (radius - 1) * cos(northAngle);
-            int16_t nLabelWidth = display->getStringWidth("N") + 2;
+                int16_t nLabelWidth = display->getStringWidth("N") + 2;
-            int16_t nLabelHeightBox = FONT_HEIGHT_SMALL + 1;
+                int16_t nLabelHeightBox = FONT_HEIGHT_SMALL + 1;
 
-            display->setColor(BLACK);
+                display->setColor(BLACK);
-            display->fillRect(nX - nLabelWidth / 2, nY - nLabelHeightBox / 2, nLabelWidth, nLabelHeightBox);
+                display->fillRect(nX - nLabelWidth / 2, nY - nLabelHeightBox / 2, nLabelWidth, nLabelHeightBox);
-            display->setColor(WHITE);
+                display->setColor(WHITE);
-            display->setFont(FONT_SMALL);
+                display->setFont(FONT_SMALL);
-            display->setTextAlignment(TEXT_ALIGN_CENTER);
+                display->setTextAlignment(TEXT_ALIGN_CENTER);
-            display->drawString(nX, nY - FONT_HEIGHT_SMALL / 2, "N");
+                display->drawString(nX, nY - FONT_HEIGHT_SMALL / 2, "N");
+            } else {
+                drawCompassStatusText(display, compassX, compassY, statusLine1, statusLine2);
+            }
         } else {
             // Portrait or square: put compass at the bottom and centered, scaled to fit available space
             // For E-Ink screens, account for navigation bar at the bottom!
-            int yBelowContent = getTextPositions(display)[5] + FONT_HEIGHT_SMALL + 2;
+            int yBelowContent = textPos[5] + FONT_HEIGHT_SMALL + 2;
             const int margin = 4;
             int availableHeight =
 #if defined(USE_EINK)
@@ -1358,25 +1381,29 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
             int compassX = x + SCREEN_WIDTH / 2;
             int compassY = yBelowContent + availableHeight / 2;
 
-            CompassRenderer::drawNodeHeading(display, compassX, compassY, compassRadius * 2, -heading);
             display->drawCircle(compassX, compassY, compassRadius);
+            if (validHeading) {
+                CompassRenderer::drawNodeHeading(display, compassX, compassY, compassRadius * 2, -heading);
 
-            // "N" label
+                // "N" label
-            float northAngle = 0;
+                float northAngle = 0;
-            if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING)
+                if (uiconfig.compass_mode != meshtastic_CompassMode_FIXED_RING)
-                northAngle = -heading;
+                    northAngle = -heading;
-            float radius = compassRadius;
+                float radius = compassRadius;
-            int16_t nX = compassX + (radius - 1) * sin(northAngle);
+                int16_t nX = compassX + (radius - 1) * sin(northAngle);
-            int16_t nY = compassY - (radius - 1) * cos(northAngle);
+                int16_t nY = compassY - (radius - 1) * cos(northAngle);
-            int16_t nLabelWidth = display->getStringWidth("N") + 2;
+                int16_t nLabelWidth = display->getStringWidth("N") + 2;
-            int16_t nLabelHeightBox = FONT_HEIGHT_SMALL + 1;
+                int16_t nLabelHeightBox = FONT_HEIGHT_SMALL + 1;
 
-            display->setColor(BLACK);
+                display->setColor(BLACK);
-            display->fillRect(nX - nLabelWidth / 2, nY - nLabelHeightBox / 2, nLabelWidth, nLabelHeightBox);
+                display->fillRect(nX - nLabelWidth / 2, nY - nLabelHeightBox / 2, nLabelWidth, nLabelHeightBox);
-            display->setColor(WHITE);
+                display->setColor(WHITE);
-            display->setFont(FONT_SMALL);
+                display->setFont(FONT_SMALL);
-            display->setTextAlignment(TEXT_ALIGN_CENTER);
+                display->setTextAlignment(TEXT_ALIGN_CENTER);
-            display->drawString(nX, nY - FONT_HEIGHT_SMALL / 2, "N");
+                display->drawString(nX, nY - FONT_HEIGHT_SMALL / 2, "N");
+            } else {
+                drawCompassStatusText(display, compassX, compassY, statusLine1, statusLine2);
+            }
         }
     }
 #endif

src/modules/WaypointModule.cpp

@@ -15,15 +15,6 @@
 
 WaypointModule *waypointModule;
 
-static inline float degToRad(float deg)
-{
-    return deg * PI / 180.0f;
-}
-static inline float radToDeg(float rad)
-{
-    return rad * 180.0f / PI;
-}
-
 ProcessMessage WaypointModule::handleReceived(const meshtastic_MeshPacket &mp)
 {
 #if defined(DEBUG_PORT) && !defined(DEBUG_MUTE)
@@ -91,9 +82,7 @@ void WaypointModule::drawFrame(OLEDDisplay *display, OLEDDisplayUiState *state,
 
     // === Header ===
     graphics::drawCommonHeader(display, x, y, titleStr);
-
+    const int *textPos = graphics::getTextPositions(display);
-    const int w = display->getWidth();
-    const int h = display->getHeight();
 
     // Decode the waypoint
     const meshtastic_MeshPacket &mp = devicestate.rx_waypoint;
@@ -108,71 +97,118 @@ void WaypointModule::drawFrame(OLEDDisplay *display, OLEDDisplayUiState *state,
     getTimeAgoStr(sinceReceived(&mp), lastStr, sizeof(lastStr));
 
     // Will contain distance information, passed as a field to drawColumns
-    char distStr[20];
+    char distStr[20] = "";
 
     // Get our node, to use our own position
     meshtastic_NodeInfoLite *ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
 
-    // Dimensions / co-ordinates for the compass/circle
+    // Match compass sizing/placement to favorite node screen logic.
-    const uint16_t compassDiam = graphics::CompassRenderer::getCompassDiam(w, h);
+    const int w = display->getWidth();
-    const int16_t compassX = x + w - (compassDiam / 2) - 5;
+    int16_t compassRadius = 8;
-    const int16_t compassY = (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT)
+    int16_t compassX = x + w - compassRadius - 8;
-                                 ? y + h / 2
+    int16_t compassY = y + display->getHeight() / 2;
-                                 : y + FONT_HEIGHT_SMALL + (h - FONT_HEIGHT_SMALL) / 2;
 
-    // If our node has a position:
+    if (SCREEN_WIDTH > SCREEN_HEIGHT) {
-    if (ourNode && (nodeDB->hasValidPosition(ourNode) || screen->hasHeading())) {
+        const int16_t topY = textPos[1];
-        const meshtastic_PositionLite &op = ourNode->position;
+        const int16_t bottomY = SCREEN_HEIGHT - (FONT_HEIGHT_SMALL - 1);
-        float myHeading;
+        const int16_t usableHeight = bottomY - topY - 5;
-        if (uiconfig.compass_mode == meshtastic_CompassMode_FREEZE_HEADING) {
+        compassRadius = usableHeight / 2;
-            myHeading = 0;
+        if (compassRadius < 8)
-        } else {
+            compassRadius = 8;
-            if (screen->hasHeading())
+        compassX = x + SCREEN_WIDTH - compassRadius - 8;
-                myHeading = degToRad(screen->getHeading());
+        compassY = topY + (usableHeight / 2) + ((FONT_HEIGHT_SMALL - 1) / 2) + 2;
-            else
+    } else {
-                myHeading = screen->estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i));
+        // Waypoint content uses rows 1..4, so place the compass below that block.
+        const int yBelowContent = textPos[4] + FONT_HEIGHT_SMALL + 2;
+        const int margin = 4;
+#if defined(USE_EINK)
+        const int iconSize = (graphics::currentResolution == graphics::ScreenResolution::High) ? 16 : 8;
+        const int navBarHeight = iconSize + 6;
+#else
+        const int navBarHeight = 0;
+#endif
+        const int availableHeight = SCREEN_HEIGHT - yBelowContent - navBarHeight - margin;
+        if (availableHeight > 0) {
+            compassRadius = availableHeight / 2;
+            if (compassRadius < 8)
+                compassRadius = 8;
+            if (compassRadius * 2 > SCREEN_WIDTH - 16)
+                compassRadius = (SCREEN_WIDTH - 16) / 2;
+            if (compassRadius < 8)
+                compassRadius = 8;
+            compassX = x + SCREEN_WIDTH / 2;
+            compassY = yBelowContent + availableHeight / 2;
         }
-        graphics::CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, (compassDiam / 2));
+    }
+    const uint16_t compassDiam = compassRadius * 2;
 
-        // Compass bearing to waypoint
+    const bool hasOwnPositionFix = (ourNode && nodeDB->hasValidPosition(ourNode));
-        float bearingToOther =
+    const char *statusLine1 = nullptr;
-            GeoCoord::bearing(DegD(op.latitude_i), DegD(op.longitude_i), DegD(wp.latitude_i), DegD(wp.longitude_i));
+    const char *statusLine2 = nullptr;
-        // If the top of the compass is a static north then bearingToOther can be drawn on the compass directly
-        // If the top of the compass is not a static north we need adjust bearingToOther based on heading
-        if (uiconfig.compass_mode != meshtastic_CompassMode_FREEZE_HEADING)
-            bearingToOther -= myHeading;
-        graphics::CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, bearingToOther);
 
-        float bearingToOtherDegrees = (bearingToOther < 0) ? bearingToOther + 2 * PI : bearingToOther;
+    // Distance only needs our own position fix; compass/bearing additionally needs heading.
-        bearingToOtherDegrees = radToDeg(bearingToOtherDegrees);
+    if (hasOwnPositionFix) {
+        const meshtastic_PositionLite &op = ourNode->position;
+        const float d =
+            GeoCoord::latLongToMeter(DegD(wp.latitude_i), DegD(wp.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
 
-        // Distance to Waypoint
+        // Always show distance once we have an own-position fix, even without heading.
-        float d = GeoCoord::latLongToMeter(DegD(wp.latitude_i), DegD(wp.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
         if (config.display.units == meshtastic_Config_DisplayConfig_DisplayUnits_IMPERIAL) {
             float feet = d * METERS_TO_FEET;
-            snprintf(distStr, sizeof(distStr), feet < (2 * MILES_TO_FEET) ? "%.0fft   %.0f°" : "%.1fmi   %.0f°",
+            snprintf(distStr, sizeof(distStr), feet < (2 * MILES_TO_FEET) ? "%.0fft" : "%.1fmi",
-                     feet < (2 * MILES_TO_FEET) ? feet : feet / MILES_TO_FEET, bearingToOtherDegrees);
+                     feet < (2 * MILES_TO_FEET) ? feet : feet / MILES_TO_FEET);
         } else {
-            snprintf(distStr, sizeof(distStr), d < 2000 ? "%.0fm   %.0f°" : "%.1fkm   %.0f°", d < 2000 ? d : d / 1000,
+            snprintf(distStr, sizeof(distStr), d < 2000 ? "%.0fm" : "%.1fkm", d < 2000 ? d : d / 1000);
-                     bearingToOtherDegrees);
         }
+
+        float myHeading = 0.0f;
+        const bool hasHeading =
+            graphics::CompassRenderer::getHeadingRadians(DegD(op.latitude_i), DegD(op.longitude_i), myHeading);
+        if (hasHeading) {
+            // Draw compass circle
+            display->drawCircle(compassX, compassY, compassRadius);
+            graphics::CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, compassRadius);
+
+            // Compass bearing to waypoint
+            float bearingToOther =
+                GeoCoord::bearing(DegD(op.latitude_i), DegD(op.longitude_i), DegD(wp.latitude_i), DegD(wp.longitude_i));
+            bearingToOther = graphics::CompassRenderer::adjustBearingForCompassMode(bearingToOther, myHeading);
+            graphics::CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, bearingToOther);
+
+            const float bearingToOtherDegrees = graphics::CompassRenderer::radiansToDegrees360(bearingToOther);
+
+            // Distance to waypoint with relative bearing when heading is available.
+            if (config.display.units == meshtastic_Config_DisplayConfig_DisplayUnits_IMPERIAL) {
+                float feet = d * METERS_TO_FEET;
+                snprintf(distStr, sizeof(distStr), feet < (2 * MILES_TO_FEET) ? "%.0fft   %.0f°" : "%.1fmi   %.0f°",
+                         feet < (2 * MILES_TO_FEET) ? feet : feet / MILES_TO_FEET, bearingToOtherDegrees);
+            } else {
+                snprintf(distStr, sizeof(distStr), d < 2000 ? "%.0fm   %.0f°" : "%.1fkm   %.0f°", d < 2000 ? d : d / 1000,
+                         bearingToOtherDegrees);
+            }
+
+        } else {
+            statusLine1 = "No";
+            statusLine2 = "Heading";
+        }
+    } else {
+        // No own fix yet, so compass/bearing data would be misleading.
+        statusLine1 = "No";
+        statusLine2 = "Fix";
     }
 
-    else {
+    if (statusLine1) {
-        display->drawString(compassX - FONT_HEIGHT_SMALL / 4, compassY - FONT_HEIGHT_SMALL / 2, "?");
+        display->drawCircle(compassX, compassY, compassRadius);
-
+        display->setTextAlignment(TEXT_ALIGN_CENTER);
-        // ? in the distance field
+        display->drawString(compassX, compassY - FONT_HEIGHT_SMALL, statusLine1);
-        snprintf(distStr, sizeof(distStr), "? %s ?°",
+        display->drawString(compassX, compassY, statusLine2);
-                 (config.display.units == meshtastic_Config_DisplayConfig_DisplayUnits_IMPERIAL) ? "mi" : "km");
     }
 
-    // Draw compass circle
-    display->drawCircle(compassX, compassY, compassDiam / 2);
-
     display->setTextAlignment(TEXT_ALIGN_LEFT); // Something above me changes to a different alignment, forcing a fix here!
-    display->drawString(0, graphics::getTextPositions(display)[line++], lastStr);
+    display->drawString(0, textPos[line++], lastStr);
-    display->drawString(0, graphics::getTextPositions(display)[line++], wp.name);
+    display->drawString(0, textPos[line++], wp.name);
-    display->drawString(0, graphics::getTextPositions(display)[line++], wp.description);
+    display->drawString(0, textPos[line++], wp.description);
-    display->drawString(0, graphics::getTextPositions(display)[line++], distStr);
+    if (distStr[0])
+        display->drawString(0, textPos[line++], distStr);
 }
 #endif

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();
-
+    heading = applyCompassOrientation(heading);
-    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;
-    }
     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) {
-
+        beginCalibrationDisplay(showingScreen);
-        if (!showingScreen) {
+        updateCalibrationExtrema(magAccel.x, magAccel.y, magAccel.z, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
-            powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration
+        finishCalibrationIfExpired(showingScreen, compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ,
-            showingScreen = true;
+                                   lowestZ);
-            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);
     }
 
-    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) {
+    heading = applyCompassOrientation(heading);
-    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;
-    }
     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;
+    seedCalibrationExtrema(magAccel.x, magAccel.y, magAccel.z, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
-    highestY = magAccel.y, lowestY = magAccel.y;
+    startCalibrationWindow(forSeconds);
-    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);
 #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) {
-
+        beginCalibrationDisplay(showingScreen);
-        if (!showingScreen) {
+        updateCalibrationExtrema(magX, magY, magZ, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
-            powerFSM.trigger(EVENT_PRESS); // keep screen alive during calibration
+        finishCalibrationIfExpired(showingScreen, compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ,
-            showingScreen = true;
+                                   lowestZ);
-            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);
     }
 
     magX -= (highestX + lowestX) / 2;
@@ -122,23 +99,7 @@ int32_t ICM20948Sensor::runOnce()
 
     float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
 
-    switch (config.display.compass_orientation) {
+    heading = applyCompassOrientation(heading);
-    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;
-    }
     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",
+    LOG_DEBUG("ICM20948 cal start %is", forSeconds);
-              highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
-    LOG_DEBUG("BMX160 calibration started for %is", forSeconds);
     if (sensor->dataReady()) {
         sensor->getAGMT();
-        highestX = sensor->agmt.mag.axes.x;
+        seedCalibrationExtrema(sensor->agmt.mag.axes.x, sensor->agmt.mag.axes.y, sensor->agmt.mag.axes.z, highestX, lowestX,
-        lowestX = sensor->agmt.mag.axes.x;
+                               highestY, lowestY, highestZ, lowestZ);
-        highestY = sensor->agmt.mag.axes.y;
-        lowestY = sensor->agmt.mag.axes.y;
-        highestZ = sensor->agmt.mag.axes.z;
-        lowestZ = sensor->agmt.mag.axes.z;
     } 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;
+    startCalibrationWindow(forSeconds);
-    uint16_t calibrateFor = forSeconds * 1000; // calibrate for seconds provided
-    endCalibrationAt = millis() + calibrateFor;
-    if (screen)
-        screen->setEndCalibration(endCalibrationAt);
 #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;
+    const int16_t width = display->getWidth();
-    sprintf(timeRemainingBuffer, "( %02d )", timeRemaining);
+    const int16_t height = display->getHeight();
-    display->setFont(FONT_SMALL);
+    const bool compactLayout = (height <= 80);
-    display->drawString(x, y + 40, timeRemainingBuffer);
+    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;
+        compassX = x + width - compassRadius - margin;
-        compassY = y + display->getHeight() / 2;
+        compassY = y + height / 2;
     } else {
-        compassX = x + display->getWidth() - compassDiam / 2 - 5;
+        compassX = x + width - compassRadius - margin;
-        compassY = y + FONT_HEIGHT_SMALL + (display->getHeight() - FONT_HEIGHT_SMALL) / 2;
+        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