mpu6050_dmp/
calibration_blocking.rs

1//! Blocking Calibration Implementation
2//!
3//! This module provides synchronous (blocking) versions of the calibration routines:
4//! - Collecting sensor readings with blocking I/O
5//! - Computing calibration offsets
6//! - Adjusting sensor calibration with blocking I2C operations
7//!
8//! The calibration process runs in a loop until all axes are within
9//! their specified thresholds, using standard blocking I/O operations.
10
11use crate::{
12    accel::{Accel, AccelFullScale},
13    calibration::{
14        CalibrationActions, CalibrationParameters, MeanAccumulator, ReferenceGravity, DELAY_MS,
15        ITERATIONS, WARMUP_ITERATIONS,
16    },
17    error::Error,
18    gyro::Gyro,
19    sensor::Mpu6050,
20};
21use embedded_hal::{delay::DelayNs, i2c::I2c};
22
23/// Perform a single loop computing the means of the readings
24pub fn collect_mean_values<I>(
25    mpu: &mut Mpu6050<I>,
26    delay: &mut impl DelayNs,
27    accel_scale: AccelFullScale,
28    gravity: ReferenceGravity,
29) -> Result<(Accel, Gyro), Error<I>>
30where
31    I: I2c,
32{
33    let mut accumulator = MeanAccumulator::new(accel_scale, gravity);
34
35    for _ in 0..WARMUP_ITERATIONS {
36        _ = mpu.accel()?;
37        _ = mpu.gyro()?;
38        delay.delay_ms(DELAY_MS);
39    }
40
41    for _ in 0..ITERATIONS {
42        let accel = mpu.accel()?;
43        let gyro = mpu.gyro()?;
44        accumulator.add(&accel, &gyro);
45        delay.delay_ms(DELAY_MS);
46    }
47
48    Ok(accumulator.means())
49}
50
51/// A single, full-fledged calibration loop (it also alters the device offset)
52pub fn calibration_loop<I>(
53    mpu: &mut Mpu6050<I>,
54    delay: &mut impl DelayNs,
55    parameters: &CalibrationParameters,
56    actions: CalibrationActions,
57) -> Result<(CalibrationActions, Accel, Gyro), Error<I>>
58where
59    I: I2c,
60{
61    let mut actions = actions;
62
63    mpu.set_accel_full_scale(parameters.accel_scale)?;
64    mpu.set_gyro_full_scale(parameters.gyro_scale)?;
65    let mut accel_offset = mpu.get_accel_calibration()?;
66    let mut gyro_offset = mpu.get_gyro_calibration()?;
67    let (accel_mean, gyro_mean) =
68        collect_mean_values(mpu, delay, parameters.accel_scale, parameters.gravity)?;
69
70    if parameters.accel_threshold.is_value_within(accel_mean.x()) {
71        actions = actions.with_accel_x(false);
72    }
73    if parameters.accel_threshold.is_value_within(accel_mean.y()) {
74        actions = actions.with_accel_y(false);
75    }
76    if parameters.accel_threshold.is_value_within(accel_mean.z()) {
77        actions = actions.with_accel_z(false);
78    }
79    if parameters.gyro_threshold.is_value_within(gyro_mean.x()) {
80        actions = actions.with_gyro_x(false);
81    }
82    if parameters.gyro_threshold.is_value_within(gyro_mean.y()) {
83        actions = actions.with_gyro_y(false);
84    }
85    if parameters.gyro_threshold.is_value_within(gyro_mean.z()) {
86        actions = actions.with_gyro_z(false);
87    }
88
89    if actions.accel_x() {
90        accel_offset.x = parameters
91            .accel_threshold
92            .next_offset(accel_mean.x(), accel_offset.x());
93    }
94    if actions.accel_y() {
95        accel_offset.y = parameters
96            .accel_threshold
97            .next_offset(accel_mean.y(), accel_offset.y());
98    }
99    if actions.accel_z() {
100        accel_offset.z = parameters
101            .accel_threshold
102            .next_offset(accel_mean.z(), accel_offset.z());
103    }
104    if actions.gyro_x() {
105        gyro_offset.x = parameters
106            .gyro_threshold
107            .next_offset(gyro_mean.x(), gyro_offset.x());
108    }
109    if actions.gyro_y() {
110        gyro_offset.y = parameters
111            .gyro_threshold
112            .next_offset(gyro_mean.y(), gyro_offset.y());
113    }
114    if actions.gyro_z() {
115        gyro_offset.z = parameters
116            .gyro_threshold
117            .next_offset(gyro_mean.z(), gyro_offset.z());
118    }
119
120    if !actions.is_empty() {
121        mpu.set_accel_calibration(&accel_offset)?;
122        mpu.set_gyro_calibration(&gyro_offset)?;
123    }
124
125    Ok((actions, accel_mean, gyro_mean))
126}
127
128/// Repeatedly perform calibration loops until the errors are within the given thresholds
129pub fn calibrate<I>(
130    mpu: &mut Mpu6050<I>,
131    delay: &mut impl DelayNs,
132    parameters: &CalibrationParameters,
133) -> Result<(Accel, Gyro), Error<I>>
134where
135    I: I2c,
136{
137    let mut actions = CalibrationActions::all();
138    while !actions.is_empty() {
139        (actions, _, _) = calibration_loop(mpu, delay, parameters, actions)?;
140    }
141    Ok((mpu.get_accel_calibration()?, mpu.get_gyro_calibration()?))
142}
mpu6050_dmp/calibration_blocking.rs

mpu6050_dmp/
calibration_blocking.rs
