diff --git a/CLAUDE.md b/CLAUDE.md index 410f9411..f05ac0a2 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -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`: diff --git a/internal_filesystem/builtin/apps/com.micropythonos.settings/assets/calibrate_imu.py b/internal_filesystem/builtin/apps/com.micropythonos.settings/assets/calibrate_imu.py index a563d346..190d8883 100644 --- a/internal_filesystem/builtin/apps/com.micropythonos.settings/assets/calibrate_imu.py +++ b/internal_filesystem/builtin/apps/com.micropythonos.settings/assets/calibrate_imu.py @@ -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): diff --git a/internal_filesystem/builtin/apps/com.micropythonos.settings/assets/check_imu_calibration.py b/internal_filesystem/builtin/apps/com.micropythonos.settings/assets/check_imu_calibration.py index 18c0bf4e..d9f0a7b8 100644 --- a/internal_filesystem/builtin/apps/com.micropythonos.settings/assets/check_imu_calibration.py +++ b/internal_filesystem/builtin/apps/com.micropythonos.settings/assets/check_imu_calibration.py @@ -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") diff --git a/internal_filesystem/lib/mpos/hardware/drivers/wsen_isds.py b/internal_filesystem/lib/mpos/hardware/drivers/wsen_isds.py index eaefeb74..631910a2 100644 --- a/internal_filesystem/lib/mpos/hardware/drivers/wsen_isds.py +++ b/internal_filesystem/lib/mpos/hardware/drivers/wsen_isds.py @@ -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 diff --git a/internal_filesystem/lib/mpos/sensor_manager.py b/internal_filesystem/lib/mpos/sensor_manager.py index ee2be064..0d585485 100644 --- a/internal_filesystem/lib/mpos/sensor_manager.py +++ b/internal_filesystem/lib/mpos/sensor_manager.py @@ -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):