#![deny(missing_docs)]
#![deny(warnings)]
#![no_std]
use embedded_hal::blocking::spi::{Transfer, Write};
use embedded_hal::digital::v2::OutputPin;
use embedded_hal::spi::{Mode};
pub const MODE: Mode = embedded_hal::spi::MODE_3;
pub struct I3G4250D<SPI, CS> {
spi: SPI,
cs: CS,
}
impl<SPI, CS, E> I3G4250D<SPI, CS>
where
SPI: Transfer<u8, Error = E> + Write<u8, Error = E>,
CS: OutputPin,
{
pub fn new(spi: SPI, cs: CS) -> Result<Self, E> {
let mut i3g4259d = I3G4250D { spi, cs };
i3g4259d.write_register(Register::CTRL_REG1, 0b00_00_1_111)?;
Ok(i3g4259d)
}
pub fn all(&mut self) -> Result<Measurements, E> {
let mut bytes = [0u8; 9];
self.read_many(Register::OUT_TEMP, &mut bytes)?;
Ok(Measurements {
gyro: I16x3 {
x: (bytes[3] as u16 + ((bytes[4] as u16) << 8)) as i16,
y: (bytes[5] as u16 + ((bytes[6] as u16) << 8)) as i16,
z: (bytes[7] as u16 + ((bytes[8] as u16) << 8)) as i16,
},
temp: bytes[1] as i8,
})
}
pub fn gyro(&mut self) -> Result<I16x3, E> {
let mut bytes = [0u8; 7];
self.read_many(Register::OUT_X_L, &mut bytes)?;
Ok(I16x3 {
x: (bytes[1] as u16 + ((bytes[2] as u16) << 8)) as i16,
y: (bytes[3] as u16 + ((bytes[4] as u16) << 8)) as i16,
z: (bytes[5] as u16 + ((bytes[6] as u16) << 8)) as i16,
})
}
pub fn temp(&mut self) -> Result<i8, E> {
Ok(self.read_register(Register::OUT_TEMP)? as i8)
}
pub fn who_am_i(&mut self) -> Result<u8, E> {
self.read_register(Register::WHO_AM_I)
}
pub fn status(&mut self) -> Result<Status, E> {
let sts = self.read_register(Register::STATUS_REG)?;
Ok(Status::from_u8(sts))
}
pub fn odr(&mut self) -> Result<Odr, E> {
let reg1 = self.read_register(Register::CTRL_REG1)?;
Ok(Odr::from_u8(reg1))
}
pub fn set_odr(&mut self, odr: Odr) -> Result<&mut Self, E> {
self.change_config(Register::CTRL_REG1, odr)
}
pub fn bandwidth(&mut self) -> Result<Bandwidth, E> {
let reg1 = self.read_register(Register::CTRL_REG1)?;
Ok(Bandwidth::from_u8(reg1))
}
pub fn set_bandwidth(&mut self, bw: Bandwidth) -> Result<&mut Self, E> {
self.change_config(Register::CTRL_REG1, bw)
}
pub fn scale(&mut self) -> Result<Scale, E> {
let scl = self.read_register(Register::CTRL_REG4)?;
Ok(Scale::from_u8(scl))
}
pub fn set_scale(&mut self, scale: Scale) -> Result<&mut Self, E> {
self.change_config(Register::CTRL_REG4, scale)
}
fn read_register(&mut self, reg: Register) -> Result<u8, E> {
let _ = self.cs.set_low();
let mut buffer = [reg.addr() | SINGLE | READ, 0];
self.spi.transfer(&mut buffer)?;
let _ = self.cs.set_high();
Ok(buffer[1])
}
fn read_many(&mut self,
start_reg: Register,
buffer: &mut [u8])
-> Result<(), E> {
let _ = self.cs.set_low();
buffer[0] = start_reg.addr() | MULTI | READ ;
self.spi.transfer(buffer)?;
let _ = self.cs.set_high();
Ok(())
}
fn write_register(&mut self, reg: Register, byte: u8) -> Result<(), E> {
let _ = self.cs.set_low();
let buffer = [reg.addr() | SINGLE | WRITE, byte];
self.spi.write(&buffer)?;
let _ = self.cs.set_high();
Ok(())
}
fn change_config<B: BitValue>(&mut self, reg: Register, bits: B) -> Result<&mut Self, E> {
let mask = B::mask() << B::shift();
let bits = (bits.value() << B::shift()) & mask;
let current = self.read_register(reg)?;
let masked = current & !mask;
let new_reg = masked | bits;
self.write_register(reg, new_reg)?;
Ok(self)
}
}
trait BitValue {
fn width() -> u8;
fn mask() -> u8 {
(1 << Self::width()) - 1
}
fn shift() -> u8;
fn value(&self) -> u8;
}
#[allow(dead_code)]
#[allow(non_camel_case_types)]
#[derive(Clone, Copy)]
enum Register {
WHO_AM_I = 0x0F,
CTRL_REG1 = 0x20,
CTRL_REG2 = 0x21,
CTRL_REG3 = 0x22,
CTRL_REG4 = 0x23,
CTRL_REG5 = 0x24,
REFERENCE = 0x25,
OUT_TEMP = 0x26,
STATUS_REG = 0x27,
OUT_X_L = 0x28,
OUT_X_H = 0x29,
OUT_Y_L = 0x2A,
OUT_Y_H = 0x2B,
OUT_Z_L = 0x2C,
OUT_Z_H = 0x2D,
FIFO_CTRL_REG = 0x2E,
FIFO_SRC_REG = 0x2F,
INT1_CFG = 0x30,
INT1_SRC = 0x31,
INT1_TSH_XH = 0x32,
INT1_TSH_XL = 0x33,
INT1_TSH_YH = 0x34,
INT1_TSH_YL = 0x35,
INT1_TSH_ZH = 0x36,
INT1_TSH_ZL = 0x37,
INT1_DURATION = 0x38,
}
#[derive(Debug, Clone, Copy)]
pub enum Odr {
Hz100 = 0x00,
Hz200 = 0x01,
Hz400 = 0x02,
Hz800 = 0x03,
}
impl BitValue for Odr {
fn width() -> u8 {
2
}
fn shift() -> u8 {
6
}
fn value(&self) -> u8 {
*self as u8
}
}
impl Odr {
fn from_u8(from: u8) -> Self {
match (from >> Odr::shift()) & Odr::mask() {
x if x == Odr::Hz100 as u8 => Odr::Hz100,
x if x == Odr::Hz200 as u8 => Odr::Hz200,
x if x == Odr::Hz400 as u8 => Odr::Hz400,
x if x == Odr::Hz800 as u8 => Odr::Hz800,
_ => unreachable!(),
}
}
}
#[derive(Debug, Clone, Copy)]
pub enum Scale {
Dps245 = 0x00,
Dps500 = 0x01,
Dps2000 = 0x03,
}
impl BitValue for Scale {
fn width() -> u8 {
2
}
fn shift() -> u8 {
4
}
fn value(&self) -> u8 {
*self as u8
}
}
impl Scale {
fn from_u8(from: u8) -> Self {
match (from >> Scale::shift()) & Scale::mask() {
x if x == Scale::Dps245 as u8 => Scale::Dps245,
x if x == Scale::Dps500 as u8 => Scale::Dps500,
x if x == Scale::Dps2000 as u8 => Scale::Dps2000,
0x02 => Scale::Dps2000,
_ => unreachable!(),
}
}
}
#[derive(Debug, Clone, Copy)]
pub enum Bandwidth {
Low = 0x00,
Medium = 0x01,
High = 0x02,
Maximum = 0x03,
}
impl BitValue for Bandwidth {
fn width() -> u8 {
2
}
fn shift() -> u8 {
4
}
fn value(&self) -> u8 {
*self as u8
}
}
impl Bandwidth {
fn from_u8(from: u8) -> Self {
match (from >> Bandwidth::shift()) & Bandwidth::mask() {
x if x == Bandwidth::Low as u8 => Bandwidth::Low,
x if x == Bandwidth::Medium as u8 => Bandwidth::Medium,
x if x == Bandwidth::High as u8 => Bandwidth::High,
x if x == Bandwidth::Maximum as u8 => Bandwidth::Maximum,
_ => unreachable!(),
}
}
}
const READ: u8 = 1 << 7;
const WRITE: u8 = 0 << 7;
const MULTI: u8 = 1 << 6;
const SINGLE: u8 = 0 << 6;
impl Register {
fn addr(self) -> u8 {
self as u8
}
}
impl Scale {
pub fn degrees(&self, val: i16) -> f32 {
match *self {
Scale::Dps245 => val as f32 * 0.00875,
Scale::Dps500 => val as f32 * 0.0175,
Scale::Dps2000 => val as f32 * 0.07,
}
}
pub fn radians(&self, val: i16) -> f32 {
self.degrees(val) * (core::f32::consts::PI / 180.0)
}
}
#[derive(Debug)]
pub struct I16x3 {
pub x: i16,
pub y: i16,
pub z: i16,
}
#[derive(Debug)]
pub struct Measurements {
pub gyro: I16x3,
pub temp: i8,
}
#[derive(Debug, Clone, Copy)]
pub struct Status {
pub overrun: bool,
pub z_overrun: bool,
pub y_overrun: bool,
pub x_overrun: bool,
pub new_data: bool,
pub z_new: bool,
pub y_new: bool,
pub x_new: bool,
}
impl Status {
fn from_u8(from: u8) -> Self {
Status {
overrun: (from & 1 << 7) != 0,
z_overrun: (from & 1 << 6) != 0,
y_overrun: (from & 1 << 5) != 0,
x_overrun: (from & 1 << 4) != 0,
new_data: (from & 1 << 3) != 0,
z_new: (from & 1 << 2) != 0,
y_new: (from & 1 << 1) != 0,
x_new: (from & 1 << 0) != 0,
}
}
}