From 2a111e26808b2028bd745dd3af9e23eb963975b6 Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Tue, 26 May 2026 10:34:09 +0200
Subject: [PATCH 01/13] feat(motion): add MPU-9250 / RAK1905 9-axis sensor
 support
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

The RAK12034 (BMX160) WisBlock module is end-of-life. RAK's official
replacement is the 6-axis RAK12033, which drops the magnetometer
entirely. RAK1905 (InvenSense MPU-9250) is the closest 9-axis WisBlock
module still in production and is a natural drop-in for users who want
to keep tilt-compensated compass functionality.

This adds a minimal direct-I2C driver — no new library dependency, ~5KB
flash on rak4631. The MPU-6500 accel/gyro die is driven directly; the
AK8963 magnetometer die is exposed via the MPU's I2C bypass mode and
talked to at 0x0C. Per-axis Fuse-ROM ASA sensitivity factors and ST2
overflow are honored. Hard-iron calibration is persisted to
/prefs/compass_mpu9250.dat, matching the BMX160 / ICM-20948 pattern.

Detection disambiguates MPU-6050 from MPU-9250/9255 by reading
WHO_AM_I (0x75) after the existing register-0x00 fallback path that
already covers ICM-20948 / BMI270 / BMX160 / SEN5X. MPU-6050 paths
are unchanged.
---
 src/configuration.h              |   3 +
 src/detect/ScanI2C.h             |   1 +
 src/detect/ScanI2CTwoWire.cpp    |  14 +-
 src/motion/AccelerometerThread.h |   4 +
 src/motion/MPU9250Sensor.cpp     | 281 +++++++++++++++++++++++++++++++
 src/motion/MPU9250Sensor.h       |  57 +++++++
 6 files changed, 357 insertions(+), 3 deletions(-)
 create mode 100644 src/motion/MPU9250Sensor.cpp
 create mode 100644 src/motion/MPU9250Sensor.h

diff --git a/src/configuration.h b/src/configuration.h
index a31198346d7..2c153f3d43c 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -292,6 +292,9 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define DA217_ADDR 0x26
 #define BMI270_ADDR 0x68
 #define BMI270_ADDR_ALT 0x69
+#define MPU9250_ADDR 0x68
+#define MPU9250_ADDR_ALT 0x69
+#define AK8963_ADDR 0x0C
 
 // -----------------------------------------------------------------------------
 // LED
diff --git a/src/detect/ScanI2C.h b/src/detect/ScanI2C.h
index 054c7854baf..775c44bc28c 100644
--- a/src/detect/ScanI2C.h
+++ b/src/detect/ScanI2C.h
@@ -92,6 +92,7 @@ class ScanI2C
         CST226SE,
         CST3530,
         BMI270,
+        MPU9250,
         SEN5X,
         SFA30,
         CW2015,
diff --git a/src/detect/ScanI2CTwoWire.cpp b/src/detect/ScanI2CTwoWire.cpp
index c1c48eac88b..070e065748d 100644
--- a/src/detect/ScanI2CTwoWire.cpp
+++ b/src/detect/ScanI2CTwoWire.cpp
@@ -796,11 +796,19 @@ void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize)
                         type = BMX160;
                         logFoundDevice("BMX160", (uint8_t)addr.address);
                         break;
-                    } else {
-                        type = MPU6050;
-                        logFoundDevice("MPU6050", (uint8_t)addr.address);
+                    }
+                    // MPU-6050 and MPU-9250/9255 share I2C addresses (0x68/0x69), and chip ID
+                    // register 0x00 reads 0x00 on MPU-9250. Disambiguate via WHO_AM_I (0x75):
+                    //   MPU-6050 -> 0x68, MPU-9250 -> 0x71, MPU-9255 -> 0x73
+                    registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x75), 1);
+                    if (registerValue == 0x71 || registerValue == 0x73) {
+                        type = MPU9250;
+                        logFoundDevice(registerValue == 0x73 ? "MPU9255" : "MPU9250", (uint8_t)addr.address);
                         break;
                     }
+                    type = MPU6050;
+                    logFoundDevice("MPU6050", (uint8_t)addr.address);
+                    break;
                 }
                 break;
 
diff --git a/src/motion/AccelerometerThread.h b/src/motion/AccelerometerThread.h
index d2205fd2a3e..bb785de2325 100755
--- a/src/motion/AccelerometerThread.h
+++ b/src/motion/AccelerometerThread.h
@@ -19,6 +19,7 @@
 #include "LIS3DHSensor.h"
 #include "LSM6DS3Sensor.h"
 #include "MPU6050Sensor.h"
+#include "MPU9250Sensor.h"
 #include "MotionSensor.h"
 #ifdef HAS_QMA6100P
 #include "QMA6100PSensor.h"
@@ -111,6 +112,9 @@ class AccelerometerThread : public concurrency::OSThread
         case ScanI2C::DeviceType::ICM20948:
             sensor = new ICM20948Sensor(device);
             break;
+        case ScanI2C::DeviceType::MPU9250:
+            sensor = new MPU9250Sensor(device);
+            break;
         case ScanI2C::DeviceType::BMM150:
             sensor = new BMM150Sensor(device);
             break;
