From 2a111e26808b2028bd745dd3af9e23eb963975b6 Mon Sep 17 00:00:00 2001 From: Egor Siniaev Date: Tue, 26 May 2026 10:34:09 +0200 Subject: [PATCH 1/6] 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 . #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 + +// 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 Date: Tue, 26 May 2026 21:09:51 +0200 Subject: [PATCH 2/6] 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 Date: Tue, 26 May 2026 21:31:39 +0200 Subject: [PATCH 3/6] 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 Date: Tue, 26 May 2026 21:55:20 +0200 Subject: [PATCH 4/6] 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 Date: Tue, 26 May 2026 22:14:23 +0200 Subject: [PATCH 5/6] 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 +// 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 Date: Tue, 26 May 2026 22:20:56 +0200 Subject: [PATCH 6/6] 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);