Skip to content

Add MPU6886 driver (tested with M5Stack FIRE) #56

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
ThomasFarstrike merged 1 commit into from
Feb 21, 2026
Merged

Add MPU6886 driver (tested with M5Stack FIRE) #56

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
83 changes: 83 additions & 0 deletions internal_filesystem/lib/drivers/imu_sensor/mpu6886.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
"""
MicroPython driver for MPU6886 3-Axis Accelerometer + 3-Axis Gyroscope.
Tested with M5Stack FIRE
https://docs.m5stack.com/en/unit/imu
https://github.com/m5stack/M5Stack/blob/master/src/utility/MPU6886.h
"""

import time

from machine import I2C
from micropython import const

_I2CADDR_DEFAULT = const(0x68)


# register addresses
_REG_PWR_MGMT_1 = const(0x6B)
_REG_ACCEL_XOUT_H = const(0x3B)
_REG_GYRO_XOUT_H = const(0x43)
_REG_ACCEL_CONFIG = const(0x1C)
_REG_GYRO_CONFIG = const(0x1B)
_REG_TEMPERATURE_OUT_H = const(0x41)

# Scale factors for converting raw sensor data to physical units:
_ACCEL_SCALE_8G = 8.0 / 32768.0 # LSB/g for +-8g range
_GYRO_SCALE_2000DPS = 2000.0 / 32768.0 # LSB/°/s for +-2000dps range
_TEMPERATURE_SCALE = 326.8 # LSB/°C
_TEMPERATURE_OFFSET = const(25) # Offset (25°C at 0 LSB)


def twos_complement(val, bits):
if val & (1 << (bits - 1)):
val -= 1 << bits
return val


class MPU6886:
def __init__(
self,
i2c_bus: I2C,
address: int = _I2CADDR_DEFAULT,
):
self.i2c = i2c_bus
self.address = address

for data in (b"\x00", b"\x80", b"\x01"): # Reset, then wake up
self._write(_REG_PWR_MGMT_1, data)
time.sleep(0.01)

self._write(_REG_ACCEL_CONFIG, b"\x10") # +-8g
time.sleep(0.001)

self._write(_REG_GYRO_CONFIG, b"\x18") # +-2000dps
time.sleep(0.001)

# Helper functions for register operations
def _write(self, reg: int, data: bytes):
self.i2c.writeto_mem(self.address, reg, data)

def _read_xyz(self, reg: int, scale: float) -> tuple[int, int, int]:
data = self.i2c.readfrom_mem(self.address, reg, 6)
x = twos_complement(data[0] << 8 | data[1], 16)
y = twos_complement(data[2] << 8 | data[3], 16)
z = twos_complement(data[4] << 8 | data[5], 16)
return (x * scale, y * scale, z * scale)

@property
def temperature(self) -> float:
buf = self.i2c.readfrom_mem(self.address, _REG_TEMPERATURE_OUT_H, 14)
temp_raw = (buf[6] << 8) | buf[7]
if temp_raw & 0x8000: # If MSB is 1, it's negative
temp_raw -= 0x10000 # Subtract 2^16 to get negative value
return temp_raw / _TEMPERATURE_SCALE + _TEMPERATURE_OFFSET

@property
def acceleration(self) -> tuple[int, int, int]:
"""Get current acceleration reading."""
return self._read_xyz(_REG_ACCEL_XOUT_H, scale=_ACCEL_SCALE_8G)

@property
def gyro(self) -> tuple[int, int, int]:
"""Get current gyroscope reading."""
return self._read_xyz(_REG_GYRO_XOUT_H, scale=_GYRO_SCALE_2000DPS)
20 changes: 17 additions & 3 deletions internal_filesystem/lib/mpos/board/m5stack_fire.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
# Hardware initialization for ESP32 M5Stack-Fire board
# Manufacturer's website at https://https://docs.m5stack.com/en/core/fire_v2.7
# Original author: https://github.com/ancebfer

import time

import drivers.display.ili9341 as ili9341
Expand All @@ -10,9 +9,9 @@
import machine
import mpos.ui
import mpos.ui.focus_direction
from machine import PWM, Pin
from machine import I2C, PWM, Pin
from micropython import const
from mpos import AudioManager, InputManager
from mpos import AudioManager, InputManager, SensorManager

