#![no_std]
#![allow(dead_code)]
#![allow(unused_variables)]
pub mod error;
use crate::data::MeasurementMode::{Single, WakeOnChange};
use crate::error::Error::{self};
pub mod data;
use crate::data::{Status, MeasurementMode, ScaledMeasurements, RawMeasurements, Resolution, Register_0x02, Register_0x01, Register_0x00,
XYZOffset};
pub mod constants;
use crate::constants::{DIGITAL_FILTER_REGISTER, DeviceAddress, Exit_mode, GAIN_REGISTER, Memory_Recall, Memory_Store,
REGISTER_00, REGISTER_01, REGISTER_03, REGISTER_04, REGISTER_05, REGISTER_06, REGISTER_07, REGISTER_08, REGISTER_09,
RES_XYZ_REGISTER, Read_Measurement, Read_Register, Reset, SENS_XY, SENS_Z, Start_Burst_Mode, Start_Single_Measurement,
Start_Wake_On_Change, Write_Register};
use embedded_hal::{i2c::I2c, delay::DelayNs, digital::InputPin};
#[cfg(feature = "async")]
use embedded_hal_async::{i2c::I2c as AsyncI2c, delay::DelayNs as AsyncDelayNs, digital::Wait as InputPinWait};
use log::Level::Info;
use log::{debug, info, warn};
use libm::{powf, truncf, roundf};
pub struct MLX90393<I2C, D, P> {
i2c: I2C,
address: DeviceAddress,
delayer: D,
t: u8, x: u8, y: u8, z: u8, conversion_time: u32, gain_sel: u8 , hall_conf: u8, resolution: Resolution, tcmp_enabled: bool,
drdy_input: Option<P>, measurement_mode: MeasurementMode,
}
#[cfg(not(feature = "async"))]
impl<I2C, D, E, P> MLX90393<I2C, D, P>
where
I2C: I2c<Error = E>,
D: DelayNs,
P: InputPin,
{
pub fn new(i2c: I2C, address: DeviceAddress, delayer: D,
t: bool, x: bool, y: bool, z: bool, pin: Option<P>) -> Self {
log::debug!("new called");
Self {
i2c,
address: address,
delayer,
t: if t {1} else {0},
x: if x {1} else {0},
y: if y {1} else {0},
z: if z {1} else {0},
conversion_time: 0,
gain_sel: 7,
resolution: Resolution { x: 0, y: 0, z: 0 },
hall_conf: 0xc,
tcmp_enabled: false,
drdy_input: pin,
measurement_mode: MeasurementMode::Single,
}
}
pub fn release(self) -> I2C {
self.i2c
}
}
#[cfg(feature = "async")]
impl<I2C, D, E, P> MLX90393<I2C, D, P>
where
I2C: AsyncI2c<Error = E>,
D: AsyncDelayNs,
P: InputPin, {
pub fn new(i2c: I2C, address: DeviceAddress, delayer: D,
t: bool, x: bool, y: bool, z: bool, pin: Option<P>) -> Self {
log::debug!("new called");
Self {
i2c,
address: address,
delayer,
t: if t {1} else {0},
x: if x {1} else {0},
y: if y {1} else {0},
z: if z {1} else {0},
conversion_time: 0,
gain_sel: 7,
resolution: Resolution { x: 0, y: 0, z: 0 },
hall_conf: 0xc,
tcmp_enabled: false,
drdy_input: pin,
measurement_mode: MeasurementMode::Single,
}
}
}
#[maybe_async_cfg::maybe(
sync(
cfg(not(feature = "async")),
self = "MLX90393",
idents(AsyncI2c(sync = "I2c"), AsyncDelayNs(sync = "DelayNs"), InputPinWait(sync = "InputPin"))
),
async(feature = "async", keep_self)
)]
impl<I2C, D, E, P> MLX90393<I2C, D, P>
where
I2C: AsyncI2c<Error = E>,
D: AsyncDelayNs,
P: InputPin,
{
async fn write_command<const N: usize>(&mut self, command_buf: [u8; N] ) -> Result<(), Error<E>> {
debug!("in write_command: command = {:#02x}", command_buf[0]);
debug!(" i2c address = {:#02x}", self.address as u8);
self.i2c
.write(self.address as u8, &command_buf).await
.map_err(Error::I2c)?;
Ok(())
}
async fn read_register( &mut self, register_address: u8, buffer: &mut [u8] ) -> Result<Status, Error<E>> {
let mut command_buffer = [0u8; 2];
command_buffer[0] = Read_Register;
command_buffer[1] = (register_address & 0x3f) << 2;
let mut read_buffer = [0u8; 3];
self.i2c
.write_read(self.address as u8, &command_buffer, &mut read_buffer).await
.map_err(Error::I2c)?;
let status: Status = Status(read_buffer[0]); buffer[0] = read_buffer[1];
buffer[1] = read_buffer[2];
Ok(status)
}
pub async fn read_register_free( &mut self, register_address: u8 ) -> Result<u16, Error<E>> {
if (register_address < 0x0a) || (register_address > 0x1f) {
return Err(Error::OutOfRange(register_address));
}
let mut command_buffer = [0u8; 2];
command_buffer[0] = Read_Register;
command_buffer[1] = (register_address & 0x3f) << 2;
let mut read_buffer = [0u8; 3];
self.i2c
.write_read(self.address as u8, &command_buffer, &mut read_buffer).await
.map_err(Error::I2c)?;
let buffer = u16::from_be_bytes([read_buffer[0], read_buffer[1] ]); Ok(buffer)
}
async fn write_register( &mut self, register_address: u8, data: &mut [u8] ) -> Result<Status, Error<E>> {
let mut command_buffer = [0u8; 4];
let mut buffer = [0u8; 1];
command_buffer[0] = Write_Register;
command_buffer[1] = data[0];
command_buffer[2] = data[1];
command_buffer[3] = (register_address & 0x3f) << 2;
self.i2c.write_read(self.address as u8, &command_buffer, &mut buffer).await
.map_err(Error::I2c)?;
let status: Status = Status(buffer[0]);
Ok(status)
}
pub async fn write_register_free(&mut self, register_address: u8, data: &mut u16 ) -> Result<(), Error<E>> {
if (register_address < 0x0a) || (register_address > 0x1f) {
return Err(Error::OutOfRange(register_address));
}
let mut command_buffer = [0u8; 4];
let mut buffer = [0u8; 1];
let data_buffer: [u8; 2] = data.to_be_bytes();
command_buffer[0] = Write_Register;
command_buffer[1] = data_buffer[0];
command_buffer[2] = data_buffer[1];
command_buffer[3] = (register_address & 0x3f) << 2;
self.i2c.write_read(self.address as u8, &command_buffer, &mut buffer).await
.map_err(Error::I2c)?;
Ok(())
}
async fn read_status_byte( &mut self, buffer: &mut [u8] ) -> Result<(), Error<E>> {
debug!("in read_status_byte()");
self.i2c.read(self.address as u8, buffer).await
.map_err(Error::I2c)?;
Ok(())
}
async fn write_get_status<const N: usize>( &mut self, command_buffer: [u8; N]) ->
Result<Status, Error<E>> {
debug!("in write_get_status");
let mut result_buffer = [0u8; 1];
self.i2c
.write_read(self.address as u8, &command_buffer, &mut result_buffer).await
.map_err(Error::I2c)?;
Ok(Status(result_buffer[0]))
}
pub async fn reset_device(&mut self) -> Result<(), Error<E>> {
debug!("in reset_device()");
let status = self.write_get_status([Exit_mode]).await?;
if status.get_error() {
return Err(Error::CommandError);
}
debug!(" wrote Exit_mode command");
self.delayer.delay_ms(2).await;
let status = self.write_get_status([Reset]).await?;
self.delayer.delay_ms(5).await;
if status.get_error() {
return Err(Error::CommandError);
} else {
return Ok(());
}
}
pub fn set_txyz(&mut self, t: bool, x: bool, y: bool, z: bool) {
self.t = if t { 1} else {0};
self.x = if x {1} else {0};
self.y = if y {1} else {0};
self.z = if z {1} else {0};
let _ = self.calc_conversion_time();
}
pub async fn init_device(&mut self) -> Result<(), Error<E>> {
debug!("in init_device()");
self.reset_device().await?;
debug!(" setting gain to 7 (default)");
self.set_gain(7).await?;
let new_gain = self.get_gain().await?;
debug!(" read back gain = {:#02x}", new_gain);
self.set_resolution(0, 0, 0).await?; let res = self.get_resolution().await?;
debug!(" new resolution is : {:#?}", res);
self.set_digital_filter(0).await?;
debug!("set digital filter to 0");
let dig_filt = self.get_digital_filter().await?;
debug!(" read back digital filter = {:#02x}", dig_filt);
self.set_over_sampling(0).await?;
let osr = self.get_over_sampling().await?;
debug!(" read back OSR = {:#02x}", osr);
self.set_temperature_over_sampling(0).await?;
let osr2 = self.get_temperature_over_sampling().await?;
debug!(" read back temperature over sampling = {:#02x}", osr2);
self.set_temperature_drift_compensation(false).await;
let tcmp_en = self.get_temperature_drift_compensation().await?;
debug!("set temperature_drift_compensation to {:#}", tcmp_en); self.set_hallconf(0x0c).await?;
self.measurement_mode = MeasurementMode::Single;
self.calc_conversion_time().await?;
match &mut self.drdy_input {
None => {
debug!(" drdy_input not defined, using calculated delay for measurement ready")
},
Some(pin) => {
debug!(" drdy_pin defined, enabling INTterrupt pin");
self.set_trig_int(true).await?;
},
}
Ok(())
}
pub async fn send_exit(&mut self) -> Result<(), Error<E>> {
let status = self.write_get_status([Exit_mode]).await?;
if status.get_error() {
return Err(Error::CommandError);
}
debug!(" wrote Exit_mode command");
self.delayer.delay_ms(2).await;
Ok(())
}
pub async fn set_gain(&mut self, gain_value: u8) -> Result<(), Error<E>> {
debug!("in set_gain()");
let mut current_value = [0u8; 2];
let status = self.read_register(GAIN_REGISTER, &mut current_value).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let z_series: u8 = current_value[1] & 0b1000_0000;
let hallconf: u8 = current_value[1] & 0b0000_1111;
let new_gain: u8 = (gain_value & 0x07) << 0x04;
let new_value: u8 = z_series | new_gain | hallconf;
let mut data = [0u8; 2]; data[1] = new_value;
data[0] = current_value[0]; let status = self.write_register(GAIN_REGISTER, &mut data ).await?;
if status.get_error() {
return Err(Error::CommandError);
}
self.hall_conf = hallconf;
self.gain_sel = gain_value;
Ok(())
}
pub async fn get_gain(&mut self) -> Result<u8, Error<E>> {
debug!(" in get_gain()");
let mut value = [0u8; 2];
let status = self.read_register(GAIN_REGISTER, &mut value).await?;
if status.get_error() {
return Err(Error::CommandError);
}
value[1] = (value[1] & 0b0111_0000 ) >> 4;
Ok(value[1])
}
pub async fn set_resolution(&mut self, x_res: u8, y_res: u8, z_res: u8) -> Result<(), Error<E>> {
debug!("in set_resultion");
let mut read_buffer = [0u8, 2];
let status = self.read_register(RES_XYZ_REGISTER, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_02:u16 = u16::from_be_bytes(read_buffer);
let mut register_02: Register_0x02 = Register_0x02(reg_02);
register_02.set_x_res(x_res);
register_02.set_y_res(y_res);
register_02.set_z_res(z_res);
let mut write_buffer = register_02.0.to_be_bytes();
let status = self.write_register(RES_XYZ_REGISTER, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
self.resolution = Resolution{x: x_res, y: y_res, z: z_res};
Ok(())
}
pub async fn get_resolution(&mut self) -> Result<Resolution, Error<E>> {
let mut current_res = [0u8; 2];
let status = self.read_register(RES_XYZ_REGISTER, &mut current_res).await?;
if (status.get_error()) {
return Err(Error::CommandError);
}
let mut value = u16::from_be_bytes(current_res);
let register_02 = Register_0x02(value);
Ok(Resolution { x: register_02.get_x_res(), y: register_02.get_y_res(), z: register_02.get_z_res() })
}
pub async fn set_digital_filter(&mut self, new_value: u8) -> Result<(), Error<E>> {
if (new_value > 0x07) {
return Err(Error::OutOfRange(new_value));
}
let mut read_buffer = [0u8, 2];
let status = self.read_register(DIGITAL_FILTER_REGISTER, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_02:u16 = u16::from_be_bytes(read_buffer);
let mut register_02: Register_0x02 = Register_0x02(reg_02);
register_02.set_dig_filt(new_value);
let mut write_buffer = register_02.0.to_be_bytes();
let status = self.write_register(DIGITAL_FILTER_REGISTER, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
self.calc_conversion_time().await?;
Ok(())
}
pub async fn get_digital_filter(&mut self) -> Result<u8, Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(DIGITAL_FILTER_REGISTER, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_02:u16 = u16::from_be_bytes(read_buffer); let register_02: Register_0x02 = Register_0x02(reg_02);
Ok(register_02.get_dig_filt())
}
pub async fn set_over_sampling (&mut self, new_value: u8) -> Result<(), Error<E>> {
if (new_value > 0x03) {
return Err(Error::OutOfRange(new_value));
}
let mut read_buffer = [0u8, 2];
let status = self.read_register(DIGITAL_FILTER_REGISTER, &mut read_buffer).await?; if status.get_error() {
return Err(Error::CommandError);
}
let reg_02:u16 = u16::from_be_bytes(read_buffer);
let mut register_02: Register_0x02 = Register_0x02(reg_02);
register_02.set_OSR(new_value & 0x03);
let mut write_buffer = register_02.0.to_be_bytes();
let status = self.write_register(DIGITAL_FILTER_REGISTER, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
self.calc_conversion_time().await?;
Ok(())
}
pub async fn get_over_sampling(&mut self) -> Result<u8, Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(DIGITAL_FILTER_REGISTER, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_02:u16 = u16::from_be_bytes(read_buffer);
let register_02: Register_0x02 = Register_0x02(reg_02);
Ok(register_02.get_OSR())
}
pub async fn set_temperature_over_sampling (&mut self, new_value: u8) -> Result<(), Error<E>> {
if (new_value > 0x03) {
return Err(Error::OutOfRange(new_value));
}
let mut read_buffer = [0u8, 2];
let status = self.read_register(DIGITAL_FILTER_REGISTER, &mut read_buffer).await?; if status.get_error() {
return Err(Error::CommandError);
}
let reg_02:u16 = u16::from_be_bytes(read_buffer);
let mut register_02: Register_0x02 = Register_0x02(reg_02);
register_02.set_OSR2(new_value & 0x03);
let mut write_buffer = register_02.0.to_be_bytes();
let status = self.write_register(DIGITAL_FILTER_REGISTER, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
self.calc_conversion_time().await?;
Ok(())
}
pub async fn get_temperature_over_sampling(&mut self) -> Result<u8, Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(DIGITAL_FILTER_REGISTER, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_02:u16 = u16::from_be_bytes(read_buffer);
let register_02: Register_0x02 = Register_0x02(reg_02);
Ok(register_02.get_OSR2())
}
pub async fn set_temperature_drift_compensation (&mut self, enable: bool) -> Result<(), Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_01, &mut read_buffer).await?; if status.get_error() {
return Err(Error::CommandError);
}
let reg_01:u16 = u16::from_be_bytes(read_buffer);
let mut register_0x01: Register_0x01 = Register_0x01(reg_01);
register_0x01.set_tcmp_en(enable);
let mut write_buffer = register_0x01.0.to_be_bytes();
debug!(" set_temperature_drift_compensation reg0x01 = {:#?}", register_0x01);
let status = self.write_register(REGISTER_01, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
self.tcmp_enabled = enable;
Ok(())
}
pub async fn get_temperature_drift_compensation(&mut self) -> Result<bool, Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_01, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_01:u16 = u16::from_be_bytes(read_buffer);
let register_0x01: Register_0x01 = Register_0x01(reg_01);
debug!(" get_temperature_drift_compensation reg0x01 = {:#?}", register_0x01);
self.tcmp_enabled = register_0x01.get_tcmp_en();
Ok(register_0x01.get_tcmp_en())
}
pub async fn set_burst_data_rate(&mut self, rate: u8) -> Result<(), Error<E>> {
if (rate & 0b1100_0000) != 0x00 {
return Err(Error::OutOfRange(0b0011_1111));
}
let mut read_buffer = [0u8, 2];
let status = self.read_register((REGISTER_01), &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_01:u16 = u16::from_be_bytes(read_buffer);
let mut register_0x01: Register_0x01 = Register_0x01(reg_01);
register_0x01.set_burst_data_rate(rate & 0b0011_1111);
let mut write_buffer = register_0x01.0.to_be_bytes();
let status = self.write_register(REGISTER_01, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn get_burst_data_rate(&mut self) -> Result<u8, Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register((REGISTER_01), &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_01:u16 = u16::from_be_bytes(read_buffer);
let register_0x01: Register_0x01 = Register_0x01(reg_01);
Ok(register_0x01.get_burst_data_rate())
}
pub async fn set_hallconf(&mut self, hallconf: u8) -> Result<(), Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_00, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_00: u16 = u16::from_be_bytes(read_buffer);
let mut register_00: Register_0x00 = Register_0x00(reg_00);
register_00.set_hallconf(hallconf & 0x0f);
let mut write_buffer = register_00.0.to_be_bytes();
let status = self.write_register(REGISTER_00, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
self.hall_conf = hallconf& 0x0f;
Ok(())
}
pub async fn get_hallconf(&mut self) -> Result<u8, Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_00, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_00: u16 = u16::from_be_bytes(read_buffer);
let mut register_00: Register_0x00 = Register_0x00(reg_00);
self.hall_conf = register_00.get_hallconf();
Ok(register_00.get_hallconf())
}
pub async fn set_burst_sel(&mut self, burst_sel: u8) -> Result<(), Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_01, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_01: u16 = u16::from_be_bytes(read_buffer);
let mut register_01: Register_0x01 = Register_0x01(reg_01);
register_01.set_burst_sel(burst_sel & 0x03);
let mut write_buffer = register_01.0.to_be_bytes();
let status = self.write_register(REGISTER_01, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn get_burst_sel(&mut self) -> Result<u8, Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_01, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_01: u16 = u16::from_be_bytes(read_buffer);
let register_01 = Register_0x01(reg_01);
Ok(register_01.get_burst_sel())
}
pub async fn set_z_series(&mut self, set: bool) -> Result<(), Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_00, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_00: u16 = u16::from_be_bytes(read_buffer);
let mut register_00: Register_0x00 = Register_0x00(reg_00);
register_00.set_z_series(set);
let mut write_buffer = register_00.0.to_be_bytes();
let status = self.write_register(REGISTER_00, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn get_z_series(&mut self) -> Result<bool, Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_00, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_00: u16 = u16::from_be_bytes(read_buffer);
let register_00 = Register_0x00(reg_00);
Ok(register_00.get_z_series())
}
pub async fn memory_recall(&mut self) -> Result<(), Error<E>> {
let status = self.write_get_status([Memory_Recall]).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn memory_store(&mut self) -> Result<(), Error<E>> {
let status = self.write_get_status([Memory_Store]).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn start_meaasurement_mode_by_props(&mut self, measurement_mode: MeasurementMode) -> Result<bool, Error<E>> {
let x_axis: bool = self.x != 0;
let y_axis: bool = self.y != 0;
let z_axis: bool = self.z != 0;
let temp: bool = self.t != 0;
return Ok(self.start_measurement_mode(measurement_mode, x_axis, y_axis, z_axis, temp).await? )
}
pub async fn start_measurement_mode(&mut self, measurement_mode: MeasurementMode, x: bool, y: bool, z: bool, t: bool) -> Result<bool, Error<E>> {
debug!("in start_measurement_mode: {:#?}", measurement_mode);
let x_axis: u8 = if (x) {1} else {0};
let y_axis: u8 = if (y) {1} else {0};
let z_axis: u8 = if (z) {1} else {0};
let temp: u8 = if (t) {1} else {0};
let sub_cmd: u8 = z_axis<<3 | y_axis <<2 | x_axis<< 1 | temp;
if (self.measurement_mode != measurement_mode) { let status = self.write_get_status([Exit_mode]).await?;
if status.get_error() {
return Err(Error::CommandError);
}
debug!("start_measurement_mode wrote Exit_mode command");
self.delayer.delay_ms(5).await;
self.measurement_mode = measurement_mode;
}
if (measurement_mode == MeasurementMode::Burst) {
self.set_burst_sel(sub_cmd).await?;
let cmd: u8 = Start_Burst_Mode | sub_cmd;
let status: Status = self.write_get_status([cmd]).await?;
if (status.get_burst_mode()) {
self.measurement_mode = measurement_mode;
debug!(" start_measurement_mode Burst Ok");
return Ok(true);
} else {
return Err(Error::CommandError);
}
} else if (measurement_mode == MeasurementMode::WakeOnChange) {
let cmd: u8 = Start_Wake_On_Change | sub_cmd;
let status: Status = self.write_get_status([cmd]).await?;
if status.get_woc_mode() {
self.measurement_mode = measurement_mode;
self.delayer.delay_ms(2).await;
debug!(" start_measurement_mode WakeOnChange Ok");
return Ok(true);
} else {
return Err(Error::CommandError);
}
} else if (measurement_mode == MeasurementMode::Single) {
let cmd: u8 = Start_Single_Measurement | sub_cmd;
let status: Status = self.write_get_status([cmd]).await?;
if status.get_single_mode() {
self.measurement_mode = measurement_mode;
self.delayer.delay_ms(2).await;
debug!(" start_measurement_mode Single Ok");
return Ok(true);
} else {
return Err(Error::CommandError);
}
} else {
return Err(Error::CommandError);
}
}
async fn start_single_measurement(&mut self) -> Result<(), Error<E>> {
let status = self.write_get_status([Exit_mode]).await?;
if status.get_error() {
return Err(Error::CommandError);
}
debug!(" wrote Exit_mode command");
let sub_cmd: u8 = self.z<<3 | self.y <<2 | self.x<< 1 | self.t;
let cmd: u8 = Start_Single_Measurement | sub_cmd;
let status: Status = self.write_get_status([cmd]).await?;
if status.get_single_mode() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn set_xyz_offset(&mut self, xyz_offset: XYZOffset) -> Result<(), Error<E>> {
let mut write_buffer = xyz_offset.x.to_be_bytes();
let status = self.write_register(REGISTER_04, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
write_buffer = xyz_offset.y.to_be_bytes();
let status = self.write_register(REGISTER_05, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
write_buffer = xyz_offset.z.to_be_bytes();
let status = self.write_register(REGISTER_06, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn get_xyz_offset(&mut self) -> Result<XYZOffset, Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_04, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let x_offset = u16::from_be_bytes(read_buffer);
let status = self.read_register(REGISTER_05, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let y_offset =u16::from_be_bytes(read_buffer);
let status = self.read_register(REGISTER_06, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let z_offset = u16::from_be_bytes(read_buffer);
let xyz_offset = XYZOffset{x: x_offset , y: y_offset , z: z_offset };
Ok(xyz_offset)
}
pub async fn set_woxy_threshold(&mut self, woxy_threshold: u16) -> Result<(), Error<E>> {
let mut write_buffer = woxy_threshold.to_be_bytes();
let status = self.write_register(REGISTER_07, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn set_woz_threshold(&mut self, woz_threshold: u16) -> Result<(), Error<E>> {
let mut write_buffer = woz_threshold.to_be_bytes();
let status = self.write_register(REGISTER_08, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn set_wot_threshold(&mut self, wot_threshold: u16) -> Result<(), Error<E>> {
let mut write_buffer = wot_threshold.to_be_bytes();
let status = self.write_register(REGISTER_09, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn set_trig_int_sel(&mut self, trig_int_as_interrupt: bool) -> Result<(), Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_01, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_01: u16 = u16::from_be_bytes(read_buffer);
let mut register_01: Register_0x01 = Register_0x01(reg_01);
register_01.set_trig_int(trig_int_as_interrupt);
let mut write_buffer = register_01.0.to_be_bytes();
let status = self.write_register(REGISTER_01, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn get_trig_int_sel(&mut self) -> Result<bool, Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_01, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_01: u16 = u16::from_be_bytes(read_buffer);
let register_01: Register_0x01 = Register_0x01(reg_01);
Ok(register_01.get_trig_int())
}
pub async fn set_ext_int_sel(&mut self, ext_trig_enable: bool) -> Result<(), Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_01, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_01: u16 = u16::from_be_bytes(read_buffer);
let mut register_01: Register_0x01 = Register_0x01(reg_01);
register_01.set_ext_trig(ext_trig_enable);
let mut write_buffer = register_01.0.to_be_bytes();
let status = self.write_register(REGISTER_01, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn get_ext_int_sel(&mut self) -> Result<bool, Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_01, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_01: u16 = u16::from_be_bytes(read_buffer);
let register_01: Register_0x01 = Register_0x01(reg_01);
Ok(register_01.get_ext_trg())
}
pub async fn set_trig_int(&mut self, trig_int: bool) -> Result<(), Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_01, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_01: u16 = u16::from_be_bytes(read_buffer);
let mut register_01: Register_0x01 = Register_0x01(reg_01);
register_01.set_trig_int(trig_int);
let mut write_buffer = register_01.0.to_be_bytes();
let status = self.write_register(REGISTER_01, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn get_trig_int(&mut self) -> Result<bool, Error<E>> {
debug!("in get_trig_int()");
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_01, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
debug!(" read_register(REGISTER_01 ... ok");
let reg_01: u16 = u16::from_be_bytes(read_buffer);
let register_01: Register_0x01 = Register_0x01(reg_01);
Ok(register_01.get_trig_int())
}
pub async fn set_woc_diff(&mut self, enable: bool) -> Result<(), Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_01, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_01: u16 = u16::from_be_bytes(read_buffer);
let mut register_01: Register_0x01 = Register_0x01(reg_01);
register_01.set_woc_diff(enable);
let mut write_buffer = register_01.0.to_be_bytes();
let status = self.write_register(REGISTER_01, &mut write_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
Ok(())
}
pub async fn get_woc_diff(&mut self) -> Result <bool, Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(REGISTER_01, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_01: u16 = u16::from_be_bytes(read_buffer);
let register_01: Register_0x01 = Register_0x01(reg_01);
Ok(register_01.get_woc_diff())
}
async fn get_measurements(&mut self, x: bool, y: bool, z: bool, t: bool) -> Result<RawMeasurements, Error<E>> {
debug!(" in get_measurements");
let x_axis: u8 = if (x) {1} else {0};
let y_axis: u8 = if (y) {1} else {0};
let z_axis: u8 = if (z) {1} else {0};
let temp: u8 = if (t) {1} else {0};
let data_length: u8 = ( temp* 2 + x_axis * 2 + y_axis * 2 + z_axis * 2) + 1;
let mut command_buffer = [0u8; 1];
command_buffer[0] = Read_Measurement | (z_axis << 3) | (y_axis << 2) |
(x_axis << 1) | (temp);
if data_length == 3 {
debug!(" reading 3 bytes");
let mut read_buffer = [0u8; 3];
self.i2c
.write_read(self.address as u8, &command_buffer, &mut read_buffer).await
.map_err(Error::I2c)?;
let status = Status(read_buffer[0]);
if status.get_error() {
return Err(Error::CommandError);
}
let measurements = self.unpack_txyz(&mut read_buffer, 3, x_axis, y_axis, z_axis, temp)?;
return Ok(measurements);
} else if data_length == 5 {
debug!(" reading 5 bytes");
let mut read_buffer = [0u8; 5];
self.i2c
.write_read(self.address as u8, &command_buffer, &mut read_buffer).await
.map_err(Error::I2c)?;
let status = Status(read_buffer[0]);
if status.get_error() {
return Err(Error::CommandError);
}
let measurements = self.unpack_txyz(&mut read_buffer, 5, x_axis, y_axis, z_axis, temp)?;
return Ok(measurements);
} else if data_length == 7 {
debug!(" reading 7 bytes");
let mut read_buffer = [0u8; 7];
self.i2c
.write_read(self.address as u8, &command_buffer,&mut read_buffer).await
.map_err(Error::I2c)?;
let status = Status(read_buffer[0]);
if status.get_error() {
return Err(Error::CommandError);
}
let measurements = self.unpack_txyz(&mut read_buffer, 7, x_axis, y_axis, z_axis, temp)?;
return Ok(measurements);
} else if data_length == 9 {
debug!(" reading 9 bytes");
let mut read_buffer = [0u8; 9];
self.i2c
.write_read(self.address as u8, &command_buffer, &mut read_buffer).await
.map_err(Error::I2c)?;
let status = Status(read_buffer[0]);
if status.get_error() {
return Err(Error::CommandError);
}
let measurements = self.unpack_txyz(&mut read_buffer, 9, x_axis, y_axis, z_axis, temp)?;
return Ok(measurements);
} else {
warn!("get_measurements invalid read length, can only happen if all t,x,y,z properties are zero");
return Err(Error::CommandError);
}
}
fn unpack_txyz(&mut self, read_buffer: &mut [u8], length: u8, x: u8, y: u8, z: u8, t:u8) -> Result<RawMeasurements, Error<E>> {
let mut temp_data: u16 = 0;
let mut x_data: u16 = 0;
let mut y_data: u16 = 0;
let mut z_data: u16 = 0;
let data_length: u8 = (t * 2 + x * 2 + y * 2 + z * 2) + 1;
let mut pos: usize = 1;
if t == 1 {
if pos+2 > length as usize {
return Err(Error::OutOfRange(pos as u8 +2))
}
temp_data = u16::from_be_bytes([read_buffer[pos], read_buffer[pos+1]]);
pos = pos + 2;
}
if (x == 1) {
if (pos+2 > length as usize) {
return Err(Error::OutOfRange(pos as u8+2))
}
x_data = u16::from_be_bytes([read_buffer[pos], read_buffer[pos+1]]);
pos = pos + 2;
}
if (y == 1) {
if (pos+2 > length as usize) {
return Err(Error::OutOfRange(pos as u8+2))
}
y_data = u16::from_be_bytes([read_buffer[pos], read_buffer[pos+1]]);
pos = pos + 2;
}
if (z == 1) {
if (pos+2 > length as usize) {
return Err(Error::OutOfRange(pos as u8+2))
}
z_data = u16::from_be_bytes([read_buffer[pos], read_buffer[pos+1]]);
}
Ok(RawMeasurements { t: temp_data, x: x_data, y: y_data, z: z_data })
}
pub async fn read_data_by_txyz_params(&mut self, t: bool, x: bool, y: bool, z: bool) -> Result<ScaledMeasurements, Error<E>> {
if (self.measurement_mode== MeasurementMode::Single) {
self.start_measurement_mode(MeasurementMode::Single, t, x, y, z).await?;
} else {
debug!(" read_data_by_txyz_params, not Single mode");
}
match &mut self.drdy_input {
None => {
debug!(" delaying {:#} msec", self.conversion_time);
self.delayer.delay_ms(self.conversion_time).await;
},
Some(pin) => {
debug!(" drdy input pin set, expecting TRIG_INT pin high when ready");
let mut counter = 0;
while ( pin.is_high().unwrap() == false) {
self.delayer.delay_ms(2).await;
counter = counter + 1;
if (self.measurement_mode == MeasurementMode::WakeOnChange) {
if (counter > 600000) { return Err(Error::MeasurementTimeout);
}
} else {
if counter > 2000 { return Err(Error::MeasurementTimeout); }
}
}
debug!(" input pin is high, after {counter} msec");
}
}
let raw_measurements = self.get_measurements(x, y, z, t).await?;
info!(" got raw_measurements = {:#?}", raw_measurements);
let scaled_measurements = self.scale_raw_measurements(raw_measurements);
return Ok(scaled_measurements);
}
pub async fn read_data_by_txyz_props(&mut self, mode: MeasurementMode) -> Result<ScaledMeasurements, Error<E>> {
debug!("in read_data_by_props");
if (mode == MeasurementMode::Single) {
self.start_meaasurement_mode_by_props(MeasurementMode::Single).await?;
}
match &mut self.drdy_input {
None => {
debug!(" delaying {:#} msec", self.conversion_time);
self.delayer.delay_ms(self.conversion_time).await
},
Some(pin) => {
let mut counter = 0;
debug!(" drdy input pin set, expecting TRIG_INT high when ready");
while ( pin.is_high().unwrap() == false) {
self.delayer.delay_ms(2).await;
counter = counter + 1;
if counter > 2000 { return Err(Error::MeasurementTimeout); }
}
}
}
let x_axis: bool = if (self.x == 1) {true} else {false};
let y_axis: bool = if (self.y == 1) {true} else {false};
let z_axis: bool = if (self.z == 1) {true} else {false};
let temp: bool = if (self.t == 1) {true} else {false};
let raw_measurements = self.get_measurements(x_axis, y_axis, z_axis, temp).await?;
info!(" got raw_measurements = {:#?}", raw_measurements);
let scaled_measurements = self.scale_raw_measurements(raw_measurements);
return Ok(scaled_measurements);
}
fn scale_raw_measurements(&mut self, raw_measurements: RawMeasurements) -> ScaledMeasurements {
let mut scaled_measurements: ScaledMeasurements = ScaledMeasurements { t: 0.0, x: 0.0, y: 0.0, z: 0.0 };
const HALLCONF_0X0: f32 = 98.0/75.0;
const HALLCONF_0XC: f32 = 1.0;
let mut xy_scaler: f32 = 0.0;
let mut z_scaler: f32 = 0.0;
if self.hall_conf == 0x0 {
xy_scaler = HALLCONF_0X0;
z_scaler = HALLCONF_0X0;
} else if self.hall_conf == 0xc {
xy_scaler = HALLCONF_0XC;
z_scaler = HALLCONF_0XC;
}
let gain_sel_xy_mult: f32 = SENS_XY[self.gain_sel as usize & 0x07];
let gain_sel_z_mult : f32 = SENS_Z[self.gain_sel as usize & 0x07];
if self.tcmp_enabled {
scaled_measurements.x = ((f32::from(raw_measurements.x) ) - 32768.0) as f32 * xy_scaler * gain_sel_xy_mult *
((1 << self.resolution.x) as f32);
} else {
if self.resolution.x == 0x00 || self.resolution.x == 0x01 {
scaled_measurements.x = (f32::from(raw_measurements.x) ) * xy_scaler * gain_sel_xy_mult * ((1 << self.resolution.x) as f32);
} else if (self.resolution.x == 0x02) {
scaled_measurements.x = ((f32::from(raw_measurements.x) ) - 32768.0) as f32 * xy_scaler * gain_sel_xy_mult *
((1 << self.resolution.x) as f32);
} else if (self.resolution.x == 0x03) {
scaled_measurements.x = ((f32::from(raw_measurements.x) ) - 16384.0) as f32 * xy_scaler * gain_sel_xy_mult *
((1 << self.resolution.x) as f32);
}
}
if self.tcmp_enabled {
scaled_measurements.y = ((f32::from(raw_measurements.y) ) - 32768.0) as f32 * xy_scaler * gain_sel_xy_mult *
((1 << self.resolution.y) as f32);
} else {
if self.resolution.y == 0x00 || self.resolution.y == 0x01 {
scaled_measurements.y = (f32::from(raw_measurements.y) ) * xy_scaler * gain_sel_xy_mult * ((1 << self.resolution.y) as f32);
} else if (self.resolution.y == 0x02) {
scaled_measurements.y = ((f32::from(raw_measurements.y) ) - 32768.0) as f32 * xy_scaler * gain_sel_xy_mult *
((1 << self.resolution.y) as f32);
} else if (self.resolution.y == 0x03) {
scaled_measurements.y = ((f32::from(raw_measurements.y) ) - 16384.0) as f32 * xy_scaler * gain_sel_xy_mult *
((1 << self.resolution.y) as f32);
}
}
if self.tcmp_enabled {
scaled_measurements.z = ((f32::from(raw_measurements.z) ) - 32768.0) as f32 * z_scaler * gain_sel_z_mult *
((1 << self.resolution.z) as f32);
} else {
if self.resolution.z == 0x00 || self.resolution.z == 0x01 {
scaled_measurements.z = (f32::from(raw_measurements.z) ) * z_scaler * gain_sel_z_mult * ((1 << self.resolution.z) as f32);
} else if (self.resolution.z == 0x02) {
scaled_measurements.z = ((f32::from(raw_measurements.z) ) - 32768.0) as f32 * z_scaler * gain_sel_z_mult *
((1 << self.resolution.y) as f32);
} else if (self.resolution.z == 0x03) {
scaled_measurements.z = ((f32::from(raw_measurements.z) ) - 16384.0) as f32 * z_scaler * gain_sel_z_mult *
((1 << self.resolution.z) as f32);
}
}
scaled_measurements.t = 25.0 + (f32::from(raw_measurements.t) - 46244.0) / 45.2;
return scaled_measurements;
}
async fn calc_conversion_time(&mut self) -> Result<(), Error<E>> {
let mut read_buffer = [0u8, 2];
let status = self.read_register(DIGITAL_FILTER_REGISTER, &mut read_buffer).await?;
if status.get_error() {
return Err(Error::CommandError);
}
let reg_02:u16 = u16::from_be_bytes(read_buffer);
let register_02: Register_0x02 = Register_0x02(reg_02);
let osr = register_02.get_OSR();
let osr2 = register_02.get_OSR2();
let dig_filt = register_02.get_dig_filt();
let conversion_time = (3.0 * (2.0 + (1 << dig_filt) as f32) * ((1 << osr) as f32) * 0.064f32 +
((1 << osr2) as f32) * 0.192f32 ) * 1.3f32;
if conversion_time < 1.0f32 {
self.conversion_time = 1;
} else {
self.conversion_time = roundf(conversion_time) as u32;
}
debug!(" conversion time = {:#}", self.conversion_time);
Ok(())
}
}