i2cdev_l3gd20/
lib.rs

1// Copyright 2017, Martin Deegan <mddeegan@gmail.com>
2//
3// Licensed under the Apache License, Version 2.0 <LICENSE-APACHE or
4// http://www.apache.org/license/LICENSE-2.0> or the MIT license
5// <LICENSE-MIT or http://opensource.org/licenses/MIT>, at your
6// option.  This file may not be copied, modified, or distributed
7// except according to those terms.
8
9// This module is implementing userland library to interact with
10// ST L3GD20 and L3GD20H Gyroscope i2c sensors.
11
12#![allow(dead_code)]
13
14extern crate i2cdev;
15extern crate i2csensors;
16extern crate byteorder;
17
18use i2csensors::Gyroscope;
19use i2csensors::Vec3;
20use std::thread;
21use std::time::Duration;
22use std::error::Error;
23use i2cdev::core::I2CDevice;
24#[cfg(any(target_os = "linux", target_os = "android"))]
25use i2cdev::linux::{LinuxI2CDevice,LinuxI2CError};
26use byteorder::{ByteOrder, BigEndian, LittleEndian};
27
28// Addresses
29
30pub const L3GD20_I2C_ADDR: u16 = 0x6A;
31pub const L3GD20H_I2C_ADDR: u16 = 0x6B;
32
33const L3GD20_CTRL_REG1: u8 = 0x20;
34const L3GD20_CTRL_REG2: u8 = 0x21;
35const L3GD20_CTRL_REG3: u8 = 0x22;
36const L3GD20_CTRL_REG4: u8 = 0x23;
37const L3GD20_CTRL_REG5: u8 = 0x24;
38
39const L3GD20_INCREMENT_BIT: u8 = 0x80;
40
41const L3GD20_OUT: u8 = 0x28;
42
43const L3GD20_WHO_AM_I_REGISTER: u8 = 0x0F;
44const L3GD20H_WHO_AM_I: u8 = 0b11010111;
45const L3GD20_WHO_AM_I: u8 = 0b11010100;
46
47#[derive(Debug, Copy, Clone)]
48pub enum L3GD20PowerMode {
49    PowerDown = 0b00000000,
50    Sleep,
51    Normal = 0b00001000
52}
53
54#[derive(Debug, Copy, Clone)]
55pub enum L3GD20GyroscopeDataRate {
56    Hz95 = 0b00,
57    Hz190 = 0b01,
58    Hz380 = 0b10,
59    Hz760 = 0b11
60}
61
62/// Reference table 21 on data sheet
63/// I believe this is the cutoff of the low pass filter
64#[derive(Debug, Copy, Clone)]
65pub enum L3GD20GyroscopeBandwidth {
66    BW1 = 0b00,
67    BW2 = 0b01,
68    BW3 = 0b10,
69    BW4 = 0b11
70}
71
72#[derive(Debug, Copy, Clone)]
73pub enum L3GD20GyroscopeFS {
74    dps250 = 0b00,
75    dps500 = 0b01,
76    dps2000 = 0b10
77}
78
79#[derive(Debug, Copy, Clone)]
80pub enum L3GD20GyroscopeHighPassFilterMode {
81    NormalModeHPRESETFILTER = 0b00,
82    ReferenceSignalForFiltering = 0b01,
83    NormalMode = 0b10,
84    AutoresetOnInterruptEvent = 0b11
85}
86
87#[derive(Debug, Copy, Clone)]
88pub enum L3GD20HighPassFilterCutOffConfig {
89    HPCF_0 = 0b0000,
90    HPCF_1 = 0b0001,
91    HPCF_2 = 0b0010,
92    HPCF_3 = 0b0011,
93    HPCF_4 = 0b0100,
94    HPCF_5 = 0b0101,
95    HPCF_6 = 0b0110,
96    HPCF_7 = 0b0111,
97    HPCF_8 = 0b1000,
98    HPCF_9 = 0b1001,
99}
100
101/// Use the [data sheet](http://www.st.com/content/ccc/resource/technical/document/datasheet/43/37/e3/06/b0/bf/48/bd/DM00036465.pdf/files/DM00036465.pdf/jcr:content/translations/en.DM00036465.pdf) to read in depth about settings
102#[derive(Debug, Copy, Clone)]
103pub struct L3GD20GyroscopeSettings {
104    /// Data measurement rate
105    pub DR: L3GD20GyroscopeDataRate,
106    /// Low pass filter cutoff
107    pub BW: L3GD20GyroscopeBandwidth,
108    /// Sleep will automatically disable '''xen''', '''yen''', '''zen'''
109    pub power_mode: L3GD20PowerMode,
110    /// Enable z axis readings
111    pub zen: bool,
112    /// Enable y axis readings
113    pub yen: bool,
114    /// Enable x axis readings
115    pub xen: bool,
116    /// Range of measurements. Lower range means more precision.
117    pub sensitivity: L3GD20GyroscopeFS,
118    /// Set to false if you do not want to update the buffer unless it has been read
119    pub continuous_update: bool,
120    pub high_pass_filter_enabled: bool,
121    pub high_pass_filter_mode: Option<L3GD20GyroscopeHighPassFilterMode>,
122    pub high_pass_filter_configuration: Option<L3GD20HighPassFilterCutOffConfig>
123}
124
125/// Get Linux I2C device at L3GD20's default address
126#[cfg(any(target_os = "linux", target_os = "android"))]
127pub fn get_linux_l3gd20_i2c_device() -> Result<LinuxI2CDevice,LinuxI2CError> {
128    let gyro = try!(LinuxI2CDevice::new("/dev/i2c-1", L3GD20_I2C_ADDR));
129    Ok(gyro)
130}
131
132pub fn get_linux_l3gd20h_i2c_device() -> Result<LinuxI2CDevice,LinuxI2CError> {
133    let gyro = try!(LinuxI2CDevice::new("/dev/i2c-1", L3GD20H_I2C_ADDR));
134    Ok(gyro)
135}
136
137pub struct L3GD20<T: I2CDevice + Sized> {
138    pub gyroscope: T,
139    g_gain: f32,
140}
141
142impl<T> L3GD20<T>
143    where T: I2CDevice + Sized
144{
145    #[cfg(any(target_os = "linux", target_os = "android"))]
146    pub fn new(mut gyro: T, mut gyro_settings: L3GD20GyroscopeSettings) -> Result<L3GD20<T>, T::Error> {
147        let who_am_i = try!(gyro.smbus_read_byte_data(L3GD20_WHO_AM_I_REGISTER));
148        assert!(who_am_i == L3GD20_WHO_AM_I || who_am_i == L3GD20H_WHO_AM_I, "L3GD20 ID didn't match for device at given I2C address.");
149        let mut ctrl_reg1: u8 = 0_u8 | ((gyro_settings.DR as u8) << 6) | ((gyro_settings.BW as u8) << 4);
150        match gyro_settings.power_mode {
151            L3GD20PowerMode::PowerDown => {
152                ctrl_reg1 |= L3GD20PowerMode::PowerDown as u8;
153            },
154            L3GD20PowerMode::Sleep => {
155                ctrl_reg1 = (ctrl_reg1 | L3GD20PowerMode::Normal as u8) & 0b11111000;
156            },
157            L3GD20PowerMode::Normal => {
158                ctrl_reg1 |= L3GD20PowerMode::Normal as u8;
159                if gyro_settings.xen { ctrl_reg1 |= 0b001 };
160                if gyro_settings.yen { ctrl_reg1 |= 0b010 };
161                if gyro_settings.zen { ctrl_reg1 |= 0b100 };
162            }
163        }
164        try!(gyro.smbus_write_byte_data(L3GD20_CTRL_REG1, ctrl_reg1));
165
166        let mut ctrl_reg2: u8 = 0_u8;
167        match gyro_settings.high_pass_filter_mode {
168            Some(mode) => {
169                ctrl_reg2 = ctrl_reg2 | ((mode as u8) << 4);
170            },
171            None => {}
172        }
173        match gyro_settings.high_pass_filter_configuration {
174            Some(config) => {
175                ctrl_reg2 = ctrl_reg2 | (config as u8);
176            },
177            None => {}
178        }
179        try!(gyro.smbus_write_byte_data(L3GD20_CTRL_REG2, ctrl_reg2));
180
181        let mut ctrl_reg4: u8 = 0_u8 | ((gyro_settings.sensitivity as u8) << 4);
182        if !gyro_settings.continuous_update {
183            ctrl_reg4 |= 0b10000000;
184        }
185        try!(gyro.smbus_write_byte_data(L3GD20_CTRL_REG4, ctrl_reg4));
186
187        let mut ctrl_reg5: u8 = 0_u8;
188        if gyro_settings.high_pass_filter_enabled {
189            ctrl_reg5 = 0b00010000;
190        }
191        try!(gyro.smbus_write_byte_data(L3GD20_CTRL_REG5, ctrl_reg5));
192
193        // Calculate gain
194        let mut g_gain: f32;
195
196        match gyro_settings.sensitivity {
197            L3GD20GyroscopeFS::dps250 => {
198                g_gain = 8.75;
199            },
200            L3GD20GyroscopeFS::dps500 => {
201                g_gain = 17.50;
202            },
203            L3GD20GyroscopeFS::dps2000 => {
204                g_gain = 70.0;
205            }
206        }
207
208        Ok(L3GD20 {
209            gyroscope: gyro,
210            g_gain: g_gain / 1000.0,
211        })
212    }
213
214    #[cfg(any(target_os = "linux", target_os = "android"))]
215    fn read_gyroscope_raw(&mut self) -> Result<(i16, i16, i16), T::Error> {
216        let mut buf: [u8; 6] = [0;6];
217        self.gyroscope.write(&[L3GD20_INCREMENT_BIT | L3GD20_OUT]);
218        self.gyroscope.read(&mut buf);
219        let x_raw = LittleEndian::read_i16(&buf[0..2]);
220        let y_raw = LittleEndian::read_i16(&buf[2..4]);
221        let z_raw = LittleEndian::read_i16(&buf[4..6]);
222        Ok((x_raw, y_raw, z_raw))
223    }
224}
225
226impl<T> Gyroscope for L3GD20<T>
227    where T: I2CDevice + Sized
228{
229    type Error = T::Error;
230
231    #[cfg(not(any(target_os = "linux", target_os = "android")))]
232    fn angular_rate_reading(&mut self) -> Result<Vec3, T::Error> {
233        Ok(Vec3::zeros())
234    }
235
236    /// Returns reading in dps
237    #[cfg(any(target_os = "linux", target_os = "android"))]
238    fn angular_rate_reading(&mut self) -> Result<Vec3, T::Error> {
239        let (x_raw, y_raw, z_raw) = try!(self.read_gyroscope_raw());
240        let angular_velocity = Vec3 {
241            x: (x_raw as f32) * self.g_gain,
242            y: (y_raw as f32) * self.g_gain,
243            z: (z_raw as f32) * self.g_gain
244        };
245        Ok(angular_velocity)
246    }
247
248}
i2cdev_l3gd20/lib.rs

i2cdev_l3gd20/
lib.rs