# Display settings:
SPI_BUS = const(1) # SPI2
Expand Down Expand Up @@ -40,6 +39,12 @@
# Buzzer
BUZZER_PIN = const(25)

# MPU6886 Sensor settings:
MPU6886_I2C_ADDR = const(0x68)
MPU6886_I2C_SCL = const(22)
MPU6886_I2C_SDA = const(21)
MPU6886_I2C_FREQ = const(400000)


print("m5stack_fire.py init buzzer")
buzzer = PWM(Pin(BUZZER_PIN, Pin.OUT, value=1), duty=5)
Expand All @@ -50,6 +55,15 @@
time.sleep(0.1)


print("m5stack_fire.py init IMU")
i2c_bus = I2C(0, scl=Pin(MPU6886_I2C_SCL), sda=Pin(MPU6886_I2C_SDA), freq=MPU6886_I2C_FREQ)
SensorManager.init(
i2c_bus=i2c_bus,
address=MPU6886_I2C_ADDR,
mounted_position=SensorManager.FACING_EARTH,
Copy link
Copy Markdown
Contributor Author

@jedie jedie Feb 18, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No idea if "FACING_EARTH" is correct

)


print("m5stack_fire.py machine.SPI.Bus() initialization")
try:
spi_bus = machine.SPI.Bus(host=SPI_BUS, mosi=LCD_MOSI, sck=LCD_SCLK)
Expand Down
160 changes: 148 additions & 12 deletions internal_filesystem/lib/mpos/sensor_manager.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -159,30 +159,48 @@ def _ensure_imu_initialized(self):
if not self._initialized or self._imu_driver is not None:
return self._imu_driver is not None

# Try QMI8658 first (Waveshare board)
if self._i2c_bus:
try:
from drivers.imu_sensor.qmi8658 import QMI8658
chip_id = self._i2c_bus.readfrom_mem(self._i2c_address, 0x00, 1)[0] # PARTID register
print("Try QMI8658 first (Waveshare board)")
# PARTID register:
chip_id = self._i2c_bus.readfrom_mem(self._i2c_address, 0x00, 1)[0]
print(f"{chip_id=:#04x}")
if chip_id == 0x05: # QMI8685_PARTID
self._imu_driver = _QMI8658Driver(self._i2c_bus, self._i2c_address)
self._register_qmi8658_sensors()
self._load_calibration()
print("Use QMI8658, ok")
return True
except:
pass
except Exception as e:
print("No QMI8658:", e)

# Try WSEN_ISDS (fri3d_2024) or LSM6DSO (fri3d_2026)
try:
from drivers.imu_sensor.wsen_isds import Wsen_Isds
chip_id = self._i2c_bus.readfrom_mem(self._i2c_address, 0x0F, 1)[0] # WHO_AM_I register - could also use Wsen_Isds.get_chip_id()
if chip_id == 0x6A or chip_id == 0x6C: # WSEN_ISDS WHO_AM_I 0x6A (Fri3d 2024) or 0x6C (Fri3d 2026)
print("Try WSEN_ISDS (fri3d_2024) or LSM6DSO (fri3d_2026)")
# WHO_AM_I register - could also use Wsen_Isds.get_chip_id():
chip_id = self._i2c_bus.readfrom_mem(self._i2c_address, 0x0F, 1)[0]
print(f"{chip_id=:#04x}")
# WSEN_ISDS WHO_AM_I 0x6A (Fri3d 2024) or 0x6C (Fri3d 2026):
if chip_id == 0x6A or chip_id == 0x6C:
self._imu_driver = _WsenISDSDriver(self._i2c_bus, self._i2c_address)
self._register_wsen_isds_sensors()
self._load_calibration()
print("Use WSEN_ISDS/LSM6DSO, ok")
return True
except:
pass
except Exception as e:
print("No WSEN_ISDS or LSM6DSO:", e)

try:
print("Try MPU6886 (M5Stack FIRE)")
chip_id = self._i2c_bus.readfrom_mem(self._i2c_address, 0x75, 1)[0]
print(f"{chip_id=:#04x}")
if chip_id == 0x19:
self._imu_driver = _MPU6886Driver(self._i2c_bus, self._i2c_address)
self._register_mpu6886_sensors()
self._load_calibration()
print("Use MPU6886, ok")
return True
except Exception as e:
print("No MPU6886:", e)

return False

Expand Down Expand Up @@ -575,7 +593,39 @@ def _register_qmi8658_sensors(self):
power_ma=0
)
]


