Mesh node t1 fixes (#10602)

* Fixes

* Remove BATTERY_LPCOMP_THRESHOLD

BATTERY_LPCOMP_THRESHOLD is dead code — in main-nrf52.cpp it's inside #ifdef BATTERY_LPCOMP_INPUT, which this board intentionally doesn't define. The threshold value is never reached.

* Trunk fix

* Update MotionSensor.cpp

* fix

---------

Co-authored-by: Ben Meadors <benmmeadors@gmail.com>
Co-authored-by: Jonathan Bennett <jbennett@incomsystems.biz>

src/graphics/Screen.cpp

@@ -233,6 +233,16 @@ static inline float wrapHeading360(float heading)
     return heading;
 }
 
+static inline float wrapDelta180(float delta)
+{
+    if (delta > 180.0f) {
+        delta -= 360.0f;
+    } else if (delta < -180.0f) {
+        delta += 360.0f;
+    }
+    return delta;
+}
+
 void Screen::setHeading(float heading)
 {
     const float wrappedHeading = wrapHeading360(heading);
@@ -244,37 +254,30 @@ void Screen::setHeading(float heading)
     }
 
     // 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;
-    }
+    float delta = wrapDelta180(wrappedHeading - compassHeading);
 
     // 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;
-    }
+    if (absDelta >= 1.0f) {
+        float alpha = 0.35f;
+        if (absDelta > 25.0f) {
+            alpha = 0.85f;
+        } else if (absDelta > 10.0f) {
+            alpha = 0.65f;
+        }
 
-    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;
+        }
 
-    float step = delta * alpha;
-    const float maxStep = 12.0f;
-    if (step > maxStep) {
-        step = maxStep;
-    } else if (step < -maxStep) {
-        step = -maxStep;
+        compassHeading = wrapHeading360(compassHeading + step);
     }
-
-    compassHeading = wrapHeading360(compassHeading + step);
 }
 
 // ==============================

src/motion/ICM42607PSensor.cpp

@@ -4,10 +4,16 @@
 
 #include "detect/ScanI2CTwoWire.h"
 #include <ICM42670P.h>
+#include <math.h>
 
 static constexpr uint16_t ICM42607P_ACCEL_ODR_HZ = 50;
 static constexpr uint16_t ICM42607P_ACCEL_FSR_G = 2;
-static constexpr float ICM42607P_COUNTS_PER_G = 32768.0f / ICM42607P_ACCEL_FSR_G;
+static constexpr float ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG_VALUE =
+#ifdef ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG
+    ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG;
+#else
+    0.0f;
+#endif
 
 #ifdef ICM_42607P_INT_PIN
 volatile static bool ICM42607P_IRQ = false;
@@ -18,10 +24,7 @@ void ICM42607PSetInterrupt()
 }
 #endif
 
-ICM42607PSensor::ICM42607PSensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice)
-{
-    wire = ScanI2CTwoWire::fetchI2CBus(foundDevice.address);
-}
+ICM42607PSensor::ICM42607PSensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {}
 
 ICM42607PSensor::~ICM42607PSensor() = default;
 
@@ -30,6 +33,7 @@ bool ICM42607PSensor::init()
     bool addressLsb = deviceAddress() == ICM42607P_ADDR_ALT;
 
     LOG_DEBUG("ICM-42607-P begin on addr 0x%02X (port=%d)", deviceAddress(), devicePort());
+    TwoWire *wire = ScanI2CTwoWire::fetchI2CBus(device.address);
     sensor.reset();
     auto newSensor = std::make_unique<ICM42670>(*wire, addressLsb);
 
@@ -82,8 +86,22 @@ int32_t ICM42607PSensor::runOnce()
         return MOTION_SENSOR_CHECK_INTERVAL_MS;
     }
 