diff --git a/src/motion/MPU9250Sensor.cpp b/src/motion/MPU9250Sensor.cpp
new file mode 100644
index 00000000000..d2748db726b
--- /dev/null
+++ b/src/motion/MPU9250Sensor.cpp
@@ -0,0 +1,281 @@
+#include "MPU9250Sensor.h"
+
+#if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C
+
+#if !defined(MESHTASTIC_EXCLUDE_SCREEN)
+// screen is defined in main.cpp
+extern graphics::Screen *screen;
+#endif
+
+namespace
+{
+// MPU-6500 die registers
+constexpr uint8_t MPU_SMPLRT_DIV = 0x19;
+constexpr uint8_t MPU_CONFIG = 0x1A;
+constexpr uint8_t MPU_GYRO_CONFIG = 0x1B;
+constexpr uint8_t MPU_ACCEL_CONFIG = 0x1C;
+constexpr uint8_t MPU_INT_PIN_CFG = 0x37;
+constexpr uint8_t MPU_ACCEL_XOUT_H = 0x3B;
+constexpr uint8_t MPU_USER_CTRL = 0x6A;
+constexpr uint8_t MPU_PWR_MGMT_1 = 0x6B;
+constexpr uint8_t MPU_WHO_AM_I = 0x75;
+
+// AK8963 magnetometer die registers
+constexpr uint8_t AK_WIA = 0x00;
+constexpr uint8_t AK_ST1 = 0x02;
+constexpr uint8_t AK_HXL = 0x03;
+constexpr uint8_t AK_ST2 = 0x09;
+constexpr uint8_t AK_CNTL1 = 0x0A;
+constexpr uint8_t AK_CNTL2 = 0x0B;
+constexpr uint8_t AK_ASAX = 0x10;
+
+// AK8963 control values
+constexpr uint8_t AK_MODE_POWERDOWN = 0x00;
+constexpr uint8_t AK_MODE_FUSE_ROM = 0x0F;
+constexpr uint8_t AK_MODE_CONT2_16BIT = 0x16; // continuous mode 2 (100 Hz), 16-bit output
+
+// Conversions
+// ±2g full scale, 16-bit signed -> g per LSB
+constexpr float ACCEL_LSB_PER_G = 16384.0f;
+// AK8963 16-bit output: 0.15 µT per LSB (datasheet 6.3)
+constexpr float MAG_UT_PER_LSB = 0.15f;
+} // namespace
+
+MPU9250Sensor::MPU9250Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {}
+
+TwoWire *MPU9250Sensor::resolveBus() const
+{
+#if defined(WIRE_INTERFACES_COUNT) && (WIRE_INTERFACES_COUNT > 1)
+    return device.address.port == ScanI2C::I2CPort::WIRE1 ? &Wire1 : &Wire;
+#else
+    return &Wire;
+#endif
+}
+
+bool MPU9250Sensor::writeRegister(uint8_t i2cAddr, uint8_t reg, uint8_t value)
+{
+    bus->beginTransmission(i2cAddr);
+    bus->write(reg);
+    bus->write(value);
+    return bus->endTransmission() == 0;
+}
+
+bool MPU9250Sensor::readRegisters(uint8_t i2cAddr, uint8_t reg, uint8_t *buf, uint8_t len)
+{
+    bus->beginTransmission(i2cAddr);
+    bus->write(reg);
+    if (bus->endTransmission(false) != 0) {
+        return false;
+    }
+    const uint8_t received = bus->requestFrom((int)i2cAddr, (int)len);
+    if (received != len) {
+        return false;
+    }
+    for (uint8_t i = 0; i < len; ++i) {
+        buf[i] = bus->read();
+    }
+    return true;
+}
+
+bool MPU9250Sensor::initMPU6500()
+{
+    const uint8_t addr = deviceAddress();
+
+    // Sanity-check WHO_AM_I — scan should have already verified this but be defensive
+    uint8_t whoAmI = 0;
+    if (!readRegisters(addr, MPU_WHO_AM_I, &whoAmI, 1)) {
+        LOG_DEBUG("MPU9250 WHO_AM_I read failed at 0x%02X", addr);
+        return false;
+    }
+    if (whoAmI != 0x71 && whoAmI != 0x73) {
+        LOG_DEBUG("MPU9250 unexpected WHO_AM_I 0x%02X at 0x%02X", whoAmI, addr);
+        return false;
+    }
+
+    // Reset device
+    if (!writeRegister(addr, MPU_PWR_MGMT_1, 0x80)) {
+        return false;
+    }
+    delay(100);
+
+    // Wake up, select PLL with X gyro reference (better stability than internal osc)
+    if (!writeRegister(addr, MPU_PWR_MGMT_1, 0x01)) {
+        return false;
+    }
+    delay(50);
+
+    // DLPF 41 Hz (CONFIG = 3), sample rate 1 kHz / (1 + SMPLRT_DIV) = 200 Hz
+    writeRegister(addr, MPU_CONFIG, 0x03);
+    writeRegister(addr, MPU_SMPLRT_DIV, 0x04);
+
+    // ±2 g accel range
+    writeRegister(addr, MPU_ACCEL_CONFIG, 0x00);
+    // ±250 dps gyro (gyro is unused for compass but configuring leaves the chip in a known state)
+    writeRegister(addr, MPU_GYRO_CONFIG, 0x00);
+
+    // Expose AK8963 on the main I2C bus: disable internal master, enable bypass
+    writeRegister(addr, MPU_USER_CTRL, 0x00);
+    writeRegister(addr, MPU_INT_PIN_CFG, 0x02);
+    delay(10);
+
+    return true;
+}
+
+bool MPU9250Sensor::initAK8963()
+{
+    uint8_t wia = 0;
+    if (!readRegisters(AK8963_ADDR, AK_WIA, &wia, 1) || wia != 0x48) {
+        LOG_DEBUG("MPU9250 AK8963 WHO_AM_I unexpected (0x%02X)", wia);
+        return false;
+    }
+
+    // Soft reset
+    writeRegister(AK8963_ADDR, AK_CNTL2, 0x01);
+    delay(100);
+
+    // Power-down then enter Fuse ROM access to read per-axis sensitivity (ASA)
+    writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_POWERDOWN);
+    delay(10);
+    writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_FUSE_ROM);
+    delay(10);
+
+    uint8_t asa[3] = {0};
+    if (!readRegisters(AK8963_ADDR, AK_ASAX, asa, 3)) {
+        LOG_DEBUG("MPU9250 AK8963 ASA read failed");
+        return false;
+    }
+    // Sensitivity adjustment: H_adj = H * ((ASA - 128) * 0.5 / 128 + 1) — datasheet 8.3.11
+    for (int i = 0; i < 3; ++i) {
+        asaScale[i] = (((float)asa[i] - 128.0f) * 0.5f / 128.0f) + 1.0f;
+    }
+
+    // Back to power-down, then enter continuous-mode-2 (100 Hz) at 16-bit resolution
+    writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_POWERDOWN);
+    delay(10);
+    writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_CONT2_16BIT);
+    delay(10);
+
+    return true;
+}
+
+bool MPU9250Sensor::init()
+{
+    bus = resolveBus();
+    if (!bus) {
+        return false;
+    }
+
+    if (!initMPU6500()) {
+        LOG_DEBUG("MPU9250 MPU-6500 init failed");
+        return false;
+    }
+    if (!initAK8963()) {
+        LOG_DEBUG("MPU9250 AK8963 init failed");
+        return false;
+    }
+
+    loadMagnetometerCalibration(compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+    LOG_DEBUG("MPU9250 init ok (asa=%.2f,%.2f,%.2f)", asaScale[0], asaScale[1], asaScale[2]);
+    return true;
+}
+
+bool MPU9250Sensor::readSensors(FusionVector &accel, FusionVector &mag)
+{
+    // Read 6 bytes of accel data
+    uint8_t raw[6] = {0};
+    if (!readRegisters(deviceAddress(), MPU_ACCEL_XOUT_H, raw, 6)) {
+        return false;
+    }
+    const int16_t ax = (int16_t)((raw[0] << 8) | raw[1]);
+    const int16_t ay = (int16_t)((raw[2] << 8) | raw[3]);
+    const int16_t az = (int16_t)((raw[4] << 8) | raw[5]);
+    accel.axis.x = (float)ax / ACCEL_LSB_PER_G;
+    accel.axis.y = (float)ay / ACCEL_LSB_PER_G;
+    accel.axis.z = (float)az / ACCEL_LSB_PER_G;
+
+    // Check magnetometer data-ready bit
+    uint8_t st1 = 0;
+    if (!readRegisters(AK8963_ADDR, AK_ST1, &st1, 1) || (st1 & 0x01) == 0) {
+        return false;
+    }
+
+    // Read 6 bytes of mag (little-endian) plus ST2 to release the data register.
+    // ST2 also exposes the magnetic overflow flag (bit 3) — discard the sample if set.
+    uint8_t magRaw[7] = {0};
+    if (!readRegisters(AK8963_ADDR, AK_HXL, magRaw, 7)) {
+        return false;
+    }
+    if (magRaw[6] & 0x08) {
+        return false;
+    }
+    const int16_t mx = (int16_t)((magRaw[1] << 8) | magRaw[0]);
+    const int16_t my = (int16_t)((magRaw[3] << 8) | magRaw[2]);
+    const int16_t mz = (int16_t)((magRaw[5] << 8) | magRaw[4]);
+
+    mag.axis.x = (float)mx * MAG_UT_PER_LSB * asaScale[0];
+    mag.axis.y = (float)my * MAG_UT_PER_LSB * asaScale[1];
+    mag.axis.z = (float)mz * MAG_UT_PER_LSB * asaScale[2];
+    return true;
+}
+
+int32_t MPU9250Sensor::runOnce()
+{
+#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
+    FusionVector accel = {0, 0, 0};
+    FusionVector mag = {0, 0, 0};
+    if (!readSensors(accel, mag)) {
+        return MOTION_SENSOR_CHECK_INTERVAL_MS;
+    }
+
+    if (doCalibration) {
+        beginCalibrationDisplay(showingScreen);
+        updateCalibrationExtrema(mag.axis.x, mag.axis.y, mag.axis.z, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+        finishCalibrationIfExpired(showingScreen, compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ,
+                                   lowestZ);
+    }
+
+    // Subtract hard-iron offsets
+    mag.axis.x -= (highestX + lowestX) / 2;
+    mag.axis.y -= (highestY + lowestY) / 2;
+    mag.axis.z -= (highestZ + lowestZ) / 2;
+
+    // The MPU-6500 accelerometer and AK8963 magnetometer dies sit inside the
+    // same package but use different axis conventions (datasheet 7.4 vs 8.1).
+    // To express the mag reading in the accel frame we swap X<->Y and negate Z.
+    // Final tweaks for board orientation are handled by config.display.compass_orientation.
+    FusionVector ga, ma;
+    ga.axis.x = accel.axis.x;
+    ga.axis.y = -accel.axis.y;
+    ga.axis.z = -accel.axis.z;
+    ma.axis.x = mag.axis.y;
+    ma.axis.y = mag.axis.x;
+    ma.axis.z = -mag.axis.z;
+
+    if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
+        ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
+        ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+    }
+
+    float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
+    heading = applyCompassOrientation(heading);
+    if (screen)
+        screen->setHeading(heading);
+#endif
+    return MOTION_SENSOR_CHECK_INTERVAL_MS;
+}
+
+void MPU9250Sensor::calibrate(uint16_t forSeconds)
+{
+#if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
+    LOG_DEBUG("MPU9250 calibration started for %is", forSeconds);
+    FusionVector accel, mag;
+    if (readSensors(accel, mag)) {
+        seedCalibrationExtrema(mag.axis.x, mag.axis.y, mag.axis.z, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+    } else {
+        seedCalibrationExtrema(0.0f, 0.0f, 0.0f, highestX, lowestX, highestY, lowestY, highestZ, lowestZ);
+    }
+    startCalibrationWindow(forSeconds);
+#endif
+}
+
+#endif
diff --git a/src/motion/MPU9250Sensor.h b/src/motion/MPU9250Sensor.h
new file mode 100644
index 00000000000..f2d26cccd3a
--- /dev/null
+++ b/src/motion/MPU9250Sensor.h
@@ -0,0 +1,57 @@
+#pragma once
+#ifndef _MPU9250_SENSOR_H_
+#define _MPU9250_SENSOR_H_
+
+#include "MotionSensor.h"
+
+#if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C
+
+#include "Fusion/Fusion.h"
+#include <Wire.h>
+
+// InvenSense MPU-9250 (and MPU-9255) 9-axis IMU driver.
+// The package contains an MPU-6500 accel/gyro die plus an AK8963 magnetometer
+// die. We drive both directly over I2C: the MPU-6500 at deviceAddress() and the
+// AK8963 at AK8963_ADDR via the MPU's I2C bypass mode.
+// This driver covers the RAK1905 WisBlock module (MPU-9250) — a viable
+// replacement for the EOL RAK12034 (BMX160) when tilt-compensated heading is
+// needed alongside RAK1904 (LIS3DH) options.
+class MPU9250Sensor : public MotionSensor
+{
+  private:
+    TwoWire *bus = nullptr;
+    bool showingScreen = false;
+
+    // Per-axis AK8963 factory sensitivity adjustment scale factors derived from
+    // the chip's Fuse ROM (ASA registers). Applied to every raw mag sample.
+    float asaScale[3] = {1.0f, 1.0f, 1.0f};
+
+    // Hard-iron calibration extrema persisted to flash.
+    static constexpr const char *compassCalibrationFileName = "/prefs/compass_mpu9250.dat";
+    float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0;
+
+    // Pick the correct TwoWire instance for the detected device.
+    TwoWire *resolveBus() const;
+
+    // Low-level I2C helpers — one address per call so we can address both dies.
+    bool writeRegister(uint8_t i2cAddr, uint8_t reg, uint8_t value);
+    bool readRegisters(uint8_t i2cAddr, uint8_t reg, uint8_t *buf, uint8_t len);
+
+    // Init helpers.
+    bool initMPU6500();
+    bool initAK8963();
+
+    // Read accel (g) and mag (µT) into Fusion vectors; returns false if mag
+    // had no fresh sample on this tick.
+    bool readSensors(FusionVector &accel, FusionVector &mag);
+
+  public:
+    explicit MPU9250Sensor(ScanI2C::FoundDevice foundDevice);
+    virtual bool init() override;
+    virtual int32_t runOnce() override;
+    virtual void calibrate(uint16_t forSeconds) override;
+};
+
+#endif
+
+#endif

From 70a821ae250a7cd82dd8e5c627ee9615044809a4 Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Tue, 26 May 2026 21:09:51 +0200
Subject: [PATCH 02/13] fix(motion): include MPU9250 in firstAccelerometer()
 type list
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Detection found the chip and set the device type correctly, but
ScanI2C::firstAccelerometer() iterates a hard-coded list that didn't
include MPU9250 — so accelerometer_found stayed unset and the
AccelerometerThread was never constructed for the new sensor.
---
 src/detect/ScanI2C.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/detect/ScanI2C.cpp b/src/detect/ScanI2C.cpp
index 75eabc9541d..d3a7acec33d 100644
--- a/src/detect/ScanI2C.cpp
+++ b/src/detect/ScanI2C.cpp
@@ -37,8 +37,9 @@ ScanI2C::FoundDevice ScanI2C::firstKeyboard() const
 
 ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const
 {
-    ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160, STK8BAXX, ICM20948, QMA6100P, BMM150, BMI270};
-    return firstOfOrNONE(10, types);
+    ScanI2C::DeviceType types[] = {MPU6050,  LIS3DH,   BMA423,  LSM6DS3, BMX160, STK8BAXX,
+                                   ICM20948, QMA6100P, BMM150, BMI270,  MPU9250};
+    return firstOfOrNONE(11, types);
 }
 
 ScanI2C::FoundDevice ScanI2C::firstAQI() const

From e532033d2b1e1a0b86c734b4d35f991d997c766e Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Tue, 26 May 2026 21:31:39 +0200
Subject: [PATCH 03/13] style(motion): apply clang-format to MPU9250 type list

---
 src/detect/ScanI2C.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/detect/ScanI2C.cpp b/src/detect/ScanI2C.cpp
index d3a7acec33d..2d11b346cf9 100644
--- a/src/detect/ScanI2C.cpp
+++ b/src/detect/ScanI2C.cpp
@@ -37,7 +37,7 @@ ScanI2C::FoundDevice ScanI2C::firstKeyboard() const
 
 ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const
 {
-    ScanI2C::DeviceType types[] = {MPU6050,  LIS3DH,   BMA423,  LSM6DS3, BMX160, STK8BAXX,
+    ScanI2C::DeviceType types[] = {MPU6050,  LIS3DH,   BMA423, LSM6DS3, BMX160, STK8BAXX,
                                    ICM20948, QMA6100P, BMM150, BMI270,  MPU9250};
     return firstOfOrNONE(11, types);
 }

From 1e867ff1f3ffd20d01720e3b6fbd11de86b4718a Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Tue, 26 May 2026 21:55:20 +0200
Subject: [PATCH 04/13] feat(motion): smooth MPU9250 accel + mag inputs for
 stable heading

Compass-only fusion is stateless, so any dynamic acceleration during
device rotation immediately showed up as overshoot on the displayed
heading. Add a per-axis exponential moving average on the raw accel
and mag samples before tilt compensation. Accel uses alpha=0.15 to
keep the gravity reference stable; mag uses alpha=0.20 for slightly
faster response. Time constant is well under half a second so the
needle still tracks deliberate rotation, but small jitters and
rotation-induced spikes are damped.
---
 src/motion/MPU9250Sensor.cpp | 19 +++++++++++++++++--
 src/motion/MPU9250Sensor.h   | 10 ++++++++++
 2 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/src/motion/MPU9250Sensor.cpp b/src/motion/MPU9250Sensor.cpp
index d2748db726b..930aae27b86 100644
--- a/src/motion/MPU9250Sensor.cpp
+++ b/src/motion/MPU9250Sensor.cpp
@@ -221,8 +221,8 @@ bool MPU9250Sensor::readSensors(FusionVector &accel, FusionVector &mag)
 int32_t MPU9250Sensor::runOnce()
 {
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
-    FusionVector accel = {0, 0, 0};
-    FusionVector mag = {0, 0, 0};
+    FusionVector accel = {{0, 0, 0}};
+    FusionVector mag = {{0, 0, 0}};
     if (!readSensors(accel, mag)) {
         return MOTION_SENSOR_CHECK_INTERVAL_MS;
     }
@@ -239,6 +239,21 @@ int32_t MPU9250Sensor::runOnce()
     mag.axis.y -= (highestY + lowestY) / 2;
     mag.axis.z -= (highestZ + lowestZ) / 2;
 
+    // Smooth raw inputs with a per-axis EMA to suppress dynamic acceleration
+    // noise during device rotation.
+    if (!filtersSeeded) {
+        accelFiltered = accel;
+        magFiltered = mag;
+        filtersSeeded = true;
+    } else {
+        for (int i = 0; i < 3; ++i) {
+            accelFiltered.array[i] = accelFilterAlpha * accel.array[i] + (1.0f - accelFilterAlpha) * accelFiltered.array[i];
+            magFiltered.array[i] = magFilterAlpha * mag.array[i] + (1.0f - magFilterAlpha) * magFiltered.array[i];
+        }
+    }
+    accel = accelFiltered;
+    mag = magFiltered;
+
     // The MPU-6500 accelerometer and AK8963 magnetometer dies sit inside the
     // same package but use different axis conventions (datasheet 7.4 vs 8.1).
     // To express the mag reading in the accel frame we swap X<->Y and negate Z.
diff --git a/src/motion/MPU9250Sensor.h b/src/motion/MPU9250Sensor.h
index f2d26cccd3a..00490656b68 100644
--- a/src/motion/MPU9250Sensor.h
+++ b/src/motion/MPU9250Sensor.h
@@ -30,6 +30,16 @@ class MPU9250Sensor : public MotionSensor
     static constexpr const char *compassCalibrationFileName = "/prefs/compass_mpu9250.dat";
     float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0;
 
+    // Exponential moving average on raw accel + mag to keep the heading stable
+    // during device rotation. The compass-only fusion path is stateless, so any
+    // dynamic acceleration shows up immediately as heading noise; filtering the
+    // inputs is the cheapest mitigation.
+    static constexpr float accelFilterAlpha = 0.15f;
+    static constexpr float magFilterAlpha = 0.20f;
+    FusionVector accelFiltered = {{0, 0, 0}};
+    FusionVector magFiltered = {{0, 0, 0}};
+    bool filtersSeeded = false;
+
     // Pick the correct TwoWire instance for the detected device.
     TwoWire *resolveBus() const;
 

From c63ef271232d67125f71000ccf0aecc563c5e6cb Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Tue, 26 May 2026 22:14:23 +0200
Subject: [PATCH 05/13] feat(motion): make MPU9250 up-axis configurable for
 vertical case mounts
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

FusionCompassCalculateHeading assumes the sensor's Z axis is world-up.
That holds when the WisBlock baseboard is mounted flat, but fails on
cases where the baseboard sits vertical (LCD perpendicular to the
board) — chip Z is then horizontal and tilt-comp becomes unstable.

Add a build-time MPU9250_UP_AXIS selector with five options:
PZ (default, baseboard flat), PX, NX, PY, NY. Implementation rotates
both accel and mag via FusionAxesSwap so the rest of the heading
pipeline still sees Z as up.

The temporary default is PX while testing on a vertical RAK case;
will be reverted to PZ before the PR is marked ready, with vertical
case users opting in via variant.h or build flags.
---
 src/motion/MPU9250Sensor.cpp | 27 +++++++++++++++++++++++++++
 src/motion/MPU9250Sensor.h   |  8 ++++++++
 2 files changed, 35 insertions(+)

diff --git a/src/motion/MPU9250Sensor.cpp b/src/motion/MPU9250Sensor.cpp
index 930aae27b86..72405494e01 100644
--- a/src/motion/MPU9250Sensor.cpp
+++ b/src/motion/MPU9250Sensor.cpp
@@ -266,6 +266,33 @@ int32_t MPU9250Sensor::runOnce()
     ma.axis.y = mag.axis.x;
     ma.axis.z = -mag.axis.z;
 
+    // Compensate for non-flat case mounting. FusionCompassCalculateHeading()
+    // assumes Z is the up axis — when the baseboard is mounted vertically
+    // (e.g. a case where the LCD sits perpendicular to the board), chip Z is
+    // horizontal and the tilt-comp math becomes unstable. Override which chip
+    // axis is treated as world-up via the MPU9250_UP_AXIS_* defines.
+    //   _PZ (default) — chip +Z up (baseboard flat)
+    //   _PX           — chip +X up (vertical mount, silkscreen "north" up)
+    //   _NX           — chip -X up
+    //   _PY           — chip +Y up
+    //   _NY           — chip -Y up
+#ifndef MPU9250_UP_AXIS
+#define MPU9250_UP_AXIS MPU9250_UP_AXIS_PX // TEMP for testing user's vertical case
+#endif
+#if MPU9250_UP_AXIS == MPU9250_UP_AXIS_PX
+    ga = FusionAxesSwap(ga, FusionAxesAlignmentNZPYPX);
+    ma = FusionAxesSwap(ma, FusionAxesAlignmentNZPYPX);
+#elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_NX
+    ga = FusionAxesSwap(ga, FusionAxesAlignmentPZPYNX);
+    ma = FusionAxesSwap(ma, FusionAxesAlignmentPZPYNX);
+#elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_PY
+    ga = FusionAxesSwap(ga, FusionAxesAlignmentPXNZPY);
+    ma = FusionAxesSwap(ma, FusionAxesAlignmentPXNZPY);
+#elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_NY
+    ga = FusionAxesSwap(ga, FusionAxesAlignmentPXPZNY);
+    ma = FusionAxesSwap(ma, FusionAxesAlignmentPXPZNY);
+#endif
+
     if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
         ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
         ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
diff --git a/src/motion/MPU9250Sensor.h b/src/motion/MPU9250Sensor.h
index 00490656b68..4e789892476 100644
--- a/src/motion/MPU9250Sensor.h
+++ b/src/motion/MPU9250Sensor.h
@@ -9,6 +9,14 @@
 #include "Fusion/Fusion.h"
 #include <Wire.h>
 
+// Numeric IDs for selecting which chip axis maps to world-up.
+// Set MPU9250_UP_AXIS to one of these in variant.h or via build flags.
+#define MPU9250_UP_AXIS_PZ 0
+#define MPU9250_UP_AXIS_PX 1
+#define MPU9250_UP_AXIS_NX 2
+#define MPU9250_UP_AXIS_PY 3
+#define MPU9250_UP_AXIS_NY 4
+
 // InvenSense MPU-9250 (and MPU-9255) 9-axis IMU driver.
 // The package contains an MPU-6500 accel/gyro die plus an AK8963 magnetometer
 // die. We drive both directly over I2C: the MPU-6500 at deviceAddress() and the

From 3b4e851ef1d1dab0b8afb719482d442ddfec25d0 Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Tue, 26 May 2026 22:20:56 +0200
Subject: [PATCH 06/13] chore(motion): restore MPU9250_UP_AXIS default to PZ

The previous commit shipped with PX as the default while validating
the remap on a vertical-mount RAK case. PZ matches the assumption
baked into BMX160 / ICM20948 (chip Z = world up), so leaving the
default at PZ keeps existing flat-mount users untouched. Vertical
mount users opt in by setting MPU9250_UP_AXIS in their variant.h or
via build_flags in platformio.ini.
---
 src/motion/MPU9250Sensor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/motion/MPU9250Sensor.cpp b/src/motion/MPU9250Sensor.cpp
index 72405494e01..2d9bc2742b3 100644
--- a/src/motion/MPU9250Sensor.cpp
+++ b/src/motion/MPU9250Sensor.cpp
@@ -277,7 +277,7 @@ int32_t MPU9250Sensor::runOnce()
     //   _PY           — chip +Y up
     //   _NY           — chip -Y up
 #ifndef MPU9250_UP_AXIS
-#define MPU9250_UP_AXIS MPU9250_UP_AXIS_PX // TEMP for testing user's vertical case
+#define MPU9250_UP_AXIS MPU9250_UP_AXIS_PZ
 #endif
 #if MPU9250_UP_AXIS == MPU9250_UP_AXIS_PX
     ga = FusionAxesSwap(ga, FusionAxesAlignmentNZPYPX);

From 8e6e691b9f855b794c0d50fb7ec43053b9c77e39 Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Thu, 18 Jun 2026 11:00:33 +0200
Subject: [PATCH 07/13] fix: detect MPU-9250 at 0x69 before BMX160 fallback;
 clang-format

Move the MPU-9250/9255 WHO_AM_I disambiguation ahead of the
BMX160-by-address fallback in ScanI2CTwoWire. Previously an MPU-9250
strapped to 0x69 (== BMX160_ADDR) was caught by the BMX160 address
shortcut and misidentified, preventing the new driver from being
selected (flagged in Copilot review).

Also reflow the accelerometer preference list in ScanI2C.cpp to satisfy
clang-format (Trunk Check).
---
 src/detect/ScanI2C.cpp        |  4 ++--
 src/detect/ScanI2CTwoWire.cpp | 18 +++++++++++++-----
 2 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/src/detect/ScanI2C.cpp b/src/detect/ScanI2C.cpp
index b0aa7ba0892..d24b20816ca 100644
--- a/src/detect/ScanI2C.cpp
+++ b/src/detect/ScanI2C.cpp
@@ -37,8 +37,8 @@ ScanI2C::FoundDevice ScanI2C::firstKeyboard() const
 
 ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const
 {
-    ScanI2C::DeviceType types[] = {MPU6050,  LIS3DH,   BMA423, LSM6DS3,   BMX160,     STK8BAXX, ICM20948,
-                                   QMA6100P, BMM150,   BMI270, ICM42607P, ISM330DHCX, MPU9250};
+    ScanI2C::DeviceType types[] = {MPU6050,  LIS3DH, BMA423, LSM6DS3,   BMX160,     STK8BAXX, ICM20948,
+                                   QMA6100P, BMM150, BMI270, ICM42607P, ISM330DHCX, MPU9250};
     return firstOfOrNONE(13, types);
 }
 
diff --git a/src/detect/ScanI2CTwoWire.cpp b/src/detect/ScanI2CTwoWire.cpp
index 8e893817528..7394b8e5138 100644
--- a/src/detect/ScanI2CTwoWire.cpp
+++ b/src/detect/ScanI2CTwoWire.cpp
@@ -820,20 +820,28 @@ void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize)
                         break;
                     }
 #endif
-                    if (addr.address == BMX160_ADDR) {
-                        type = BMX160;
-                        logFoundDevice("BMX160", (uint8_t)addr.address);
-                        break;
-                    }
                     // MPU-6050 and MPU-9250/9255 share I2C addresses (0x68/0x69), and chip ID
                     // register 0x00 reads 0x00 on MPU-9250. Disambiguate via WHO_AM_I (0x75):
                     //   MPU-6050 -> 0x68, MPU-9250 -> 0x71, MPU-9255 -> 0x73
+                    // This must run before the BMX160-by-address fallback below, otherwise an
+                    // MPU-9250 strapped to 0x69 (== BMX160_ADDR) is misidentified as a BMX160.
                     registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x75), 1);
                     if (registerValue == 0x71 || registerValue == 0x73) {
                         type = MPU9250;
                         logFoundDevice(registerValue == 0x73 ? "MPU9255" : "MPU9250", (uint8_t)addr.address);
                         break;
                     }
