You've already forked MicroPythonOS
mirror of
https://github.com/m5stack/MicroPythonOS.git
synced 2026-05-20 11:51:27 -07:00
Fri3d 2024 Board: add support for WSEN ISDS IMU
This commit is contained in:
Thomas Farstrike
@@ -114,6 +114,33 @@ The `c_mpos/src/webcam.c` module provides webcam support for desktop builds usin
|
||||
|
||||
## Build System
|
||||
|
||||
### Development Workflow (IMPORTANT)
|
||||
|
||||
**For most development, you do NOT need to rebuild the firmware!**
|
||||
|
||||
When you run `scripts/install.sh`, it copies files from `internal_filesystem/` to the device storage. These files override the frozen filesystem because the storage paths are first in `sys.path`. This means:
|
||||
|
||||
```bash
|
||||
# Fast development cycle (recommended):
|
||||
# 1. Edit Python files in internal_filesystem/
|
||||
# 2. Install to device:
|
||||
./scripts/install.sh waveshare-esp32-s3-touch-lcd-2
|
||||
|
||||
# That's it! Your changes are live on the device.
|
||||
```
|
||||
|
||||
**You only need to rebuild firmware (`./scripts/build_mpos.sh esp32`) when:**
|
||||
- Testing the frozen `lib/` for production releases
|
||||
- Modifying C extension modules (`c_mpos/`, `secp256k1-embedded-ecdh/`)
|
||||
- Changing MicroPython core or LVGL bindings
|
||||
- Creating a fresh firmware image for distribution
|
||||
|
||||
**Desktop development** always uses the unfrozen files, so you never need to rebuild for Python changes:
|
||||
```bash
|
||||
# Edit internal_filesystem/ files
|
||||
./scripts/run_desktop.sh # Changes are immediately active
|
||||
```
|
||||
|
||||
### Building Firmware
|
||||
|
||||
The main build script is `scripts/build_mpos.sh`:
|
||||
|
||||
@@ -256,57 +256,78 @@ class CalibrateIMUActivity(Activity):
|
||||
def calibration_thread_func(self):
|
||||
"""Background thread for calibration process."""
|
||||
try:
|
||||
print("[CalibrateIMU] === Calibration thread started ===")
|
||||
|
||||
# Step 1: Check stationarity
|
||||
print("[CalibrateIMU] Step 1: Checking stationarity...")
|
||||
if self.is_desktop:
|
||||
stationarity = {'is_stationary': True, 'message': 'Mock: Stationary'}
|
||||
else:
|
||||
print("[CalibrateIMU] Calling SensorManager.check_stationarity(samples=30)...")
|
||||
stationarity = SensorManager.check_stationarity(samples=30)
|
||||
print(f"[CalibrateIMU] Stationarity result: {stationarity}")
|
||||
|
||||
if stationarity is None or not stationarity['is_stationary']:
|
||||
msg = stationarity['message'] if stationarity else "Stationarity check failed"
|
||||
print(f"[CalibrateIMU] Device not stationary: {msg}")
|
||||
self.update_ui_threadsafe_if_foreground(self.handle_calibration_error,
|
||||
f"Device not stationary!\n\n{msg}\n\nPlace on flat surface and try again.")
|
||||
return
|
||||
|
||||
print("[CalibrateIMU] Device is stationary, proceeding to calibration")
|
||||
|
||||
# Step 2: Perform calibration
|
||||
print("[CalibrateIMU] Step 2: Performing calibration...")
|
||||
self.update_ui_threadsafe_if_foreground(lambda: self.set_state(CalibrationState.CALIBRATING))
|
||||
time.sleep(0.5) # Brief pause for user to see status change
|
||||
|
||||
if self.is_desktop:
|
||||
# Mock calibration
|
||||
print("[CalibrateIMU] Mock calibration (desktop)")
|
||||
time.sleep(2)
|
||||
accel_offsets = (0.1, -0.05, 0.15)
|
||||
gyro_offsets = (0.2, -0.1, 0.05)
|
||||
else:
|
||||
# Real calibration
|
||||
print("[CalibrateIMU] Real calibration (hardware)")
|
||||
accel = SensorManager.get_default_sensor(SensorManager.TYPE_ACCELEROMETER)
|
||||
gyro = SensorManager.get_default_sensor(SensorManager.TYPE_GYROSCOPE)
|
||||
print(f"[CalibrateIMU] Accel sensor: {accel}, Gyro sensor: {gyro}")
|
||||
|
||||
if accel:
|
||||
print("[CalibrateIMU] Calibrating accelerometer (100 samples)...")
|
||||
accel_offsets = SensorManager.calibrate_sensor(accel, samples=100)
|
||||
print(f"[CalibrateIMU] Accel offsets: {accel_offsets}")
|
||||
else:
|
||||
accel_offsets = None
|
||||
|
||||
if gyro:
|
||||
print("[CalibrateIMU] Calibrating gyroscope (100 samples)...")
|
||||
gyro_offsets = SensorManager.calibrate_sensor(gyro, samples=100)
|
||||
print(f"[CalibrateIMU] Gyro offsets: {gyro_offsets}")
|
||||
else:
|
||||
gyro_offsets = None
|
||||
|
||||
# Step 3: Verify results
|
||||
print("[CalibrateIMU] Step 3: Verifying calibration...")
|
||||
self.update_ui_threadsafe_if_foreground(lambda: self.set_state(CalibrationState.VERIFYING))
|
||||
time.sleep(0.5)
|
||||
|
||||
if self.is_desktop:
|
||||
verify_quality = self.get_mock_quality(good=True)
|
||||
else:
|
||||
print("[CalibrateIMU] Checking calibration quality (50 samples)...")
|
||||
verify_quality = SensorManager.check_calibration_quality(samples=50)
|
||||
print(f"[CalibrateIMU] Verification quality: {verify_quality}")
|
||||
|
||||
if verify_quality is None:
|
||||
print("[CalibrateIMU] Verification failed")
|
||||
self.update_ui_threadsafe_if_foreground(self.handle_calibration_error,
|
||||
"Calibration completed but verification failed")
|
||||
return
|
||||
|
||||
# Step 4: Show results
|
||||
print("[CalibrateIMU] Step 4: Showing results...")
|
||||
rating = verify_quality['quality_rating']
|
||||
score = verify_quality['quality_score']
|
||||
|
||||
@@ -316,10 +337,14 @@ class CalibrateIMUActivity(Activity):
|
||||
if gyro_offsets:
|
||||
result_msg += f"\n\nGyro offsets:\nX:{gyro_offsets[0]:.3f} Y:{gyro_offsets[1]:.3f} Z:{gyro_offsets[2]:.3f}"
|
||||
|
||||
print(f"[CalibrateIMU] Calibration complete! Result: {result_msg[:80]}")
|
||||
self.update_ui_threadsafe_if_foreground(self.show_calibration_complete, result_msg)
|
||||
print("[CalibrateIMU] === Calibration thread finished ===")
|
||||
|
||||
except Exception as e:
|
||||
print(f"[CalibrateIMU] Calibration error: {e}")
|
||||
import sys
|
||||
sys.print_exception(e)
|
||||
self.update_ui_threadsafe_if_foreground(self.handle_calibration_error, str(e))
|
||||
|
||||
def show_calibration_complete(self, result_msg):
|
||||
|
||||
+2
-1
@@ -151,7 +151,8 @@ class CheckIMUCalibrationActivity(Activity):
|
||||
if self.is_desktop:
|
||||
quality = self.get_mock_quality()
|
||||
else:
|
||||
quality = SensorManager.check_calibration_quality(samples=30)
|
||||
# Use only 5 samples for real-time display (faster, less blocking)
|
||||
quality = SensorManager.check_calibration_quality(samples=5)
|
||||
|
||||
if quality is None:
|
||||
self.status_label.set_text("Error reading IMU")
|
||||
|
||||
@@ -126,8 +126,9 @@ class Wsen_Isds:
|
||||
acc_range: Accelerometer range ("2g", "4g", "8g", "16g")
|
||||
acc_data_rate: Accelerometer data rate ("0", "1.6Hz", "12.5Hz", ...)
|
||||
gyro_range: Gyroscope range ("125dps", "250dps", "500dps", "1000dps", "2000dps")
|
||||
gyro_data_rate: Gyroscope data rate ("0", "12.5Hz", "26Hz", ...)
|
||||
gyro_data_rate: Gyroscope data rate ("0", "12.5Hz", "26Hz", ...")
|
||||
"""
|
||||
print(f"[WSEN_ISDS] __init__ called with address={hex(address)}, acc_range={acc_range}, acc_data_rate={acc_data_rate}, gyro_range={gyro_range}, gyro_data_rate={gyro_data_rate}")
|
||||
self.i2c = i2c
|
||||
self.address = address
|
||||
|
||||
@@ -149,10 +150,31 @@ class Wsen_Isds:
|
||||
self.GYRO_NUM_SAMPLES_CALIBRATION = 5
|
||||
self.GYRO_CALIBRATION_DELAY_MS = 10
|
||||
|
||||
print("[WSEN_ISDS] Configuring accelerometer...")
|
||||
self.set_acc_range(acc_range)
|
||||
self.set_acc_data_rate(acc_data_rate)
|
||||
print("[WSEN_ISDS] Accelerometer configured")
|
||||
|
||||
print("[WSEN_ISDS] Configuring gyroscope...")
|
||||
self.set_gyro_range(gyro_range)
|
||||
self.set_gyro_data_rate(gyro_data_rate)
|
||||
print("[WSEN_ISDS] Gyroscope configured")
|
||||
|
||||
# Give sensors time to stabilize and start producing data
|
||||
# Especially important for gyroscope which may need warmup time
|
||||
print("[WSEN_ISDS] Waiting 100ms for sensors to stabilize...")
|
||||
time.sleep_ms(100)
|
||||
|
||||
# Debug: Read all control registers to see full sensor state
|
||||
print("[WSEN_ISDS] === Sensor State After Initialization ===")
|
||||
for reg_addr in [0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19]:
|
||||
try:
|
||||
reg_val = self.i2c.readfrom_mem(self.address, reg_addr, 1)[0]
|
||||
print(f"[WSEN_ISDS] Reg 0x{reg_addr:02x} (CTRL{reg_addr-0x0f}): 0x{reg_val:02x} = 0b{reg_val:08b}")
|
||||
except:
|
||||
pass
|
||||
|
||||
print("[WSEN_ISDS] Initialization complete")
|
||||
|
||||
def get_chip_id(self):
|
||||
"""Get chip ID for detection. Returns WHO_AM_I register value."""
|
||||
@@ -166,10 +188,12 @@ class Wsen_Isds:
|
||||
opt = Wsen_Isds._options[option]
|
||||
try:
|
||||
bits = opt["val_to_bits"][value]
|
||||
config_value = self.i2c.readfrom_mem(self.address, opt["reg"], 1)[0]
|
||||
old_value = self.i2c.readfrom_mem(self.address, opt["reg"], 1)[0]
|
||||
config_value = old_value
|
||||
config_value &= opt["mask"]
|
||||
config_value |= (bits << opt["shift_left"])
|
||||
self.i2c.writeto_mem(self.address, opt["reg"], bytes([config_value]))
|
||||
print(f"[WSEN_ISDS] _write_option: {option}={value} → reg {hex(opt['reg'])}: {hex(old_value)} → {hex(config_value)}")
|
||||
except KeyError as err:
|
||||
print(f"Invalid option: {option}, or invalid option value: {value}.", err)
|
||||
|
||||
@@ -300,15 +324,19 @@ class Wsen_Isds:
|
||||
|
||||
def _read_raw_accelerations(self):
|
||||
"""Read raw accelerometer data."""
|
||||
print("[WSEN_ISDS] _read_raw_accelerations: checking data ready...")
|
||||
if not self._acc_data_ready():
|
||||
print("[WSEN_ISDS] _read_raw_accelerations: DATA NOT READY!")
|
||||
raise Exception("sensor data not ready")
|
||||
|
||||
print("[WSEN_ISDS] _read_raw_accelerations: data ready, reading registers...")
|
||||
raw = self.i2c.readfrom_mem(self.address, Wsen_Isds._REG_A_X_OUT_L, 6)
|
||||
|
||||
raw_a_x = self._convert_from_raw(raw[0], raw[1])
|
||||
raw_a_y = self._convert_from_raw(raw[2], raw[3])
|
||||
raw_a_z = self._convert_from_raw(raw[4], raw[5])
|
||||
|
||||
print(f"[WSEN_ISDS] _read_raw_accelerations: raw values = ({raw_a_x}, {raw_a_y}, {raw_a_z})")
|
||||
return raw_a_x, raw_a_y, raw_a_z
|
||||
|
||||
def gyro_calibrate(self, samples=None):
|
||||
@@ -351,15 +379,19 @@ class Wsen_Isds:
|
||||
|
||||
def _read_raw_angular_velocities(self):
|
||||
"""Read raw gyroscope data."""
|
||||
print("[WSEN_ISDS] _read_raw_angular_velocities: checking data ready...")
|
||||
if not self._gyro_data_ready():
|
||||
print("[WSEN_ISDS] _read_raw_angular_velocities: DATA NOT READY!")
|
||||
raise Exception("sensor data not ready")
|
||||
|
||||
print("[WSEN_ISDS] _read_raw_angular_velocities: data ready, reading registers...")
|
||||
raw = self.i2c.readfrom_mem(self.address, Wsen_Isds._REG_G_X_OUT_L, 6)
|
||||
|
||||
raw_g_x = self._convert_from_raw(raw[0], raw[1])
|
||||
raw_g_y = self._convert_from_raw(raw[2], raw[3])
|
||||
raw_g_z = self._convert_from_raw(raw[4], raw[5])
|
||||
|
||||
print(f"[WSEN_ISDS] _read_raw_angular_velocities: raw values = ({raw_g_x}, {raw_g_y}, {raw_g_z})")
|
||||
return raw_g_x, raw_g_y, raw_g_z
|
||||
|
||||
def read_angular_velocities_accelerations(self):
|
||||
@@ -426,10 +458,15 @@ class Wsen_Isds:
|
||||
Returns:
|
||||
Tuple (acc_data_ready, gyro_data_ready, temp_data_ready)
|
||||
"""
|
||||
raw = self.i2c.readfrom_mem(self.address, Wsen_Isds._ISDS_STATUS_REG, 4)
|
||||
# STATUS_REG (0x1E) is a single byte with bit flags:
|
||||
# Bit 0: XLDA (accelerometer data available)
|
||||
# Bit 1: GDA (gyroscope data available)
|
||||
# Bit 2: TDA (temperature data available)
|
||||
status = self.i2c.readfrom_mem(self.address, Wsen_Isds._ISDS_STATUS_REG, 1)[0]
|
||||
|
||||
acc_data_ready = True if raw[0] == 1 else False
|
||||
gyro_data_ready = True if raw[1] == 1 else False
|
||||
temp_data_ready = True if raw[2] == 1 else False
|
||||
acc_data_ready = bool(status & 0x01) # Bit 0
|
||||
gyro_data_ready = bool(status & 0x02) # Bit 1
|
||||
temp_data_ready = bool(status & 0x04) # Bit 2
|
||||
|
||||
print(f"[WSEN_ISDS] Status register: 0x{status:02x} = 0b{status:08b}, acc_ready={acc_data_ready}, gyro_ready={gyro_data_ready}, temp_ready={temp_data_ready}")
|
||||
return acc_data_ready, gyro_data_ready, temp_data_ready
|
||||
|
||||
@@ -72,27 +72,55 @@ class Sensor:
|
||||
|
||||
|
||||
def init(i2c_bus, address=0x6B):
|
||||
"""Initialize SensorManager with I2C bus. Auto-detects IMU type and MCU temperature.
|
||||
|
||||
Tries to detect QMI8658 (chip ID 0x05) or WSEN_ISDS (WHO_AM_I 0x6A).
|
||||
Also detects ESP32 MCU internal temperature sensor.
|
||||
Loads calibration from SharedPreferences if available.
|
||||
"""Initialize SensorManager. MCU temperature initializes immediately, IMU initializes on first use.
|
||||
|
||||
Args:
|
||||
i2c_bus: machine.I2C instance (can be None if only MCU temperature needed)
|
||||
address: I2C address (default 0x6B for both QMI8658 and WSEN_ISDS)
|
||||
|
||||
Returns:
|
||||
bool: True if any sensor detected and initialized successfully
|
||||
bool: True if initialized successfully
|
||||
"""
|
||||
global _initialized, _imu_driver, _sensor_list, _i2c_bus, _i2c_address, _has_mcu_temperature
|
||||
|
||||
if _initialized:
|
||||
print("[SensorManager] Already initialized")
|
||||
return True
|
||||
global _i2c_bus, _i2c_address, _initialized, _has_mcu_temperature
|
||||
|
||||
_i2c_bus = i2c_bus
|
||||
_i2c_address = address
|
||||
|
||||
# Initialize MCU temperature sensor immediately (fast, no I2C needed)
|
||||
try:
|
||||
import esp32
|
||||
# Test if mcu_temperature() is available
|
||||
_ = esp32.mcu_temperature()
|
||||
_has_mcu_temperature = True
|
||||
_register_mcu_temperature_sensor()
|
||||
print("[SensorManager] Detected MCU internal temperature sensor")
|
||||
except Exception as e:
|
||||
print(f"[SensorManager] MCU temperature not available: {e}")
|
||||
|
||||
# Mark as initialized (but IMU driver is still None - will be initialized lazily)
|
||||
_initialized = True
|
||||
print("[SensorManager] init() called - IMU initialization deferred until first use")
|
||||
return True
|
||||
|
||||
|
||||
def _ensure_imu_initialized():
|
||||
"""Perform IMU initialization on first use (lazy initialization).
|
||||
|
||||
Tries to detect QMI8658 (chip ID 0x05) or WSEN_ISDS (WHO_AM_I 0x6A).
|
||||
Loads calibration from SharedPreferences if available.
|
||||
|
||||
Returns:
|
||||
bool: True if IMU detected and initialized successfully
|
||||
"""
|
||||
global _imu_driver, _sensor_list, _i2c_bus, _i2c_address
|
||||
|
||||
# If already initialized, return
|
||||
if _imu_driver is not None:
|
||||
return True
|
||||
|
||||
print("[SensorManager] _ensure_imu_initialized: Starting lazy IMU initialization...")
|
||||
i2c_bus = _i2c_bus
|
||||
address = _i2c_address
|
||||
imu_detected = False
|
||||
|
||||
# Try QMI8658 first (Waveshare board)
|
||||
@@ -114,65 +142,71 @@ def init(i2c_bus, address=0x6B):
|
||||
|
||||
# Try WSEN_ISDS (Fri3d badge)
|
||||
if not imu_detected:
|
||||
print(f"[SensorManager] Trying to detect WSEN_ISDS at address {hex(address)}...")
|
||||
try:
|
||||
from mpos.hardware.drivers.wsen_isds import Wsen_Isds
|
||||
print("[SensorManager] Reading WHO_AM_I register (0x0F)...")
|
||||
chip_id = i2c_bus.readfrom_mem(address, 0x0F, 1)[0] # WHO_AM_I register
|
||||
print(f"[SensorManager] WHO_AM_I = {hex(chip_id)}")
|
||||
if chip_id == 0x6A: # WSEN_ISDS WHO_AM_I value
|
||||
print("[SensorManager] Detected WSEN_ISDS IMU")
|
||||
print("[SensorManager] Detected WSEN_ISDS IMU - initializing driver...")
|
||||
_imu_driver = _WsenISDSDriver(i2c_bus, address)
|
||||
print("[SensorManager] WSEN_ISDS driver initialized, registering sensors...")
|
||||
_register_wsen_isds_sensors()
|
||||
print("[SensorManager] Loading calibration...")
|
||||
_load_calibration()
|
||||
imu_detected = True
|
||||
print("[SensorManager] WSEN_ISDS initialization complete!")
|
||||
else:
|
||||
print(f"[SensorManager] Chip ID {hex(chip_id)} doesn't match WSEN_ISDS (expected 0x6A)")
|
||||
except Exception as e:
|
||||
print(f"[SensorManager] WSEN_ISDS detection failed: {e}")
|
||||
import sys
|
||||
sys.print_exception(e)
|
||||
|
||||
# Try MCU internal temperature sensor (ESP32)
|
||||
try:
|
||||
import esp32
|
||||
# Test if mcu_temperature() is available
|
||||
_ = esp32.mcu_temperature()
|
||||
_has_mcu_temperature = True
|
||||
_register_mcu_temperature_sensor()
|
||||
print("[SensorManager] Detected MCU internal temperature sensor")
|
||||
except Exception as e:
|
||||
print(f"[SensorManager] MCU temperature not available: {e}")
|
||||
|
||||
_initialized = True
|
||||
|
||||
if not imu_detected and not _has_mcu_temperature:
|
||||
print("[SensorManager] No sensors detected")
|
||||
return False
|
||||
|
||||
return True
|
||||
print(f"[SensorManager] _ensure_imu_initialized: IMU initialization complete, success={imu_detected}")
|
||||
return imu_detected
|
||||
|
||||
|
||||
def is_available():
|
||||
"""Check if sensors are available.
|
||||
|
||||
Does NOT trigger IMU initialization (to avoid boot-time initialization).
|
||||
Use get_default_sensor() or read_sensor() to lazily initialize IMU.
|
||||
|
||||
Returns:
|
||||
bool: True if SensorManager is initialized with hardware
|
||||
bool: True if SensorManager is initialized (may only have MCU temp, not IMU)
|
||||
"""
|
||||
return _initialized and _imu_driver is not None
|
||||
return _initialized
|
||||
|
||||
|
||||
def get_sensor_list():
|
||||
"""Get list of all available sensors.
|
||||
|
||||
Performs lazy IMU initialization on first call.
|
||||
|
||||
Returns:
|
||||
list: List of Sensor objects
|
||||
"""
|
||||
_ensure_imu_initialized()
|
||||
return _sensor_list.copy() if _sensor_list else []
|
||||
|
||||
|
||||
def get_default_sensor(sensor_type):
|
||||
"""Get default sensor of given type.
|
||||
|
||||
Performs lazy IMU initialization on first call.
|
||||
|
||||
Args:
|
||||
sensor_type: Sensor type constant (TYPE_ACCELEROMETER, etc.)
|
||||
|
||||
Returns:
|
||||
Sensor object or None if not available
|
||||
"""
|
||||
# Only initialize IMU if requesting IMU sensor types
|
||||
if sensor_type in (TYPE_ACCELEROMETER, TYPE_GYROSCOPE):
|
||||
_ensure_imu_initialized()
|
||||
|
||||
for sensor in _sensor_list:
|
||||
if sensor.type == sensor_type:
|
||||
return sensor
|
||||
@@ -182,6 +216,8 @@ def get_default_sensor(sensor_type):
|
||||
def read_sensor(sensor):
|
||||
"""Read sensor data synchronously.
|
||||
|
||||
Performs lazy IMU initialization on first call for IMU sensors.
|
||||
|
||||
Args:
|
||||
sensor: Sensor object from get_default_sensor()
|
||||
|
||||
@@ -193,35 +229,58 @@ def read_sensor(sensor):
|
||||
if sensor is None:
|
||||
return None
|
||||
|
||||
# Only initialize IMU if reading IMU sensor
|
||||
if sensor.type in (TYPE_ACCELEROMETER, TYPE_GYROSCOPE):
|
||||
_ensure_imu_initialized()
|
||||
|
||||
if _lock:
|
||||
_lock.acquire()
|
||||
|
||||
try:
|
||||
if sensor.type == TYPE_ACCELEROMETER:
|
||||
if _imu_driver:
|
||||
return _imu_driver.read_acceleration()
|
||||
elif sensor.type == TYPE_GYROSCOPE:
|
||||
if _imu_driver:
|
||||
return _imu_driver.read_gyroscope()
|
||||
elif sensor.type == TYPE_IMU_TEMPERATURE:
|
||||
if _imu_driver:
|
||||
return _imu_driver.read_temperature()
|
||||
elif sensor.type == TYPE_SOC_TEMPERATURE:
|
||||
if _has_mcu_temperature:
|
||||
import esp32
|
||||
return esp32.mcu_temperature()
|
||||
elif sensor.type == TYPE_TEMPERATURE:
|
||||
# Generic temperature - return first available (backward compatibility)
|
||||
if _imu_driver:
|
||||
temp = _imu_driver.read_temperature()
|
||||
if temp is not None:
|
||||
return temp
|
||||
if _has_mcu_temperature:
|
||||
import esp32
|
||||
return esp32.mcu_temperature()
|
||||
return None
|
||||
except Exception as e:
|
||||
print(f"[SensorManager] Error reading sensor {sensor.name}: {e}")
|
||||
# Retry logic for "sensor data not ready" (WSEN_ISDS needs time after init)
|
||||
max_retries = 3
|
||||
retry_delay_ms = 20 # Wait 20ms between retries
|
||||
|
||||
for attempt in range(max_retries):
|
||||
try:
|
||||
if sensor.type == TYPE_ACCELEROMETER:
|
||||
if _imu_driver:
|
||||
return _imu_driver.read_acceleration()
|
||||
elif sensor.type == TYPE_GYROSCOPE:
|
||||
if _imu_driver:
|
||||
return _imu_driver.read_gyroscope()
|
||||
elif sensor.type == TYPE_IMU_TEMPERATURE:
|
||||
if _imu_driver:
|
||||
return _imu_driver.read_temperature()
|
||||
elif sensor.type == TYPE_SOC_TEMPERATURE:
|
||||
if _has_mcu_temperature:
|
||||
import esp32
|
||||
return esp32.mcu_temperature()
|
||||
elif sensor.type == TYPE_TEMPERATURE:
|
||||
# Generic temperature - return first available (backward compatibility)
|
||||
if _imu_driver:
|
||||
temp = _imu_driver.read_temperature()
|
||||
if temp is not None:
|
||||
return temp
|
||||
if _has_mcu_temperature:
|
||||
import esp32
|
||||
return esp32.mcu_temperature()
|
||||
return None
|
||||
except Exception as e:
|
||||
error_msg = str(e)
|
||||
# Retry if sensor data not ready, otherwise fail immediately
|
||||
if "data not ready" in error_msg and attempt < max_retries - 1:
|
||||
import time
|
||||
time.sleep_ms(retry_delay_ms)
|
||||
continue
|
||||
else:
|
||||
# Final attempt failed or different error
|
||||
if attempt == max_retries - 1:
|
||||
print(f"[SensorManager] Error reading sensor {sensor.name} after {max_retries} retries: {e}")
|
||||
else:
|
||||
print(f"[SensorManager] Error reading sensor {sensor.name}: {e}")
|
||||
return None
|
||||
|
||||
return None
|
||||
finally:
|
||||
if _lock:
|
||||
@@ -231,6 +290,7 @@ def read_sensor(sensor):
|
||||
def calibrate_sensor(sensor, samples=100):
|
||||
"""Calibrate sensor and save to SharedPreferences.
|
||||
|
||||
Performs lazy IMU initialization on first call.
|
||||
Device must be stationary for accelerometer/gyroscope calibration.
|
||||
|
||||
Args:
|
||||
@@ -240,18 +300,25 @@ def calibrate_sensor(sensor, samples=100):
|
||||
Returns:
|
||||
tuple: Calibration offsets (x, y, z) or None if failed
|
||||
"""
|
||||
print(f"[SensorManager] calibrate_sensor called for {sensor.name} with {samples} samples")
|
||||
_ensure_imu_initialized()
|
||||
if not is_available() or sensor is None:
|
||||
print("[SensorManager] calibrate_sensor: sensor not available")
|
||||
return None
|
||||
|
||||
print("[SensorManager] calibrate_sensor: acquiring lock...")
|
||||
if _lock:
|
||||
_lock.acquire()
|
||||
print("[SensorManager] calibrate_sensor: lock acquired")
|
||||
|
||||
try:
|
||||
offsets = None
|
||||
if sensor.type == TYPE_ACCELEROMETER:
|
||||
print(f"[SensorManager] Calling _imu_driver.calibrate_accelerometer({samples})...")
|
||||
offsets = _imu_driver.calibrate_accelerometer(samples)
|
||||
print(f"[SensorManager] Accelerometer calibrated: {offsets}")
|
||||
elif sensor.type == TYPE_GYROSCOPE:
|
||||
print(f"[SensorManager] Calling _imu_driver.calibrate_gyroscope({samples})...")
|
||||
offsets = _imu_driver.calibrate_gyroscope(samples)
|
||||
print(f"[SensorManager] Gyroscope calibrated: {offsets}")
|
||||
else:
|
||||
@@ -260,15 +327,21 @@ def calibrate_sensor(sensor, samples=100):
|
||||
|
||||
# Save calibration
|
||||
if offsets:
|
||||
print("[SensorManager] Saving calibration...")
|
||||
_save_calibration()
|
||||
print("[SensorManager] Calibration saved")
|
||||
|
||||
return offsets
|
||||
except Exception as e:
|
||||
print(f"[SensorManager] Error calibrating sensor {sensor.name}: {e}")
|
||||
import sys
|
||||
sys.print_exception(e)
|
||||
return None
|
||||
finally:
|
||||
print("[SensorManager] calibrate_sensor: releasing lock...")
|
||||
if _lock:
|
||||
_lock.release()
|
||||
print("[SensorManager] calibrate_sensor: lock released")
|
||||
|
||||
|
||||
# Helper functions for calibration quality checking (module-level to avoid nested def issues)
|
||||
@@ -294,6 +367,8 @@ def _calc_variance(samples_list):
|
||||
def check_calibration_quality(samples=50):
|
||||
"""Check quality of current calibration.
|
||||
|
||||
Performs lazy IMU initialization on first call.
|
||||
|
||||
Args:
|
||||
samples: Number of samples to collect (default 50)
|
||||
|
||||
@@ -308,12 +383,12 @@ def check_calibration_quality(samples=50):
|
||||
- issues: list of strings describing problems
|
||||
None if IMU not available
|
||||
"""
|
||||
_ensure_imu_initialized()
|
||||
if not is_available():
|
||||
return None
|
||||
|
||||
if _lock:
|
||||
_lock.acquire()
|
||||
|
||||
# Don't acquire lock here - let read_sensor() handle it per-read
|
||||
# (avoids deadlock since read_sensor also acquires the lock)
|
||||
try:
|
||||
accel = get_default_sensor(TYPE_ACCELEROMETER)
|
||||
gyro = get_default_sensor(TYPE_GYROSCOPE)
|
||||
@@ -413,9 +488,6 @@ def check_calibration_quality(samples=50):
|
||||
except Exception as e:
|
||||
print(f"[SensorManager] Error checking calibration quality: {e}")
|
||||
return None
|
||||
finally:
|
||||
if _lock:
|
||||
_lock.release()
|
||||
|
||||
|
||||
def check_stationarity(samples=30, variance_threshold_accel=0.5, variance_threshold_gyro=5.0):
|
||||
@@ -434,12 +506,12 @@ def check_stationarity(samples=30, variance_threshold_accel=0.5, variance_thresh
|
||||
- message: string describing result
|
||||
None if IMU not available
|
||||
"""
|
||||
_ensure_imu_initialized()
|
||||
if not is_available():
|
||||
return None
|
||||
|
||||
if _lock:
|
||||
_lock.acquire()
|
||||
|
||||
# Don't acquire lock here - let read_sensor() handle it per-read
|
||||
# (avoids deadlock since read_sensor also acquires the lock)
|
||||
try:
|
||||
accel = get_default_sensor(TYPE_ACCELEROMETER)
|
||||
gyro = get_default_sensor(TYPE_GYROSCOPE)
|
||||
@@ -498,9 +570,6 @@ def check_stationarity(samples=30, variance_threshold_accel=0.5, variance_thresh
|
||||
except Exception as e:
|
||||
print(f"[SensorManager] Error checking stationarity: {e}")
|
||||
return None
|
||||
finally:
|
||||
if _lock:
|
||||
_lock.release()
|
||||
|
||||
|
||||
# ============================================================================
|
||||
@@ -583,39 +652,53 @@ class _QMI8658Driver(_IMUDriver):
|
||||
|
||||
def calibrate_accelerometer(self, samples):
|
||||
"""Calibrate accelerometer (device must be stationary)."""
|
||||
print(f"[QMI8658Driver] calibrate_accelerometer: starting with {samples} samples")
|
||||
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
|
||||
|
||||
for _ in range(samples):
|
||||
for i in range(samples):
|
||||
if i % 10 == 0:
|
||||
print(f"[QMI8658Driver] Sample {i}/{samples}: about to read acceleration...")
|
||||
ax, ay, az = self.sensor.acceleration
|
||||
if i % 10 == 0:
|
||||
print(f"[QMI8658Driver] Sample {i}/{samples}: read complete, values=({ax:.3f}, {ay:.3f}, {az:.3f}), sleeping...")
|
||||
# Convert to m/s²
|
||||
sum_x += ax * _GRAVITY
|
||||
sum_y += ay * _GRAVITY
|
||||
sum_z += az * _GRAVITY
|
||||
time.sleep_ms(10)
|
||||
if i % 10 == 0:
|
||||
print(f"[QMI8658Driver] Sample {i}/{samples}: sleep complete")
|
||||
|
||||
print(f"[QMI8658Driver] All {samples} samples collected, calculating offsets...")
|
||||
# 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 # Expect +1G on Z
|
||||
|
||||
print(f"[QMI8658Driver] Calibration complete: offsets = {tuple(self.accel_offset)}")
|
||||
return tuple(self.accel_offset)
|
||||
|
||||
def calibrate_gyroscope(self, samples):
|
||||
"""Calibrate gyroscope (device must be stationary)."""
|
||||
print(f"[QMI8658Driver] calibrate_gyroscope: starting with {samples} samples")
|
||||
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
|
||||
|
||||
for _ in range(samples):
|
||||
for i in range(samples):
|
||||
if i % 20 == 0:
|
||||
print(f"[QMI8658Driver] Reading sample {i}/{samples}...")
|
||||
gx, gy, gz = self.sensor.gyro
|
||||
sum_x += gx
|
||||
sum_y += gy
|
||||
sum_z += gz
|
||||
time.sleep_ms(10)
|
||||
|
||||
print(f"[QMI8658Driver] All {samples} samples collected, calculating offsets...")
|
||||
# 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
|
||||
|
||||
print(f"[QMI8658Driver] Calibration complete: offsets = {tuple(self.gyro_offset)}")
|
||||
return tuple(self.gyro_offset)
|
||||
|
||||
def get_calibration(self):
|
||||
|
||||
Reference in New Issue
Block a user