use crate::registers::Write as RegWrite;
use crate::registers::*;
use crate::Stk8ba58;
use core::{fmt::Debug, marker::Copy};
use accelerometer_hal::Error;
use embedded_hal::i2c::I2c;
impl<I2C, E> RegWrite<I2C, E> for Stk8ba58<I2C>
where
I2C: I2c<Error = E>,
E: Debug,
{
fn set_range(&mut self, range: RangeSel) -> Result<&mut Self, Error<E>> {
self.write_register(Register::RANGESEL, range as u8)?;
self.range = range;
Ok(self)
}
fn set_bandwidth(&mut self, bandwidth: BwSel) -> Result<&mut Self, Error<E>> {
self.write_register(Register::BWSEL, bandwidth as u8)?;
Ok(self)
}
fn set_sleep_duration(&mut self, sleep_duration: SleepDuration) -> Result<&mut Self, Error<E>> {
let mut buf = [0u8];
self.read_register(Register::POWMODE, &mut buf)?;
let mut powmode = buf[0] & 0b111_0000_1; powmode |= sleep_duration as u8;
self.write_register(Register::POWMODE, powmode)?;
Ok(self)
}
fn set_int_latch(&mut self, latch: IntLatch) -> Result<&mut Self, Error<E>> {
let mut buf = [0u8];
self.read_register(Register::INTCFG2, &mut buf)?;
let mut int_latch = buf[0] & 0b1111_0000; int_latch |= latch as u8;
self.write_register(Register::INTCFG2, int_latch)?;
Ok(self)
}
fn set_flags<F: Flags<Bits = u8> + Into<Register> + Copy>(
&mut self,
flags: F,
) -> Result<&mut Self, Error<E>> {
let mut buf = [0u8];
self.read_register(flags.into(), &mut buf)?;
let mut register = buf[0] & F::complement(F::all()).bits(); register |= flags.bits();
self.write_register(flags.into(), register)?;
Ok(self)
}
fn reset_all(&mut self) -> Result<&mut Self, Error<E>> {
self.write_register(Register::SWRST, 0xB6)?;
Ok(self)
}
}