mirror of
https://github.com/m5stack/MicroPythonOS.git
synced 2026-05-20 11:51:27 -07:00
Thomas Farstrike
94401dbbde
This fixes: Traceback (most recent call last): File "<stdin>", line 31, in <module> File "lib/mpos/main.py", line 150, in <module> File "lib/mpos/board/linux.py", line 133, in <module> File "lib/mpos/sensor_manager.py", line 286, in class_method File "lib/mpos/sensor_manager.py", line 109, in init_iio File "lib/mpos/imu/manager.py", line 52, in init_iio File "lib/mpos/imu/drivers/iio.py", line 21, in __init__ File "lib/mpos/imu/drivers/iio.py", line 129, in ensure_sampling_frequency_max TypeError: unsupported types for __add__: 'NoneType', 'str'
303 lines
9.2 KiB
Python
303 lines
9.2 KiB
Python
import os
|
|
|
|
from mpos.imu.drivers.base import IMUDriverBase
|
|
|
|
|
|
class IIODriver(IMUDriverBase):
|
|
"""
|
|
Read sensor data via Linux IIO sysfs.
|
|
|
|
Typical base path:
|
|
/sys/bus/iio/devices/iio:device0
|
|
"""
|
|
|
|
accel_path: str
|
|
mag_path: str
|
|
gyro_path: str
|
|
|
|
def __init__(self):
|
|
super().__init__()
|
|
self.accel_path = self.find_iio_device_with_file("in_accel_x_raw")
|
|
self.mag_path = self.find_iio_device_with_file("in_magn_x_raw")
|
|
self.gyro_path = self.find_iio_device_with_file("in_anglvel_x_raw")
|
|
self.available = any((self.accel_path, self.mag_path, self.gyro_path))
|
|
|
|
if not self.available:
|
|
print("IIO: no IIO sensors detected")
|
|
return
|
|
|
|
if self.accel_path:
|
|
self.ensure_sampling_frequency_max(self.accel_path)
|
|
if self.mag_path:
|
|
self.ensure_sampling_frequency_max(self.mag_path)
|
|
if self.gyro_path:
|
|
self.ensure_sampling_frequency_max(self.gyro_path)
|
|
|
|
def _p(self, name: str):
|
|
return self.accel_path + "/" + name
|
|
|
|
def _exists(self, name):
|
|
try:
|
|
os.stat(name)
|
|
return True
|
|
except OSError:
|
|
return False
|
|
|
|
def _is_dir(self, path):
|
|
# MicroPython: stat tuple, mode is [0]
|
|
try:
|
|
st = os.stat(path)
|
|
mode = st[0]
|
|
# directory bit (POSIX): 0o040000
|
|
return (mode & 0o170000) == 0o040000
|
|
except OSError:
|
|
return False
|
|
|
|
def find_iio_device_with_file(self, filename, base_dir="/sys/bus/iio/devices/"):
|
|
"""
|
|
Returns full path to iio:deviceX that contains given filename,
|
|
e.g. "/sys/bus/iio/devices/iio:device0"
|
|
|
|
Returns None if not found.
|
|
"""
|
|
|
|
print("Is dir? ", self._is_dir(base_dir), base_dir)
|
|
try:
|
|
entries = os.listdir(base_dir)
|
|
except OSError:
|
|
print("Error listing dir")
|
|
return None
|
|
|
|
for entry in entries:
|
|
print("Entry:", entry)
|
|
if not entry.startswith("iio:device"):
|
|
continue
|
|
|
|
dev_path = base_dir + "/" + entry
|
|
if not self._is_dir(dev_path):
|
|
continue
|
|
|
|
if self._exists(dev_path + "/" + filename):
|
|
return dev_path
|
|
|
|
return None
|
|
|
|
def _read_text(self, name: str) -> str:
|
|
if False:
|
|
print("Read: ", name)
|
|
f = open(name, "r")
|
|
try:
|
|
return f.readline().strip()
|
|
finally:
|
|
f.close()
|
|
|
|
def _parse_available_freqs(self, text):
|
|
"""
|
|
IIO typically uses either:
|
|
"12.5 25 50 100"
|
|
or
|
|
"0.5 1 2 4 8 16"
|
|
|
|
Returns list of floats.
|
|
"""
|
|
out = []
|
|
for tok in text.replace(",", " ").split():
|
|
out.append(float(tok))
|
|
return out
|
|
|
|
def _format_freq_for_sysfs(self, f):
|
|
"""
|
|
Kernel sysfs usually accepts either integer or decimal.
|
|
We'll keep it minimal:
|
|
- if f is whole number -> "100"
|
|
- else -> "12.5"
|
|
"""
|
|
if int(f) == f:
|
|
return str(int(f))
|
|
# avoid scientific notation
|
|
s = ("%.6f" % f).rstrip("0").rstrip(".")
|
|
return s
|
|
|
|
def _try_set_via_sudo_tee(self, path, value_str):
|
|
"""
|
|
Executes:
|
|
sh -c 'echo VALUE | sudo tee PATH'
|
|
Returns True if command returns 0.
|
|
"""
|
|
cmd = "sh -c 'echo %s | sudo tee %s >/dev/null'" % (value_str, path)
|
|
rc = os.system(cmd)
|
|
return rc == 0
|
|
|
|
def ensure_sampling_frequency_max(self, dev_path):
|
|
"""
|
|
dev_path: "/sys/bus/iio/devices/iio:deviceX"
|
|
|
|
Returns:
|
|
(changed: bool, max_freq: float or None, current: float or None)
|
|
"""
|
|
if not dev_path:
|
|
return (False, None, None)
|
|
|
|
sf = dev_path + "/sampling_frequency"
|
|
sfa = dev_path + "/sampling_frequency_available"
|
|
|
|
# read current
|
|
cur_s = self._read_text(sf)
|
|
cur = float(cur_s)
|
|
|
|
avail_s = self._read_text(sfa)
|
|
avail = self._parse_available_freqs(avail_s)
|
|
|
|
maxf = max(avail)
|
|
|
|
# already max (tolerate float fuzz)
|
|
if abs(cur - maxf) < 1e-6:
|
|
print("Already at max frequency")
|
|
return (False, maxf, cur)
|
|
|
|
max_str = self._format_freq_for_sysfs(maxf)
|
|
|
|
# Fallback: sudo tee
|
|
ok = self._try_set_via_sudo_tee(sf, max_str)
|
|
if not ok:
|
|
print("Can't switch to max frequency")
|
|
return (False, maxf, cur)
|
|
|
|
new_cur = float(self._read_text(sf))
|
|
|
|
return (True, maxf, new_cur)
|
|
|
|
def ensure_sampling_frequency_max_for_device_with_file(self, filename):
|
|
"""
|
|
Convenience wrapper:
|
|
- finds iio device containing filename
|
|
- sets sampling_frequency to maximum
|
|
"""
|
|
dev = self.find_iio_device_with_file(filename)
|
|
if dev is None:
|
|
return (None, False, None, None)
|
|
|
|
changed, maxf, cur = self.ensure_sampling_frequency_max(dev)
|
|
return (dev, changed, maxf, cur)
|
|
|
|
def _read_float(self, name: str) -> float:
|
|
return float(self._read_text(name))
|
|
|
|
def _read_int(self, name: str) -> int:
|
|
return int(self._read_text(name), 10)
|
|
|
|
def _read_raw_scaled(self, raw_name: str, scale_name: str) -> float:
|
|
raw = self._read_int(raw_name)
|
|
scale = self._read_float(scale_name)
|
|
return raw * scale
|
|
|
|
def read_temperature(self) -> float:
|
|
"""
|
|
Tries common IIO patterns:
|
|
- in_temp_input (already scaled, usually millidegree C)
|
|
- in_temp_raw + in_temp_scale
|
|
"""
|
|
return 12.34
|
|
if not self.accel_path:
|
|
return None
|
|
|
|
raw_path = self.accel_path + "/" + "in_temp_raw"
|
|
scale_path = self.accel_path + "/" + "in_temp_scale"
|
|
if not self._exists(raw_path) or not self._exists(scale_path):
|
|
return None
|
|
return self._read_raw_scaled(raw_path, scale_path)
|
|
|
|
def _read_mount_matrix(self, p):
|
|
"""
|
|
Reads IIO mount matrix from *mount_matrix
|
|
|
|
Format example:
|
|
"0, 1, 0; -1, 0, 0; 0, 0, 1"
|
|
|
|
Returns:
|
|
3x3 matrix as tuple of tuples (float)
|
|
"""
|
|
path = p + "/" + "in_accel_mount_matrix"
|
|
if not self._exists(path):
|
|
# Strange, librem 5 has different filename
|
|
path = self.accel_path + "/" + "mount_matrix"
|
|
if not self._exists(path):
|
|
return None
|
|
|
|
text = self._read_text(path).strip()
|
|
|
|
rows = []
|
|
for row in text.split(";"):
|
|
rows.append(tuple(float(x.strip()) for x in row.split(",")))
|
|
|
|
if len(rows) != 3 or any(len(r) != 3 for r in rows):
|
|
raise ValueError("Invalid mount matrix format")
|
|
|
|
return tuple(rows)
|
|
|
|
|
|
def _apply_mount_matrix(self, ax, ay, az, p):
|
|
"""
|
|
Applies IIO mount matrix to acceleration vector.
|
|
|
|
Returns rotated (ax, ay, az).
|
|
"""
|
|
M = self._read_mount_matrix(p)
|
|
if M is None:
|
|
return (ax, ay, az)
|
|
|
|
x = M[0][0]*ax + M[0][1]*ay + M[0][2]*az
|
|
y = M[1][0]*ax + M[1][1]*ay + M[1][2]*az
|
|
z = M[2][0]*ax + M[2][1]*ay + M[2][2]*az
|
|
|
|
return (x, y, z)
|
|
|
|
def _raw_acceleration_mps2(self):
|
|
if not self.accel_path:
|
|
return (0.0, 0.0, 0.0)
|
|
scale_name = self.accel_path + "/" + "in_accel_scale"
|
|
|
|
ax = self._read_raw_scaled(self.accel_path + "/" + "in_accel_x_raw", scale_name)
|
|
ay = self._read_raw_scaled(self.accel_path + "/" + "in_accel_y_raw", scale_name)
|
|
az = self._read_raw_scaled(self.accel_path + "/" + "in_accel_z_raw", scale_name)
|
|
|
|
return self._apply_mount_matrix(ax, ay, az, self.accel_path)
|
|
|
|
def _raw_gyroscope_dps(self):
|
|
if not self.gyro_path:
|
|
return (0.0, 0.0, 0.0)
|
|
scale_name = self.gyro_path + "/" + "in_anglvel_scale"
|
|
mul = 57.2957795
|
|
|
|
gx = mul * self._read_raw_scaled(self.gyro_path + "/" + "in_anglvel_x_raw", scale_name)
|
|
gy = mul * self._read_raw_scaled(self.gyro_path + "/" + "in_anglvel_y_raw", scale_name)
|
|
gz = mul * self._read_raw_scaled(self.gyro_path + "/" + "in_anglvel_z_raw", scale_name)
|
|
|
|
return self._apply_mount_matrix(gx, gy, gz, self.gyro_path)
|
|
|
|
def read_acceleration(self):
|
|
ax, ay, az = self._raw_acceleration_mps2()
|
|
return (
|
|
ax - self.accel_offset[0],
|
|
ay - self.accel_offset[1],
|
|
az - self.accel_offset[2],
|
|
)
|
|
|
|
def read_gyroscope(self):
|
|
gx, gy, gz = self._raw_gyroscope_dps()
|
|
return (
|
|
gx - self.gyro_offset[0],
|
|
gy - self.gyro_offset[1],
|
|
gz - self.gyro_offset[2],
|
|
)
|
|
|
|
def read_magnetometer(self) -> tuple[float, float, float]:
|
|
if not self.mag_path:
|
|
return (0.0, 0.0, 0.0)
|
|
|
|
gx = self._read_raw_scaled(self.mag_path + "/" + "in_magn_x_raw", self.mag_path + "/" + "in_magn_x_scale")
|
|
gy = self._read_raw_scaled(self.mag_path + "/" + "in_magn_y_raw", self.mag_path + "/" + "in_magn_y_scale")
|
|
gz = self._read_raw_scaled(self.mag_path + "/" + "in_magn_z_raw", self.mag_path + "/" + "in_magn_z_scale")
|
|
|
|
return self._apply_mount_matrix(gx, gy, gz, self.mag_path)
|