use crate::{
types::{
AccelBandwidth, AccelerometerId, GyroFilterBandwidth, GyroRange, GyroStatusFlags,
GyroscopeId, MagOpMode, MagnetometerId, StatusFlags,
},
AccelRange,
};
pub trait RegRead<D = u8> {
type Output;
const ADDR: u8;
fn from_data(data: D) -> Self::Output;
}
pub trait RegWrite<D = u8>: RegRead<D> {
fn data(&self) -> D;
}
macro_rules! register {
(@impl_reg_read $ty:ident, $addr:literal, $output:ident) => {
impl RegRead for $ty {
type Output = $output;
const ADDR: u8 = $addr;
fn from_data(data: u8) -> Self::Output {
Self::Output::from_bits_truncate(data)
}
}
};
(@impl_reg_write $ty:ident, $addr:literal, $output:ident) => {
register!(@impl_reg_read $ty, $addr, Self);
impl RegWrite for $ty {
fn data(&self) -> u8 {
self.bits()
}
}
};
(
#[doc = $name:expr]
$(#[$meta:meta])*
$vis:vis type $ty:ident: $addr:literal = $ty2:ident;
) => {
#[doc = concat!($name, " register (`", stringify!($addr), "`)")]
$(#[$meta])*
$vis enum $ty {}
register!(@impl_reg_read $ty, $addr, $ty2);
};
(
#[doc = $name:expr]
$(#[$meta:meta])*
$vis:vis struct $ty:ident: $addr:literal {
$(const $bit_name:ident = $bit_val:expr;)*
}
) => {
::bitflags::bitflags! {
#[doc = concat!($name, " register (`", stringify!($addr), "`)")]
$(#[$meta])*
$vis struct $ty: u8 {
$(const $bit_name = $bit_val;)*
}
}
register!(@impl_reg_write $ty, $addr, Self);
};
}
register! {
pub type WhoAmIA: 0x00 = AccelerometerId;
}
impl WhoAmIA {
pub(crate) const ID: u8 = 0b1111_1010;
}
register! {
#[derive(Debug, Copy, Clone)]
pub struct BgwSoftresetA: 0x14 {
const TRIGGER_SOFTRESET = 0xb6;
}
}
register! {
#[derive(Debug, Default, Copy, Clone)]
pub struct IntEn1A: 0x17 {
const INT_FWM_EN = 0b01000000;
const INT_FFULL_EN = 0b00100000;
const DATA_EN = 0b00010000;
const LOW_EN = 0b00001000;
const HIGH_EN_Z = 0b00000100;
const HIGH_EN_Y = 0b00000010;
const HIGH_EN_X = 0b00000001;
}
}
register! {
#[derive(Debug, Default, Copy, Clone)]
pub struct PmuRangeA: 0x10 {
const RANGE3 = 0b00001000;
const RANGE2 = 0b00000100;
const RANGE1 = 0b00000010;
const RANGE0 = 0b00000001;
const RANGE = Self::RANGE3.bits() | Self::RANGE2.bits() | Self::RANGE1.bits()| Self::RANGE0.bits();
}
}
impl PmuRangeA {
pub const fn range(&self) -> AccelRange {
match self.intersection(Self::RANGE).bits() {
0b0011 => AccelRange::G2,
0b0101 => AccelRange::G4,
0b1000 => AccelRange::G8,
_ => AccelRange::G16,
}
}
pub const fn with_range(self, range: AccelRange) -> Self {
let reg = self.difference(Self::RANGE);
match range {
AccelRange::G2 => reg.union(Self::RANGE1).union(Self::RANGE0),
AccelRange::G4 => reg.union(Self::RANGE2).union(Self::RANGE0),
AccelRange::G8 => reg.union(Self::RANGE3),
AccelRange::G16 => reg.union(Self::RANGE3).union(Self::RANGE2),
}
}
}
register! {
#[derive(Debug, Default, Copy, Clone)]
pub struct PmuBwA: 0x10 {
const BW4 = 0b00010000;
const BW3 = 0b00001000;
const BW2 = 0b00000100;
const BW1 = 0b00000010;
const BW0 = 0b00000001;
const BW = Self::BW4.bits() | Self::BW3.bits() | Self::BW2.bits() | Self::BW1.bits()| Self::BW0.bits();
}
}
impl PmuBwA {
pub const fn with_bw(self, odr: AccelBandwidth) -> Self {
let reg = self.difference(Self::BW);
match odr {
AccelBandwidth::Hz7_81 => reg.union(Self::BW3),
AccelBandwidth::Hz15_63 => reg.union(Self::BW3).union(Self::BW0),
AccelBandwidth::Hz31_25 => reg.union(Self::BW3).union(Self::BW1),
AccelBandwidth::Hz62_5 => reg.union(Self::BW3).union(Self::BW1).union(Self::BW0),
AccelBandwidth::Hz125 => reg.union(Self::BW3).union(Self::BW2),
AccelBandwidth::Hz250 => reg.union(Self::BW3).union(Self::BW2).union(Self::BW0),
AccelBandwidth::Hz500 => reg.union(Self::BW3).union(Self::BW2).union(Self::BW1),
AccelBandwidth::Hz1000 => reg
.union(Self::BW3)
.union(Self::BW2)
.union(Self::BW1)
.union(Self::BW0),
}
}
}
register! {
#[derive(Debug, Copy, Clone)]
pub type IntStatus1A: 0x0a = StatusFlags;
}
register! {
#[derive(Debug, Copy, Clone)]
pub struct BgwSoftresetG: 0x14 {
const TRIGGER_SOFTRESET = 0xb6;
}
}
register! {
#[derive(Debug, Default, Copy, Clone)]
pub struct IntEn0G: 0x15 {
const DATA_EN = 0b10000000;
const FIFO_EN = 0b01000000;
const AUTO_OFFSET_EN = 0b00000010;
}
}
register! {
#[derive(Debug, Default, Copy, Clone)]
pub struct BgwSpi3WdtG: 0x34 {
const EXT_FIFO_S_EN = 0b00100000;
const EXT_FIFO_S_SEL = 0b00010000;
const IC2_WDT_EN = 0b00000100;
const IC2_WDT_SEL = 0b00000010;
const SPI3 = 0b00000001;
}
}
register! {
#[derive(Debug, Default, Copy, Clone)]
pub struct Lpm1G: 0x11 {
const SUSPEND = 0b10000000;
const DEEP_SUSPEND = 0b00100000;
const SLEEP_DUR_2 = 0b00001000;
const SLEEP_DUR_1 = 0b00000100;
const SLEEP_DUR_0 = 0b00000010;
}
}
register! {
#[derive(Debug, Default, Copy, Clone)]
pub struct RangeG: 0x10 {
const RANGE2 = 0b00000100;
const RANGE1 = 0b00000010;
const RANGE0 = 0b00000001;
const ZERO = 0b00000000;
const RANGE = Self::RANGE2.bits() | Self::RANGE1.bits()| Self::RANGE0.bits();
}
}
impl RangeG {
pub const fn range(&self) -> GyroRange {
match self.intersection(Self::RANGE).bits() {
0b001 => GyroRange::R1000,
0b010 => GyroRange::R500,
0b011 => GyroRange::R250,
0b100 => GyroRange::R125,
_ => GyroRange::R2000,
}
}
pub const fn with_range(self, range: GyroRange) -> Self {
let reg = self.difference(Self::RANGE);
match range {
GyroRange::R2000 => reg.union(Self::ZERO),
GyroRange::R1000 => reg.union(Self::RANGE0),
GyroRange::R500 => reg.union(Self::RANGE1),
GyroRange::R250 => reg.union(Self::RANGE1).union(Self::RANGE0),
GyroRange::R125 => reg.union(Self::RANGE2),
}
}
}
register! {
#[derive(Debug, Default, Copy, Clone)]
pub struct BwG: 0x10 {
const BW3 = 0b00001000;
const BW2 = 0b00000100;
const BW1 = 0b00000010;
const BW0 = 0b00000001;
const ZERO = 0b00000000;
const BW = Self::BW3.bits() | Self::BW2.bits() | Self::BW1.bits()| Self::BW0.bits();
}
}
impl BwG {
pub const fn with_bw(self, bw: GyroFilterBandwidth) -> Self {
let reg = self.difference(Self::BW);
match bw {
GyroFilterBandwidth::Hz32 => reg.union(Self::BW2).union(Self::BW1).union(Self::BW0),
GyroFilterBandwidth::Hz64 => reg.union(Self::BW2).union(Self::BW1),
GyroFilterBandwidth::Hz12 => reg.union(Self::BW2).union(Self::BW0),
GyroFilterBandwidth::Hz23 => reg.union(Self::BW2),
GyroFilterBandwidth::Hz47 => reg.union(Self::BW1).union(Self::BW0),
GyroFilterBandwidth::Hz116 => reg.union(Self::BW1),
GyroFilterBandwidth::Hz230 => reg.union(Self::BW0),
GyroFilterBandwidth::Hz523 => reg.union(Self::ZERO),
}
}
}
register! {
#[derive(Debug, Copy, Clone)]
pub type IntStatus1G: 0x0a = GyroStatusFlags;
}
register! {
pub type WhoAmIG: 0x00 = GyroscopeId;
}
impl WhoAmIG {
pub(crate) const ID: u8 = 0b0000_1111;
}
register! {
pub type WhoAmIM: 0x40 = MagnetometerId;
}
impl WhoAmIM {
pub(crate) const ID: u8 = 0b0011_0010;
}
register! {
#[derive(Debug, Default, Copy, Clone)]
pub struct PwrCntl1M: 0x4b {
const SOFT_RESET_2 = 0b1000_0000;
const SPI3_EN = 0b0000_0100;
const SOFT_RESET_1 = 0b0000_0010;
const POWER_CONTROL = 0b0000_0001;
}
}
register! {
#[derive(Debug, Default, Copy, Clone)]
pub struct OpmOdrStM: 0x4c {
const ADVANCED_SELF_TEST_1 = 0b1000_0000;
const ADVANCED_SELF_TEST_0 = 0b0100_0000;
const ODR_2 = 0b0010_0000;
const ODR_1 = 0b0001_0000;
const ODR_0 = 0b0000_1000;
const OP_MODE_1 = 0b0000_0100;
const OP_MODE_0 = 0b0000_0010;
const SELF_TEST = 0b0000_0001;
const OP_MODE = Self::OP_MODE_1.bits() | Self::OP_MODE_0.bits();
}
}
impl OpmOdrStM {
#[allow(dead_code)]
pub const fn op_mode(&self) -> MagOpMode {
match self.intersection(Self::OP_MODE).bits() {
0b01 => MagOpMode::Forced,
0b11 => MagOpMode::Sleep,
_ => MagOpMode::Normal,
}
}
pub const fn with_op_mode(self, op_mode: MagOpMode) -> Self {
let reg = self.difference(Self::OP_MODE);
match op_mode {
MagOpMode::Normal => reg,
MagOpMode::Forced => reg.union(Self::OP_MODE_0),
MagOpMode::Sleep => reg.union(Self::OP_MODE_0).union(Self::OP_MODE_1),
}
}
}