mt6701 0.2.5

MT6701 driver in Rust
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

#![no_std]

use embedded_hal::blocking::spi::Transfer;
use embedded_hal::digital::v2::OutputPin;

use core::f32::consts::PI;
use core::fmt::Debug;
use libm::fabsf;
use nb;

const _2PI: f32 = PI * 2.0;

#[derive(Debug)]
pub enum MT6701Error {
    SpiError,
    CsnError,
}

pub trait AngleSensorTrait {
    type Error: Debug;

    fn init(&mut self) -> nb::Result<(), Self::Error>;
    fn read_raw_angle(&mut self) -> nb::Result<u16, Self::Error>;
    fn update(&mut self, now_us: u64) -> nb::Result<(), Self::Error>;
    fn get_angle(&mut self) -> f32;
    fn get_turns(&mut self) -> i64;
    fn get_position(&mut self) -> f64;
    fn get_velocity(&mut self) -> f32;
}

#[derive(Debug)]
pub struct MT6701Spi<SPI, CS> {
    spi: SPI,
    cs: CS,
    turns: i64,
    angle: f32,
    angle_prev: f32,
    position: f64,
    position_prev: f64,
    velocity: f32,
    prev_ns: u64,
}

impl<SPI, CS> MT6701Spi<SPI, CS>
where
    SPI: Transfer<u16>,
    CS: OutputPin,
{
    pub fn new(spi: SPI, cs: CS) -> Self {
        MT6701Spi {
            spi,
            cs,
            turns: 0,
            position: 0.0,
            position_prev: 0.0,
            angle: 0.0,
            angle_prev: 0.0,
            velocity: 0.0,
            prev_ns: 0,
        }
    }

    fn cal_velocity(&mut self, ts_ns: u64) {
        if ts_ns == 0 {
            self.velocity = 0.0;
            return;
        }

        let mut ts = (ts_ns - self.prev_ns) as f32 * 1e-6;
        if ts < 0.0 {
            ts = 1e-3;
        }

        self.velocity = (self.position - self.position_prev) as f32 / ts;

        self.position_prev = self.position;
    }
}

impl<SPI, CS> AngleSensorTrait for MT6701Spi<SPI, CS>
where
    SPI: Transfer<u16>,
    CS: OutputPin,
{
    type Error = MT6701Error;

    fn init(&mut self) -> nb::Result<(), MT6701Error> {
        self.cs.set_high().map_err(|_| MT6701Error::CsnError)?;
        Ok(())
    }

    fn read_raw_angle(&mut self) -> nb::Result<u16, MT6701Error> {
        let mut buffer: [u16; 1] = [0];

        self.cs.set_low().map_err(|_| MT6701Error::CsnError)?;

        self.spi
            .transfer(&mut buffer)
            .map_err(|_| MT6701Error::SpiError)?;

        self.cs.set_high().map_err(|_| MT6701Error::CsnError)?;

        Ok((buffer[0] >> 1) & 0x3FFF)
    }

    fn update(&mut self, ts_ns: u64) -> nb::Result<(), MT6701Error> {
        let raw_angle = self.read_raw_angle()?;

        self.angle = (raw_angle as f32 / 16384_f32) * _2PI;
        let move_angle = self.angle - self.angle_prev;

        if fabsf(move_angle) > (0.8 * _2PI) {
            self.turns += if move_angle > 0.0 { -1 } else { 1 };
        }

        self.position = (self.turns as f32 * _2PI + self.angle) as f64;

        self.cal_velocity(ts_ns);

        self.angle_prev = self.angle;
        self.prev_ns = ts_ns;

        Ok(())
    }

    fn get_angle(&mut self) -> f32 {
        self.angle
    }

    fn get_turns(&mut self) -> i64 {
        self.turns
    }

    fn get_position(&mut self) -> f64 {
        self.position
    }

    fn get_velocity(&mut self) -> f32 {
        self.velocity
    }
}