diff --git a/examples/imu.rs b/examples/imu.rs
new file mode 100644
index 0000000000000000000000000000000000000000..78bc15d99144918ae2aa02b45ab1af5b44f0a8a3
--- /dev/null
+++ b/examples/imu.rs
@@ -0,0 +1,140 @@
+//! Interfacing the LSM9DS1 3D accelerometer, gyroscope, and magnetometer
+//! using SPI3.
+//!
+//! Using the SparkFun LSM9DS1 breakout board, connect:
+//! PA_8 -> CS_AG (Chip Select for accelerometer/gyro)
+//! PA_9 -> CS_M (Chip Select for magnetometer)
+//! PB_3 -> SCL (SPI clock)
+//! PB_4 -> SDO_M and SDO_AG (Combined SPI MISO)
+//! PB_5 -> SDA (SPI MOSI)
+#![deny(unsafe_code)]
+#![deny(warnings)]
+#![feature(const_fn)]
+#![feature(proc_macro)]
+#![feature(lang_items)]
+#![no_std]
+
+extern crate cortex_m;
+extern crate cortex_m_rtfm as rtfm;
+#[macro_use]
+extern crate f4;
+extern crate heapless;
+extern crate stm32f40x;
+
+use cortex_m::peripheral::SystClkSource;
+use core::ops::Deref;
+use f4::lsm9ds1::{ImuSettings, Lsm9ds1};
+use f4::Serial;
+use f4::Spi;
+use f4::dma::{Buffer, Dma1Channel6};
+use f4::time::Hertz;
+use f4::clock;
+use rtfm::{app, Threshold};
+
+const BAUD_RATE: Hertz = Hertz(115_200);
+const FREQUENCY: u32 = 10;
+const MAX_TX_LEN: usize = 256;
+
+app! {
+ device: f4::stm32f40x,
+
+ resources: {
+ static IMU_SETTINGS: ImuSettings = ImuSettings::new();
+ static TX_BUFFER: Buffer<[u8; MAX_TX_LEN], Dma1Channel6> = Buffer::new([0; MAX_TX_LEN]);
+ },
+
+ tasks: {
+ SYS_TICK: {
+ path: sys_tick,
+ priority: 1,
+ resources: [TX_BUFFER, IMU_SETTINGS, DMA1, USART2, SPI3, GPIOA],
+ },
+ DMA1_STREAM6: {
+ path: tx_done,
+ priority: 2,
+ resources: [TX_BUFFER, DMA1],
+ },
+ },
+}
+
+fn sys_tick(t: &mut Threshold, mut r: SYS_TICK::Resources) {
+ let spi = Spi(&**r.SPI3);
+ let imu = Lsm9ds1(&**r.SPI3);
+ let g = imu.read_gyro(&spi, r.GPIOA, &r.IMU_SETTINGS);
+ let a = imu.read_acc(&spi, r.GPIOA, &r.IMU_SETTINGS);
+ let m = imu.read_mag(&spi, r.GPIOA, &r.IMU_SETTINGS);
+
+ uprint!(
+ t,
+ r.USART2,
+ r.DMA1,
+ r.TX_BUFFER,
+ "a=({0: <11}, {1: <11}, {2: <11}), g=({3: <11}, {4: <11}, {5: <11}), m=({6: <11}, {7: <11}, {8: <11})\r\n",
+ a.x,a.y,a.z,g.x,g.y,g.z,m.x,m.y,m.z
+ );
+}
+
+fn init(p: init::Peripherals, r: init::Resources) {
+ let clk = clock::set_84_mhz(&p.RCC, &p.FLASH);
+ p.SYST.set_clock_source(SystClkSource::Core);
+ p.SYST.set_reload(clk / FREQUENCY);
+ p.SYST.enable_interrupt();
+ p.SYST.enable_counter();
+
+ // Start the serial port
+ let serial = Serial(p.USART2);
+ serial.init(BAUD_RATE.invert(), Some(p.DMA1), p.GPIOA, p.RCC);
+
+ // Setup CS pins
+ {
+ // Power on GPIOA
+ p.RCC.ahb1enr.modify(|_, w| w.gpioaen().set_bit());
+ // Set PA_8 and PA_9 as outputs
+ p.GPIOA
+ .moder
+ .modify(|_, w| w.moder8().bits(1).moder9().bits(1));
+ // Highest output speed
+ p.GPIOA
+ .ospeedr
+ .modify(|_, w| w.ospeedr8().bits(3).ospeedr9().bits(3));
+ // Default to high (CS disabled)
+ p.GPIOA
+ .odr
+ .modify(|_, w| w.odr8().bit(true).odr9().bit(true));
+ }
+
+ // Init the SPI peripheral
+ let spi = Spi(p.SPI3);
+ spi.init(p.GPIOA, p.GPIOB, p.RCC);
+
+ // For the LSM9DS1, the second clock transition is
+ // the first data capture edge
+ // RM0368 20.5.1
+ p.SPI3.cr1.modify(|_, w| w.cpha().set_bit());
+ spi.enable();
+
+ let imu = Lsm9ds1(p.SPI3);
+ imu.init_gyro(&spi, &p.GPIOA, &r.IMU_SETTINGS);
+ imu.init_acc(&spi, &p.GPIOA, &r.IMU_SETTINGS);
+ imu.init_mag(&spi, &p.GPIOA, &r.IMU_SETTINGS);
+}
+
+// Interrupt for serial transmit DMA
+fn tx_done(t: &mut Threshold, r: DMA1_STREAM6::Resources) {
+ use rtfm::Resource;
+ // We need to unlock the DMA to use it again in the future
+ r.TX_BUFFER.claim(t, |tx, t| {
+ r.DMA1.claim(t, |dma, _| tx.release(dma).unwrap());
+ });
+ // Clear the transmit buffer so we do not retransmit old stuff
+ r.TX_BUFFER.claim_mut(t, |tx, _| {
+ let array = &**tx.deref();
+ array.borrow_mut()[..MAX_TX_LEN].clone_from_slice(&[0; MAX_TX_LEN]);
+ });
+}
+
+fn idle() -> ! {
+ loop {
+ rtfm::wfi();
+ }
+}
diff --git a/examples/spi1.rs b/examples/spi1.rs
index ba39e88cf64d44ca2ffc28cc546a4184ec5927ef..db24b7b50f9155b2e0e65696ff0c71991b3de8ac 100644
--- a/examples/spi1.rs
+++ b/examples/spi1.rs
@@ -82,11 +82,7 @@ fn disable_ag(gpioa: &mut GPIOA) {
fn send_receive(spi: &f4::Spi<f4::stm32f40x::SPI3>, addr: u8) -> u8 {
// Send and receive using the hal crate
while spi.send(addr).is_err() {}
- loop {
- if let Ok(_) = spi.read() {
- break;
- }
- }
+ while spi.read().is_err() {}
while spi.send(0).is_err() {}
loop {
if let Ok(byte) = spi.read() {
diff --git a/src/led.rs b/src/led.rs
index bca68e662ee293ce0ca1672b989afcd22e681c2b..71be9a56ab9dd2e6f049e31319a43e5723ac56dd 100644
--- a/src/led.rs
+++ b/src/led.rs
@@ -31,4 +31,9 @@ impl PA5 {
(*GPIOA.get()).odr.modify(|_, w| w.odr5().bit(false));
}
}
+
+ /// True if LED is ON, false otherwise.
+ pub fn is_on(&self) -> bool {
+ unsafe { (*GPIOA.get()).odr.read().odr5().bit_is_set() }
+ }
}
diff --git a/src/lib.rs b/src/lib.rs
index 96a1ca9e362522fdfd2b39ebda537f588ae75d14..8974f0aef41ae62c2243c86cd5cfa715f8cf945d 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -45,8 +45,9 @@ pub mod pwm;
pub mod capture;
pub mod clock;
pub mod spi;
-
+pub mod lsm9ds1;
pub mod frequency;
+
use frequency::*;
pub use hal::prelude;
@@ -56,6 +57,7 @@ pub use timer::{Channel, Timer};
pub use pwm::Pwm;
pub use capture::Capture;
pub use spi::Spi;
+pub use lsm9ds1::{ImuSettings, Lsm9ds1, Vector3};
/// println over semihosting
#[macro_export]
diff --git a/src/lsm9ds1.rs b/src/lsm9ds1.rs
new file mode 100644
index 0000000000000000000000000000000000000000..ae4aebe7bdcbd9d8fe7d8e69ebf440a54719df04
--- /dev/null
+++ b/src/lsm9ds1.rs
@@ -0,0 +1,695 @@
+//! Interfacing the LSM9DS1 3D accelerometer, gyroscope, and magnetometer
+//! using SPI.
+
+extern crate stm32f40x;
+use stm32f40x::{SPI1, SPI2, SPI3, GPIOA};
+use spi::Spi;
+use prelude::*;
+
+/////////////////////////////////////////
+// Lsm9ds1 Accel/Gyro (XL/G) Registers //
+/////////////////////////////////////////
+const ACT_THS: u8 = 0x04;
+const ACT_DUR: u8 = 0x05;
+const INT_GEN_CFG_XL: u8 = 0x06;
+const INT_GEN_THS_X_XL: u8 = 0x07;
+const INT_GEN_THS_Y_XL: u8 = 0x08;
+const INT_GEN_THS_Z_XL: u8 = 0x09;
+const INT_GEN_DUR_XL: u8 = 0x0A;
+const REFERENCE_G: u8 = 0x0B;
+const INT1_CTRL: u8 = 0x0C;
+const INT2_CTRL: u8 = 0x0D;
+const WHO_AM_I_XG: u8 = 0x0F;
+const CTRL_REG1_G: u8 = 0x10;
+const CTRL_REG2_G: u8 = 0x11;
+const CTRL_REG3_G: u8 = 0x12;
+const ORIENT_CFG_G: u8 = 0x13;
+const INT_GEN_SRC_G: u8 = 0x14;
+const OUT_TEMP_L: u8 = 0x15;
+const OUT_TEMP_H: u8 = 0x16;
+const STATUS_REG_0: u8 = 0x17;
+const OUT_X_L_G: u8 = 0x18;
+const OUT_X_H_G: u8 = 0x19;
+const OUT_Y_L_G: u8 = 0x1A;
+const OUT_Y_H_G: u8 = 0x1B;
+const OUT_Z_L_G: u8 = 0x1C;
+const OUT_Z_H_G: u8 = 0x1D;
+const CTRL_REG4: u8 = 0x1E;
+const CTRL_REG5_XL: u8 = 0x1F;
+const CTRL_REG6_XL: u8 = 0x20;
+const CTRL_REG7_XL: u8 = 0x21;
+const CTRL_REG8: u8 = 0x22;
+const CTRL_REG9: u8 = 0x23;
+const CTRL_REG10: u8 = 0x24;
+const INT_GEN_SRC_XL: u8 = 0x26;
+const STATUS_REG_1: u8 = 0x27;
+const OUT_X_L_XL: u8 = 0x28;
+const OUT_X_H_XL: u8 = 0x29;
+const OUT_Y_L_XL: u8 = 0x2A;
+const OUT_Y_H_XL: u8 = 0x2B;
+const OUT_Z_L_XL: u8 = 0x2C;
+const OUT_Z_H_XL: u8 = 0x2D;
+const FIFO_CTRL: u8 = 0x2E;
+const FIFO_SRC: u8 = 0x2F;
+const INT_GEN_CFG_G: u8 = 0x30;
+const INT_GEN_THS_XH_G: u8 = 0x31;
+const INT_GEN_THS_XL_G: u8 = 0x32;
+const INT_GEN_THS_YH_G: u8 = 0x33;
+const INT_GEN_THS_YL_G: u8 = 0x34;
+const INT_GEN_THS_ZH_G: u8 = 0x35;
+const INT_GEN_THS_ZL_G: u8 = 0x36;
+const INT_GEN_DUR_G: u8 = 0x37;
+
+///////////////////////////////
+// Lsm9ds1 Magneto Registers //
+///////////////////////////////
+const OFFSET_X_REG_L_M: u8 = 0x05;
+const OFFSET_X_REG_H_M: u8 = 0x06;
+const OFFSET_Y_REG_L_M: u8 = 0x07;
+const OFFSET_Y_REG_H_M: u8 = 0x08;
+const OFFSET_Z_REG_L_M: u8 = 0x09;
+const OFFSET_Z_REG_H_M: u8 = 0x0A;
+const WHO_AM_I_M: u8 = 0x0F;
+const CTRL_REG1_M: u8 = 0x20;
+const CTRL_REG2_M: u8 = 0x21;
+const CTRL_REG3_M: u8 = 0x22;
+const CTRL_REG4_M: u8 = 0x23;
+const CTRL_REG5_M: u8 = 0x24;
+const STATUS_REG_M: u8 = 0x27;
+const OUT_X_L_M: u8 = 0x28;
+const OUT_X_H_M: u8 = 0x29;
+const OUT_Y_L_M: u8 = 0x2A;
+const OUT_Y_H_M: u8 = 0x2B;
+const OUT_Z_L_M: u8 = 0x2C;
+const OUT_Z_H_M: u8 = 0x2D;
+const INT_CFG_M: u8 = 0x30;
+const INT_SRC_M: u8 = 0x31;
+const INT_THS_L_M: u8 = 0x32;
+const INT_THS_H_M: u8 = 0x33;
+
+////////////////////////////////
+// Lsm9ds1 WHO_AM_I Responses //
+////////////////////////////////
+const WHO_AM_I_AG_RSP: u8 = 0x68;
+const WHO_AM_I_M_RSP: u8 = 0x3D;
+
+/// Accelerometer settings
+pub struct AccelSettings {
+ ///
+ pub enabled: bool,
+ /// accel scale can be 2, 4, 8, or 16
+ pub scale: u16,
+ /// accel sample rate can be 1-6
+ /// 1 = 10 Hz 4 = 238 Hz
+ /// 2 = 50 Hz 5 = 476 Hz
+ /// 3 = 119 Hz 6 = 952 Hz
+ pub sample_rate: u8,
+ ///
+ pub enable_x: bool,
+ ///
+ pub enable_y: bool,
+ ///
+ pub enable_z: bool,
+ /// Accel cutoff freqeuncy can be any value between -1 - 3.
+ /// -1 = bandwidth determined by sample rate
+ /// 0 = 408 Hz 2 = 105 Hz
+ /// 1 = 211 Hz 3 = 50 Hz
+ pub bandwidth: i8,
+ ///
+ pub high_res_enable: bool,
+ /// accelHighResBandwidth can be any value between 0-3
+ /// LP cutoff is set to a factor of sample rate
+ /// 0 = ODR/50 2 = ODR/9
+ /// 1 = ODR/100 3 = ODR/400
+ pub high_res_bandwidth: u8,
+}
+
+/// Gyroscope settings
+pub struct GyroSettings {
+ ///
+ pub enabled: bool,
+ /// gyro scale can be 245, 500, or 2000
+ pub scale: u16,
+ /// gyro sample rate: value between 1-6
+ /// 1 = 14.9 4 = 238
+ /// 2 = 59.5 5 = 476
+ /// 3 = 119 6 = 952
+ pub sample_rate: u8,
+ /// gyro cutoff frequency: value between 0-3
+ /// Actual value of cutoff frequency depends
+ /// on sample rate.
+ pub bandwidth: u8,
+ ///
+ pub low_power_enable: bool,
+ ///
+ pub hpf_enable: bool,
+ /// Gyro HPF cutoff frequency: value between 0-9
+ /// Actual value depends on sample rate. Only applies
+ /// if gyroHPFEnable is true.
+ pub hpf_cutoff: u8,
+ ///
+ pub flip_x: bool,
+ ///
+ pub flip_y: bool,
+ ///
+ pub flip_z: bool,
+ ///
+ pub orientation: u8,
+ ///
+ pub enable_x: bool,
+ ///
+ pub enable_y: bool,
+ ///
+ pub enable_z: bool,
+ ///
+ pub latch_interrupt: bool,
+}
+
+/// Magnetometer settings
+pub struct MagSettings {
+ ///
+ pub enabled: bool,
+ /// mag scale can be 4, 8, 12, or 16
+ pub scale: u8,
+ ///mag data rate can be 0-7
+ /// 0 = 0.625 Hz 4 = 10 Hz
+ /// 1 = 1.25 Hz 5 = 20 Hz
+ /// 2 = 2.5 Hz 6 = 40 Hz
+ /// 3 = 5 Hz 7 = 80 Hz
+ pub sample_rate: u8,
+ ///
+ pub temp_compensation_enable: bool,
+ /// magPerformance can be any value between 0-3
+ /// 0 = Low power mode 2 = high performance
+ /// 1 = medium performance 3 = ultra-high performance
+ pub xy_performance: u8,
+ ///
+ pub z_performance: u8,
+ ///
+ pub low_power_enable: bool,
+ /// magOperatingMode can be 0-2
+ /// 0 = continuous conversion
+ /// 1 = single-conversion
+ /// 2 = power down
+ pub operating_mode: u8,
+}
+
+/// Settings for lsm9ds1
+pub struct ImuSettings {
+ ///
+ pub mag: MagSettings,
+ ///
+ pub accel: AccelSettings,
+ ///
+ pub gyro: GyroSettings,
+}
+
+/// Generic 3D vector
+pub struct Vector3<T> {
+ /// X axis value
+ pub x: T,
+ /// Y axis value
+ pub y: T,
+ /// Z axis value
+ pub z: T,
+}
+
+/// Default settings for lsm9ds1
+impl ImuSettings {
+ ///
+ pub const fn new() -> Self {
+ ImuSettings {
+ accel: AccelSettings {
+ enabled: true,
+ scale: 2,
+ sample_rate: 6,
+ enable_x: true,
+ enable_y: true,
+ enable_z: true,
+ bandwidth: -1,
+ high_res_enable: false,
+ high_res_bandwidth: 0,
+ },
+ gyro: GyroSettings {
+ enabled: true,
+ scale: 245,
+ sample_rate: 6,
+ bandwidth: 0,
+ low_power_enable: false,
+ hpf_enable: false,
+ hpf_cutoff: 0,
+ flip_x: false,
+ flip_y: false,
+ flip_z: false,
+ orientation: 0,
+ enable_x: true,
+ enable_y: true,
+ enable_z: true,
+ latch_interrupt: true,
+ },
+ mag: MagSettings {
+ enabled: true,
+ scale: 4,
+ sample_rate: 7,
+ temp_compensation_enable: false,
+ xy_performance: 3,
+ z_performance: 3,
+ low_power_enable: false,
+ operating_mode: 0,
+ },
+ }
+ }
+}
+
+const SENSITIVITY_ACCELEROMETER_2: f32 = 0.000061;
+const SENSITIVITY_ACCELEROMETER_4: f32 = 0.000122;
+const SENSITIVITY_ACCELEROMETER_8: f32 = 0.000244;
+const SENSITIVITY_ACCELEROMETER_16: f32 = 0.000732;
+const SENSITIVITY_GYROSCOPE_245: f32 = 0.00875;
+const SENSITIVITY_GYROSCOPE_500: f32 = 0.0175;
+const SENSITIVITY_GYROSCOPE_2000: f32 = 0.07;
+const SENSITIVITY_MAGNETOMETER_4: f32 = 0.00014;
+const SENSITIVITY_MAGNETOMETER_8: f32 = 0.00029;
+const SENSITIVITY_MAGNETOMETER_12: f32 = 0.00043;
+const SENSITIVITY_MAGNETOMETER_16: f32 = 0.00058;
+
+
+/// LSM9DS1 for SPI communication
+pub struct Lsm9ds1<'a, T>(pub &'a T)
+where
+ T: 'a;
+
+macro_rules! impl_Lsm9ds1 {
+ ($SPI:ident) => {
+ impl<'a> Lsm9ds1<'a, $SPI> {
+
+ fn enable_m(&self, gpioa: &GPIOA) {
+ gpioa.odr.modify(|_, w| w.odr9().bit(false));
+ }
+
+ fn disable_m(&self, gpioa: &GPIOA) {
+ gpioa.odr.modify(|_, w| w.odr9().bit(true));
+ }
+
+ fn enable_ag(&self, gpioa: &GPIOA) {
+ gpioa.odr.modify(|_, w| w.odr8().bit(false));
+ }
+
+ fn disable_ag(&self, gpioa: &GPIOA) {
+ gpioa.odr.modify(|_, w| w.odr8().bit(true));
+ }
+
+ fn write_bytes(&self, spi: &Spi<$SPI>, addr: u8, data: [u8; 128], length: u8) {
+ let mut x: u8 = 0;
+ while spi.send(addr).is_err() {}
+ while spi.read().is_err() {}
+ while x < length {
+ while spi.send(data[x as usize]).is_err() {}
+ while spi.read().is_err() {}
+ x = x + 1;
+ }
+ }
+
+ fn read_bytes(&self, spi: &Spi<$SPI>, addr: u8, length: u8) -> [u8; 128] {
+ let mut response: [u8; 128] = [0; 128];
+ let mut x: u8 = 0;
+ while spi.send(addr).is_err() {}
+ while spi.read().is_err() {}
+ while x < length {
+ while spi.send(0).is_err() {}
+ loop {
+ if let Ok(byte) = spi.read() {
+ response[x as usize] = byte;
+ break;
+ }
+ }
+ x = x + 1;
+ }
+ response
+ }
+
+ fn ag_read_bytes(&self, spi: &Spi<$SPI>, gpioa: &GPIOA, addr: u8, length: u8) -> [u8; 128] {
+ let addr = 0x80 | (addr & 0x7F);
+ self.enable_ag(&gpioa);
+ let ans = self.read_bytes(spi, addr, length);
+ self.disable_ag(&gpioa);
+ ans
+ }
+
+ fn ag_read_byte(&self, spi: &Spi<$SPI>, gpioa: &GPIOA, addr: u8) -> u8 {
+ self.ag_read_bytes(spi,gpioa,addr,1)[0]
+ }
+
+ fn ag_write_bytes(&self, spi: &Spi<$SPI>, gpioa: &GPIOA, addr: u8, data: [u8; 128], length: u8) {
+ let addr = !0x80 & (addr & 0x7F);
+ self.enable_ag(&gpioa);
+ self.write_bytes(spi, addr, data, length);
+ self.disable_ag(&gpioa);
+ }
+
+ fn ag_write_byte(&self, spi: &Spi<$SPI>, gpioa: &GPIOA, addr: u8, data: u8) {
+ self.ag_write_bytes(spi, gpioa, addr, [data; 128], 1);
+ }
+
+ fn m_read_bytes(&self, spi: &Spi<$SPI>, gpioa: &GPIOA, addr: u8, length: u8) -> [u8; 128] {
+ let addr = 0x80 | 0x40 | (addr & 0x3F);
+ self.enable_m(&gpioa);
+ let ans = self.read_bytes(spi, addr, length);
+ self.disable_m(&gpioa);
+ ans
+ }
+
+ fn m_read_byte(&self, spi: &Spi<$SPI>, gpioa: &GPIOA, addr: u8) -> u8 {
+ self.m_read_bytes(spi,gpioa,addr,1)[0]
+ }
+
+ fn m_write_bytes(&self, spi: &Spi<$SPI>, gpioa: &GPIOA, addr: u8, data: [u8; 128], length: u8) {
+ let addr = !0x80 & (0x40 | addr & 0x7F);
+ self.enable_m(&gpioa);
+ self.write_bytes(spi, addr, data, length);
+ self.disable_m(&gpioa);
+ }
+
+ fn m_write_byte(&self, spi: &Spi<$SPI>, gpioa: &GPIOA, addr: u8, data: u8) {
+ self.m_write_bytes(spi, gpioa, addr, [data; 128], 1);
+ }
+
+ /// Initalizes the gyroscope with given settings
+ pub fn init_gyro(&self, spi: &Spi<$SPI>, gpioa: &GPIOA, settings: &ImuSettings) {
+ // Check that we can read from acc/gyro
+ assert_eq!(WHO_AM_I_AG_RSP, self.ag_read_byte(&spi, &gpioa, WHO_AM_I_XG));
+
+ let mut temp: u8 = 0;
+ // CTRL_REG1_G (Default value: 0x00)
+ // [ODR_G2][ODR_G1][ODR_G0][FS_G1][FS_G0][0][BW_G1][BW_G0]
+ // ODR_G[2:0] - Output data rate selection
+ // FS_G[1:0] - Gyroscope full-scale selection
+ // BW_G[1:0] - Gyroscope bandwidth selection
+
+ // To disable gyro, set sample rate bits to 0. We'll only set sample
+ // rate if the gyro is enabled.
+ if settings.gyro.enabled {
+ temp = (settings.gyro.sample_rate & 0x07) << 5;
+ }
+ temp |= match settings.gyro.scale {
+ 500 => (0x1 << 3),
+ 2000 => (0x3 << 3),
+ _ => 0, // Otherwise we'll set it to 245 dps (0x0 << 4)
+ };
+ temp |= settings.gyro.bandwidth & 0x3;
+ self.ag_write_byte(&spi, &gpioa, CTRL_REG1_G, temp);
+
+ // Check that we can both read and write
+ assert_eq!(temp, self.ag_read_byte(&spi, &gpioa, CTRL_REG1_G));
+
+ // CTRL_REG2_G (Default value: 0x00)
+ // [0][0][0][0][INT_SEL1][INT_SEL0][OUT_SEL1][OUT_SEL0]
+ // INT_SEL[1:0] - INT selection configuration
+ // OUT_SEL[1:0] - Out selection configuration
+ self.ag_write_byte(&spi, &gpioa, CTRL_REG2_G, 0x00);
+ assert_eq!(0x00, self.ag_read_byte(&spi, &gpioa, CTRL_REG2_G));
+
+ // CTRL_REG3_G (Default value: 0x00)
+ // [LP_mode][HP_EN][0][0][HPCF3_G][HPCF2_G][HPCF1_G][HPCF0_G]
+ // LP_mode - Low-power mode enable (0: disabled, 1: enabled)
+ // HP_EN - HPF enable (0:disabled, 1: enabled)
+ // HPCF_G[3:0] - HPF cutoff frequency
+ temp = 0;
+ if settings.gyro.low_power_enable {
+ temp = 1 << 7;
+ }
+ if settings.gyro.hpf_enable {
+ temp |= (1 << 6) | (settings.gyro.hpf_cutoff & 0x0F);
+ }
+ self.ag_write_byte(&spi, &gpioa, CTRL_REG3_G, temp);
+ assert_eq!(temp, self.ag_read_byte(&spi, &gpioa, CTRL_REG3_G));
+
+ // CTRL_REG4 (Default value: 0x38)
+ // [0][0][Zen_G][Yen_G][Xen_G][0][LIR_XL1][4D_XL1]
+ // Zen_G - Z-axis output enable (0:disable, 1:enable)
+ // Yen_G - Y-axis output enable (0:disable, 1:enable)
+ // Xen_G - X-axis output enable (0:disable, 1:enable)
+ // LIR_XL1 - Latched interrupt (0:not latched, 1:latched)
+ // 4D_XL1 - 4D option on interrupt (0:6D used, 1:4D used)
+ temp = 0;
+ if settings.gyro.enable_z {
+ temp |= 1 << 5
+ }
+ if settings.gyro.enable_y {
+ temp |= 1 << 4
+ }
+ if settings.gyro.enable_x {
+ temp |= 1 << 3
+ }
+ if settings.gyro.latch_interrupt {
+ temp |= 1 << 1
+ }
+ self.ag_write_byte(&spi, &gpioa, CTRL_REG4, temp);
+ assert_eq!(temp, self.ag_read_byte(&spi, &gpioa, CTRL_REG4));
+
+ // ORIENT_CFG_G (Default value: 0x00)
+ // [0][0][SignX_G][SignY_G][SignZ_G][Orient_2][Orient_1][Orient_0]
+ // SignX_G - Pitch axis (X) angular rate sign (0: positive, 1: negative)
+ // Orient [2:0] - Directional user orientation selection
+ temp = 0;
+ if settings.gyro.flip_x {
+ temp |= 1 << 5
+ }
+ if settings.gyro.flip_y {
+ temp |= 1 << 4
+ }
+ if settings.gyro.flip_z {
+ temp |= 1 << 3
+ }
+ self.ag_write_byte(&spi, &gpioa, ORIENT_CFG_G, temp);
+ assert_eq!(temp, self.ag_read_byte(&spi, &gpioa, ORIENT_CFG_G));
+ }
+
+ /// Reads the raw values from the gyro X/Y/Z axis
+ pub fn read_gyro_raw(&self,spi: &Spi<$SPI>, gpioa: &GPIOA) -> Vector3<i16> {
+ // Read 6 bytes, beginning at OUT_X_L_G
+ let temp = self.ag_read_bytes(&spi, &gpioa, OUT_X_L_G, 6);
+ Vector3 {
+ x:(((temp[1] as u16) << 8) | temp[0] as u16) as i16,
+ y:(((temp[3] as u16) << 8) | temp[2] as u16) as i16,
+ z:(((temp[5] as u16) << 8) | temp[4] as u16) as i16,
+ }
+ }
+
+ /// Reads the values from the gyro X/Y/Z axis in DPS
+ pub fn read_gyro(&self,spi: &Spi<$SPI>, gpioa: &GPIOA, settings: &ImuSettings) -> Vector3<f32> {
+ let g_res = match settings.gyro.scale {
+ 245 => SENSITIVITY_GYROSCOPE_245,
+ 500 => SENSITIVITY_GYROSCOPE_500,
+ 2000 => SENSITIVITY_GYROSCOPE_2000,
+ _ => panic!("Invalid sensitivity")
+ };
+ let g_raw = self.read_gyro_raw(&spi, &gpioa);
+ Vector3 {
+ x:g_raw.x as f32 * g_res,
+ y:g_raw.y as f32 * g_res,
+ z:g_raw.z as f32 * g_res,
+ }
+ }
+
+ /// Initalizes the accelerometer with given settings
+ pub fn init_acc(&self, spi: &Spi<$SPI>, gpioa: &GPIOA, settings: &ImuSettings) {
+ // Check that we can read from acc/gyro
+ assert_eq!(WHO_AM_I_AG_RSP, self.ag_read_byte(&spi, &gpioa, WHO_AM_I_XG));
+
+ let mut temp: u8 = 0;
+
+ // CTRL_REG5_XL (0x1F) (Default value: 0x38)
+ // [DEC_1][DEC_0][Zen_XL][Yen_XL][Zen_XL][0][0][0]
+ // DEC[0:1] - Decimation of accel data on OUT REG and FIFO.
+ // 00: None, 01: 2 samples, 10: 4 samples 11: 8 samples
+ // Zen_XL - Z-axis output enabled
+ // Yen_XL - Y-axis output enabled
+ // Xen_XL - X-axis output enabled
+ if settings.accel.enable_z {temp |= 1<<5};
+ if settings.accel.enable_y {temp |= 1<<4};
+ if settings.accel.enable_x {temp |= 1<<3};
+
+ self.ag_write_byte(&spi, &gpioa, CTRL_REG5_XL, temp);
+ assert_eq!(temp, self.ag_read_byte(&spi, &gpioa, CTRL_REG5_XL));
+
+ // CTRL_REG6_XL (0x20) (Default value: 0x00)
+ // [ODR_XL2][ODR_XL1][ODR_XL0][FS1_XL][FS0_XL][BW_SCAL_ODR][BW_XL1][BW_XL0]
+ // ODR_XL[2:0] - Output data rate & power mode selection
+ // FS_XL[1:0] - Full-scale selection
+ // BW_SCAL_ODR - Bandwidth selection
+ // BW_XL[1:0] - Anti-aliasing filter bandwidth selection
+ temp = 0;
+ // To disable the accel, set the sampleRate bits to 0.
+ if settings.accel.enabled
+ {
+ temp |= (settings.accel.sample_rate & 0x07) << 5;
+ }
+
+ temp |= match settings.accel.scale {
+ 4 => (0x2 << 3),
+ 8 => (0x3 << 3),
+ 16 => (0x1 << 3),
+ _ => 0, // Otherwise it'll be set to 2g (0x0 << 3)
+ };
+
+ if settings.accel.bandwidth >= 0 {
+ temp |= 1<<2; // Set BW_SCAL_ODR
+ temp |= settings.accel.bandwidth as u8 & 0x03;
+ }
+ self.ag_write_byte(&spi, &gpioa, CTRL_REG6_XL, temp);
+ assert_eq!(temp, self.ag_read_byte(&spi, &gpioa, CTRL_REG6_XL));
+
+ // CTRL_REG7_XL (0x21) (Default value: 0x00)
+ // [HR][DCF1][DCF0][0][0][FDS][0][HPIS1]
+ // HR - High resolution mode (0: disable, 1: enable)
+ // DCF[1:0] - Digital filter cutoff frequency
+ // FDS - Filtered data selection
+ // HPIS1 - HPF enabled for interrupt function
+ temp = 0;
+ if settings.accel.high_res_enable
+ {
+ temp |= 1<<7; // Set HR bit
+ temp |= (settings.accel.high_res_bandwidth & 0x3) << 5;
+ }
+ self.ag_write_byte(&spi, &gpioa, CTRL_REG7_XL, temp);
+ assert_eq!(temp, self.ag_read_byte(&spi, &gpioa, CTRL_REG7_XL));
+ }
+
+ /// Reads the raw values from the accelerometer X/Y/Z axis
+ pub fn read_acc_raw(&self,spi: &Spi<$SPI>, gpioa: &GPIOA) -> Vector3<i16> {
+ // Read 6 bytes, beginning at OUT_X_L_XL
+ let temp = self.ag_read_bytes(&spi, &gpioa, OUT_X_L_XL, 6);
+ Vector3 {
+ x:(((temp[1] as u16) << 8) | temp[0] as u16) as i16,
+ y:(((temp[3] as u16) << 8) | temp[2] as u16) as i16,
+ z:(((temp[5] as u16) << 8) | temp[4] as u16) as i16,
+ }
+ }
+
+ /// Reads the values from the accelerometer X/Y/Z axis in g's
+ pub fn read_acc(&self,spi: &Spi<$SPI>, gpioa: &GPIOA, settings: &ImuSettings) -> Vector3<f32> {
+ let a_res = match settings.accel.scale {
+ 2 => SENSITIVITY_ACCELEROMETER_2,
+ 4=> SENSITIVITY_ACCELEROMETER_4,
+ 8=> SENSITIVITY_ACCELEROMETER_8,
+ 16=> SENSITIVITY_ACCELEROMETER_16,
+ _ => panic!("Invalid sensitivity"),
+ };
+ let a_raw = self.read_acc_raw(&spi, &gpioa);
+ Vector3 {
+ x:a_raw.x as f32 * a_res,
+ y:a_raw.y as f32 * a_res,
+ z:a_raw.z as f32 * a_res,
+ }
+ }
+
+ /// Initalizes the magnetometer with given settings
+ pub fn init_mag(&self, spi: &Spi<$SPI>, gpioa: &GPIOA, settings: &ImuSettings) -> bool {
+ // Check that we can read from acc/gyro
+ assert_eq!(WHO_AM_I_M_RSP, self.m_read_byte(&spi, &gpioa, WHO_AM_I_M));
+
+ let mut temp: u8 = 0;
+
+ // CTRL_REG1_M (Default value: 0x10)
+ // [TEMP_COMP][OM1][OM0][DO2][DO1][DO0][0][ST]
+ // TEMP_COMP - Temperature compensation
+ // OM[1:0] - X & Y axes op mode selection
+ // 00:low-power, 01:medium performance
+ // 10: high performance, 11:ultra-high performance
+ // DO[2:0] - Output data rate selection
+ // ST - Self-test enable
+ if settings.mag.temp_compensation_enable {temp |= 1<<7};
+ temp |= (settings.mag.xy_performance & 0x3) << 5;
+ temp |= (settings.mag.sample_rate & 0x7) << 2;
+ self.m_write_byte(&spi, &gpioa, CTRL_REG1_M, temp);
+ assert_eq!(temp, self.m_read_byte(&spi, &gpioa, CTRL_REG1_M));
+
+ // CTRL_REG2_M (Default value 0x00)
+ // [0][FS1][FS0][0][REBOOT][SOFT_RST][0][0]
+ // FS[1:0] - Full-scale configuration
+ // REBOOT - Reboot memory content (0:normal, 1:reboot)
+ // SOFT_RST - Reset config and user registers (0:default, 1:reset)
+ temp = 0;
+ temp |= match settings.mag.scale
+ {
+ 8 => 0x1 << 5,
+ 12 => 0x2 << 5,
+ 16 => 0x3 << 5,
+ _ => 0,
+ // Otherwise we'll default to 4 gauss (00)
+ };
+ self.m_write_byte(&spi, &gpioa, CTRL_REG2_M, temp); // +/-4Gauss
+ assert_eq!(temp, self.m_read_byte(&spi, &gpioa, CTRL_REG2_M));
+
+ // CTRL_REG3_M (Default value: 0x03)
+ // [I2C_DISABLE][0][LP][0][0][SIM][MD1][MD0]
+ // I2C_DISABLE - Disable I2C interace (0:enable, 1:disable)
+ // LP - Low-power mode cofiguration (1:enable)
+ // SIM - SPI mode selection (0:write-only, 1:read/write enable)
+ // MD[1:0] - Operating mode
+ // 00:continuous conversion, 01:single-conversion,
+ // 10,11: Power-down
+ temp = 0;
+ if settings.mag.low_power_enable {temp |= 1<<5};
+ temp |= settings.mag.operating_mode & 0x3;
+ self.m_write_byte(&spi, &gpioa, CTRL_REG3_M, temp); // Continuous conversion mode
+ assert_eq!(temp, self.m_read_byte(&spi, &gpioa, CTRL_REG3_M));
+
+ // CTRL_REG4_M (Default value: 0x00)
+ // [0][0][0][0][OMZ1][OMZ0][BLE][0]
+ // OMZ[1:0] - Z-axis operative mode selection
+ // 00:low-power mode, 01:medium performance
+ // 10:high performance, 10:ultra-high performance
+ // BLE - Big/little endian data
+ temp = (settings.mag.z_performance & 0x3) << 2;
+ self.m_write_byte(&spi, &gpioa, CTRL_REG4_M, temp);
+ assert_eq!(temp, self.m_read_byte(&spi, &gpioa, CTRL_REG4_M));
+
+ // CTRL_REG5_M (Default value: 0x00)
+ // [0][BDU][0][0][0][0][0][0]
+ // BDU - Block data update for magnetic data
+ // 0:continuous, 1:not updated until MSB/LSB are read
+ temp = 0;
+ self.m_write_byte(&spi, &gpioa, CTRL_REG5_M, temp);
+ assert_eq!(temp, self.m_read_byte(&spi, &gpioa, CTRL_REG5_M));
+
+ true
+ }
+
+ /// Reads the raw values from the magnetometer X/Y/Z axis
+ pub fn read_mag_raw(&self,spi: &Spi<$SPI>, gpioa: &GPIOA) -> Vector3<i16> {
+
+ // Read 6 bytes, beginning at OUT_X_L_M
+ let temp = self.m_read_bytes(&spi, &gpioa, OUT_X_L_M, 6);
+ Vector3 {
+ x:(((temp[1] as u16) << 8) | temp[0] as u16) as i16,
+ y:(((temp[3] as u16) << 8) | temp[2] as u16) as i16,
+ z:(((temp[5] as u16) << 8) | temp[4] as u16) as i16,
+ }
+ }
+
+ /// Reads the values from the magnetometer X/Y/Z axis in Gauss
+ pub fn read_mag(&self,spi: &Spi<$SPI>, gpioa: &GPIOA, settings: &ImuSettings) -> Vector3<f32> {
+ let m_res = match settings.mag.scale {
+ 4 => SENSITIVITY_MAGNETOMETER_4,
+ 8 => SENSITIVITY_MAGNETOMETER_8,
+ 12 => SENSITIVITY_MAGNETOMETER_12,
+ 16 => SENSITIVITY_MAGNETOMETER_16,
+ _ => panic!("Invalid sensitivity")
+ };
+ let m_raw = self.read_mag_raw(&spi, &gpioa);
+ Vector3 {
+ x:m_raw.x as f32 * m_res,
+ y:m_raw.y as f32 * m_res,
+ z:m_raw.z as f32 * m_res,
+ }
+ }
+
+ }
+ }
+}
+
+impl_Lsm9ds1!(SPI1);
+impl_Lsm9ds1!(SPI2);
+impl_Lsm9ds1!(SPI3);