-    // LOG_DEBUG("ICM-42607-P accel read x=%.3fg y=%.3fg z=%.3fg", (float)event.accel[0] / ICM42607P_COUNTS_PER_G,
-    //           (float)event.accel[1] / ICM42607P_COUNTS_PER_G, (float)event.accel[2] / ICM42607P_COUNTS_PER_G);
+    float ax = static_cast<float>(event.accel[0]);
+    float ay = static_cast<float>(event.accel[1]);
+    const float az = static_cast<float>(event.accel[2]);
+
+    if (ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG_VALUE != 0.0f) {
+        static const float rotRad = ICM42607P_ACCEL_TO_COMPASS_ROTATION_DEG_VALUE * DEG_TO_RAD;
+        static const float cosTheta = cosf(rotRad);
+        static const float sinTheta = sinf(rotRad);
+        const float rotatedX = (ax * cosTheta) - (ay * sinTheta);
+        const float rotatedY = (ax * sinTheta) + (ay * cosTheta);
+        ax = rotatedX;
+        ay = rotatedY;
+    }
+
+    // Match the accel sign convention used by other FusionCompass sensor paths.
+    publishCompassAccelSample(ax, -ay, -az);
 
     return MOTION_SENSOR_CHECK_INTERVAL_MS;
 #endif

src/motion/ICM42607PSensor.h

@@ -14,7 +14,6 @@ class ICM42607PSensor : public MotionSensor
 {
   private:
     std::unique_ptr<ICM42670> sensor;
-    TwoWire *wire = nullptr;
 
   public:
     explicit ICM42607PSensor(ScanI2C::FoundDevice foundDevice);

src/motion/MMC5983MASensor.cpp

@@ -2,6 +2,7 @@
 
 #if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && __has_include(<SparkFun_MMC5983MA_Arduino_Library.h>)
 
+#include "Fusion/Fusion.h"
 #include "detect/ScanI2CTwoWire.h"
 
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN)
@@ -10,8 +11,11 @@ extern graphics::Screen *screen;
 
 static constexpr float MMC5983MA_ZERO_FIELD = 131072.0f;
 static constexpr float MMC5983MA_COUNTS_PER_GAUSS = 16384.0f;
-static constexpr uint16_t MMC5983MA_CONTINUOUS_FREQUENCY_HZ = 10;
+static constexpr uint16_t MMC5983MA_CONTINUOUS_FREQUENCY_HZ = 50;
+static constexpr int32_t MMC5983MA_UPDATE_INTERVAL_MS = 20;
 static constexpr float MMC5983MA_HEADING_OFFSET_DEG = 180.0f;
+static constexpr uint32_t MMC5983MA_ACCEL_STALE_MS = 300;
+static constexpr float MMC5983MA_MIN_AXIS_RADIUS = 1e-4f;
 
 MMC5983MASensor::MMC5983MASensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {}
 
@@ -62,7 +66,7 @@ int32_t MMC5983MASensor::runOnce()
 {
     float magX = 0, magY = 0, magZ = 0;
     if (!readMagnetometer(magX, magY, magZ)) {
-        return MOTION_SENSOR_CHECK_INTERVAL_MS;
+        return MMC5983MA_UPDATE_INTERVAL_MS;
     }
 
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN)
@@ -74,25 +78,53 @@ int32_t MMC5983MASensor::runOnce()
     }
 #endif
 
-    magX -= (highestX + lowestX) / 2;
-    magY -= (highestY + lowestY) / 2;
-    magZ -= (highestZ + lowestZ) / 2;
+    // Hard-iron bias removal.
+    magX -= (highestX + lowestX) * 0.5f;
+    magY -= (highestY + lowestY) * 0.5f;
+    magZ -= (highestZ + lowestZ) * 0.5f;
+
+    // Soft-iron diagonal scaling from calibration extrema.
+    const float radiusX = (highestX - lowestX) * 0.5f;
+    const float radiusY = (highestY - lowestY) * 0.5f;
+    const float radiusZ = (highestZ - lowestZ) * 0.5f;
+    const float avgRadius = (radiusX + radiusY + radiusZ) / 3.0f;
+    magX *= (radiusX > MMC5983MA_MIN_AXIS_RADIUS) ? (avgRadius / radiusX) : 1.0f;
+    magY *= (radiusY > MMC5983MA_MIN_AXIS_RADIUS) ? (avgRadius / radiusY) : 1.0f;
+    magZ *= (radiusZ > MMC5983MA_MIN_AXIS_RADIUS) ? (avgRadius / radiusZ) : 1.0f;
 
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
-    float heading = atan2f(magY, magX) * RAD_TO_DEG + MMC5983MA_HEADING_OFFSET_DEG;
-    if (heading < 0.0f) {
-        heading += 360.0f;
-    } else if (heading >= 360.0f) {
-        heading -= 360.0f;
+    float heading;
+    float accelX = 0.0f;
+    float accelY = 0.0f;
+    float accelZ = 0.0f;
+    uint32_t accelAgeMs = 0;
+
+    if (getLatestCompassAccelSample(accelX, accelY, accelZ, accelAgeMs) && accelAgeMs <= MMC5983MA_ACCEL_STALE_MS) {
+        FusionVector ga = {.axis = {accelX, accelY, accelZ}};
+        FusionVector ma = {.axis = {magX, magY, magZ}};
+        if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
+            ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
+            ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+        }
+        heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma) + MMC5983MA_HEADING_OFFSET_DEG;
+    } else {
+        heading = atan2f(magY, magX) * RAD_TO_DEG + MMC5983MA_HEADING_OFFSET_DEG;
     }
 