def _register_mpu6886_sensors(self):
"""Register MPU6886 sensors in sensor list."""
self._sensor_list = [
Sensor(
name="MPU6886 Accelerometer",
sensor_type=TYPE_ACCELEROMETER,
vendor="InvenSense",
version=1,
max_range="±16g",
resolution="0.0024 m/s²",
power_ma=0.2,
),
Sensor(
name="MPU6886 Gyroscope",
sensor_type=TYPE_GYROSCOPE,
vendor="InvenSense",
version=1,
max_range="±256 deg/s",
resolution="0.002 deg/s",
power_ma=0.7,
),
Sensor(
name="MPU6886 Temperature",
sensor_type=TYPE_IMU_TEMPERATURE,
vendor="InvenSense",
version=1,
max_range="-40°C to +85°C",
resolution="0.05°C",
power_ma=0,
),
]

def _register_wsen_isds_sensors(self):
"""Register WSEN_ISDS sensors in sensor list."""
self._sensor_list = [
Expand Down Expand Up @@ -813,6 +863,92 @@ def set_calibration(self, accel_offsets, gyro_offsets):
self.gyro_offset = list(gyro_offsets)


class _MPU6886Driver(_IMUDriver):
Copy link
Copy Markdown
Contributor Author

@jedie jedie Feb 18, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Most parts of this is copied from _QMI8658Driver
Don't know if everything is correct. If so: It's possible to merge many code parts by create a base class or mixin.

"""Wrapper for MPU6886 IMU (Waveshare board)."""

def __init__(self, i2c_bus, address):
from drivers.imu_sensor.mpu6886 import MPU6886

self.sensor = MPU6886(i2c_bus, address=address)
# Software calibration offsets (MPU6886 has no built-in calibration)
self.accel_offset = [0.0, 0.0, 0.0]
self.gyro_offset = [0.0, 0.0, 0.0]

def read_temperature(self):
"""Read temperature in °C."""
return self.sensor.temperature

def read_acceleration(self):
"""Read acceleration in m/s² (converts from G)."""
ax, ay, az = self.sensor.acceleration
# Convert G to m/s² and apply calibration
return (
(ax * _GRAVITY) - self.accel_offset[0],
(ay * _GRAVITY) - self.accel_offset[1],
(az * _GRAVITY) - self.accel_offset[2],
)

def read_gyroscope(self):
"""Read gyroscope in deg/s (already in correct units)."""
gx, gy, gz = self.sensor.gyro
# Apply calibration
return (
gx - self.gyro_offset[0],
gy - self.gyro_offset[1],
gz - self.gyro_offset[2],
)

def calibrate_accelerometer(self, samples):
"""Calibrate accelerometer (device must be stationary)."""
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0

for _ in range(samples):
ax, ay, az = self.sensor.acceleration
sum_x += ax * _GRAVITY
sum_y += ay * _GRAVITY
sum_z += az * _GRAVITY
time.sleep_ms(10)

if FACING_EARTH == FACING_EARTH:
sum_z *= -1

# Average offsets (assuming Z-axis should read +9.8 m/s²)
self.accel_offset[0] = sum_x / samples
self.accel_offset[1] = sum_y / samples
self.accel_offset[2] = (sum_z / samples) - _GRAVITY

return tuple(self.accel_offset)

def calibrate_gyroscope(self, samples):
"""Calibrate gyroscope (device must be stationary)."""
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0

for _ in range(samples):
gx, gy, gz = self.sensor.gyro
sum_x += gx
sum_y += gy
sum_z += gz
time.sleep_ms(10)

# Average offsets (should be 0 when stationary)
self.gyro_offset[0] = sum_x / samples
self.gyro_offset[1] = sum_y / samples
self.gyro_offset[2] = sum_z / samples

return tuple(self.gyro_offset)

def get_calibration(self):
"""Get current calibration."""
return {"accel_offsets": self.accel_offset, "gyro_offsets": self.gyro_offset}

def set_calibration(self, accel_offsets, gyro_offsets):
"""Set calibration from saved values."""
if accel_offsets:
self.accel_offset = list(accel_offsets)
if gyro_offsets:
self.gyro_offset = list(gyro_offsets)


class _WsenISDSDriver(_IMUDriver):
"""Wrapper for WSEN_ISDS IMU (Fri3d badge)."""

Expand Down
Loading