+                    if (registerValue == 0x68) {
+                        type = MPU6050;
+                        logFoundDevice("MPU6050", (uint8_t)addr.address);
+                        break;
+                    }
+                    if (addr.address == BMX160_ADDR) {
+                        type = BMX160;
+                        logFoundDevice("BMX160", (uint8_t)addr.address);
+                        break;
+                    }
+                    // Fallback for the shared 0x68 address: assume MPU-6050
                     type = MPU6050;
                     logFoundDevice("MPU6050", (uint8_t)addr.address);
                     break;

From 03b019518350153e3b043a886615eea1f6e83612 Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Wed, 1 Jul 2026 17:14:48 +0200
Subject: [PATCH 08/13] fix: harden MPU9250 init and calibration per CodeRabbit
 review

- initMPU6500/initAK8963: return false on any required writeRegister()
  failure so a partially configured driver is never kept alive
- runOnce: run finishCalibrationIfExpired() even when readSensors() fails,
  so missed AK8963 samples cannot stall the calibration window open
- axis remap: add explicit MPU9250_UP_AXIS_PZ branch and #error default so
  an invalid MPU9250_UP_AXIS build flag fails at compile time
---
 src/motion/MPU9250Sensor.cpp | 48 +++++++++++++++++++++++++++---------
 1 file changed, 37 insertions(+), 11 deletions(-)

