Skip to content

Commit 1b60a26

Browse files
jediejedie
committed
Add MPU6886 driver (tested with M5Stack FIRE)
Will fix #44
1 parent 403cd8d commit 1b60a26

3 files changed

Lines changed: 247 additions & 15 deletions

File tree

‎internal_filesystem/lib/mpos/board/m5stack_fire.py‎

Lines changed: 14 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,6 @@
11
# Hardware initialization for ESP32 M5Stack-Fire board
22
# Manufacturer's website at https://https://docs.m5stack.com/en/core/fire_v2.7
33
# Original author: https://github.com/ancebfer
4-
54
import time
65

76
import drivers.display.ili9341 as ili9341
@@ -10,9 +9,9 @@
109
import machine
1110
import mpos.ui
1211
import mpos.ui.focus_direction
13-
from machine import PWM, Pin
12+
from machine import I2C, PWM, Pin
1413
from micropython import const
15-
from mpos import AudioManager, InputManager
14+
from mpos import AudioManager, InputManager, SensorManager
1615

1716
# Display settings:
1817
SPI_BUS = const(1) # SPI2
@@ -50,6 +49,18 @@
5049
time.sleep(0.1)
5150

5251

52+
print("m5stack_fire.py init IMU")
53+
MPU6886_I2C_ADDR = const(0x68)
54+
MPU6886_I2C_SCL = const(22)
55+
MPU6886_I2C_SDA = const(21)
56+
i2c_bus = I2C(0, scl=Pin(MPU6886_I2C_SCL), sda=Pin(MPU6886_I2C_SDA), freq=400000)
57+
SensorManager.init(
58+
i2c_bus=i2c_bus,
59+
address=MPU6886_I2C_ADDR,
60+
mounted_position=SensorManager.FACING_EARTH,
61+
)
62+
63+
5364
print("m5stack_fire.py machine.SPI.Bus() initialization")
5465
try:
5566
spi_bus = machine.SPI.Bus(host=SPI_BUS, mosi=LCD_MOSI, sck=LCD_SCLK)

‎internal_filesystem/lib/mpos/mpu6886.py‎

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
# TODO: Move to internal_filesystem/lib/drivers/imu_sensor/mpu6886.py
2+
3+
"""
4+
MicroPython driver for MPU6886 3-Axis Accelerometer + 3-Axis Gyroscope.
5+
Tested with M5Stack FIRE
6+
https://docs.m5stack.com/en/unit/imu
7+
https://github.com/m5stack/M5Stack/blob/master/src/utility/MPU6886.h
8+
"""
9+
10+
import time
11+
12+
from machine import I2C
13+
from micropython import const
14+
15+
_MPU6886_I2CADDR_DEFAULT = const(0x68)
16+
17+
18+
# MPU6886 register addresses (typical InvenSense MPU6xxx style)
19+
_MPU6886_REG_PWR_MGMT_1 = const(0x6B)
20+
_MPU6886_REG_ACCEL_XOUT_H = const(0x3B)
21+
_MPU6886_REG_GYRO_XOUT_H = const(0x43)
22+
MPU6886_ACCEL_CONFIG = 0x1C
23+
MPU6886_GYRO_CONFIG = 0x1B
24+
25+
# Temperature register
26+
MPU6886_TEMP_OUT_H = const(0x41)
27+
28+
ACCEL_SCALE_8G = 8.0 / 32768.0 # LSB/g for +-8g range
29+
GYRO_SCALE_2000DPS = 2000.0 / 32768.0 # LSB/°/s for +-2000dps range
30+
31+
32+
def twos_complement(val, bits):
33+
if val & (1 << (bits - 1)):
34+
val -= 1 << bits
35+
return val
36+
37+
38+
class MPU6886:
39+
def __init__(
40+
self,
41+
i2c_bus: I2C,
42+
address: int = _MPU6886_I2CADDR_DEFAULT,
43+
):
44+
self.i2c = i2c_bus
45+
self.address = address
46+
47+
for data in (b"\x00", b"\x80", b"\x01"): # Reset, then wake up
48+
self._write(_MPU6886_REG_PWR_MGMT_1, data)
49+
time.sleep(0.01)
50+
51+
self._write(MPU6886_ACCEL_CONFIG, b"\x10") # +-8g
52+
time.sleep(0.001)
53+
54+
self._write(MPU6886_GYRO_CONFIG, b"\x18") # +-2000dps
55+
time.sleep(0.001)
56+
57+
# Helper functions for register operations
58+
def _write(self, reg: int, data: bytes):
59+
self.i2c.writeto_mem(self.address, reg, data)
60+
61+
def _read_xyz(self, reg: int, scale: float) -> tuple[int, int, int]:
62+
data = self.i2c.readfrom_mem(self.address, reg, 6)
63+
x = twos_complement(data[0] << 8 | data[1], 16)
64+
y = twos_complement(data[2] << 8 | data[3], 16)
65+
z = twos_complement(data[4] << 8 | data[5], 16)
66+
return (x * scale, y * scale, z * scale)
67+
68+
@property
69+
def temperature(self) -> float:
70+
buf = self.i2c.readfrom_mem(self.address, MPU6886_TEMP_OUT_H, 14)
71+
temp_raw = (buf[6] << 8) | buf[7]
72+
if temp_raw & 0x8000: # If MSB is 1, it's negative
73+
temp_raw -= 0x10000 # Subtract 2^16 to get negative value
74+
return temp_raw / 326.8 + 25 # Convert to °C using MPU6886 formula
75+
76+
@property
77+
def acceleration(self) -> tuple[int, int, int]:
78+
"""Get current acceleration reading."""
79+
return self._read_xyz(_MPU6886_REG_ACCEL_XOUT_H, scale=ACCEL_SCALE_8G)
80+
81+
@property
82+
def gyro(self) -> tuple[int, int, int]:
83+
"""Get current gyroscope reading."""
84+
return self._read_xyz(_MPU6886_REG_GYRO_XOUT_H, scale=GYRO_SCALE_2000DPS)

