bmi270 0.1.1

Rust BMI270
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

mod bmi270;

pub use bmi270::*;
pub use embedded_hal::blocking::spi::*;
pub use linux_embedded_hal::spidev::{SpiModeFlags, SpidevOptions};
pub use linux_embedded_hal::Spidev;

use std::time::Duration;

pub struct Bmi270<C> {
    communication: C,
    acc_range: f64,
    gyr_range: f64,
}

impl Bmi270Write for Bmi270<Spidev> {
    fn bmi270_write(&mut self, addr: u8, value: &[u8]) {
        let mut words = vec![addr];
        words.extend_from_slice(value);
        self.communication.write(&words).unwrap();
    }
}

impl Bmi270ReadWrite for Bmi270<Spidev> {
    fn bmi270_read_write(&mut self, addr: u8, len: usize) -> Vec<u8> {
        let mut tx_buff = vec![addr | 0x80];
        tx_buff.extend_from_slice(&vec![0xff; len + 1]);
        self.communication.transfer(&mut tx_buff).unwrap();
        tx_buff[2..].to_vec()
    }
}

impl Bmi270<Spidev> {
    pub fn new(spi: Spidev, config: Bmi270Config) -> Self {
        let mut imu = Self {
            communication: spi,
            acc_range: 0.0,
            gyr_range: 0.0,
        };
        // 检测陀螺仪连接
        while imu.bmi270_read_write(ADDR_CHIP_ID, 1)[0] != 0x24 {
            println!("等待陀螺仪连接，检查陀螺仪连线");
        }

        println!("已检测到陀螺仪，尝试初始化");
        imu.bmi270_write(ADDR_POWER_CNF, &[0x00]);
        imu.bmi270_write(ADDR_INIT_CTRL, &[0x00]);
        std::thread::sleep(Duration::from_millis(100)); // 保证初始化时间
        imu.bmi270_write(ADDR_INIT_WRITE, &BMI270_CONFIG_FILE);
        imu.bmi270_write(ADDR_INIT_CTRL, &[0x01]);
        std::thread::sleep(Duration::from_secs(2)); // 保证初始化时间

        // 检测初始化状态
        while imu.bmi270_read_write(ADDR_INIT_CHECK, 1)[0] & 0x01 == 0 {
            println!("初始化失败，继续等待可能会完成初始化，检测配置文件或者延长等待时间");
        }

        if config.acc_ord.is_some() {
            // 有一个就代表都配置了，启动加速度计,默认高性能
            unsafe {
                let conf_value = (0_u8 | config.acc_ord.unwrap_unchecked() as u8)
                    | (0_u8 | ((config.acc_bwp.unwrap_unchecked() as u8) << 4)) & 0x80;

                imu.bmi270_write(ADDR_ACC_CONF, &[conf_value]);
                imu.bmi270_write(ADDR_ACC_RANGE, &[config.acc_range.unwrap_unchecked() as u8]);
            }
        }

        if config.gyr_ord.is_some() {
            // 有一个就代表都配置了，启动加速度计,默认高性能
            unsafe {
                let conf_value = (0_u8 | config.gyr_ord.unwrap_unchecked() as u8)
                    | (0_u8 | ((config.gyr_bwp.unwrap_unchecked() as u8) << 4)) & 0xc0;

                imu.bmi270_write(ADDR_GYR_CONF, &[conf_value]);
                imu.bmi270_write(ADDR_GYR_RANGE, &[config.gyr_range.unwrap_unchecked() as u8]);
            }
        }

        // 默认开启所有传感器(AUX除外)
        imu.bmi270_write(ADDR_POWER_CTRL, &[0x0e]);
        unsafe {
            imu.acc_range = config.acc_range.unwrap_unchecked().to_sensitivity();
            imu.gyr_range = config.gyr_range.unwrap_unchecked().to_sensitivity();
        }
        imu
    }
}

impl Bmi270Data for Bmi270<Spidev> {
    fn acc(&mut self) -> [f64; 3] {
        unsafe {
            self.bmi270_read_write(ADDR_ACC_DATA, 6)
                .chunks_exact(2)
                .map(|blocks| {
                    i16::from_le_bytes(blocks.try_into().unwrap_unchecked()) as f64 / self.acc_range
                })
                .collect::<Vec<f64>>()
                .try_into()
                .unwrap()
        }
    }
    fn acc_gyr(&mut self) -> [f64; 6] {
        unsafe {
            let mut f: [f64; 6] = self
                .bmi270_read_write(ADDR_ACC_DATA, 12)
                .chunks_exact(2)
                .map(|blocks| i16::from_le_bytes(blocks.try_into().unwrap_unchecked()) as f64)
                .collect::<Vec<f64>>()
                .try_into()
                .unwrap();
            f[0] = f[0] / self.acc_range;
            f[1] = f[1] / self.acc_range;
            f[2] = f[2] / self.acc_range;
            f[3] = f[3] / self.gyr_range;
            f[4] = f[4] / self.gyr_range;
            f[5] = f[5] / self.gyr_range;
            f
        }
    }
    fn gyr(&mut self) -> [f64; 3] {
        unsafe {
            self.bmi270_read_write(ADDR_GYR_DATA, 6)
                .chunks_exact(2)
                .map(|blocks| {
                    i16::from_le_bytes(blocks.try_into().unwrap_unchecked()) as f64 / self.gyr_range
                })
                .collect::<Vec<f64>>()
                .try_into()
                .unwrap()
        }
    }
    fn temperature(&mut self) -> f64 {
        todo!()
    }
}