diff --git a/src/motion/MPU9250Sensor.cpp b/src/motion/MPU9250Sensor.cpp
index 2d9bc2742b3..c3f79290d84 100644
--- a/src/motion/MPU9250Sensor.cpp
+++ b/src/motion/MPU9250Sensor.cpp
@@ -105,17 +105,23 @@ bool MPU9250Sensor::initMPU6500()
     delay(50);
 
     // DLPF 41 Hz (CONFIG = 3), sample rate 1 kHz / (1 + SMPLRT_DIV) = 200 Hz
-    writeRegister(addr, MPU_CONFIG, 0x03);
-    writeRegister(addr, MPU_SMPLRT_DIV, 0x04);
+    if (!writeRegister(addr, MPU_CONFIG, 0x03) || !writeRegister(addr, MPU_SMPLRT_DIV, 0x04)) {
+        return false;
+    }
 
     // ±2 g accel range
-    writeRegister(addr, MPU_ACCEL_CONFIG, 0x00);
+    if (!writeRegister(addr, MPU_ACCEL_CONFIG, 0x00)) {
+        return false;
+    }
     // ±250 dps gyro (gyro is unused for compass but configuring leaves the chip in a known state)
-    writeRegister(addr, MPU_GYRO_CONFIG, 0x00);
+    if (!writeRegister(addr, MPU_GYRO_CONFIG, 0x00)) {
+        return false;
+    }
 
     // Expose AK8963 on the main I2C bus: disable internal master, enable bypass