‎internal_filesystem/lib/mpos/sensor_manager.py‎

Lines changed: 149 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -159,30 +159,48 @@ def _ensure_imu_initialized(self):
159159
if not self._initialized or self._imu_driver is not None:
160160
return self._imu_driver is not None
161161

162-
# Try QMI8658 first (Waveshare board)
163162
if self._i2c_bus:
164163
try:
165-
from drivers.imu_sensor.qmi8658 import QMI8658
166-
chip_id = self._i2c_bus.readfrom_mem(self._i2c_address, 0x00, 1)[0] # PARTID register
164+
print("Try QMI8658 first (Waveshare board)")
165+
# PARTID register:
166+
chip_id = self._i2c_bus.readfrom_mem(self._i2c_address, 0x00, 1)[0]
167+
print(f"{chip_id=:#04x}")
167168
if chip_id == 0x05: # QMI8685_PARTID
168169
self._imu_driver = _QMI8658Driver(self._i2c_bus, self._i2c_address)
169170
self._register_qmi8658_sensors()
170171
self._load_calibration()
172+
print("Use QMI8658, ok")
171173
return True
172-
except:
173-
pass
174+
except Exception as e:
175+
print("No QMI8658:", e)
174176

175-
# Try WSEN_ISDS (fri3d_2024) or LSM6DSO (fri3d_2026)
176177
try:
177-
from drivers.imu_sensor.wsen_isds import Wsen_Isds
178-
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()
179-
if chip_id == 0x6A or chip_id == 0x6C: # WSEN_ISDS WHO_AM_I 0x6A (Fri3d 2024) or 0x6C (Fri3d 2026)
178+
print("Try WSEN_ISDS (fri3d_2024) or LSM6DSO (fri3d_2026)")
179+
# WHO_AM_I register - could also use Wsen_Isds.get_chip_id():
180+
chip_id = self._i2c_bus.readfrom_mem(self._i2c_address, 0x0F, 1)[0]
181+
print(f"{chip_id=:#04x}")
182+
# WSEN_ISDS WHO_AM_I 0x6A (Fri3d 2024) or 0x6C (Fri3d 2026):
183+
if chip_id == 0x6A or chip_id == 0x6C:
180184
self._imu_driver = _WsenISDSDriver(self._i2c_bus, self._i2c_address)
181185
self._register_wsen_isds_sensors()
182186
self._load_calibration()
187+
print("Use WSEN_ISDS/LSM6DSO, ok")
183188
return True
184-
except:
185-
pass
189+
except Exception as e:
190+
print("No WSEN_ISDS or LSM6DSO:", e)
191+
192+
try:
193+
print("Try MPU6886 (M5Stack FIRE)")
194+
chip_id = self._i2c_bus.readfrom_mem(self._i2c_address, 0x75, 1)[0]
195+
print(f"{chip_id=:#04x}")
196+
if chip_id == 0x19:
197+
self._imu_driver = _MPU6886Driver(self._i2c_bus, self._i2c_address)
198+
self._register_mpu6886_sensors()
199+
self._load_calibration()
200+
print("Use MPU6886, ok")
201+
return True
202+
except Exception as e:
203+
print("No MPU6886:", e)
186204

187205
return False
188206

