bmx055 0.1.0

use embedded_hal::blocking::delay::DelayMs;

use crate::{
    interface::{I2cInterface, ReadData, SpiInterface, WriteData},
    mode,
    register_address::{
        BgwSoftresetA, BgwSoftresetG, BgwSpi3WdtG, BwG, IntEn0G, IntEn1A, IntStatus1A, IntStatus1G,
        OpmOdrStM, PmuBwA, PmuRangeA, PwrCntl1M, RangeG, WhoAmIA, WhoAmIG, WhoAmIM,
    },
    types::{
        AngularRate, GyroStatus, GyroscopeId, MagOpMode, MagneticFieldData, MagnetometerId,
        Temperature, TrimData, TrimX1Y1, TrimXY1XY2, TrimXYZ,
    },
    Acceleration, AccelerometerId, Bmx055, Error, PhantomData, Status,
};

impl<I2C> Bmx055<I2cInterface<I2C>, mode::MagOneShot> {
    /// Create new instance of the BMX055 device communicating through I2C.
    pub fn new_with_i2c(i2c: I2C) -> Self {
        Bmx055 {
            iface: I2cInterface { i2c },
            int_en_1_a: IntEn1A::default(),
            pmu_range_a: PmuRangeA::default(),
            pmu_bw_a: PmuBwA::default(),
            bgw_spi3_wdt_g: BgwSpi3WdtG::default(),
            int_en_0_g: IntEn0G::default(),
            bw_g: BwG::default(),
            range_g: RangeG::default(),
            pwr_cntl_1_m: PwrCntl1M::default(),
            opm_odr_st_m: OpmOdrStM::default(),
            mag_trim_data: None,
            _mag_mode: PhantomData,
        }
    }
}

impl<I2C, MODE> Bmx055<I2cInterface<I2C>, MODE> {
    /// Destroy driver instance, return I2C bus.
    pub fn destroy(self) -> I2C {
        self.iface.i2c
    }
}

impl<SPI, CSXL, CSGYR, CSMAG> Bmx055<SpiInterface<SPI, CSXL, CSGYR, CSMAG>, mode::MagOneShot> {
    /// Create new instance of the BMX055 device communicating through SPI.
    pub fn new_with_spi(
        spi: SPI,
        chip_select_accel: CSXL,
        chip_select_gyro: CSGYR,
        chip_select_mag: CSMAG,
    ) -> Self {
        Bmx055 {
            iface: SpiInterface {
                spi,
                cs_xl: chip_select_accel,
                cs_gyr: chip_select_gyro,
                cs_mag: chip_select_mag,
            },
            int_en_1_a: IntEn1A::default(),
            pmu_range_a: PmuRangeA::default(),
            pmu_bw_a: PmuBwA::default(),
            bgw_spi3_wdt_g: BgwSpi3WdtG::default(),
            int_en_0_g: IntEn0G::default(),
            bw_g: BwG::default(),
            range_g: RangeG::default(),
            pwr_cntl_1_m: PwrCntl1M::default(),
            opm_odr_st_m: OpmOdrStM::default(),
            mag_trim_data: None,
            _mag_mode: PhantomData,
        }
    }
}

impl<SPI, CSXL, CSGYR, CSMAG, MODE> Bmx055<SpiInterface<SPI, CSXL, CSGYR, CSMAG>, MODE> {
    /// Destroy driver instance, return SPI bus instance and chip select pin.
    pub fn destroy(self) -> (SPI, CSXL, CSGYR, CSMAG) {
        (
            self.iface.spi,
            self.iface.cs_xl,
            self.iface.cs_gyr,
            self.iface.cs_mag,
        )
    }
}