-    writeRegister(addr, MPU_USER_CTRL, 0x00);
-    writeRegister(addr, MPU_INT_PIN_CFG, 0x02);
+    if (!writeRegister(addr, MPU_USER_CTRL, 0x00) || !writeRegister(addr, MPU_INT_PIN_CFG, 0x02)) {
+        return false;
+    }
     delay(10);
 
     return true;
@@ -130,13 +136,19 @@ bool MPU9250Sensor::initAK8963()
     }
 
     // Soft reset
-    writeRegister(AK8963_ADDR, AK_CNTL2, 0x01);
+    if (!writeRegister(AK8963_ADDR, AK_CNTL2, 0x01)) {
+        return false;
+    }
     delay(100);
 
     // Power-down then enter Fuse ROM access to read per-axis sensitivity (ASA)
-    writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_POWERDOWN);
+    if (!writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_POWERDOWN)) {
+        return false;
+    }
     delay(10);
-    writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_FUSE_ROM);
+    if (!writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_FUSE_ROM)) {
+        return false;
+    }
     delay(10);
 
     uint8_t asa[3] = {0};
@@ -150,9 +162,13 @@ bool MPU9250Sensor::initAK8963()
     }
 
     // Back to power-down, then enter continuous-mode-2 (100 Hz) at 16-bit resolution
