use core::default::Default;
use core::marker::PhantomData;
use super::types;
#[derive(Copy, Clone, Debug)]
pub enum GyroTempDataRate {
FChoice0, FChoice1, DlpfConf(Dlpf),
}
impl Default for GyroTempDataRate {
fn default() -> Self {
GyroTempDataRate::DlpfConf(Dlpf::default())
}
}
impl GyroTempDataRate {
pub(crate) fn fchoice_b_bits(&self) -> u8 {
match self {
GyroTempDataRate::FChoice0 => 0b01,
GyroTempDataRate::FChoice1 => 0b10,
GyroTempDataRate::DlpfConf(_) => 0b00,
}
}
pub(crate) fn dlpf_bits(&self) -> u8 {
match self {
GyroTempDataRate::FChoice0 => 0b000,
GyroTempDataRate::FChoice1 => 0b000,
GyroTempDataRate::DlpfConf(dlpf) => *dlpf as u8,
}
}
}
#[derive(Copy, Clone, Debug)]
pub enum AccelDataRate {
FChoice0, DlpfConf(Dlpf),
}
impl Default for AccelDataRate {
fn default() -> Self {
AccelDataRate::DlpfConf(Dlpf::default())
}
}
impl AccelDataRate {
pub(crate) fn accel_config_bits(&self) -> u8 {
match self {
AccelDataRate::FChoice0 => 0b00001000,
AccelDataRate::DlpfConf(dlpf) => 0b01000000 | (*dlpf as u8),
}
}
}
#[derive(Copy, Clone, Debug)]
pub enum Dlpf {
_0 = 0,
_1 = 1,
_2 = 2,
_3 = 3,
_4 = 4,
_5 = 5,
_6 = 6,
_7 = 7,
}
impl Default for Dlpf {
fn default() -> Self {
Dlpf::_0
}
}
#[allow(non_camel_case_types)]
#[derive(Copy, Clone, Debug)]
pub enum GyroScale {
_250DPS = 0,
_500DPS,
_1000DPS,
_2000DPS,
}
impl GyroScale {
pub(crate) fn resolution(&self) -> f32 {
match self {
GyroScale::_250DPS => 250.0 / 32768.0,
GyroScale::_500DPS => 500.0 / 32768.0,
GyroScale::_1000DPS => 1000.0 / 32768.0,
GyroScale::_2000DPS => 2000.0 / 32768.0,
}
}
}
impl Default for GyroScale {
fn default() -> Self {
GyroScale::_250DPS
}
}
#[allow(non_camel_case_types)]
#[derive(Copy, Clone, Debug)]
pub enum AccelScale {
_2G = 0,
_4G,
_8G,
_16G,
}
impl AccelScale {
pub(crate) fn resolution(&self) -> f32 {
match self {
AccelScale::_2G => 2.0 / 32768.0,
AccelScale::_4G => 4.0 / 32768.0,
AccelScale::_8G => 8.0 / 32768.0,
AccelScale::_16G => 16.0 / 32768.0,
}
}
}
impl Default for AccelScale {
fn default() -> Self {
AccelScale::_2G
}
}
#[allow(non_camel_case_types)]
#[derive(Copy, Clone, Debug)]
pub enum MagScale {
_14BITS = 0,
_16BITS,
}
impl MagScale {
pub(crate) fn resolution(&self) -> f32 {
match self {
MagScale::_14BITS => 10. * 4912. / 8192.,
MagScale::_16BITS => 10. * 4912. / 32768.0,
}
}
}
impl Default for MagScale {
fn default() -> Self {
MagScale::_14BITS
}
}
#[derive(Copy, Clone, Debug)]
pub enum DmpRate {
_200Hz = 0,
_100Hz = 1,
_50Hz = 3,
_40Hz = 4,
_25Hz = 8,
_20Hz = 9,
_10Hz = 19,
_8Hz = 24,
_5Hz = 39,
_4Hz = 49,
}
impl Default for DmpRate {
fn default() -> Self {
DmpRate::_100Hz
}
}
#[derive(Copy, Clone, Debug)]
pub enum Orientation {
ZUp = 0x088,
ZDown = 0x18c,
XUp = 0x00e,
XDown = 0x10a,
YUp = 0x070,
YDown = 0x150,
XForward = 0x085,
XBackward = 0x0a1,
}
#[cfg(feature = "dmp")]
impl Orientation {
pub(crate) fn gyro_axes(&self) -> [u8; 3] {
const AXES: [u8; 3] = [0x4c, 0xcd, 0x6c];
[AXES[*self as usize & 3],
AXES[(*self as usize >> 3) & 3],
AXES[(*self as usize >> 6) & 3]]
}
pub(crate) fn accel_axes(&self) -> [u8; 3] {
const AXES: [u8; 3] = [0x0c, 0xc9, 0x2c];
[AXES[*self as usize & 3],
AXES[(*self as usize >> 3) & 3],
AXES[(*self as usize >> 6) & 3]]
}
pub(crate) fn gyro_signs(&self) -> [u8; 3] {
let mut sign: [u8; 3] = [0x36, 0x56, 0x76];
if *self as u16 & 0x002 != 0 {
sign[0] |= 1;
}
if *self as u16 & 0x020 != 0 {
sign[1] |= 1;
}
if *self as u16 & 0x100 != 0 {
sign[2] |= 1;
}
sign
}
pub(crate) fn accel_signs(&self) -> [u8; 3] {
let mut sign: [u8; 3] = [0x26, 0x46, 0x66];
if *self as u16 & 0x002 != 0 {
sign[0] |= 1;
}
if *self as u16 & 0x020 != 0 {
sign[1] |= 1;
}
if *self as u16 & 0x100 != 0 {
sign[2] |= 1;
}
sign
}
}
impl Default for Orientation {
fn default() -> Self {
Orientation::ZUp
}
}
#[derive(Copy, Clone, Debug)]
pub struct DmpFeatures {
pub raw_gyro: bool,
pub raw_accel: bool,
pub tap: bool,
pub android_orient: bool,
pub quat6: bool,
pub quat: bool,
pub gyro_auto_calibrate: bool,
}
#[cfg(feature = "dmp")]
impl DmpFeatures {
pub(crate) fn packet_size(self) -> usize {
let mut size = 0;
if self.raw_gyro {
size += 6
}
if self.raw_accel {
size += 6
}
if self.quat | self.quat6 {
size += 16
}
if self.android_orient | self.tap {
size += 4
}
size
}
}
impl Default for DmpFeatures {
fn default() -> Self {
DmpFeatures { raw_gyro: true,
raw_accel: true,
tap: false,
android_orient: false,
quat6: true,
quat: false,
gyro_auto_calibrate: false }
}
}
#[derive(Copy, Clone, Debug)]
pub struct DmpConfiguration {
pub(crate) orientation: Orientation,
pub(crate) features: DmpFeatures,
pub(crate) rate: DmpRate,
}
impl Default for DmpConfiguration {
fn default() -> Self {
DmpConfiguration { orientation: Default::default(),
features: Default::default(),
rate: Default::default() }
}
}
#[derive(Copy, Clone, Debug)]
pub struct MpuConfig<MODE> {
pub(crate) gyro_scale: Option<GyroScale>,
pub(crate) accel_scale: Option<AccelScale>,
pub(crate) mag_scale: Option<MagScale>,
pub(crate) accel_data_rate: Option<AccelDataRate>,
pub(crate) gyro_temp_data_rate: Option<GyroTempDataRate>,
pub(crate) sample_rate_divisor: Option<u8>,
pub(crate) dmp_configuration: Option<DmpConfiguration>,
_mode: PhantomData<MODE>,
}
impl MpuConfig<types::Imu> {
pub fn imu() -> Self {
MpuConfig { gyro_scale: None,
accel_scale: None,
mag_scale: None,
accel_data_rate: None,
gyro_temp_data_rate: None,
sample_rate_divisor: None,
dmp_configuration: None,
_mode: PhantomData }
}
}
impl MpuConfig<types::Marg> {
pub fn marg() -> Self {
MpuConfig { gyro_scale: None,
accel_scale: None,
mag_scale: None,
accel_data_rate: None,
gyro_temp_data_rate: None,
sample_rate_divisor: None,
dmp_configuration: None,
_mode: PhantomData }
}
}
#[cfg(feature = "dmp")]
impl MpuConfig<types::Dmp> {
pub fn dmp() -> Self {
MpuConfig { gyro_scale: Some(GyroScale::_2000DPS),
accel_scale: Some(AccelScale::_8G),
mag_scale: None,
accel_data_rate: Some(AccelDataRate::DlpfConf(Dlpf::_1)),
gyro_temp_data_rate:
Some(GyroTempDataRate::DlpfConf(Dlpf::_1)),
sample_rate_divisor: Some(4),
dmp_configuration: None,
_mode: PhantomData }
}
pub fn dmp_rate(&mut self, rate: DmpRate) -> &mut Self {
match self.dmp_configuration.as_mut() {
Some(mut x) => x.rate = rate,
None => {
self.dmp_configuration =
Some(DmpConfiguration { rate,
..Default::default() })
},
}
self
}
pub fn dmp_orientation(&mut self, orientation: Orientation) -> &mut Self {
match self.dmp_configuration.as_mut() {
Some(mut x) => x.orientation = orientation,
None => {
self.dmp_configuration =
Some(DmpConfiguration { orientation,
..Default::default() })
},
}
self
}
pub fn dmp_features_raw_gyro(&mut self, feature: bool) -> &mut Self {
match self.dmp_configuration.as_mut() {
Some(mut x) => x.features.raw_gyro = feature,
None => self.dmp_configuration =
Some(DmpConfiguration { features:
DmpFeatures { raw_gyro: feature,
..Default::default() },
..Default::default() }),
}
self
}
pub fn dmp_features_raw_accel(&mut self, feature: bool) -> &mut Self {
match self.dmp_configuration.as_mut() {
Some(mut x) => x.features.raw_accel = feature,
None => self.dmp_configuration =
Some(DmpConfiguration { features:
DmpFeatures { raw_accel: feature,
..Default::default() },
..Default::default() }),
}
self
}
pub fn dmp_features_tap(&mut self, feature: bool) -> &mut Self {
match self.dmp_configuration.as_mut() {
Some(mut x) => x.features.tap = feature,
None => self.dmp_configuration =
Some(DmpConfiguration { features:
DmpFeatures { tap: feature,
..Default::default() },
..Default::default() }),
}
self
}
pub fn dmp_features_quat(&mut self, feature: bool) -> &mut Self {
match self.dmp_configuration.as_mut() {
Some(mut x) => x.features.quat = feature,
None => self.dmp_configuration =
Some(DmpConfiguration { features:
DmpFeatures { quat: feature,
..Default::default() },
..Default::default() }),
}
self
}
pub fn dmp_features_quat6(&mut self, feature: bool) -> &mut Self {
match self.dmp_configuration.as_mut() {
Some(mut x) => x.features.quat6 = feature,
None => self.dmp_configuration =
Some(DmpConfiguration { features:
DmpFeatures { quat6: feature,
..Default::default() },
..Default::default() }),
}
self
}
pub fn dmp_features_gyro_auto_calibrate(&mut self,
feature: bool)
-> &mut Self {
match self.dmp_configuration.as_mut() {
Some(mut x) => x.features.gyro_auto_calibrate = feature,
None => self.dmp_configuration =
Some(DmpConfiguration { features:
DmpFeatures { gyro_auto_calibrate:
feature,
..Default::default() },
..Default::default() }),
}
self
}
pub fn dmp_features_orientation(&mut self, feature: bool) -> &mut Self {
match self.dmp_configuration.as_mut() {
Some(mut x) => x.features.android_orient = feature,
None => self.dmp_configuration =
Some(DmpConfiguration { features:
DmpFeatures { android_orient:
feature,
..Default::default() },
..Default::default() }),
}
self
}
}
impl<MODE> MpuConfig<MODE> {
pub fn gyro_scale(&mut self, scale: GyroScale) -> &mut Self {
self.gyro_scale = Some(scale);
self
}
pub fn accel_scale(&mut self, scale: AccelScale) -> &mut Self {
self.accel_scale = Some(scale);
self
}
pub fn accel_data_rate(&mut self, data_rate: AccelDataRate) -> &mut Self {
self.accel_data_rate = Some(data_rate);
self
}
pub fn gyro_temp_data_rate(&mut self,
data_rate: GyroTempDataRate)
-> &mut Self {
self.gyro_temp_data_rate = Some(data_rate);
self
}
pub fn sample_rate_divisor(&mut self, smplrt_div: u8) -> &mut Self {
self.sample_rate_divisor = Some(smplrt_div);
self
}
}
impl MpuConfig<types::Marg> {
pub fn mag_scale(&mut self, scale: MagScale) -> &mut Self {
self.mag_scale = Some(scale);
self
}
}
bitflags! {
#[allow(non_camel_case_types)]
pub struct InterruptEnable: u8 {
const WOM_EN = 0b0100_0000;
const FIFO_OVERFLOW_EN = 0b0001_0000;
const FSYNC_INT_EN = 0b0000_1000;
const RAW_RDY_EN = 0b0000_0001;
}
}
bitflags! {
#[allow(non_camel_case_types)]
pub struct InterruptConfig: u8 {
const ACL = 0b1000_0000;
const OPEN = 0b0100_0000;
const LATCH_INT_EN = 0b0010_0000;
const INT_ANYRD_CLEAR = 0b0001_0000;
const ACTL_FSYNC = 0b0000_1000;
const FSYNC_INT_MODE_EN = 0b0000_0100;
const BYPASS_EN = 0b0000_0010;
}
}