ROS#209: LSM6DSO driver implementation and usage example

Cargo.toml

@@ -9,6 +9,9 @@ members = [
     "arch/x86/aerugo-x86-hal",
     "utils/env-parser",
     "utils/env-parser-tests",
+    "utils/lsm6dso",
+    "utils/bitfield-enum",
+    "utils/bounded-int",
     "calldwell/calldwell-rs",
 ]
 exclude = ["demos", "examples", "testbins", "calldwell/examples"]

arch/cortex-m/samv71-hal/src/spi/embedded_hal.rs

@@ -83,6 +83,15 @@ impl<Instance: SPIMetadata> SpiBus<u8> for Spi<Instance, Master> {
     /// # Parameters
     /// * `words` - Slice of words to be transmitted.
     ///
+    /// # Remarks
+    /// This function may behave unexpectedly in some scenarios, as it waits for RX data register
+    /// full event, instead of TX data register empty event, as the latter doesn't happen after
+    /// first word is transmitted for unknown reasons in some SPI configurations. The issue may
+    /// happen if the device expects the write to be a single transaction, and SPI is configured in
+    /// a way that will make every word to be a separate transaction with CS deactivations in
+    /// between. Verify if this function works for your scenario, or use `transfer`/`transfer_in_place`
+    /// for writing.
+    ///
     /// # Returns
     /// Ok(()) on success, [`SpiError`] on SPI error.
     fn write(&mut self, words: &[u8]) -> Result<(), Self::Error> {
@@ -91,10 +100,13 @@ impl<Instance: SPIMetadata> SpiBus<u8> for Spi<Instance, Master> {
 
         for &word in words {
             self.transmit_value(word as u16);
-            status_reader.wait_for_status(
-                |status| status.interrupts.tx_data_register_empty,
-                usize::MAX,
-            );
+            // This should, in theory, check the state of TX data register. However, when it does
+            // that, the transfer hangs at 2nd word, as the flag never rises again. A workaround
+            // is to wait for RX data flag, as it'll be risen after the word is transmitted, but it's
+            // not guaranteed to work in every case (as it doesn't guarantee that the transfer will
+            // be a single SPI transaction, which may sometimes be necessary).
+            status_reader
+                .wait_for_status(|status| status.interrupts.rx_data_register_full, usize::MAX);
             // Dummy read to prevent overrun error
             self.get_received_data();
         }

examples/samv71-lsm6dso/.cargo/config.toml

@@ -0,0 +1,11 @@
+[build]
+target = "thumbv7em-none-eabihf"
+
+[env]
+AERUGO_TASKLET_COUNT = { value = "2" }
+
+[target.thumbv7em-none-eabihf]
+rustflags = [
+    "-C", "link-arg=--nmagic", # Disable page alignment of sections (to prevent issues with binary size)
+    "-C", "link-arg=-Tlink.x", # Use cortex-m-rt's linker script
+]

examples/samv71-lsm6dso/Cargo.toml

@@ -0,0 +1,25 @@
+[package]
+name = "samv71-lsm6dso"
+authors = ["Wojciech Olech <wojciech_olech@hotmail.com>"]
+edition = "2021"
+version = "0.1.0"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]
+aerugo = { version = "0.1.0", path = "../..", features = [
+    "use-aerugo-cortex-m",
+    "rt",
+] }
+cortex-m = { version = "0.7.7", features = ["critical-section-single-core"] }
+cortex-m-rt = { version = "0.7.3", features = ["device"] }
+lsm6dso = { version = "*", path = "../../utils/lsm6dso" }
+panic-rtt-target = { version = "0.1.2", features = ["cortex-m"] }
+
+[features]
+rt = ["aerugo/rt"]
+
+[profile.release]
+codegen-units = 1
+lto = true
+debug = true

examples/samv71-lsm6dso/build.rs

@@ -0,0 +1,14 @@
+use std::env;
+use std::fs::File;
+use std::io::Write;
+use std::path::PathBuf;
+
+fn main() {
+    // Put the linker script somewhere the linker can find it
+    let out = &PathBuf::from(env::var_os("OUT_DIR").unwrap());
+    File::create(out.join("memory.x"))
+        .unwrap()
+        .write_all(include_bytes!("memory.x"))
+        .unwrap();
+    println!("cargo:rustc-link-search={}", out.display());
+}

examples/samv71-lsm6dso/memory.x

@@ -0,0 +1,6 @@
+/* Linker script for SAMV71Q21 */
+MEMORY
+{
+    FLASH (rx) : ORIGIN = 0x00400000, LENGTH = 0x00200000
+    RAM (rwx) : ORIGIN = 0x20400000, LENGTH = 0x00060000
+}

examples/samv71-lsm6dso/src/hardware_config.rs

@@ -0,0 +1,82 @@
+use aerugo::hal::{
+    drivers::{
+        pio::{
+            pin::{InputMode, Peripheral, PeripheralMode},
+            Pin, Port,
+        },
+        pmc::{config::PeripheralId, PMC},
+        spi::{
+            chip_config::{
+                BitsPerTransfer, ChipConfig, ChipSelectBehavior, ClockPhase, ClockPolarity,
+                SerialClockDivider,
+            },
+            config::{MasterConfig, SelectedChip},
+            interrupts::Interrupts,
+            Master, NotConfigured, Spi,
+        },
+    },
+    user_peripherals::{PIOD, SPI0},
+};
+
+/// LSM6DSO chip select signal.
+const LSM6DSO_CHIP: SelectedChip = SelectedChip::Chip1;
+/// LSM6DSO SPI clock divider. Default peripheral clock is 12MHz, LSM6DSO can work with up to 10MHz.
+const LSM6DSO_SPI_CLOCK_DIVIDER: u8 = 100;
+
+pub struct LSM6DSOPins {
+    pub miso: Pin<PeripheralMode>,
+    pub mosi: Pin<PeripheralMode>,
+    pub sck: Pin<PeripheralMode>,
+    pub cs: Pin<PeripheralMode>,
+    pub int1: Pin<InputMode>,
+}
+
+pub fn configure_pio(port: Port<PIOD>) -> LSM6DSOPins {
+    let mut pins = port.into_pins();
+
+    let miso = pins[20].take().unwrap().into_peripheral_pin(Peripheral::B);
+    let mosi = pins[21].take().unwrap().into_peripheral_pin(Peripheral::B);
+    let sck = pins[22].take().unwrap().into_peripheral_pin(Peripheral::B);
+    let cs = pins[25].take().unwrap().into_peripheral_pin(Peripheral::B);
+    let int1 = pins[28].take().unwrap().into_input_pin();
+
+    LSM6DSOPins {
+        miso,
+        mosi,
+        sck,
+        cs,
+        int1,
+    }
+}
+
+pub fn configure_spi(spi: Spi<SPI0, NotConfigured>) -> Spi<SPI0, Master> {
+    let mut spi = spi.into_master(MasterConfig::new(LSM6DSO_CHIP));
+    spi.configure_chip(
+        LSM6DSO_CHIP,
+        ChipConfig {
+            clock_polarity: ClockPolarity::HighWhenInactive,
+            clock_phase: ClockPhase::DataChangedOnLeadingEdge,
+            chip_select_behavior: ChipSelectBehavior::DeactivateAfterLastTransfer,
+            bits_per_transfer: BitsPerTransfer::Bits8,
+            clock_divider: SerialClockDivider::new(LSM6DSO_SPI_CLOCK_DIVIDER).unwrap(),
+            delay_before_first_clock: 0,
+            delay_between_consecutive_transfers: 0,
+        },
+    );
+    spi.set_interrupts_state(Interrupts {
+        rx_data_register_full: true,
+        tx_data_register_empty: true,
+        mode_fault_error: true,
+        overrun_error: true,
+        nss_rising: false,
+        tx_registers_empty: true,
+        underrun_error: false,
+    });
+
+    spi
+}
+
+pub fn configure_pmc(pmc: &mut PMC) {
+    pmc.enable_peripheral_clock(PeripheralId::SPI0);
+    pmc.enable_peripheral_clock(PeripheralId::PIOD);
+}

examples/samv71-lsm6dso/src/main.rs

@@ -0,0 +1,102 @@
+#![no_std]
+#![no_main]
+
+extern crate cortex_m;
+extern crate cortex_m_rt as rt;
+extern crate lsm6dso;
+extern crate panic_rtt_target;
+
+use aerugo::{
+    hal::drivers::{pio::Port, spi::Spi},
+    logln, Aerugo, InitApi, SystemHardwareConfig,
+};
+use lsm6dso::{
+    config::{
+        control::{
+            AccelerometerConfig, AccelerometerDataRate, AccelerometerOutputSelection,
+            AccelerometerScale, AccelerometerTestMode, GyroscopeConfig, GyroscopeDataRate,
+            GyroscopeScale, GyroscopeTestMode,
+        },
+        fifo::config::{
+            AccelerometerBatchingRate, DataRateChangeBatching, FifoConfig, FifoMode,
+            FifoWatermarkThreshold, GyroscopeBatchingRate, StopOnWatermarkThreshold,
+        },
+    },
+    LSM6DSO,
+};
+use rt::entry;
+
+use crate::tasklets::init_system;
+
+mod hardware_config;
+mod tasklets;
+
+#[entry]
+fn main() -> ! {
+    let (aerugo, mut peripherals) = Aerugo::initialize(SystemHardwareConfig::default());
+    logln!("Hello, world! Aerugo initialized! Initializing hardware...");
+
+    let mut pmc = peripherals.pmc.take().unwrap();
+    let spi = Spi::new(peripherals.spi_0.take().unwrap());
+    let port_d = Port::new(peripherals.pio_d.take().unwrap());
+
+    hardware_config::configure_pmc(&mut pmc);
+    let _lsm_pins = hardware_config::configure_pio(port_d);
+    let lsm_spi = hardware_config::configure_spi(spi);
+
+    let mut lsm: LSM6DSO<_, 32> = LSM6DSO::new(lsm_spi).unwrap();
+
+    logln!("LSM id: {:2X?}", lsm.id());
+    logln!("Is LSM alive? {:?}", lsm.is_alive());
+    lsm.software_reset().unwrap();
+    lsm.reboot_memory_content().unwrap();
+
+    let fifo_config = FifoConfig {
+        watermark_threshold: FifoWatermarkThreshold::new(50).unwrap(),
+        odr_change_batched: DataRateChangeBatching::Enabled,
+        stop_on_watermark: StopOnWatermarkThreshold::No,
+        gyroscope_batching_rate: GyroscopeBatchingRate::Batch12_5Hz,
+        accelerometer_batching_rate: AccelerometerBatchingRate::Batch12_5Hz,
+        mode: FifoMode::Fifo,
+    };
+    lsm.set_fifo_config(fifo_config).unwrap();
+    logln!("New LSM FIFO config: {:#?}", lsm.get_fifo_config());
+
+    let accelerometer_config = AccelerometerConfig {
+        data_rate: AccelerometerDataRate::Rate12_5Hz,
+        scale: AccelerometerScale::Scale4g,
+        output_selection: AccelerometerOutputSelection::FirstStageFilter,
+    };
+
+    let gyroscope_config = GyroscopeConfig {
+        data_rate: GyroscopeDataRate::Rate12_5Hz,
+        scale: GyroscopeScale::Scale500dps,
+    };
+
+    lsm.set_accelerometer_test_mode(AccelerometerTestMode::Negative)
+        .unwrap();
+    lsm.set_gyroscope_test_mode(GyroscopeTestMode::Positive)
+        .unwrap();
+
+    logln!(
+        "Accelerometer mode: {:?}, gyroscope mode: {:?}",
+        lsm.get_accelerometer_test_mode(),
+        lsm.get_gyroscope_test_mode()
+    );
+
+    lsm.set_accelerometer_config(accelerometer_config).unwrap();
+    lsm.set_gyroscope_config(gyroscope_config).unwrap();
+    logln!(
+        "New LSM accelerometer config: {:#?}",
+        lsm.get_accelerometer_config()
+    );
+    logln!(
+        "New LSM gyroscope config: {:#?}",
+        lsm.get_gyroscope_config()
+    );
+
+    init_system(aerugo, lsm);
+
+    logln!("System is starting!");
+    aerugo.start();
+}

examples/samv71-lsm6dso/src/tasklets.rs

@@ -0,0 +1,49 @@
+use aerugo::{
+    hal::{
+        drivers::spi::{Master, Spi},
+        user_peripherals::SPI0,
+    },
+    logln, Duration, InitApi, RuntimeApi, TaskletConfig, TaskletStorage,
+};
+use lsm6dso::LSM6DSO;
+
+static LSM_TASK_STORAGE: TaskletStorage<(), LSMTaskContext, 0> = TaskletStorage::new();
+
+type Lsm = LSM6DSO<Spi<SPI0, Master>>;
+
+pub struct LSMTaskContext {
+    pub lsm: Lsm,
+}
+
+pub fn lsm_task(_: (), context: &mut LSMTaskContext, _: &'static dyn RuntimeApi) {
+    let fifo_status = context.lsm.get_fifo_status().unwrap();
+    for _ in 0..fifo_status.stored_words {
+        logln!("{:?}", context.lsm.get_next_fifo_word().unwrap());
+    }
+}
+
+pub fn init_system(aerugo: &'static impl InitApi, lsm: Lsm) {
+    logln!("Initializing tasks...");
+
+    let lsm_task_config = TaskletConfig {
+        name: "LSMTask",
+        ..Default::default()
+    };
+
+    let lsm_task_context = LSMTaskContext { lsm };
+
+    aerugo.create_tasklet_with_context(
+        lsm_task_config,
+        lsm_task,
+        lsm_task_context,
+        &LSM_TASK_STORAGE,
+    );
+
+    let lsm_task_handle = LSM_TASK_STORAGE.create_handle().unwrap();
+
+    logln!("Tasks created, subscribing...");
+
+    aerugo.subscribe_tasklet_to_cyclic(&lsm_task_handle, Some(Duration::millis(100)), None);
+
+    logln!("Tasks created and subscribed!");
+}

utils/bitfield-enum/Cargo.toml

@@ -0,0 +1,17 @@
+[package]
+name = "bitfield-enum"
+version = "1.0.0"
+authors.workspace = true
+edition.workspace = true
+rust-version.workspace = true
+repository.workspace = true
+license.workspace = true
+description = "Library with macro for creating bitfield enums with automatic conversion from/to bytes. Also provides traits for bitfield-to-bytes/bytes-to-bitfield conversion"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]
+derive_more = { version = "1.0.0-beta.6", features = [
+    "try_from",
+    "try_into",
+], default-features = false }