Skip to content

Commit 70915a7

Browse files
ThomasFarstrikeThomasFarstrike
committed
Rework IMU drivers
1 parent aea339a commit 70915a7

10 files changed

Lines changed: 1026 additions & 1099 deletions

File tree

‎internal_filesystem/lib/mpos/imu/__init__.py‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
from mpos.imu.constants import (
2+
TYPE_ACCELEROMETER,
3+
TYPE_MAGNETIC_FIELD,
4+
TYPE_GYROSCOPE,
5+
TYPE_TEMPERATURE,
6+
TYPE_IMU_TEMPERATURE,
7+
TYPE_SOC_TEMPERATURE,
8+
FACING_EARTH,
9+
FACING_SKY,
10+
GRAVITY,
11+
IMU_CALIBRATION_FILENAME,
12+
)
13+
from mpos.imu.sensor import Sensor
14+
from mpos.imu.manager import ImuManager
15+
16+
__all__ = [
17+
"TYPE_ACCELEROMETER",
18+
"TYPE_MAGNETIC_FIELD",
19+
"TYPE_GYROSCOPE",
20+
"TYPE_TEMPERATURE",
21+
"TYPE_IMU_TEMPERATURE",
22+
"TYPE_SOC_TEMPERATURE",
23+
"FACING_EARTH",
24+
"FACING_SKY",
25+
"GRAVITY",
26+
"IMU_CALIBRATION_FILENAME",
27+
"Sensor",
28+
"ImuManager",
29+
]

‎internal_filesystem/lib/mpos/imu/constants.py‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
TYPE_ACCELEROMETER = 1 # Units: m/s² (meters per second squared)
2+
TYPE_MAGNETIC_FIELD = 2 # Units: μT (micro teslas)
3+
TYPE_GYROSCOPE = 4 # Units: deg/s (degrees per second)
4+
TYPE_TEMPERATURE = 13 # Units: °C (generic, returns first available - deprecated)
5+
TYPE_IMU_TEMPERATURE = 14 # Units: °C (IMU chip temperature)
6+
TYPE_SOC_TEMPERATURE = 15 # Units: °C (MCU/SoC internal temperature)
7+
8+
# mounted_position:
9+
FACING_EARTH = 20 # underside of PCB, like fri3d_2024
10+
FACING_SKY = 21 # top of PCB, like waveshare_esp32_s3_lcd_touch_2 (default)
11+
12+
# Gravity constant for unit conversions
13+
GRAVITY = 9.80665 # m/s²
14+
15+
IMU_CALIBRATION_FILENAME = "imu_calibration.json"

‎internal_filesystem/lib/mpos/imu/drivers/base.py‎

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
import time
2+
3+
from mpos.imu.constants import GRAVITY, FACING_EARTH
4+
5+
6+
class IMUDriverBase:
7+
"""Base class for IMU drivers with shared calibration logic."""
8+
9+
def __init__(self):
10+
self.accel_offset = [0.0, 0.0, 0.0]
11+
self.gyro_offset = [0.0, 0.0, 0.0]
12+
13+
def read_acceleration(self):
14+
"""Returns (x, y, z) in m/s²"""
15+
raise NotImplementedError
16+
17+
def read_gyroscope(self):
18+
"""Returns (x, y, z) in deg/s"""
19+
raise NotImplementedError
20+
21+
def read_temperature(self):
22+
"""Returns temperature in °C"""
23+
raise NotImplementedError
24+
25+
def _raw_acceleration_mps2(self):
26+
"""Returns raw (x, y, z) in m/s² for calibration sampling."""
27+
raise NotImplementedError
28+
29+
def _raw_gyroscope_dps(self):
30+
"""Returns raw (x, y, z) in deg/s for calibration sampling."""
31+
raise NotImplementedError
32+
33+
def calibrate_accelerometer(self, samples):
34+
"""Calibrate accel, return (x, y, z) offsets in m/s²"""
35+
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
36+
37+
for _ in range(samples):
38+
ax, ay, az = self._raw_acceleration_mps2()
39+
sum_x += ax
40+
sum_y += ay
41+
sum_z += az
42+
time.sleep_ms(10)
43+
44+
if FACING_EARTH == FACING_EARTH:
45+
sum_z *= -1
46+
47+
self.accel_offset[0] = sum_x / samples
48+
self.accel_offset[1] = sum_y / samples
49+
self.accel_offset[2] = (sum_z / samples) - GRAVITY
50+
51+
return tuple(self.accel_offset)
52+
53+
def calibrate_gyroscope(self, samples):
54+
"""Calibrate gyro, return (x, y, z) offsets in deg/s"""
55+
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
56+
57+
for _ in range(samples):
58+
gx, gy, gz = self._raw_gyroscope_dps()
59+
sum_x += gx
60+
sum_y += gy
61+
sum_z += gz
62+
time.sleep_ms(10)
63+
64+
self.gyro_offset[0] = sum_x / samples
65+
self.gyro_offset[1] = sum_y / samples
66+
self.gyro_offset[2] = sum_z / samples
67+
68+
return tuple(self.gyro_offset)
69+
70+
def get_calibration(self):
71+
"""Return dict with 'accel_offsets' and 'gyro_offsets' keys"""
72+
return {
73+
"accel_offsets": self.accel_offset,
74+
"gyro_offsets": self.gyro_offset,
75+
}
76+
77+
def set_calibration(self, accel_offsets, gyro_offsets):
78+
"""Set calibration offsets from saved values"""
79+
if accel_offsets:
80+
self.accel_offset = list(accel_offsets)
81+
if gyro_offsets:
82+
self.gyro_offset = list(gyro_offsets)