@@ -575,7 +593,39 @@ def _register_qmi8658_sensors(self):
575593
power_ma=0
576594
)
577595
]
578-
596+
597+
def _register_mpu6886_sensors(self):
598+
"""Register MPU6886 sensors in sensor list."""
599+
self._sensor_list = [
600+
Sensor(
601+
name="MPU6886 Accelerometer",
602+
sensor_type=TYPE_ACCELEROMETER,
603+
vendor="InvenSense",
604+
version=1,
605+
max_range="±16g",
606+
resolution="0.0024 m/s²",
607+
power_ma=0.2,
608+
),
609+
Sensor(
610+
name="MPU6886 Gyroscope",
611+
sensor_type=TYPE_GYROSCOPE,
612+
vendor="InvenSense",
613+
version=1,
614+
max_range="±256 deg/s",
615+
resolution="0.002 deg/s",
616+
power_ma=0.7,
617+
),
618+
Sensor(
619+
name="MPU6886 Temperature",
620+
sensor_type=TYPE_IMU_TEMPERATURE,
621+
vendor="InvenSense",
622+
version=1,
623+
max_range="-40°C to +85°C",
624+
resolution="0.05°C",
625+
power_ma=0,
626+
),
627+
]
628+
579629
def _register_wsen_isds_sensors(self):
580630
"""Register WSEN_ISDS sensors in sensor list."""
581631
self._sensor_list = [
@@ -813,6 +863,93 @@ def set_calibration(self, accel_offsets, gyro_offsets):
813863
self.gyro_offset = list(gyro_offsets)
814864

815865

866+
class _MPU6886Driver(_IMUDriver):
867+
"""Wrapper for MPU6886 IMU (Waveshare board)."""
868+
869+
def __init__(self, i2c_bus, address):
870+
from mpos.mpu6886 import MPU6886 # TODO: Move to "drivers.imu_sensor.mpu6886":
871+
# from drivers.imu_sensor.mpu6886 import MPU6886
872+
873+
self.sensor = MPU6886(i2c_bus, address=address)
874+
# Software calibration offsets (MPU6886 has no built-in calibration)
875+
self.accel_offset = [0.0, 0.0, 0.0]
876+
self.gyro_offset = [0.0, 0.0, 0.0]
877+
878+
def read_temperature(self):
879+
"""Read temperature in °C."""
880+
return self.sensor.temperature
881+
882+
def read_acceleration(self):
883+
"""Read acceleration in m/s² (converts from G)."""
884+
ax, ay, az = self.sensor.acceleration
885+
# Convert G to m/s² and apply calibration
886+
return (
887+
(ax * _GRAVITY) - self.accel_offset[0],
888+
(ay * _GRAVITY) - self.accel_offset[1],
889+
(az * _GRAVITY) - self.accel_offset[2],
890+
)
891+
892+
def read_gyroscope(self):
893+
"""Read gyroscope in deg/s (already in correct units)."""
894+
gx, gy, gz = self.sensor.gyro
895+
# Apply calibration
896+
return (
897+
gx - self.gyro_offset[0],
898+
gy - self.gyro_offset[1],
899+
gz - self.gyro_offset[2],
900+
)
901+
902+
def calibrate_accelerometer(self, samples):
903+
"""Calibrate accelerometer (device must be stationary)."""
904+
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
905+
906+
for _ in range(samples):
907+
ax, ay, az = self.sensor.acceleration
908+
sum_x += ax * _GRAVITY
909+
sum_y += ay * _GRAVITY
910+
sum_z += az * _GRAVITY
911+
time.sleep_ms(10)
912+
913+
if FACING_EARTH == FACING_EARTH:
914+
sum_z *= -1
915+
916+
# Average offsets (assuming Z-axis should read +9.8 m/s²)
917+
self.accel_offset[0] = sum_x / samples
918+
self.accel_offset[1] = sum_y / samples
919+
self.accel_offset[2] = (sum_z / samples) - _GRAVITY
920+
921+
return tuple(self.accel_offset)
922+
923+
def calibrate_gyroscope(self, samples):
924+
"""Calibrate gyroscope (device must be stationary)."""
925+
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
926+
927+
for _ in range(samples):
928+
gx, gy, gz = self.sensor.gyro
929+
sum_x += gx
930+
sum_y += gy
931+
sum_z += gz
932+
time.sleep_ms(10)
933+
934+
# Average offsets (should be 0 when stationary)
935+
self.gyro_offset[0] = sum_x / samples
936+
self.gyro_offset[1] = sum_y / samples
937+
self.gyro_offset[2] = sum_z / samples
938+
939+
return tuple(self.gyro_offset)
940+
941+
def get_calibration(self):
942+
"""Get current calibration."""
943+
return {"accel_offsets": self.accel_offset, "gyro_offsets": self.gyro_offset}
944+
945+
def set_calibration(self, accel_offsets, gyro_offsets):
946+
"""Set calibration from saved values."""
947+
if accel_offsets:
948+
self.accel_offset = list(accel_offsets)
949+
if gyro_offsets:
950+
self.gyro_offset = list(gyro_offsets)
951+
952+
816953
class _WsenISDSDriver(_IMUDriver):
817954
"""Wrapper for WSEN_ISDS IMU (Fri3d badge)."""
818955

0 commit comments

Comments
 (0)