Skip to content

Commit 7dbc813

Browse files
ThomasFarstrikeThomasFarstrike
committed
Fix calibration
1 parent 5199a92 commit 7dbc813

File tree

1 file changed

+102
-8
lines changed
  • internal_filesystem/builtin/apps/com.micropythonos.settings/assets

1 file changed

+102
-8
lines changed

‎internal_filesystem/builtin/apps/com.micropythonos.settings/assets/calibrate_imu.py‎

Lines changed: 102 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
import mpos.ui
1818
import mpos.sensor_manager as SensorManager
1919
import mpos.apps
20+
from mpos.ui.testing import wait_for_render
2021

2122

2223
class CalibrationState:
@@ -246,14 +247,106 @@ def handle_quality_error(self, error_msg):
246247
self.detail_label.set_text("Check IMU connection and try again")
247248

248249
def start_calibration_process(self):
249-
"""Start the calibration process."""
250-
self.set_state(CalibrationState.CHECKING_STATIONARITY)
250+
"""Start the calibration process.
251251
252-
# Run in background thread
253-
_thread.stack_size(mpos.apps.good_stack_size())
254-
_thread.start_new_thread(self.calibration_thread_func, ())
252+
Note: Runs in main thread - UI will freeze during calibration (~1 second).
253+
This avoids threading issues with I2C/sensor access.
254+
"""
255+
try:
256+
print("[CalibrateIMU] === Calibration started ===")
257+
258+
# Step 1: Check stationarity
259+
print("[CalibrateIMU] Step 1: Checking stationarity...")
260+
self.set_state(CalibrationState.CHECKING_STATIONARITY)
261+
wait_for_render() # Let UI update
262+
263+
if self.is_desktop:
264+
stationarity = {'is_stationary': True, 'message': 'Mock: Stationary'}
265+
else:
266+
print("[CalibrateIMU] Calling SensorManager.check_stationarity(samples=30)...")
267+
stationarity = SensorManager.check_stationarity(samples=30)
268+
print(f"[CalibrateIMU] Stationarity result: {stationarity}")
269+
270+
if stationarity is None or not stationarity['is_stationary']:
271+
msg = stationarity['message'] if stationarity else "Stationarity check failed"
272+
print(f"[CalibrateIMU] Device not stationary: {msg}")
273+
self.handle_calibration_error(
274+
f"Device not stationary!\n\n{msg}\n\nPlace on flat surface and try again.")
275+
return
276+
277+
print("[CalibrateIMU] Device is stationary, proceeding to calibration")
278+
279+
# Step 2: Perform calibration
280+
print("[CalibrateIMU] Step 2: Performing calibration...")
281+
self.set_state(CalibrationState.CALIBRATING)
282+
self.status_label.set_text("Calibrating IMU...\n\nUI will freeze for ~2 seconds\nPlease wait...")
283+
wait_for_render() # Let UI update before blocking
284+
285+
if self.is_desktop:
286+
print("[CalibrateIMU] Mock calibration (desktop)")
287+
time.sleep(2)
288+
accel_offsets = (0.1, -0.05, 0.15)
289+
gyro_offsets = (0.2, -0.1, 0.05)
290+
else:
291+
# Real calibration - UI will freeze here
292+
print("[CalibrateIMU] Real calibration (hardware)")
293+
accel = SensorManager.get_default_sensor(SensorManager.TYPE_ACCELEROMETER)
294+
gyro = SensorManager.get_default_sensor(SensorManager.TYPE_GYROSCOPE)
295+
print(f"[CalibrateIMU] Accel sensor: {accel}, Gyro sensor: {gyro}")
296+
297+
if accel:
298+
print("[CalibrateIMU] Calibrating accelerometer (100 samples)...")
299+
accel_offsets = SensorManager.calibrate_sensor(accel, samples=100)
300+
print(f"[CalibrateIMU] Accel offsets: {accel_offsets}")
301+
else:
302+
accel_offsets = None
255303

256-
def calibration_thread_func(self):
304+
if gyro:
305+
print("[CalibrateIMU] Calibrating gyroscope (100 samples)...")
306+
gyro_offsets = SensorManager.calibrate_sensor(gyro, samples=100)
307+
print(f"[CalibrateIMU] Gyro offsets: {gyro_offsets}")
308+
else:
309+
gyro_offsets = None
310+
311+
# Step 3: Verify results
312+
print("[CalibrateIMU] Step 3: Verifying calibration...")
313+
self.set_state(CalibrationState.VERIFYING)
314+
wait_for_render()
315+
316+
if self.is_desktop:
317+
verify_quality = self.get_mock_quality(good=True)
318+
else:
319+
print("[CalibrateIMU] Checking calibration quality (50 samples)...")
320+
verify_quality = SensorManager.check_calibration_quality(samples=50)
321+
print(f"[CalibrateIMU] Verification quality: {verify_quality}")
322+
323+
if verify_quality is None:
324+
print("[CalibrateIMU] Verification failed")
325+
self.handle_calibration_error("Calibration completed but verification failed")
326+
return
327+
328+
# Step 4: Show results
329+
print("[CalibrateIMU] Step 4: Showing results...")
330+
rating = verify_quality['quality_rating']
331+
score = verify_quality['quality_score']
332+
333+
result_msg = f"Calibration successful!\n\nNew quality: {rating} ({score*100:.0f}%)"
334+
if accel_offsets:
335+
result_msg += f"\n\nAccel offsets:\nX:{accel_offsets[0]:.3f} Y:{accel_offsets[1]:.3f} Z:{accel_offsets[2]:.3f}"
336+
if gyro_offsets:
337+
result_msg += f"\n\nGyro offsets:\nX:{gyro_offsets[0]:.3f} Y:{gyro_offsets[1]:.3f} Z:{gyro_offsets[2]:.3f}"
338+
339+
print(f"[CalibrateIMU] Calibration complete! Result: {result_msg[:80]}")
340+
self.show_calibration_complete(result_msg)
341+
print("[CalibrateIMU] === Calibration finished ===")
342+
343+
except Exception as e:
344+
print(f"[CalibrateIMU] Calibration error: {e}")
345+
import sys
346+
sys.print_exception(e)
347+
self.handle_calibration_error(str(e))
348+
349+
def old_calibration_thread_func_UNUSED(self):
257350
"""Background thread for calibration process."""
258351
try:
259352
print("[CalibrateIMU] === Calibration thread started ===")
@@ -337,16 +430,17 @@ def calibration_thread_func(self):
337430
if gyro_offsets:
338431
result_msg += f"\n\nGyro offsets:\nX:{gyro_offsets[0]:.3f} Y:{gyro_offsets[1]:.3f} Z:{gyro_offsets[2]:.3f}"
339432

340-
print(f"[CalibrateIMU] Calibration complete! Result: {result_msg[:80]}")
433+
print(f"[CalibrateIMU] Calibration compl ete! Result: {result_msg[:80]}")
341434
self.update_ui_threadsafe_if_foreground(self.show_calibration_complete, result_msg)
435+
342436
print("[CalibrateIMU] === Calibration thread finished ===")
343437

344438
except Exception as e:
345439
print(f"[CalibrateIMU] Calibration error: {e}")
346440
import sys
347441
sys.print_exception(e)
348442
self.update_ui_threadsafe_if_foreground(self.handle_calibration_error, str(e))
349-
443+
350444
def show_calibration_complete(self, result_msg):
351445
"""Show calibration completion message."""
352446
self.status_label.set_text(result_msg)

0 commit comments

Comments
 (0)