-    writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_POWERDOWN);
+    if (!writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_POWERDOWN)) {
+        return false;
+    }
     delay(10);
-    writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_CONT2_16BIT);
+    if (!writeRegister(AK8963_ADDR, AK_CNTL1, AK_MODE_CONT2_16BIT)) {
+        return false;
+    }
     delay(10);
 
     return true;
@@ -224,6 +240,12 @@ int32_t MPU9250Sensor::runOnce()
     FusionVector accel = {{0, 0, 0}};
     FusionVector mag = {{0, 0, 0}};
     if (!readSensors(accel, mag)) {
+        // Missed samples must not stall calibration: close the window on timeout
+        // even when no fresh magnetometer data is available.
+        if (doCalibration) {
+            finishCalibrationIfExpired(showingScreen, compassCalibrationFileName, highestX, lowestX, highestY, lowestY,
+                                       highestZ, lowestZ);
+        }
         return MOTION_SENSOR_CHECK_INTERVAL_MS;
     }
 
@@ -282,6 +304,8 @@ int32_t MPU9250Sensor::runOnce()
 #if MPU9250_UP_AXIS == MPU9250_UP_AXIS_PX
     ga = FusionAxesSwap(ga, FusionAxesAlignmentNZPYPX);
     ma = FusionAxesSwap(ma, FusionAxesAlignmentNZPYPX);
+#elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_PZ
+    // Default orientation (chip +Z up), no remap needed.
 #elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_NX
     ga = FusionAxesSwap(ga, FusionAxesAlignmentPZPYNX);
     ma = FusionAxesSwap(ma, FusionAxesAlignmentPZPYNX);
@@ -291,6 +315,8 @@ int32_t MPU9250Sensor::runOnce()
 #elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_NY
     ga = FusionAxesSwap(ga, FusionAxesAlignmentPXPZNY);
     ma = FusionAxesSwap(ma, FusionAxesAlignmentPXPZNY);
+#else
+#error "MPU9250_UP_AXIS must be one of MPU9250_UP_AXIS_PZ/PX/NX/PY/NY"
 #endif
 
     if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {

From c8e7bd8b920f14a256024cffb0ae005c80705a93 Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Thu, 2 Jul 2026 09:07:30 +0200
Subject: [PATCH 09/13] docs: add Doxygen docstrings to MPU9250Sensor methods

Documents all ten MPU9250Sensor member functions to satisfy the CodeRabbit
docstring-coverage check (was 12.5%, threshold 80%). Comment-only change.
---
 src/motion/MPU9250Sensor.cpp | 63 ++++++++++++++++++++++++++++++++++++
 1 file changed, 63 insertions(+)

diff --git a/src/motion/MPU9250Sensor.cpp b/src/motion/MPU9250Sensor.cpp
index c3f79290d84..9b57352818a 100644
--- a/src/motion/MPU9250Sensor.cpp
+++ b/src/motion/MPU9250Sensor.cpp
@@ -41,8 +41,16 @@ constexpr float ACCEL_LSB_PER_G = 16384.0f;
 constexpr float MAG_UT_PER_LSB = 0.15f;
 } // namespace
 
+/**
+ * @brief Construct the driver for a detected MPU-9250/MPU-9255 device.
+ * @param foundDevice I2C scan result identifying the chip's address and bus port.
+ */
 MPU9250Sensor::MPU9250Sensor(ScanI2C::FoundDevice foundDevice) : MotionSensor::MotionSensor(foundDevice) {}
 
+/**
+ * @brief Select the TwoWire bus instance the detected device lives on.
+ * @return Pointer to Wire1 when the device was scanned on the second port, else Wire.
+ */
 TwoWire *MPU9250Sensor::resolveBus() const
 {
 #if defined(WIRE_INTERFACES_COUNT) && (WIRE_INTERFACES_COUNT > 1)
@@ -52,6 +60,13 @@ TwoWire *MPU9250Sensor::resolveBus() const
 #endif
 }
 
+/**
+ * @brief Write a single byte to a register on the given I2C address.
+ * @param i2cAddr Target die address (MPU-6500 or AK8963).
+ * @param reg Register address to write.
+ * @param value Byte value to store.
+ * @return true if the I2C transaction completed successfully.
+ */
 bool MPU9250Sensor::writeRegister(uint8_t i2cAddr, uint8_t reg, uint8_t value)
 {
     bus->beginTransmission(i2cAddr);
@@ -60,6 +75,14 @@ bool MPU9250Sensor::writeRegister(uint8_t i2cAddr, uint8_t reg, uint8_t value)
     return bus->endTransmission() == 0;
 }
 
+/**
+ * @brief Read a block of consecutive registers from an I2C address.
+ * @param i2cAddr Target die address (MPU-6500 or AK8963).
+ * @param reg First register address to read.
+ * @param buf Destination buffer; must hold at least @p len bytes.
+ * @param len Number of bytes to read.
+ * @return true if exactly @p len bytes were received.
+ */
 bool MPU9250Sensor::readRegisters(uint8_t i2cAddr, uint8_t reg, uint8_t *buf, uint8_t len)
 {
     bus->beginTransmission(i2cAddr);
@@ -77,6 +100,14 @@ bool MPU9250Sensor::readRegisters(uint8_t i2cAddr, uint8_t reg, uint8_t *buf, ui
     return true;
 }
 
+/**
+ * @brief Reset and configure the MPU-6500 accelerometer/gyroscope die.
+ *
+ * Verifies WHO_AM_I, wakes the device onto the gyro PLL, sets the DLPF, sample
+ * rate and full-scale ranges, then enables I2C bypass so the on-package AK8963
+ * magnetometer becomes reachable on the main bus.
+ * @return true only if every required register write succeeded.
+ */
 bool MPU9250Sensor::initMPU6500()
 {
     const uint8_t addr = deviceAddress();
@@ -127,6 +158,13 @@ bool MPU9250Sensor::initMPU6500()
     return true;
 }
 
+/**
+ * @brief Initialise the AK8963 magnetometer die.
+ *
+ * Confirms WHO_AM_I, reads the Fuse-ROM per-axis sensitivity adjustment (ASA)
+ * into asaScale, then switches the magnetometer into 16-bit continuous mode 2.
+ * @return true if the die responded and all setup writes succeeded.
+ */
 bool MPU9250Sensor::initAK8963()
 {
     uint8_t wia = 0;
@@ -174,6 +212,13 @@ bool MPU9250Sensor::initAK8963()
     return true;
 }
 
+/**
+ * @brief Bring the sensor fully online.
+ *
+ * Resolves the I2C bus, initialises both the MPU-6500 and AK8963 dies, and
+ * loads any persisted hard-iron calibration.
+ * @return true when the device is ready to produce headings.
+ */
 bool MPU9250Sensor::init()
 {
     bus = resolveBus();
@@ -195,6 +240,12 @@ bool MPU9250Sensor::init()
     return true;
 }
 
+/**
+ * @brief Read one accelerometer and magnetometer sample.
+ * @param[out] accel Acceleration vector in g.
+ * @param[out] mag Magnetic field vector in µT, sensitivity- and axis-scaled.
+ * @return false if no fresh magnetometer sample was ready or the reading overflowed.
+ */
 bool MPU9250Sensor::readSensors(FusionVector &accel, FusionVector &mag)
 {
     // Read 6 bytes of accel data
@@ -234,6 +285,14 @@ bool MPU9250Sensor::readSensors(FusionVector &accel, FusionVector &mag)
     return true;
 }
 
+/**
+ * @brief Periodic worker: sample, filter, compute tilt-compensated heading, publish.
+ *
+ * Applies hard-iron correction and a per-axis EMA, remaps the accel/mag frames
+ * to a common orientation, computes the compass heading via the Fusion library,
+ * and pushes it to the screen. Also drives the calibration state machine.
+ * @return Milliseconds until the next desired invocation.
+ */
 int32_t MPU9250Sensor::runOnce()
 {
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN
@@ -332,6 +391,10 @@ int32_t MPU9250Sensor::runOnce()
     return MOTION_SENSOR_CHECK_INTERVAL_MS;
 }
 
+/**
+ * @brief Begin hard-iron calibration, seeding extrema from the current sample.
+ * @param forSeconds Duration of the calibration window in seconds.
+ */
 void MPU9250Sensor::calibrate(uint16_t forSeconds)
 {
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN) && HAS_SCREEN

From 32defbb0e5f404ce3d80163e032f324765503158 Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Thu, 2 Jul 2026 10:00:59 +0200
Subject: [PATCH 10/13] docs: trim verbose comments per CodeRabbit nitpicks

Shorten the WHO_AM_I disambiguation comment and the MPU9250Sensor.h class /
EMA comments to the repo's one-to-two-line guideline, keeping the non-obvious
rationale (detection ordering, axis-remap, filtering). Comment-only.
---
 src/detect/ScanI2CTwoWire.cpp |  7 ++-----
 src/motion/MPU9250Sensor.h    | 16 +++++-----------
 2 files changed, 7 insertions(+), 16 deletions(-)

diff --git a/src/detect/ScanI2CTwoWire.cpp b/src/detect/ScanI2CTwoWire.cpp
index c5ad8439118..4adde34561c 100644
--- a/src/detect/ScanI2CTwoWire.cpp
+++ b/src/detect/ScanI2CTwoWire.cpp
@@ -875,11 +875,8 @@ void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize)
                         break;
                     }
 #endif
