Skip to content

Commit 23df0f5

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

3 files changed

Lines changed: 248 additions & 15 deletions

File tree

‎internal_filesystem/lib/drivers/imu_sensor/mpu6886.py‎

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

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

Lines changed: 17 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
@@ -40,6 +39,12 @@
4039
# Buzzer
4140
BUZZER_PIN = const(25)
4241

42+
# MPU6886 Sensor settings:
43+
MPU6886_I2C_ADDR = const(0x68)
44+
MPU6886_I2C_SCL = const(22)
45+
MPU6886_I2C_SDA = const(21)
46+
MPU6886_I2C_FREQ = const(400000)
47+
4348

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

5257

58+
print("m5stack_fire.py init IMU")
59+
i2c_bus = I2C(0, scl=Pin(MPU6886_I2C_SCL), sda=Pin(MPU6886_I2C_SDA), freq=MPU6886_I2C_FREQ)
60+
SensorManager.init(
61+
i2c_bus=i2c_bus,
62+
address=MPU6886_I2C_ADDR,
63+
mounted_position=SensorManager.FACING_EARTH,
64+
)
65+
66+
5367
print("m5stack_fire.py machine.SPI.Bus() initialization")
5468
try:
5569
spi_bus = machine.SPI.Bus(host=SPI_BUS, mosi=LCD_MOSI, sck=LCD_SCLK)

‎internal_filesystem/lib/mpos/sensor_manager.py‎

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

0 commit comments

Comments
 (0)