‎internal_filesystem/lib/mpos/imu/drivers/iio.py‎

Lines changed: 140 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,140 @@
1+
import os
2+
3+
from mpos.imu.drivers.base import IMUDriverBase
4+
5+
6+
class IIODriver(IMUDriverBase):
7+
"""
8+
Read sensor data via Linux IIO sysfs.
9+
10+
Typical base path:
11+
/sys/bus/iio/devices/iio:device0
12+
"""
13+
14+
accel_path: str
15+
16+
def __init__(self):
17+
super().__init__()
18+
self.accel_path = self.find_iio_device_with_file("in_accel_x_raw")
19+
print("path:", self.accel_path)
20+
21+
def _p(self, name: str):
22+
return self.accel_path + "/" + name
23+
24+
def _exists(self, name):
25+
try:
26+
os.stat(name)
27+
return True
28+
except OSError:
29+
return False
30+
31+
def _is_dir(self, path):
32+
# MicroPython: stat tuple, mode is [0]
33+
try:
34+
st = os.stat(path)
35+
mode = st[0]
36+
# directory bit (POSIX): 0o040000
37+
return (mode & 0o170000) == 0o040000
38+
except OSError:
39+
return False
40+
41+
def find_iio_device_with_file(self, filename, base_dir="/sys/bus/iio/devices/"):
42+
"""
43+
Returns full path to iio:deviceX that contains given filename,
44+
e.g. "/sys/bus/iio/devices/iio:device0"
45+
46+
Returns None if not found.
47+
"""
48+
49+
print("Is dir? ", self._is_dir(base_dir), base_dir)
50+
try:
51+
entries = os.listdir(base_dir)
52+
except OSError:
53+
print("Error listing dir")
54+
return None
55+
56+
for entry in entries:
57+
print("Entry:", entry)
58+
if not entry.startswith("iio:device"):
59+
continue
60+
61+
dev_path = base_dir + "/" + entry
62+
if not self._is_dir(dev_path):
63+
continue
64+
65+
if self._exists(dev_path + "/" + filename):
66+
return dev_path
67+
68+
return None
69+
70+
def _read_text(self, name: str) -> str:
71+
print("Read: ", name)
72+
f = open(name, "r")
73+
try:
74+
return f.readline().strip()
75+
finally:
76+
f.close()
77+
78+
def _read_float(self, name: str) -> float:
79+
return float(self._read_text(name))
80+
81+
def _read_int(self, name: str) -> int:
82+
return int(self._read_text(name), 10)
83+
84+
def _read_raw_scaled(self, raw_name: str, scale_name: str) -> float:
85+
raw = self._read_int(raw_name)
86+
scale = self._read_float(scale_name)
87+
return raw * scale
88+
89+
def read_temperature(self) -> float:
90+
"""
91+
Tries common IIO patterns:
92+
- in_temp_input (already scaled, usually millidegree C)
93+
- in_temp_raw + in_temp_scale
94+
"""
95+
if not self.accel_path:
96+
return None
97+
98+
raw_path = self.accel_path + "/" + "in_temp_raw"
99+
scale_path = self.accel_path + "/" + "in_temp_scale"
100+
if not self._exists(raw_path) or not self._exists(scale_path):
101+
return None
102+
return self._read_raw_scaled(raw_path, scale_path)
103+
104+
def _raw_acceleration_mps2(self):
105+
if not self.accel_path:
106+
return (0.0, 0.0, 0.0)
107+
scale_name = self.accel_path + "/" + "in_accel_scale"
108+
109+
ax = self._read_raw_scaled(self.accel_path + "/" + "in_accel_x_raw", scale_name)
110+
ay = self._read_raw_scaled(self.accel_path + "/" + "in_accel_y_raw", scale_name)
111+
az = self._read_raw_scaled(self.accel_path + "/" + "in_accel_z_raw", scale_name)
112+
113+
return (ax, ay, az)
114+
115+
def _raw_gyroscope_dps(self):
116+
if not self.accel_path:
117+
return (0.0, 0.0, 0.0)
118+
scale_name = self.accel_path + "/" + "in_anglvel_scale"
119+
120+
gx = self._read_raw_scaled(self.accel_path + "/" + "in_anglvel_x_raw", scale_name)
121+
gy = self._read_raw_scaled(self.accel_path + "/" + "in_anglvel_y_raw", scale_name)
122+
gz = self._read_raw_scaled(self.accel_path + "/" + "in_anglvel_z_raw", scale_name)
123+
124+
return (gx, gy, gz)
125+
126+
def read_acceleration(self):
127+
ax, ay, az = self._raw_acceleration_mps2()
128+
return (
129+
ax - self.accel_offset[0],
130+
ay - self.accel_offset[1],
131+
az - self.accel_offset[2],
132+
)
133+
134+
def read_gyroscope(self):
135+
gx, gy, gz = self._raw_gyroscope_dps()
136+
return (
137+
gx - self.gyro_offset[0],
138+
gy - self.gyro_offset[1],
139+
gz - self.gyro_offset[2],
140+
)

