diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index 7052f398264..26e7c18dfaa 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/src/motion/BMX160Sensor.cpp @@ -43,13 +43,56 @@ int32_t BMX160Sensor::runOnce() magAccel.x -= (highestX + lowestX) / 2; magAccel.y -= (highestY + lowestY) / 2; magAccel.z -= (highestZ + lowestZ) / 2; + + // Smooth raw inputs with a per-axis EMA to suppress dynamic acceleration + // noise during rotation (stateless FusionCompass turns it into heading jitter). + FusionVector accel = {{gAccel.x, gAccel.y, gAccel.z}}; + FusionVector mag = {{magAccel.x, magAccel.y, magAccel.z}}; + 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; + FusionVector ga, ma; - ga.axis.x = -gAccel.x; // default location for the BMX160 is on the rear of the board - ga.axis.y = -gAccel.y; - ga.axis.z = gAccel.z; - ma.axis.x = -magAccel.x; - ma.axis.y = -magAccel.y; - ma.axis.z = magAccel.z * 3; + ga.axis.x = -accel.axis.x; // default location for the BMX160 is on the rear of the board + ga.axis.y = -accel.axis.y; + ga.axis.z = accel.axis.z; + ma.axis.x = -mag.axis.x; + ma.axis.y = -mag.axis.y; + ma.axis.z = mag.axis.z * 3; + + // Compensate for non-flat case mounting. FusionCompass() assumes chip Z is + // world-up; on a vertical mount (LCD 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 BMX160_UP_AXIS_* defines. +#ifndef BMX160_UP_AXIS +#define BMX160_UP_AXIS BMX160_UP_AXIS_PZ +#endif +#if BMX160_UP_AXIS == BMX160_UP_AXIS_PX + ga = FusionRemap(ga, FusionRemapAlignmentNZPYPX); + ma = FusionRemap(ma, FusionRemapAlignmentNZPYPX); +#elif BMX160_UP_AXIS == BMX160_UP_AXIS_PZ + // Default orientation (chip +Z up), no remap needed. +#elif BMX160_UP_AXIS == BMX160_UP_AXIS_NX + ga = FusionRemap(ga, FusionRemapAlignmentPZPYNX); + ma = FusionRemap(ma, FusionRemapAlignmentPZPYNX); +#elif BMX160_UP_AXIS == BMX160_UP_AXIS_PY + ga = FusionRemap(ga, FusionRemapAlignmentPXNZPY); + ma = FusionRemap(ma, FusionRemapAlignmentPXNZPY); +#elif BMX160_UP_AXIS == BMX160_UP_AXIS_NY + ga = FusionRemap(ga, FusionRemapAlignmentPXPZNY); + ma = FusionRemap(ma, FusionRemapAlignmentPXPZNY); +#else +#error "BMX160_UP_AXIS must be one of BMX160_UP_AXIS_PZ/PX/NX/PY/NY" +#endif // If we're set to one of the inverted positions if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) { diff --git a/src/motion/BMX160Sensor.h b/src/motion/BMX160Sensor.h index d0d21d3dda5..7ce38644ddb 100755 --- a/src/motion/BMX160Sensor.h +++ b/src/motion/BMX160Sensor.h @@ -12,6 +12,14 @@ #include "Fusion/Fusion.h" #include +// Numeric IDs for selecting which chip axis maps to world-up (vertical case mounts). +// Set BMX160_UP_AXIS to one of these via build flags (platformio.ini). +#define BMX160_UP_AXIS_PZ 0 +#define BMX160_UP_AXIS_PX 1 +#define BMX160_UP_AXIS_NX 2 +#define BMX160_UP_AXIS_PY 3 +#define BMX160_UP_AXIS_NY 4 + class BMX160Sensor : public MotionSensor { private: @@ -20,6 +28,14 @@ class BMX160Sensor : public MotionSensor static constexpr const char *compassCalibrationFileName = "/prefs/compass_bmx160.dat"; float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0; + // 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}}; + FusionVector magFiltered = {{0, 0, 0}}; + bool filtersSeeded = false; + public: explicit BMX160Sensor(ScanI2C::FoundDevice foundDevice); virtual bool init() override;