-                    // MPU-6050 and MPU-9250/9255 share I2C addresses (0x68/0x69), and chip ID
-                    // register 0x00 reads 0x00 on MPU-9250. Disambiguate via WHO_AM_I (0x75):
-                    //   MPU-6050 -> 0x68, MPU-9250 -> 0x71, MPU-9255 -> 0x73
-                    // This must run before the BMX160-by-address fallback below, otherwise an
-                    // MPU-9250 strapped to 0x69 (== BMX160_ADDR) is misidentified as a BMX160.
+                    // Disambiguate shared 0x68/0x69 addresses via WHO_AM_I (0x75: 0x71 MPU-9250,
+                    // 0x73 MPU-9255, 0x68 MPU-6050); must run before the BMX160-by-address fallback.
                     registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x75), 1);
                     if (registerValue == 0x71 || registerValue == 0x73) {
                         type = MPU9250;
diff --git a/src/motion/MPU9250Sensor.h b/src/motion/MPU9250Sensor.h
index 4e789892476..03e37de2fcd 100644
--- a/src/motion/MPU9250Sensor.h
+++ b/src/motion/MPU9250Sensor.h
@@ -17,13 +17,9 @@
 #define MPU9250_UP_AXIS_PY 3
 #define MPU9250_UP_AXIS_NY 4
 
-// InvenSense MPU-9250 (and MPU-9255) 9-axis IMU driver.
-// The package contains an MPU-6500 accel/gyro die plus an AK8963 magnetometer
-// die. We drive both directly over I2C: the MPU-6500 at deviceAddress() and the
-// AK8963 at AK8963_ADDR via the MPU's I2C bypass mode.
-// This driver covers the RAK1905 WisBlock module (MPU-9250) — a viable
-// replacement for the EOL RAK12034 (BMX160) when tilt-compensated heading is
-// needed alongside RAK1904 (LIS3DH) options.
+// InvenSense MPU-9250/MPU-9255 9-axis IMU driver (e.g. RAK1905 WisBlock).
+// Drives the MPU-6500 accel die at deviceAddress() and the AK8963 magnetometer
+// at AK8963_ADDR over I2C bypass mode.
 class MPU9250Sensor : public MotionSensor
 {
   private:
@@ -38,10 +34,8 @@ class MPU9250Sensor : public MotionSensor
     static constexpr const char *compassCalibrationFileName = "/prefs/compass_mpu9250.dat";
     float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0;
 
-    // Exponential moving average on raw accel + mag to keep the heading stable
-    // during device rotation. The compass-only fusion path is stateless, so any
-    // dynamic acceleration shows up immediately as heading noise; filtering the
-    // inputs is the cheapest mitigation.
+    // Per-axis EMA on raw accel + mag: the stateless compass fusion turns dynamic
+    // acceleration into heading noise, so filtering the inputs steadies rotation.
     static constexpr float accelFilterAlpha = 0.15f;
     static constexpr float magFilterAlpha = 0.20f;
     FusionVector accelFiltered = {{0, 0, 0}};

From 336bb4a0dc30ab728518711862e2cdf1e4c14d6f Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Thu, 2 Jul 2026 15:19:39 +0200
Subject: [PATCH 11/13] style: fix trunk formatting (ascii-dash + clang-format)

Replace Unicode em-dashes with ASCII hyphens (ascii-dash linter) and apply
clang-format 16 in MPU9250Sensor.{cpp,h}. Formatting-only.
---
 src/motion/MPU9250Sensor.cpp | 22 +++++++++++-----------
 src/motion/MPU9250Sensor.h   |  2 +-
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/motion/MPU9250Sensor.cpp b/src/motion/MPU9250Sensor.cpp
index 9b57352818a..b5af4b10212 100644
--- a/src/motion/MPU9250Sensor.cpp
+++ b/src/motion/MPU9250Sensor.cpp
@@ -112,7 +112,7 @@ bool MPU9250Sensor::initMPU6500()
 {
     const uint8_t addr = deviceAddress();
 
-    // Sanity-check WHO_AM_I — scan should have already verified this but be defensive
+    // Sanity-check WHO_AM_I - scan should have already verified this but be defensive
     uint8_t whoAmI = 0;
     if (!readRegisters(addr, MPU_WHO_AM_I, &whoAmI, 1)) {
         LOG_DEBUG("MPU9250 WHO_AM_I read failed at 0x%02X", addr);
@@ -194,7 +194,7 @@ bool MPU9250Sensor::initAK8963()
         LOG_DEBUG("MPU9250 AK8963 ASA read failed");
         return false;
     }
-    // Sensitivity adjustment: H_adj = H * ((ASA - 128) * 0.5 / 128 + 1) — datasheet 8.3.11
+    // Sensitivity adjustment: H_adj = H * ((ASA - 128) * 0.5 / 128 + 1) - datasheet 8.3.11
     for (int i = 0; i < 3; ++i) {
         asaScale[i] = (((float)asa[i] - 128.0f) * 0.5f / 128.0f) + 1.0f;
     }
@@ -267,7 +267,7 @@ bool MPU9250Sensor::readSensors(FusionVector &accel, FusionVector &mag)
     }
 
     // Read 6 bytes of mag (little-endian) plus ST2 to release the data register.
-    // ST2 also exposes the magnetic overflow flag (bit 3) — discard the sample if set.
+    // ST2 also exposes the magnetic overflow flag (bit 3) - discard the sample if set.
     uint8_t magRaw[7] = {0};
     if (!readRegisters(AK8963_ADDR, AK_HXL, magRaw, 7)) {
         return false;
@@ -302,8 +302,8 @@ int32_t MPU9250Sensor::runOnce()
         // Missed samples must not stall calibration: close the window on timeout
         // even when no fresh magnetometer data is available.
         if (doCalibration) {
-            finishCalibrationIfExpired(showingScreen, compassCalibrationFileName, highestX, lowestX, highestY, lowestY,
-                                       highestZ, lowestZ);
+            finishCalibrationIfExpired(showingScreen, compassCalibrationFileName, highestX, lowestX, highestY, lowestY, highestZ,
+                                       lowestZ);
         }
         return MOTION_SENSOR_CHECK_INTERVAL_MS;
     }
@@ -348,15 +348,15 @@ int32_t MPU9250Sensor::runOnce()
     ma.axis.z = -mag.axis.z;
 
     // Compensate for non-flat case mounting. FusionCompassCalculateHeading()
-    // assumes Z is the up axis — when the baseboard is mounted vertically
+    // assumes Z is the up axis - when the baseboard is mounted vertically
     // (e.g. a case where the LCD sits perpendicular to the board), chip Z is
     // horizontal and the tilt-comp math becomes unstable. Override which chip
     // axis is treated as world-up via the MPU9250_UP_AXIS_* defines.
-    //   _PZ (default) — chip +Z up (baseboard flat)
-    //   _PX           — chip +X up (vertical mount, silkscreen "north" up)
-    //   _NX           — chip -X up
-    //   _PY           — chip +Y up
-    //   _NY           — chip -Y up
+    //   _PZ (default) - chip +Z up (baseboard flat)
+    //   _PX           - chip +X up (vertical mount, silkscreen "north" up)
+    //   _NX           - chip -X up
+    //   _PY           - chip +Y up
+    //   _NY           - chip -Y up
 #ifndef MPU9250_UP_AXIS
 #define MPU9250_UP_AXIS MPU9250_UP_AXIS_PZ
 #endif
diff --git a/src/motion/MPU9250Sensor.h b/src/motion/MPU9250Sensor.h
index 03e37de2fcd..8b62cf9317d 100644
--- a/src/motion/MPU9250Sensor.h
+++ b/src/motion/MPU9250Sensor.h
@@ -45,7 +45,7 @@ class MPU9250Sensor : public MotionSensor
     // Pick the correct TwoWire instance for the detected device.
     TwoWire *resolveBus() const;
 
-    // Low-level I2C helpers — one address per call so we can address both dies.
+    // Low-level I2C helpers - one address per call so we can address both dies.
     bool writeRegister(uint8_t i2cAddr, uint8_t reg, uint8_t value);
     bool readRegisters(uint8_t i2cAddr, uint8_t reg, uint8_t *buf, uint8_t len);
 

From 4257e1b0eba86dd7b8be1c08624d4e0cd7b51080 Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Mon, 6 Jul 2026 20:36:17 +0200
Subject: [PATCH 12/13] fix(motion): update MPU9250 to upstream Fusion library
 API

develop switched to the upstream meshtastic/Fusion library (#10724),
which renamed the axes-remap API. Update MPU9250Sensor to match the
other sensors:
- FusionAxesSwap -> FusionRemap
- FusionAxesAlignment* -> FusionRemapAlignment*
- FusionCompassCalculateHeading(conv, a, m) -> FusionCompass(a, m, conv)
---
 src/motion/MPU9250Sensor.cpp | 24 ++++++++++++------------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/motion/MPU9250Sensor.cpp b/src/motion/MPU9250Sensor.cpp
index b5af4b10212..792d7883b39 100644
--- a/src/motion/MPU9250Sensor.cpp
+++ b/src/motion/MPU9250Sensor.cpp
@@ -347,7 +347,7 @@ int32_t MPU9250Sensor::runOnce()
     ma.axis.y = mag.axis.x;
     ma.axis.z = -mag.axis.z;
 
-    // Compensate for non-flat case mounting. FusionCompassCalculateHeading()
+    // Compensate for non-flat case mounting. FusionCompass()
     // assumes Z is the up axis - when the baseboard is mounted vertically
     // (e.g. a case where the LCD sits perpendicular to the board), chip Z is
     // horizontal and the tilt-comp math becomes unstable. Override which chip
@@ -361,29 +361,29 @@ int32_t MPU9250Sensor::runOnce()
 #define MPU9250_UP_AXIS MPU9250_UP_AXIS_PZ
 #endif
 #if MPU9250_UP_AXIS == MPU9250_UP_AXIS_PX
-    ga = FusionAxesSwap(ga, FusionAxesAlignmentNZPYPX);
-    ma = FusionAxesSwap(ma, FusionAxesAlignmentNZPYPX);
+    ga = FusionRemap(ga, FusionRemapAlignmentNZPYPX);
+    ma = FusionRemap(ma, FusionRemapAlignmentNZPYPX);
 #elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_PZ
     // Default orientation (chip +Z up), no remap needed.
 #elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_NX
-    ga = FusionAxesSwap(ga, FusionAxesAlignmentPZPYNX);
-    ma = FusionAxesSwap(ma, FusionAxesAlignmentPZPYNX);
+    ga = FusionRemap(ga, FusionRemapAlignmentPZPYNX);
+    ma = FusionRemap(ma, FusionRemapAlignmentPZPYNX);
 #elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_PY
-    ga = FusionAxesSwap(ga, FusionAxesAlignmentPXNZPY);
-    ma = FusionAxesSwap(ma, FusionAxesAlignmentPXNZPY);
+    ga = FusionRemap(ga, FusionRemapAlignmentPXNZPY);
+    ma = FusionRemap(ma, FusionRemapAlignmentPXNZPY);
 #elif MPU9250_UP_AXIS == MPU9250_UP_AXIS_NY
-    ga = FusionAxesSwap(ga, FusionAxesAlignmentPXPZNY);
-    ma = FusionAxesSwap(ma, FusionAxesAlignmentPXPZNY);
+    ga = FusionRemap(ga, FusionRemapAlignmentPXPZNY);
+    ma = FusionRemap(ma, FusionRemapAlignmentPXPZNY);
 #else
 #error "MPU9250_UP_AXIS must be one of MPU9250_UP_AXIS_PZ/PX/NX/PY/NY"
 #endif
 
     if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
-        ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
-        ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+        ma = FusionRemap(ma, FusionRemapAlignmentNXNYPZ);
+        ga = FusionRemap(ga, FusionRemapAlignmentNXNYPZ);
     }
 
-    float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
+    float heading = FusionCompass(ga, ma, FusionConventionNed);
     heading = applyCompassOrientation(heading);
     if (screen)
         screen->setHeading(heading);

From 19cd3112be4a0220c07e7901685512aeab902eea Mon Sep 17 00:00:00 2001
From: Egor Siniaev <egr.snv@gmail.com>
Date: Sun, 26 Jul 2026 16:41:51 +0200
Subject: [PATCH 13/13] fix(motion): use real screen declaration from main.h in
 MPU9250Sensor

The file re-declared the global screen as a raw pointer
(extern graphics::Screen *screen), conflicting with the real
declaration in main.h (extern std::unique_ptr<graphics::Screen> screen).
Build variants whose include chain pulled in main.h failed with
'conflicting declaration'. Include main.h instead; if (screen) and
screen->setHeading() work unchanged on the unique_ptr.
---
 src/motion/MPU9250Sensor.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/motion/MPU9250Sensor.cpp b/src/motion/MPU9250Sensor.cpp
index 792d7883b39..64bcbf0fb2a 100644
--- a/src/motion/MPU9250Sensor.cpp
+++ b/src/motion/MPU9250Sensor.cpp
@@ -3,8 +3,7 @@
 #if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C
 
 #if !defined(MESHTASTIC_EXCLUDE_SCREEN)
-// screen is defined in main.cpp
-extern graphics::Screen *screen;
+#include "main.h" // global std::unique_ptr<graphics::Screen> screen
 #endif
 
 namespace