‎internal_filesystem/lib/mpos/imu/drivers/mpu6886.py‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
from mpos.imu.constants import GRAVITY
2+
from mpos.imu.drivers.base import IMUDriverBase
3+
4+
5+
class MPU6886Driver(IMUDriverBase):
6+
"""Wrapper for MPU6886 IMU (Waveshare board)."""
7+
8+
def __init__(self, i2c_bus, address):
9+
super().__init__()
10+
from drivers.imu_sensor.mpu6886 import MPU6886
11+
12+
self.sensor = MPU6886(i2c_bus, address=address)
13+
14+
def _raw_acceleration_mps2(self):
15+
ax, ay, az = self.sensor.acceleration
16+
return (ax * GRAVITY, ay * GRAVITY, az * GRAVITY)
17+
18+
def _raw_gyroscope_dps(self):
19+
gx, gy, gz = self.sensor.gyro
20+
return (gx, gy, gz)
21+
22+
def read_temperature(self):
23+
return self.sensor.temperature
24+
25+
def read_acceleration(self):
26+
ax, ay, az = self._raw_acceleration_mps2()
27+
return (
28+
ax - self.accel_offset[0],
29+
ay - self.accel_offset[1],
30+
az - self.accel_offset[2],
31+
)
32+
33+
def read_gyroscope(self):
34+
gx, gy, gz = self._raw_gyroscope_dps()
35+
return (
36+
gx - self.gyro_offset[0],
37+
gy - self.gyro_offset[1],
38+
gz - self.gyro_offset[2],
39+
)

‎internal_filesystem/lib/mpos/imu/drivers/qmi8658.py‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
from mpos.imu.constants import GRAVITY
2+
from mpos.imu.drivers.base import IMUDriverBase
3+
4+
5+
class QMI8658Driver(IMUDriverBase):
6+
"""Wrapper for QMI8658 IMU (Waveshare board)."""
7+
8+
def __init__(self, i2c_bus, address):
9+
super().__init__()
10+
from drivers.imu_sensor.qmi8658 import QMI8658
11+
12+
_ACCELSCALE_RANGE_8G = 0b10
13+
_GYROSCALE_RANGE_256DPS = 0b100
14+
self.sensor = QMI8658(
15+
i2c_bus,
16+
address=address,
17+
accel_scale=_ACCELSCALE_RANGE_8G,
18+
gyro_scale=_GYROSCALE_RANGE_256DPS,
19+
)
20+
21+
def _raw_acceleration_mps2(self):
22+
ax, ay, az = self.sensor.acceleration
23+
return (ax * GRAVITY, ay * GRAVITY, az * GRAVITY)
24+
25+
def _raw_gyroscope_dps(self):
26+
gx, gy, gz = self.sensor.gyro
27+
return (gx, gy, gz)
28+
29+
def read_acceleration(self):
30+
ax, ay, az = self._raw_acceleration_mps2()
31+
return (
32+
ax - self.accel_offset[0],
33+
ay - self.accel_offset[1],
34+
az - self.accel_offset[2],
35+
)
36+
37+
def read_gyroscope(self):
38+
gx, gy, gz = self._raw_gyroscope_dps()
39+
return (
40+
gx - self.gyro_offset[0],
41+
gy - self.gyro_offset[1],
42+
gz - self.gyro_offset[2],
43+
)
44+
45+
def read_temperature(self):
46+
return self.sensor.temperature

0 commit comments

Comments
 (0)