From 23df0f51f2aa8c8e1d11f2deeca6f5abf3242e43 Mon Sep 17 00:00:00 2001 From: JensDiemer Date: Wed, 18 Feb 2026 21:21:15 +0100 Subject: [PATCH] Add MPU6886 driver (tested with M5Stack FIRE) Will fix https://github.com/MicroPythonOS/MicroPythonOS/issues/44 --- .../lib/drivers/imu_sensor/mpu6886.py | 83 +++++++++ .../lib/mpos/board/m5stack_fire.py | 20 ++- .../lib/mpos/sensor_manager.py | 160 ++++++++++++++++-- 3 files changed, 248 insertions(+), 15 deletions(-) create mode 100644 internal_filesystem/lib/drivers/imu_sensor/mpu6886.py diff --git a/internal_filesystem/lib/drivers/imu_sensor/mpu6886.py b/internal_filesystem/lib/drivers/imu_sensor/mpu6886.py new file mode 100644 index 00000000..0cf8d137 --- /dev/null +++ b/internal_filesystem/lib/drivers/imu_sensor/mpu6886.py @@ -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) diff --git a/internal_filesystem/lib/mpos/board/m5stack_fire.py b/internal_filesystem/lib/mpos/board/m5stack_fire.py index 3f41a0f8..9aac9953 100644 --- a/internal_filesystem/lib/mpos/board/m5stack_fire.py +++ b/internal_filesystem/lib/mpos/board/m5stack_fire.py @@ -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 @@ -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 @@ -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) @@ -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, +) + + print("m5stack_fire.py machine.SPI.Bus() initialization") try: spi_bus = machine.SPI.Bus(host=SPI_BUS, mosi=LCD_MOSI, sck=LCD_SCLK) diff --git a/internal_filesystem/lib/mpos/sensor_manager.py b/internal_filesystem/lib/mpos/sensor_manager.py index fa5eab57..c9bc86d3 100644 --- a/internal_filesystem/lib/mpos/sensor_manager.py +++ b/internal_filesystem/lib/mpos/sensor_manager.py @@ -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 @@ -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 = [ @@ -813,6 +863,92 @@ def set_calibration(self, accel_offsets, gyro_offsets): self.gyro_offset = list(gyro_offsets) +class _MPU6886Driver(_IMUDriver): + """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)."""