Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
55 changes: 49 additions & 6 deletions src/motion/BMX160Sensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
16 changes: 16 additions & 0 deletions src/motion/BMX160Sensor.h
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,14 @@
#include "Fusion/Fusion.h"
#include <Rak_BMX160.h>

// 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:
Expand All @@ -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;
Expand Down