impl<DI, CommE, PinE, MODE> Bmx055<DI, MODE>
where
    DI: ReadData<Error = Error<CommE, PinE>> + WriteData<Error = Error<CommE, PinE>>,
{
    /// Initialize registers
    pub fn init<D: DelayMs<u32>>(&mut self, delay: &mut D) -> Result<(), Error<CommE, PinE>> {
        self.acc_perform_soft_reset(delay)?;
        self.acc_enable_new_data_interrupt()?;
        self.gyr_enable_new_data_interrupt()?;
        self.mag_to_sleep_mode(delay)
    }

    /// Perform softreset
    #[inline]
    fn acc_perform_soft_reset<D: DelayMs<u32>>(
        &mut self,
        delay: &mut D,
    ) -> Result<(), Error<CommE, PinE>> {
        let reg = BgwSoftresetA::TRIGGER_SOFTRESET;
        self.iface.write_accel_register(reg)?;
        delay.delay_ms(100u32);
        Ok(())
    }

    /// Enable interrupt for new data for accelerometer.
    #[inline]
    fn acc_enable_new_data_interrupt(&mut self) -> Result<(), Error<CommE, PinE>> {
        let reg = self.int_en_1_a | IntEn1A::DATA_EN;
        self.iface.write_accel_register(reg)?;
        self.int_en_1_a = reg;

        Ok(())
    }

    /// Accelerometer status
    pub fn accel_status(&mut self) -> Result<Status, Error<CommE, PinE>> {
        self.iface
            .read_accel_register::<IntStatus1A>()
            .map(Status::new)
    }

    /// Get measured acceleration.
    pub fn acceleration(&mut self) -> Result<Acceleration, Error<CommE, PinE>> {
        let (x, y, z) = self.iface.read_accel_3_double_registers::<Acceleration>()?;

        Ok(Acceleration {
            x,
            y,
            z,
            mode: self.get_accel_mode(),
            range: self.get_accel_range(),
        })
    }

    /// Get the accelerometer device ID.
    pub fn accelerometer_id(&mut self) -> Result<AccelerometerId, Error<CommE, PinE>> {
        self.iface.read_accel_register::<WhoAmIA>()
    }

    /// Get measured temperature.
    pub fn temperature(&mut self) -> Result<Temperature, Error<CommE, PinE>> {
        self.iface.read_accel_register::<Temperature>()
    }

    /// Perform softreset
    #[allow(dead_code)]
    #[inline]
    fn gyr_perform_soft_reset<D: DelayMs<u32>>(
        &mut self,
        delay: &mut D,
    ) -> Result<(), Error<CommE, PinE>> {
        let gyro_reg = BgwSoftresetG::TRIGGER_SOFTRESET;
        self.iface.write_gyro_register(gyro_reg)?;
        delay.delay_ms(100u32);
        Ok(())
    }

    /// Set watchdog timer to 50ms
    #[inline]
    fn gyr_enable_wdt(&mut self) -> Result<(), Error<CommE, PinE>> {
        let reg = self.bgw_spi3_wdt_g | BgwSpi3WdtG::IC2_WDT_EN | BgwSpi3WdtG::IC2_WDT_SEL;
        self.iface.write_gyro_register(reg)?;
        self.bgw_spi3_wdt_g = reg;

        Ok(())
    }

    /// Enable interrupt for new data for gyroscope.
    #[inline]
    fn gyr_enable_new_data_interrupt(&mut self) -> Result<(), Error<CommE, PinE>> {
        let reg = self.int_en_0_g | IntEn0G::DATA_EN;
        self.iface.write_gyro_register(reg)?;
        self.int_en_0_g = reg;

        Ok(())
    }

    /// Gyroscope status
    pub fn gyro_status(&mut self) -> Result<GyroStatus, Error<CommE, PinE>> {
        self.iface
            .read_gyro_register::<IntStatus1G>()
            .map(GyroStatus::new)
    }

    /// Get measured angular rate.
    pub fn angular_rate(&mut self) -> Result<AngularRate, Error<CommE, PinE>> {
        let (x, y, z) = self.iface.read_gyro_3_double_registers::<AngularRate>()?;

        Ok(AngularRate {
            x,
            y,
            z,
            mode: self.get_gyro_mode(),
            range: self.get_gyro_range(),
        })
    }

    /// Get the gyroscope device ID.
    pub fn gyroscope_id(&mut self) -> Result<GyroscopeId, Error<CommE, PinE>> {
        self.iface.read_gyro_register::<WhoAmIG>()
    }

    /// Get the magnetometer device ID.
    pub fn magnetometer_id(&mut self) -> Result<MagnetometerId, Error<CommE, PinE>> {
        self.iface.read_mag_register::<WhoAmIM>()
    }

    /// Read magnetometer trim data
    pub fn mag_trim_data(&mut self) -> Result<TrimData, Error<CommE, PinE>> {
        let trim_x1y1 = self.iface.read_mag_2_registers::<TrimX1Y1>()?;
        let trim_xyz = self.iface.read_mag_4_registers::<TrimXYZ>()?;
        let trim_xy1xy2 = self.iface.read_mag_10_registers::<TrimXY1XY2>()?;
        Ok((trim_x1y1, trim_xyz, trim_xy1xy2).into())
    }
}

impl<DI, CommE, PinE> Bmx055<DI, mode::MagOneShot>
where
    DI: ReadData<Error = Error<CommE, PinE>> + WriteData<Error = Error<CommE, PinE>>,
{
    /// Get measured magnetic field data
    pub fn magnetic_field_data(&mut self) -> Result<MagneticFieldData, Error<CommE, PinE>> {
        if self.mag_trim_data.is_none() {
            let trim_data = self.mag_trim_data()?;
            self.mag_trim_data.replace(trim_data);
        }
        let trim_data = self.mag_trim_data.unwrap();
        self.mag_to_op_mode(MagOpMode::Forced)?;
        let (x, y, z, rhall) = self
            .iface
            .read_mag_4_double_registers::<MagneticFieldData>()?;

        Ok(MagneticFieldData {
            x,
            y,
            z,
            rhall,
            trim_data,
        })
    }
}