+    if (heading >= 360.0f)
+        heading -= 360.0f;
+    else if (heading < 0.0f)
+        heading += 360.0f;
+    heading = 360.0f - heading;
+    if (heading >= 360.0f)
+        heading -= 360.0f;
+
     heading = applyCompassOrientation(heading);
-    if (screen) {
+    if (screen)
         screen->setHeading(heading);
-    }
 #endif
 
-    return MOTION_SENSOR_CHECK_INTERVAL_MS;
+    return MMC5983MA_UPDATE_INTERVAL_MS;
 }
 
 void MMC5983MASensor::calibrate(uint16_t forSeconds)

src/motion/MagnetometerThread.h

@@ -47,9 +47,8 @@ class MagnetometerThread : public concurrency::OSThread
     {
         canSleep = true;
 
-        if (isInitialised) {
+        if (isInitialised)
             return sensor->runOnce();
-        }
 
         return MOTION_SENSOR_CHECK_INTERVAL_MS;
     }

src/motion/MotionSensor.cpp

@@ -2,6 +2,7 @@
 #include "FSCommon.h"
 #include "SPILock.h"
 #include "SafeFile.h"
+#include "concurrency/LockGuard.h"
 #include "graphics/draw/CompassRenderer.h"
 
 #if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C
@@ -30,6 +31,17 @@ bool isRangeValid(float highest, float lowest)
     // NaN/Inf guard without pulling in extra math helpers.
     return (highest == highest) && (lowest == lowest) && (highest > lowest);
 }
+
+struct CompassAccelSample {
+    float x = 0.0f;
+    float y = 0.0f;
+    float z = 0.0f;
+    uint32_t sampledAtMs = 0;
+    bool valid = false;
+};
+
+concurrency::Lock latestCompassAccelLock;
+CompassAccelSample latestCompassAccelSample;
 } // namespace
 
 // screen is defined in main.cpp
@@ -204,6 +216,35 @@ float MotionSensor::applyCompassOrientation(float heading)
     }
 }
 
+void MotionSensor::publishCompassAccelSample(float x, float y, float z)
+{
+    concurrency::LockGuard guard(&latestCompassAccelLock);
+    latestCompassAccelSample.x = x;
+    latestCompassAccelSample.y = y;
+    latestCompassAccelSample.z = z;
+    latestCompassAccelSample.sampledAtMs = millis();
+    latestCompassAccelSample.valid = true;
+}
+
+bool MotionSensor::getLatestCompassAccelSample(float &x, float &y, float &z, uint32_t &ageMs)
+{
+    uint32_t sampledAtMs = 0;
+    {
+        concurrency::LockGuard guard(&latestCompassAccelLock);
+        if (!latestCompassAccelSample.valid) {
+            return false;
+        }
+
+        x = latestCompassAccelSample.x;
+        y = latestCompassAccelSample.y;
+        z = latestCompassAccelSample.z;
+        sampledAtMs = latestCompassAccelSample.sampledAtMs;
+    }
+
+    ageMs = millis() - sampledAtMs;
+    return true;
+}
+
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
 void MotionSensor::drawFrameCalibration(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
 {

src/motion/MotionSensor.h

@@ -67,6 +67,8 @@ class MotionSensor
     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);
+    static void publishCompassAccelSample(float x, float y, float z);
+    static bool getLatestCompassAccelSample(float &x, float &y, float &z, uint32_t &ageMs);
 
     ScanI2C::FoundDevice device;