This commit is contained in:
Thomas Farstrike
2025-12-07 09:12:31 +01:00
parent 421140cd7b
commit 331cf14178
5 changed files with 116 additions and 415 deletions
+26 -16
View File
@@ -116,30 +116,40 @@ The `c_mpos/src/webcam.c` module provides webcam support for desktop builds usin
### Development Workflow (IMPORTANT)
**For most development, you do NOT need to rebuild the firmware!**
**⚠️ CRITICAL: Desktop vs Hardware Testing**
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:
📖 **See**: [docs/os-development/running-on-desktop.md](../docs/docs/os-development/running-on-desktop.md) for complete guide.
**Desktop testing (recommended for ALL Python development):**
```bash
# Fast development cycle (recommended):
# 1. Edit Python files in internal_filesystem/
# 2. Install to device:
# 1. Edit files in internal_filesystem/
nano internal_filesystem/builtin/apps/com.micropythonos.settings/assets/settings.py
# 2. Run on desktop - changes are IMMEDIATELY active!
./scripts/run_desktop.sh
# That's it! NO build, NO install needed.
```
**❌ DO NOT run `./scripts/install.sh` for desktop testing!** It's only for hardware deployment.
The desktop binary runs **directly from `internal_filesystem/`**, so any Python file changes are instantly available. This is the fastest development cycle.
**Hardware deployment (only after desktop testing):**
```bash
# Deploy to physical ESP32 device via USB/serial
./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/`)
This copies files from `internal_filesystem/` to device storage, which overrides the frozen filesystem.
**When you need to rebuild firmware (`./scripts/build_mpos.sh`):**
- Modifying C extension modules (`c_mops/`, `secp256k1-embedded-ecdh/`)
- Changing MicroPython core or LVGL bindings
- Creating a fresh firmware image for distribution
- Testing frozen filesystem for production releases
- Creating firmware 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
```
**For 99% of development work on Python code**: Just edit `internal_filesystem/` and run `./scripts/run_desktop.sh`.
### Building Firmware
@@ -1,42 +1,34 @@
"""Calibrate IMU Activity.
Guides user through IMU calibration process:
1. Check current calibration quality
2. Ask if user wants to recalibrate
3. Check stationarity
4. Perform calibration
5. Verify results
6. Save to new location
1. Show calibration instructions
2. Check stationarity when user clicks "Calibrate Now"
3. Perform calibration
4. Show results
"""
import lvgl as lv
import time
import _thread
import sys
from mpos.app.activity import Activity
import mpos.ui
import mpos.sensor_manager as SensorManager
import mpos.apps
from mpos.ui.testing import wait_for_render
class CalibrationState:
"""Enum for calibration states."""
IDLE = 0
CHECKING_QUALITY = 1
AWAITING_CONFIRMATION = 2
CHECKING_STATIONARITY = 3
CALIBRATING = 4
VERIFYING = 5
COMPLETE = 6
ERROR = 7
READY = 0
CALIBRATING = 1
COMPLETE = 2
ERROR = 3
class CalibrateIMUActivity(Activity):
"""Guide user through IMU calibration process."""
# State
current_state = CalibrationState.IDLE
current_state = CalibrationState.READY
calibration_thread = None
# Widgets
@@ -120,9 +112,8 @@ class CalibrateIMUActivity(Activity):
self.action_button.add_state(lv.STATE.DISABLED)
return
# Start by checking current quality
self.set_state(CalibrationState.IDLE)
self.action_button_label.set_text("Check Quality")
# Show calibration instructions
self.set_state(CalibrationState.READY)
def onPause(self, screen):
# Stop any running calibration
@@ -138,55 +129,31 @@ class CalibrateIMUActivity(Activity):
def update_ui_for_state(self):
"""Update UI based on current state."""
if self.current_state == CalibrationState.IDLE:
self.status_label.set_text("Ready to check calibration quality")
self.action_button_label.set_text("Check Quality")
if self.current_state == CalibrationState.READY:
self.status_label.set_text("Place device on flat, stable surface\n\nKeep device completely still during calibration")
self.detail_label.set_text("Calibration will take ~2 seconds\nUI will freeze during calibration")
self.action_button_label.set_text("Calibrate Now")
self.action_button.remove_state(lv.STATE.DISABLED)
self.progress_bar.add_flag(lv.obj.FLAG.HIDDEN)
self.cancel_button.remove_flag(lv.obj.FLAG.HIDDEN)
elif self.current_state == CalibrationState.CHECKING_QUALITY:
self.status_label.set_text("Checking current calibration...")
self.action_button.add_state(lv.STATE.DISABLED)
self.progress_bar.remove_flag(lv.obj.FLAG.HIDDEN)
self.progress_bar.set_value(20, True)
self.cancel_button.remove_flag(lv.obj.FLAG.HIDDEN)
elif self.current_state == CalibrationState.AWAITING_CONFIRMATION:
# Status will be set by quality check result
self.action_button_label.set_text("Calibrate Now")
self.action_button.remove_state(lv.STATE.DISABLED)
self.progress_bar.set_value(30, True)
self.cancel_button.remove_flag(lv.obj.FLAG.HIDDEN)
elif self.current_state == CalibrationState.CHECKING_STATIONARITY:
self.status_label.set_text("Checking if device is stationary...")
self.detail_label.set_text("Keep device still on flat surface")
self.action_button.add_state(lv.STATE.DISABLED)
self.progress_bar.set_value(40, True)
self.cancel_button.add_flag(lv.obj.FLAG.HIDDEN)
elif self.current_state == CalibrationState.CALIBRATING:
self.status_label.set_text("Calibrating IMU...")
self.detail_label.set_text("Do not move device!\nCollecting samples...")
self.detail_label.set_text("Do not move device!")
self.action_button.add_state(lv.STATE.DISABLED)
self.progress_bar.set_value(60, True)
self.cancel_button.add_flag(lv.obj.FLAG.HIDDEN)
elif self.current_state == CalibrationState.VERIFYING:
self.status_label.set_text("Verifying calibration...")
self.action_button.add_state(lv.STATE.DISABLED)
self.progress_bar.set_value(90, True)
self.progress_bar.remove_flag(lv.obj.FLAG.HIDDEN)
self.progress_bar.set_value(50, True)
self.cancel_button.add_flag(lv.obj.FLAG.HIDDEN)
elif self.current_state == CalibrationState.COMPLETE:
self.status_label.set_text("Calibration complete!")
# Status text will be set by calibration results
self.action_button_label.set_text("Done")
self.action_button.remove_state(lv.STATE.DISABLED)
self.progress_bar.set_value(100, True)
self.cancel_button.add_flag(lv.obj.FLAG.HIDDEN)
elif self.current_state == CalibrationState.ERROR:
# Status text will be set by error handler
self.action_button_label.set_text("Retry")
self.action_button.remove_state(lv.STATE.DISABLED)
self.progress_bar.add_flag(lv.obj.FLAG.HIDDEN)
@@ -194,261 +161,70 @@ class CalibrateIMUActivity(Activity):
def action_button_clicked(self, event):
"""Handle action button clicks based on current state."""
if self.current_state == CalibrationState.IDLE:
self.start_quality_check()
elif self.current_state == CalibrationState.AWAITING_CONFIRMATION:
if self.current_state == CalibrationState.READY:
self.start_calibration_process()
elif self.current_state == CalibrationState.COMPLETE:
self.finish()
elif self.current_state == CalibrationState.ERROR:
self.set_state(CalibrationState.IDLE)
self.set_state(CalibrationState.READY)
def start_quality_check(self):
"""Check current calibration quality."""
self.set_state(CalibrationState.CHECKING_QUALITY)
# Run in background thread
_thread.stack_size(mpos.apps.good_stack_size())
_thread.start_new_thread(self.quality_check_thread, ())
def quality_check_thread(self):
"""Background thread for quality check."""
try:
if self.is_desktop:
quality = self.get_mock_quality()
else:
quality = SensorManager.check_calibration_quality(samples=50)
if quality is None:
self.update_ui_threadsafe_if_foreground(self.handle_quality_error, "Failed to read IMU")
return
# Update UI with results
self.update_ui_threadsafe_if_foreground(self.show_quality_results, quality)
except Exception as e:
print(f"[CalibrateIMU] Quality check error: {e}")
self.update_ui_threadsafe_if_foreground(self.handle_quality_error, str(e))
def show_quality_results(self, quality):
"""Show quality check results and ask for confirmation."""
rating = quality['quality_rating']
score = quality['quality_score']
issues = quality['issues']
# Build status message
if rating == "Good":
msg = f"Current calibration: {rating} ({score*100:.0f}%)\n\nCalibration looks good!"
else:
msg = f"Current calibration: {rating} ({score*100:.0f}%)\n\nRecommend recalibrating."
if issues:
msg += "\n\nIssues found:\n" + "\n".join(f"- {issue}" for issue in issues[:3]) # Show first 3
self.status_label.set_text(msg)
self.set_state(CalibrationState.AWAITING_CONFIRMATION)
def handle_quality_error(self, error_msg):
"""Handle error during quality check."""
self.set_state(CalibrationState.ERROR)
self.status_label.set_text(f"Error: {error_msg}")
self.detail_label.set_text("Check IMU connection and try again")
def start_calibration_process(self):
"""Start the calibration process.
Note: Runs in main thread - UI will freeze during calibration (~1 second).
Note: Runs in main thread - UI will freeze during calibration (~2 seconds).
This avoids threading issues with I2C/sensor access.
"""
try:
print("[CalibrateIMU] === Calibration started ===")
# Step 1: Check stationarity
print("[CalibrateIMU] Step 1: Checking stationarity...")
self.set_state(CalibrationState.CHECKING_STATIONARITY)
self.set_state(CalibrationState.CALIBRATING)
wait_for_render() # Let UI update
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.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.set_state(CalibrationState.CALIBRATING)
self.status_label.set_text("Calibrating IMU...\n\nUI will freeze for ~2 seconds\nPlease wait...")
wait_for_render() # Let UI update before blocking
if self.is_desktop:
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 - UI will freeze here
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.set_state(CalibrationState.VERIFYING)
wait_for_render()
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.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']
result_msg = f"Calibration successful!\n\nNew quality: {rating} ({score*100:.0f}%)"
# Step 3: Show results
result_msg = "Calibration successful!"
if accel_offsets:
result_msg += f"\n\nAccel offsets:\nX:{accel_offsets[0]:.3f} Y:{accel_offsets[1]:.3f} Z:{accel_offsets[2]:.3f}"
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.show_calibration_complete(result_msg)
print("[CalibrateIMU] === Calibration finished ===")
except Exception as e:
print(f"[CalibrateIMU] Calibration error: {e}")
import sys
sys.print_exception(e)
self.handle_calibration_error(str(e))
def old_calibration_thread_func_UNUSED(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 (30 samples)...")
accel_offsets = SensorManager.calibrate_sensor(accel, samples=30)
print(f"[CalibrateIMU] Accel offsets: {accel_offsets}")
else:
accel_offsets = None
if gyro:
print("[CalibrateIMU] Calibrating gyroscope (30 samples)...")
gyro_offsets = SensorManager.calibrate_sensor(gyro, samples=30)
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']
result_msg = f"Calibration successful!\n\nNew quality: {rating} ({score*100:.0f}%)"
if accel_offsets:
result_msg += f"\n\nAccel offsets:\nX:{accel_offsets[0]:.3f} Y:{accel_offsets[1]:.3f} Z:{accel_offsets[2]:.3f}"
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 compl ete! 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):
"""Show calibration completion message."""
self.status_label.set_text(result_msg)
@@ -461,29 +237,3 @@ class CalibrateIMUActivity(Activity):
self.status_label.set_text(f"Calibration failed:\n\n{error_msg}")
self.detail_label.set_text("")
def get_mock_quality(self, good=False):
"""Generate mock quality data for desktop testing."""
import random
if good:
# Simulate excellent calibration after calibration
return {
'accel_mean': (random.uniform(-0.05, 0.05), random.uniform(-0.05, 0.05), 9.8 + random.uniform(-0.1, 0.1)),
'accel_variance': (random.uniform(0.001, 0.02), random.uniform(0.001, 0.02), random.uniform(0.001, 0.02)),
'gyro_mean': (random.uniform(-0.1, 0.1), random.uniform(-0.1, 0.1), random.uniform(-0.1, 0.1)),
'gyro_variance': (random.uniform(0.01, 0.2), random.uniform(0.01, 0.2), random.uniform(0.01, 0.2)),
'quality_score': random.uniform(0.90, 0.99),
'quality_rating': "Good",
'issues': []
}
else:
# Simulate mediocre calibration before calibration
return {
'accel_mean': (random.uniform(-1.0, 1.0), random.uniform(-1.0, 1.0), 9.8 + random.uniform(-2.0, 2.0)),
'accel_variance': (random.uniform(0.2, 0.5), random.uniform(0.2, 0.5), random.uniform(0.2, 0.5)),
'gyro_mean': (random.uniform(-3.0, 3.0), random.uniform(-3.0, 3.0), random.uniform(-3.0, 3.0)),
'gyro_variance': (random.uniform(2.0, 5.0), random.uniform(2.0, 5.0), random.uniform(2.0, 5.0)),
'quality_score': random.uniform(0.4, 0.6),
'quality_rating': "Fair",
'issues': ["High accelerometer variance", "Gyro not near zero"]
}
@@ -38,6 +38,18 @@ class CheckIMUCalibrationActivity(Activity):
screen = lv.obj()
screen.set_style_pad_all(mpos.ui.pct_of_display_width(2), 0)
screen.set_flex_flow(lv.FLEX_FLOW.COLUMN)
self.setContentView(screen)
def onResume(self, screen):
super().onResume(screen)
print(f"[CheckIMU] onResume called, is_desktop={self.is_desktop}")
# Clear the screen and recreate UI (to avoid stale widget references)
screen.clean()
# Reset widget lists
self.accel_labels = []
self.gyro_labels = []
# Title
title = lv.label(screen)
@@ -118,20 +130,18 @@ class CheckIMUCalibrationActivity(Activity):
calibrate_label.center()
calibrate_btn.add_event_cb(self.start_calibration, lv.EVENT.CLICKED, None)
self.setContentView(screen)
def onResume(self, screen):
super().onResume(screen)
# Check if IMU is available
if not self.is_desktop and not SensorManager.is_available():
print("[CheckIMU] IMU not available, stopping")
self.status_label.set_text("IMU not available on this device")
self.quality_score_label.set_text("N/A")
return
# Start real-time updates
print("[CheckIMU] Starting real-time updates")
self.updating = True
self.update_timer = lv.timer_create(self.update_display, self.UPDATE_INTERVAL, None)
print(f"[CheckIMU] Timer created: {self.update_timer}")
def onPause(self, screen):
# Stop updates
@@ -195,8 +205,17 @@ class CheckIMUCalibrationActivity(Activity):
self.issues_label.set_text(issues_text)
self.status_label.set_text("Real-time monitoring (place on flat surface)")
except:
# Widgets were deleted (activity closed), stop updating
except Exception as e:
# Log the actual error for debugging
print(f"[CheckIMU] Error in update_display: {e}")
import sys
sys.print_exception(e)
# If widgets were deleted (activity closed), stop updating
try:
self.status_label.set_text(f"Error: {str(e)}")
except:
# Widgets really were deleted
pass
self.updating = False
def get_mock_quality(self):
@@ -232,8 +251,11 @@ class CheckIMUCalibrationActivity(Activity):
def start_calibration(self, event):
"""Navigate to calibration activity."""
print("[CheckIMU] start_calibration called!")
from mpos.content.intent import Intent
from calibrate_imu import CalibrateIMUActivity
intent = Intent(activity_class=CalibrateIMUActivity)
print("[CheckIMU] Starting CalibrateIMUActivity...")
self.startActivity(intent)
print("[CheckIMU] startActivity returned")
@@ -128,7 +128,6 @@ class Wsen_Isds:
gyro_range: Gyroscope range ("125dps", "250dps", "500dps", "1000dps", "2000dps")
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
@@ -150,23 +149,15 @@ 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...")
# Give sensors time to stabilize
time.sleep_ms(100)
print("[WSEN_ISDS] Initialization complete")
def get_chip_id(self):
"""Get chip ID for detection. Returns WHO_AM_I register value."""
try:
@@ -270,14 +261,11 @@ class Wsen_Isds:
if samples is None:
samples = self.ACC_NUM_SAMPLES_CALIBRATION
print(f"[WSEN_ISDS] Calibrating accelerometer with {samples} samples...")
self.acc_offset_x = 0
self.acc_offset_y = 0
self.acc_offset_z = 0
for i in range(samples):
if i % 10 == 0:
print(f"[WSEN_ISDS] Accel sample {i}/{samples}")
for _ in range(samples):
x, y, z = self._read_raw_accelerations()
self.acc_offset_x += x
self.acc_offset_y += y
@@ -287,7 +275,6 @@ class Wsen_Isds:
self.acc_offset_x //= samples
self.acc_offset_y //= samples
self.acc_offset_z //= samples
print(f"[WSEN_ISDS] Accelerometer calibration complete: offsets=({self.acc_offset_x}, {self.acc_offset_y}, {self.acc_offset_z})")
def _acc_calc_sensitivity(self):
"""Calculate accelerometer sensitivity based on range (in mg/digit)."""
@@ -338,14 +325,11 @@ class Wsen_Isds:
if samples is None:
samples = self.GYRO_NUM_SAMPLES_CALIBRATION
print(f"[WSEN_ISDS] Calibrating gyroscope with {samples} samples...")
self.gyro_offset_x = 0
self.gyro_offset_y = 0
self.gyro_offset_z = 0
for i in range(samples):
if i % 10 == 0:
print(f"[WSEN_ISDS] Gyro sample {i}/{samples}")
for _ in range(samples):
x, y, z = self._read_raw_angular_velocities()
self.gyro_offset_x += x
self.gyro_offset_y += y
@@ -355,7 +339,6 @@ class Wsen_Isds:
self.gyro_offset_x //= samples
self.gyro_offset_y //= samples
self.gyro_offset_z //= samples
print(f"[WSEN_ISDS] Gyroscope calibration complete: offsets=({self.gyro_offset_x}, {self.gyro_offset_y}, {self.gyro_offset_z})")
def read_angular_velocities(self):
"""Read calibrated gyroscope data.
+32 -96
View File
@@ -89,17 +89,13 @@ def init(i2c_bus, address=0x6B):
# 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}")
except:
pass
# 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
@@ -112,60 +108,37 @@ def _ensure_imu_initialized():
Returns:
bool: True if IMU detected and initialized successfully
"""
global _imu_driver, _sensor_list, _i2c_bus, _i2c_address
global _imu_driver, _sensor_list
# 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
if not _initialized or _imu_driver is not None:
return _imu_driver is not None
# Try QMI8658 first (Waveshare board)
if i2c_bus:
if _i2c_bus:
try:
from mpos.hardware.drivers.qmi8658 import QMI8658
# QMI8658 constants (can't import const() values)
_QMI8685_PARTID = 0x05
_REG_PARTID = 0x00
chip_id = i2c_bus.readfrom_mem(address, _REG_PARTID, 1)[0]
if chip_id == _QMI8685_PARTID:
print("[SensorManager] Detected QMI8658 IMU")
_imu_driver = _QMI8658Driver(i2c_bus, address)
chip_id = _i2c_bus.readfrom_mem(_i2c_address, 0x00, 1)[0] # PARTID register
if chip_id == 0x05: # QMI8685_PARTID
_imu_driver = _QMI8658Driver(_i2c_bus, _i2c_address)
_register_qmi8658_sensors()
_load_calibration()
imu_detected = True
except Exception as e:
print(f"[SensorManager] QMI8658 detection failed: {e}")
return True
except:
pass
# 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 - 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:
from mpos.hardware.drivers.wsen_isds import Wsen_Isds
chip_id = _i2c_bus.readfrom_mem(_i2c_address, 0x0F, 1)[0] # WHO_AM_I register
if chip_id == 0x6A: # WSEN_ISDS WHO_AM_I
_imu_driver = _WsenISDSDriver(_i2c_bus, _i2c_address)
_register_wsen_isds_sensors()
_load_calibration()
return True
except:
pass
print(f"[SensorManager] _ensure_imu_initialized: IMU initialization complete, success={imu_detected}")
return imu_detected
return False
def is_available():
@@ -274,11 +247,6 @@ def read_sensor(sensor):
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
@@ -300,48 +268,31 @@ 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:
print(f"[SensorManager] Sensor type {sensor.type} does not support calibration")
return None
# 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)
print(f"[SensorManager] Calibration error: {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)
@@ -652,14 +603,10 @@ class _QMI8658Driver(_IMUDriver):
def calibrate_accelerometer(self, samples):
"""Calibrate accelerometer (device must be stationary)."""
print(f"[QMI8658Driver] Calibrating accelerometer with {samples} samples...")
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
for i in range(samples):
if i % 10 == 0:
print(f"[QMI8658Driver] Accel sample {i}/{samples}")
for _ in range(samples):
ax, ay, az = self.sensor.acceleration
# Convert to m/s²
sum_x += ax * _GRAVITY
sum_y += ay * _GRAVITY
sum_z += az * _GRAVITY
@@ -668,19 +615,15 @@ class _QMI8658Driver(_IMUDriver):
# 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
self.accel_offset[2] = (sum_z / samples) - _GRAVITY
print(f"[QMI8658Driver] Accelerometer 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] Calibrating gyroscope with {samples} samples...")
sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
for i in range(samples):
if i % 10 == 0:
print(f"[QMI8658Driver] Gyro sample {i}/{samples}")
for _ in range(samples):
gx, gy, gz = self.sensor.gyro
sum_x += gx
sum_y += gy
@@ -692,7 +635,6 @@ class _QMI8658Driver(_IMUDriver):
self.gyro_offset[1] = sum_y / samples
self.gyro_offset[2] = sum_z / samples
print(f"[QMI8658Driver] Gyroscope calibration complete: offsets = {tuple(self.gyro_offset)}")
return tuple(self.gyro_offset)
def get_calibration(self):
@@ -899,7 +841,6 @@ def _load_calibration():
gyro_offsets = prefs_old.get_list("gyro_offsets")
if accel_offsets or gyro_offsets:
print("[SensorManager] Migrating calibration from old to new location...")
# Save to new location
editor = prefs_new.edit()
if accel_offsets:
@@ -907,23 +848,20 @@ def _load_calibration():
if gyro_offsets:
editor.put_list("gyro_offsets", gyro_offsets)
editor.commit()
print("[SensorManager] Migration complete")
if accel_offsets or gyro_offsets:
_imu_driver.set_calibration(accel_offsets, gyro_offsets)
print(f"[SensorManager] Loaded calibration: accel={accel_offsets}, gyro={gyro_offsets}")
except Exception as e:
print(f"[SensorManager] Failed to load calibration: {e}")
except:
pass
def _save_calibration():
"""Save calibration to SharedPreferences (new location)."""
"""Save calibration to SharedPreferences."""
if not _imu_driver:
return
try:
from mpos.config import SharedPreferences
# NEW LOCATION: com.micropythonos.settings/sensors.json
prefs = SharedPreferences("com.micropythonos.settings", filename="sensors.json")
editor = prefs.edit()
@@ -931,7 +869,5 @@ def _save_calibration():
editor.put_list("accel_offsets", list(cal['accel_offsets']))
editor.put_list("gyro_offsets", list(cal['gyro_offsets']))
editor.commit()
print(f"[SensorManager] Saved calibration to settings: accel={cal['accel_offsets']}, gyro={cal['gyro_offsets']}")
except Exception as e:
print(f"[SensorManager] Failed to save calibration: {e}")
except:
pass