mpu6050-dmp 0.6.1

Platform-independent I2C driver for MPU6050. Supports DMP usage. Fork of drogue-mpu-6050 which uses only embedded-hal traits (no dependency on embedded-time or drogue-embedded-timer).
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142

//! Blocking Calibration Implementation
//!
//! This module provides synchronous (blocking) versions of the calibration routines:
//! - Collecting sensor readings with blocking I/O
//! - Computing calibration offsets
//! - Adjusting sensor calibration with blocking I2C operations
//!
//! The calibration process runs in a loop until all axes are within
//! their specified thresholds, using standard blocking I/O operations.

use crate::{
    accel::{Accel, AccelFullScale},
    calibration::{
        CalibrationActions, CalibrationParameters, MeanAccumulator, ReferenceGravity, DELAY_MS,
        ITERATIONS, WARMUP_ITERATIONS,
    },
    error::Error,
    gyro::Gyro,
    sensor::Mpu6050,
};
use embedded_hal::{delay::DelayNs, i2c::I2c};

/// Perform a single loop computing the means of the readings
pub fn collect_mean_values<I>(
    mpu: &mut Mpu6050<I>,
    delay: &mut impl DelayNs,
    accel_scale: AccelFullScale,
    gravity: ReferenceGravity,
) -> Result<(Accel, Gyro), Error<I>>
where
    I: I2c,
{
    let mut accumulator = MeanAccumulator::new(accel_scale, gravity);

    for _ in 0..WARMUP_ITERATIONS {
        _ = mpu.accel()?;
        _ = mpu.gyro()?;
        delay.delay_ms(DELAY_MS);
    }

    for _ in 0..ITERATIONS {
        let accel = mpu.accel()?;
        let gyro = mpu.gyro()?;
        accumulator.add(&accel, &gyro);
        delay.delay_ms(DELAY_MS);
    }

    Ok(accumulator.means())
}

/// A single, full-fledged calibration loop (it also alters the device offset)
pub fn calibration_loop<I>(
    mpu: &mut Mpu6050<I>,
    delay: &mut impl DelayNs,
    parameters: &CalibrationParameters,
    actions: CalibrationActions,
) -> Result<(CalibrationActions, Accel, Gyro), Error<I>>
where
    I: I2c,
{
    let mut actions = actions;

    mpu.set_accel_full_scale(parameters.accel_scale)?;
    mpu.set_gyro_full_scale(parameters.gyro_scale)?;
    let mut accel_offset = mpu.get_accel_calibration()?;
    let mut gyro_offset = mpu.get_gyro_calibration()?;
    let (accel_mean, gyro_mean) =
        collect_mean_values(mpu, delay, parameters.accel_scale, parameters.gravity)?;

    if parameters.accel_threshold.is_value_within(accel_mean.x()) {
        actions = actions.with_accel_x(false);
    }
    if parameters.accel_threshold.is_value_within(accel_mean.y()) {
        actions = actions.with_accel_y(false);
    }
    if parameters.accel_threshold.is_value_within(accel_mean.z()) {
        actions = actions.with_accel_z(false);
    }
    if parameters.gyro_threshold.is_value_within(gyro_mean.x()) {
        actions = actions.with_gyro_x(false);
    }
    if parameters.gyro_threshold.is_value_within(gyro_mean.y()) {
        actions = actions.with_gyro_y(false);
    }
    if parameters.gyro_threshold.is_value_within(gyro_mean.z()) {
        actions = actions.with_gyro_z(false);
    }

    if actions.accel_x() {
        accel_offset.x = parameters
            .accel_threshold
            .next_offset(accel_mean.x(), accel_offset.x());
    }
    if actions.accel_y() {
        accel_offset.y = parameters
            .accel_threshold
            .next_offset(accel_mean.y(), accel_offset.y());
    }
    if actions.accel_z() {
        accel_offset.z = parameters
            .accel_threshold
            .next_offset(accel_mean.z(), accel_offset.z());
    }
    if actions.gyro_x() {
        gyro_offset.x = parameters
            .gyro_threshold
            .next_offset(gyro_mean.x(), gyro_offset.x());
    }
    if actions.gyro_y() {
        gyro_offset.y = parameters
            .gyro_threshold
            .next_offset(gyro_mean.y(), gyro_offset.y());
    }
    if actions.gyro_z() {
        gyro_offset.z = parameters
            .gyro_threshold
            .next_offset(gyro_mean.z(), gyro_offset.z());
    }

    if !actions.is_empty() {
        mpu.set_accel_calibration(&accel_offset)?;
        mpu.set_gyro_calibration(&gyro_offset)?;
    }

    Ok((actions, accel_mean, gyro_mean))
}

/// Repeatedly perform calibration loops until the errors are within the given thresholds
pub fn calibrate<I>(
    mpu: &mut Mpu6050<I>,
    delay: &mut impl DelayNs,
    parameters: &CalibrationParameters,
) -> Result<(Accel, Gyro), Error<I>>
where
    I: I2c,
{
    let mut actions = CalibrationActions::all();
    while !actions.is_empty() {
        (actions, _, _) = calibration_loop(mpu, delay, parameters, actions)?;
    }
    Ok((mpu.get_accel_calibration()?, mpu.get_gyro_calibration()?))
}