#![no_std]
use core::mem;
use embedded_hal as hal;
use crate::hal::blocking::i2c::{Write, WriteRead};
use bitflags::bitflags;
#[derive(Debug)]
pub enum FullScale {
Fs4g,
Fs8g,
Fs12g,
Fs16g
}
#[derive(Debug)]
pub enum OperatingMode {
LowPower,
MediumPerformance,
HighPerformance,
UltraHighPerformance
}
#[derive(Debug)]
pub enum MeasurementMode {
Idle,
SingleMeasurement,
Continuous
}
#[allow(non_camel_case_types)]
#[derive(Debug)]
pub enum DataRate {
ODR_0_625Hz,
ODR_1_25Hz,
ODR_2_5Hz,
ODR_5Hz,
ODR_10Hz,
ODR_20Hz,
ODR_40Hz,
ODR_80Hz,
ODR_Fast
}
#[derive(Debug)]
pub enum Error {
CommunicationError,
InvalidValue,
IncorrectDeviceIdFound,
}
fn i2c_error<E>(_: E) -> Error {
Error::CommunicationError
}
#[derive(Debug)]
pub struct I16xyz {
pub x: i16,
pub y: i16,
pub z: i16,
}
#[derive(Debug)]
pub struct I32xyz {
pub x: i32,
pub y: i32,
pub z: i32,
}
pub struct Lis3mdl<I2C> {
i2c: I2C,
address: u8,
}
impl<I2C, E> Lis3mdl<I2C> where I2C: WriteRead<Error=E> + Write<Error=E>,
{
pub fn new (i2c: I2C) -> Result<Self,Error> {
let mut lis3mdl = Lis3mdl {
i2c,
address: ADDRESS
};
if lis3mdl.who_am_i()? != LIS3MDL_DEVICE_ID {
return Err(Error::IncorrectDeviceIdFound)
}
lis3mdl.set_full_scale(FullScale::Fs12g)?;
lis3mdl.set_operating_mode(OperatingMode::UltraHighPerformance)?;
lis3mdl.set_measurement_mode(MeasurementMode::Continuous)?;
lis3mdl.set_temperature_sensor_enable(true)?;
lis3mdl.set_data_rate(DataRate::ODR_Fast)?;
lis3mdl.set_block_data_update(true)?;
Ok(lis3mdl)
}
pub fn who_am_i(&mut self) -> Result<u8, Error> {
self.read_register(Register::WHO_AM_I).map_err(i2c_error)
}
pub fn get_raw_mag_axes(&mut self) -> Result<I16xyz, Error> {
let x = self.read_x_raw()?;
let y = self.read_y_raw()?;
let z = self.read_z_raw()?;
Ok(I16xyz {
x,
y,
z
})
}
pub fn xyz_data_available(&mut self) -> Result<bool, Error> {
Ok(self.read_device_status()?.contains(StatusRegisterBits::ZYXDA))
}
pub fn get_mag_axes_mgauss(&mut self) -> Result<I32xyz, Error> {
let mag_data = self.get_raw_mag_axes()?;
let fullscale = FullScaleBits::from_bits(self.read_register(Register::CTRL_REG2)?).unwrap();
let sensitivity: f64 = match fullscale {
FullScaleBits::FS4G => Ok(1000_f64/6842_f64),
FullScaleBits::FS8G => Ok(1000_f64/3421_f64),
FullScaleBits::FS12G => Ok(1000_f64/2281_f64),
FullScaleBits::FS16G => Ok(1000_f64/1711_f64),
_ => Err(Error::InvalidValue)
}?;
Ok(I32xyz {
x: (mag_data.x as f64 * sensitivity) as i32,
y: (mag_data.y as f64 * sensitivity) as i32,
z: (mag_data.z as f64 * sensitivity) as i32
})
}
pub fn set_full_scale(&mut self, scale: FullScale) -> Result<(),Error> {
let fs_mask = !(ControlRegister2Bits::FS1 | ControlRegister2Bits::FS0);
let fs = match scale {
FullScale::Fs4g => FullScaleBits::FS4G,
FullScale::Fs8g => FullScaleBits::FS8G,
FullScale::Fs12g => FullScaleBits::FS12G,
FullScale::Fs16g => FullScaleBits::FS16G,
};
let existing_settings = self.read_control_register_2()? & fs_mask;
let reg2bits = ControlRegister2Bits::from_bits_truncate(fs.bits());
self.set_control_register_2(reg2bits | existing_settings)
}
pub fn set_operating_mode(&mut self, mode: OperatingMode) -> Result<(), Error> {
let reg1_mask = !(ControlRegister1Bits::OM1 | ControlRegister1Bits::OM0);
let reg4_mask = !(ControlRegister4Bits::OMZ1 | ControlRegister4Bits::OMZ0);
let om = match mode {
OperatingMode::LowPower => (ControlRegister1Bits::empty(), ControlRegister4Bits::empty()),
OperatingMode::MediumPerformance => (ControlRegister1Bits::OM0, ControlRegister4Bits::OMZ0),
OperatingMode::HighPerformance => (ControlRegister1Bits::OM1, ControlRegister4Bits::OMZ1),
OperatingMode::UltraHighPerformance => (ControlRegister1Bits::OM1
| ControlRegister1Bits::OM0,
ControlRegister4Bits::OMZ1
| ControlRegister4Bits::OMZ0),
};
let existing_reg_1_settings = self.read_control_register_1()? & reg1_mask;
let existing_reg_4_settings = self.read_control_register_4()? & reg4_mask;
self.set_control_register_1(existing_reg_1_settings | om.0)?;
self.set_control_register_4(existing_reg_4_settings | om.1)?;
Ok(())
}
pub fn set_measurement_mode(&mut self, mode: MeasurementMode) -> Result<(), Error> {
let mm_mask = !(ControlRegister3Bits::MD1 | ControlRegister3Bits::MD0);
let mm = match mode {
MeasurementMode::Idle => ControlRegister3Bits::MD1,
MeasurementMode::SingleMeasurement => ControlRegister3Bits::MD0,
MeasurementMode::Continuous => ControlRegister3Bits::empty(),
};
let existing_reg_3_settings = self.read_control_register_3()? & mm_mask;
self.set_control_register_3(existing_reg_3_settings | mm)
}
pub fn set_data_rate(&mut self, rate: DataRate) -> Result<(), Error> {
let odr_mask = !(ControlRegister1Bits::DO2 | ControlRegister1Bits::DO1
| ControlRegister1Bits::DO0 | ControlRegister1Bits::FAST_ODR);
let odr = match rate {
DataRate::ODR_0_625Hz => ControlRegister1Bits::empty(),
DataRate::ODR_1_25Hz => ControlRegister1Bits::DO0,
DataRate::ODR_2_5Hz => ControlRegister1Bits::DO1,
DataRate::ODR_5Hz => ControlRegister1Bits::DO1 | ControlRegister1Bits::DO0,
DataRate::ODR_10Hz => ControlRegister1Bits::DO2,
DataRate::ODR_20Hz => ControlRegister1Bits::DO2 | ControlRegister1Bits::DO0,
DataRate::ODR_40Hz => ControlRegister1Bits::DO2 | ControlRegister1Bits::DO1,
DataRate::ODR_80Hz => ControlRegister1Bits::DO2 | ControlRegister1Bits::DO1
| ControlRegister1Bits::DO0,
DataRate::ODR_Fast => ControlRegister1Bits::FAST_ODR,
};
let existing_reg_1_settings = self.read_control_register_1()? & odr_mask;
self.set_control_register_1(existing_reg_1_settings | odr)
}
pub fn set_block_data_update(&mut self, block: bool) -> Result<(), Error> {
let bdu_mask = !ControlRegister5Bits::BDU;
let existing_reg_5_settings = self.read_control_register_5()? & bdu_mask;
if block {
self.set_control_register_5(existing_reg_5_settings | ControlRegister5Bits::BDU)
}
else {
self.set_control_register_5(existing_reg_5_settings)
}
}
pub fn set_temperature_sensor_enable(&mut self, enabled: bool) -> Result<(), Error> {
let temp_mask = !ControlRegister1Bits::TEMP_EN;
let existing_reg_1_settings = self.read_control_register_1()? & temp_mask;
if enabled {
self.set_control_register_1(existing_reg_1_settings | ControlRegister1Bits::TEMP_EN)
}
else {
self.set_control_register_1(existing_reg_1_settings)
}
}
fn read_x_raw(&mut self) -> Result<i16,Error> {
self.read_register_i16(Register::OUT_X_H,Register::OUT_X_L)
}
fn read_y_raw(&mut self) -> Result<i16,Error> {
self.read_register_i16(Register::OUT_Y_H,Register::OUT_Y_L)
}
fn read_z_raw(&mut self) -> Result<i16,Error> {
self.read_register_i16(Register::OUT_Z_H,Register::OUT_Z_L)
}
fn read_device_status(&mut self) -> Result<StatusRegisterBits, Error> {
Ok(StatusRegisterBits::from_bits_truncate(self.read_register(Register::STATUS_REG)?))
}
fn set_control_register_1(&mut self, bits: ControlRegister1Bits) -> Result<(),Error> {
Ok(self.write_register(Register::CTRL_REG1, bits.bits())?)
}
fn set_control_register_2(&mut self, bits: ControlRegister2Bits) -> Result<(),Error> {
Ok(self.write_register(Register::CTRL_REG2, bits.bits())?)
}
fn set_control_register_3(&mut self, bits: ControlRegister3Bits) -> Result<(),Error> {
Ok(self.write_register(Register::CTRL_REG3, bits.bits())?)
}
fn set_control_register_4(&mut self, bits: ControlRegister4Bits) -> Result<(),Error> {
Ok(self.write_register(Register::CTRL_REG4, bits.bits())?)
}
fn set_control_register_5(&mut self, bits: ControlRegister5Bits) -> Result<(),Error> {
Ok(self.write_register(Register::CTRL_REG5, bits.bits())?)
}
fn read_control_register_1(&mut self) -> Result<ControlRegister1Bits, Error> {
Ok(ControlRegister1Bits::from_bits_truncate(self.read_register(Register::CTRL_REG1)?))
}
fn read_control_register_2(&mut self) -> Result<ControlRegister2Bits, Error> {
Ok(ControlRegister2Bits::from_bits_truncate(self.read_register(Register::CTRL_REG2)?))
}
fn read_control_register_3(&mut self) -> Result<ControlRegister3Bits, Error> {
Ok(ControlRegister3Bits::from_bits_truncate(self.read_register(Register::CTRL_REG3)?))
}
fn read_control_register_4(&mut self) -> Result<ControlRegister4Bits, Error> {
Ok(ControlRegister4Bits::from_bits_truncate(self.read_register(Register::CTRL_REG4)?))
}
fn read_control_register_5(&mut self) -> Result<ControlRegister5Bits, Error> {
Ok(ControlRegister5Bits::from_bits_truncate(self.read_register(Register::CTRL_REG5)?))
}
fn read_register_i16(&mut self, reg_low: Register, reg_high: Register) -> Result<i16, Error> {
let low = self.read_register(reg_low)?;
let high = self.read_register(reg_high)?;
let signed = i16::from_le_bytes([high, low]);
Ok(signed)
}
fn read_register(&mut self, reg: Register) -> Result<u8, Error> {
let mut buffer: [u8; 1] = unsafe { mem::uninitialized() };
self.i2c.write_read(self.address, &[reg.addr()], &mut buffer).map_err(i2c_error)?;
Ok(buffer[0])
}
fn write_register(&mut self, reg: Register, byte: u8) -> Result<(), Error> {
self.i2c.write(self.address, &[reg.addr(), byte]).map_err(i2c_error)
}
}
const ADDRESS: u8 = 0b0011_1100;
const LIS3MDL_DEVICE_ID: u8 = 0x3D;
#[allow(non_camel_case_types, dead_code)]
#[derive(Debug, Copy, Clone)]
enum Register {
OFFSET_X_REG_L_M = 0x05,
OFFSET_X_REG_H_M = 0x06,
OFFSET_Y_REG_L_M = 0x07,
OFFSET_Y_REG_H_M = 0x08,
OFFSET_Z_REG_L_M = 0x09,
OFFSET_Z_REG_H_M = 0x0A,
WHO_AM_I = 0x0F,
CTRL_REG1 = 0x20,
CTRL_REG2 = 0x21,
CTRL_REG3 = 0x22,
CTRL_REG4 = 0x23,
CTRL_REG5 = 0x24,
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,
TEMP_OUT_L = 0x2E,
TEMP_OUT_H = 0x2F,
INT_CFG = 0x30,
INT_SRC = 0x31,
INT_THS_L = 0x32,
INT_THS_H = 0x33
}
impl Register {
fn addr(self) -> u8 {
self as u8
}
}
bitflags! {
#[allow(non_camel_case_types, dead_code)]
struct ControlRegister1Bits: u8 {
const TEMP_EN = 0b1000_0000;
const OM1 = 0b0100_0000;
const OM0 = 0b0010_0000;
const DO2 = 0b0001_0000;
const DO1 = 0b0000_1000;
const DO0 = 0b0000_0100;
const FAST_ODR = 0b0000_0010;
const ST = 0b0000_0001;
}
}
bitflags! {
#[allow(non_camel_case_types, dead_code)]
struct ControlRegister2Bits: u8 {
const FS1 = 0b0100_0000;
const FS0 = 0b0010_0000;
const REBOOT = 0b0000_1000;
const SOFT_RST = 0b0000_0100;
}
}
bitflags! {
#[allow(non_camel_case_types, dead_code)]
struct ControlRegister3Bits: u8 {
const LP = 0b0010_0000;
const SIM = 0b0000_0100;
const MD1 = 0b0000_0010;
const MD0 = 0b0000_0001;
}
}
bitflags! {
struct ControlRegister4Bits: u8 {
const OMZ1 = 0b0000_1000;
const OMZ0 = 0b0000_0100;
const BLE = 0b0000_0010;
const UHP = 0b0000_1100;
}
}
bitflags! {
struct ControlRegister5Bits: u8 {
const FAST_READ = 0b1000_0000;
const BDU = 0b0100_0000;
}
}
bitflags! {
struct StatusRegisterBits: u8 {
const ZYXOR = 0b1000_0000;
const ZOR = 0b0100_0000;
const YOR = 0b0010_0000;
const XOR = 0b0001_0000;
const ZYXDA = 0b0000_1000;
const ZDA = 0b0000_0100;
const YDA = 0b0000_0010;
const XDA = 0b0000_0001;
}
}
bitflags! {
#[allow(non_camel_case_types, dead_code)]
struct FullScaleBits: u8 {
const FS4G = 0b0000_0000;
const FS8G = 0b0010_0000;
const FS12G = 0b0100_0000;
const FS16G = 0b0110_0000;
}
}