diff --git a/.gitignore b/.gitignore index ce77d05..7aec685 100644 --- a/.gitignore +++ b/.gitignore @@ -11,6 +11,7 @@ __pycache__/ .ruff_cache/ .dmypy.json *.pyi +.venv/ # Databases *.db diff --git a/LSM6DSV_README.md b/LSM6DSV_README.md new file mode 100644 index 0000000..e2d5d39 --- /dev/null +++ b/LSM6DSV_README.md @@ -0,0 +1,184 @@ +# LSM6DSV IMU Driver Implementation + +This document describes the implementation of the LSM6DSV 6-axis IMU driver for the IMU Python package. + +## Overview + +The LSM6DSV is a 6-axis IMU (Inertial Measurement Unit) from STMicroelectronics that combines a 3-axis accelerometer and 3-axis gyroscope. This implementation provides SPI communication support for reading sensor data. + +## Features + +- **SPI Communication**: Full SPI interface support with configurable settings +- **Accelerometer**: 3-axis acceleration data with configurable ranges (±2g, ±4g, ±8g, ±16g) +- **Gyroscope**: 3-axis angular velocity data with configurable ranges (±125 to ±4000 dps) +- **Temperature**: Built-in temperature sensor +- **Background Threading**: Continuous data acquisition in background thread +- **Configurable ODR**: Adjustable output data rates for both sensors +- **Python Integration**: Seamless integration with the existing IMU package + +## Hardware Requirements + +- LSM6DSV sensor connected via SPI +- Linux system with SPI support +- Appropriate permissions to access SPI devices (typically `/dev/spidev*`) + +## SPI Connection + +The LSM6DSV uses SPI mode 0 (CPOL=0, CPHA=0) with the following connections: + +- **MOSI**: Master Out Slave In +- **MISO**: Master In Slave Out +- **SCLK**: Serial Clock +- **CS**: Chip Select (active low) +- **VCC**: 1.71V to 3.6V +- **GND**: Ground + +## Usage + +### Basic Usage + +```python +from imu import create_lsm6dsvtr + +# Create an LSM6DSV reader +reader = create_lsm6dsvtr("/dev/spidev1.0") + +# Read sensor data +data = reader.get_data() + +if data.get('accelerometer'): + accel = data['accelerometer'] + print(f"Acceleration: x={accel.x:.3f}, y={accel.y:.3f}, z={accel.z:.3f} m/s²") + +if data.get('gyroscope'): + gyro = data['gyroscope'] + print(f"Angular velocity: x={gyro.x:.3f}, y={gyro.y:.3f}, z={gyro.z:.3f} deg/s") + +if data.get('temperature') is not None: + print(f"Temperature: {data['temperature']:.1f}°C") + +# Clean up +reader.stop() +``` + +### Advanced Configuration + +The driver supports runtime configuration of sensor ranges and output data rates: + +```python +# Note: These methods would need to be exposed in the Python bindings +# reader.set_accel_range(AccelRange.G4) # ±4g range +# reader.set_gyro_range(GyroRange.Dps500) # ±500 dps range +# reader.set_accel_odr(AccelOdr.Hz208) # 208 Hz output rate +# reader.set_gyro_odr(GyroOdr.Hz208) # 208 Hz output rate +``` + +## Implementation Details + +### File Structure + +``` +imu/drivers/lsm6dsv/ +├── Cargo.toml # Rust package configuration +├── README.md # Driver-specific documentation +└── src/ + ├── lib.rs # Main driver implementation + ├── registers.rs # Register definitions and constants + └── spi.rs # SPI communication layer +``` + +### Key Components + +1. **Register Definitions** (`registers.rs`): + - All LSM6DSV register addresses + - Output data rate (ODR) settings + - Full-scale range settings + - Sensitivity constants + +2. **SPI Communication** (`spi.rs`): + - Low-level SPI read/write operations + - Register-based communication + - Error handling for SPI operations + +3. **Main Driver** (`lib.rs`): + - High-level sensor interface + - Data conversion and scaling + - Background thread management + - Configuration commands + +### Data Conversion + +- **Accelerometer**: Raw values are converted from mg to m/s² using appropriate sensitivity factors +- **Gyroscope**: Raw values are converted from mdps to deg/s using appropriate sensitivity factors +- **Temperature**: Raw values are converted to Celsius using the formula: `temp = (raw * 1/256) + 25` + +### Error Handling + +The driver provides comprehensive error handling for: +- SPI communication failures +- Invalid chip ID detection +- Register read/write errors +- Thread synchronization issues + +## Testing + +A test script is provided (`test_lsm6dsv.py`) that can be used to verify the implementation: + +```bash +python3 test_lsm6dsv.py +``` + +This script will: +1. Test import functionality +2. Attempt to create an LSM6DSV reader +3. Try to read sensor data +4. Test cleanup procedures + +## Dependencies + +- `spidev`: Linux SPI device interface +- `byteorder`: Byte order conversion utilities +- `log`: Logging framework +- `imu-traits`: Common IMU interface traits + +## Limitations + +- Linux-only support (SPI device access) +- Requires appropriate permissions for SPI device access +- No magnetometer support (LSM6DSV doesn't include a magnetometer) +- No quaternion/euler angle calculation (accelerometer + gyroscope only) + +## Future Enhancements + +Potential improvements could include: +- I2C communication support +- Advanced filtering and sensor fusion +- Interrupt-driven data acquisition +- Additional sensor features (if available in LSM6DSV variants) + +## Troubleshooting + +### Common Issues + +1. **Permission Denied**: Ensure the user has access to SPI devices + ```bash + sudo usermod -a -G spi $USER + ``` + +2. **Device Not Found**: Verify the SPI device path exists + ```bash + ls -la /dev/spidev* + ``` + +3. **Invalid Chip ID**: Check SPI connections and power supply + +4. **No Data**: Verify sensor initialization and SPI communication + +### Debug Mode + +Enable debug logging to troubleshoot issues: +```python +import logging +logging.basicConfig(level=logging.DEBUG) +``` + diff --git a/imu/Cargo.toml b/imu/Cargo.toml index f1fab98..c9682cf 100644 --- a/imu/Cargo.toml +++ b/imu/Cargo.toml @@ -22,3 +22,4 @@ hiwonder = "0.6.4" linux_bno055 = "0.1.3" linux_bmi088 = "0.1.3" hexmove = "0.2.3" +lsm6dsv = { path = "drivers/lsm6dsv" } diff --git a/imu/__init__.py b/imu/__init__.py index d20fdec..31c73ff 100644 --- a/imu/__init__.py +++ b/imu/__init__.py @@ -9,6 +9,7 @@ create_bno055_reader, create_hexmove_reader, create_hiwonder_reader, + create_lsm6dsvtr_reader, ) @@ -45,6 +46,10 @@ def create_hexmove(can_interface: str = "can0", node_id: int = 1, param_id: int """Create a Hexmove IMU reader on the specified CAN interface.""" return create_hexmove_reader(can_interface, node_id, param_id) +def create_lsm6dsvtr(spi_device: str = "/dev/spi-1") -> ImuReader: + """Create a LSM6DSVTR IMU reader on the specified SPI device.""" + return create_lsm6dsvtr_reader(spi_device) + __all__ = [ # Data types diff --git a/imu/bindings/__init__.py b/imu/bindings/__init__.py new file mode 100644 index 0000000..6f47006 --- /dev/null +++ b/imu/bindings/__init__.py @@ -0,0 +1,18 @@ +"""Python bindings for the IMU package.""" + +# Import the compiled extension module from parent directory +import sys +import os +import importlib.util + +# Get the path to the compiled binary +parent_dir = os.path.dirname(os.path.dirname(__file__)) +binary_path = os.path.join(parent_dir, 'bindings.cpython-311-aarch64-linux-gnu.so') + +# Load the compiled binary directly +spec = importlib.util.spec_from_file_location("bindings", binary_path) +bindings_binary = importlib.util.module_from_spec(spec) +spec.loader.exec_module(bindings_binary) + +# Make all symbols available in this module's namespace +globals().update({name: getattr(bindings_binary, name) for name in dir(bindings_binary) if not name.startswith('_')}) diff --git a/imu/bindings/src/lib.rs b/imu/bindings/src/lib.rs index 880b3c8..6367607 100644 --- a/imu/bindings/src/lib.rs +++ b/imu/bindings/src/lib.rs @@ -331,6 +331,26 @@ fn create_hexmove_reader(can_interface: &str, node_id: u8, param_id: u8) -> PyRe } } +#[pyfunction] +fn create_lsm6dsvtr_reader(spi_device: &str) -> PyResult { + #[cfg(target_os = "linux")] + { + match imu::Lsm6dsvReader::new(spi_device) { + Ok(reader) => Ok(PyImuReader { + reader: Box::new(reader), + }), + Err(e) => Err(PyRuntimeError::new_err(e.to_string())), + } + } + #[cfg(not(target_os = "linux"))] + { + let _ = spi_device; + Err(PyRuntimeError::new_err( + "LSM6DSV reader is only available on Linux systems.", + )) + } +} + #[pymodule] fn bindings(m: &Bound) -> PyResult<()> { m.add_class::()?; @@ -346,5 +366,7 @@ fn bindings(m: &Bound) -> PyResult<()> { m.add_function(wrap_pyfunction!(create_hexmove_reader, m)?)?; + m.add_function(wrap_pyfunction!(create_lsm6dsvtr_reader, m)?)?; + Ok(()) } diff --git a/imu/drivers/lsm6dsv/Cargo.toml b/imu/drivers/lsm6dsv/Cargo.toml new file mode 100644 index 0000000..15d6235 --- /dev/null +++ b/imu/drivers/lsm6dsv/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "linux_lsm6dsv" +readme = "README.md" +description = "SPI driver for LSM6DSV 6-axis IMU sensor" +version = "0.1.3" +authors.workspace = true +edition.workspace = true +repository.workspace = true +license.workspace = true + +[lib] +name = "linux_lsm6dsv" +crate-type = ["rlib"] + +[dependencies] +byteorder = "1.5" +log = "0.4" +imu-traits = "0.1" diff --git a/imu/drivers/lsm6dsv/README.md b/imu/drivers/lsm6dsv/README.md new file mode 100644 index 0000000..8021fcf --- /dev/null +++ b/imu/drivers/lsm6dsv/README.md @@ -0,0 +1,21 @@ +# LSM6DSV Driver + +SPI driver for the LSM6DSV 6-axis IMU sensor from STMicroelectronics. + +## Features + +- Accelerometer and gyroscope data reading +- SPI communication interface +- Configurable output data rates and full-scale ranges +- Temperature reading +- Background thread for continuous data acquisition + +## Usage + +```rust +use lsm6dsv::Lsm6dsvReader; + +let reader = Lsm6dsvReader::new("/dev/spidev1.0")?; +let data = reader.get_data()?; +``` + diff --git a/imu/drivers/lsm6dsv/src/lib.rs b/imu/drivers/lsm6dsv/src/lib.rs new file mode 100644 index 0000000..60b477b --- /dev/null +++ b/imu/drivers/lsm6dsv/src/lib.rs @@ -0,0 +1,341 @@ +// LSM6DSV 6-axis IMU driver for SPI communication + +use byteorder::{ByteOrder, LittleEndian}; +use imu_traits::{ImuData, ImuError, ImuReader, Vector3}; +use log::{debug, error, warn}; +use std::sync::{mpsc, Arc, RwLock}; +use std::thread; +use std::time::Duration; + +mod registers; +mod spi; + +use registers::*; +use spi::Lsm6dsvSpi; + +/// Low-level LSM6DSV driver +pub struct Lsm6dsv { + spi: Lsm6dsvSpi, + accel_range: AccelRange, + gyro_range: GyroRange, +} + +impl Lsm6dsv { + /// Initialize the LSM6DSV sensor on the given SPI device + pub fn new(spi_device: &str) -> Result { + debug!("Initializing LSM6DSV..."); + + let mut spi = Lsm6dsvSpi::new(spi_device)?; + + // Verify chip ID + let chip_id = spi.read_register(Register::WhoAmI as u8)?; + if chip_id != Constants::WHO_AM_I_VALUE { + return Err(ImuError::DeviceError(format!( + "Invalid chip ID: expected 0x{:02X}, got 0x{:02X}", + Constants::WHO_AM_I_VALUE, chip_id + ))); + } + debug!("LSM6DSV chip ID verified: 0x{:02X}", chip_id); + + // Soft reset + spi.write_register(Register::Ctrl3C as u8, Constants::SOFT_RESET_CMD)?; + thread::sleep(Duration::from_millis(100)); + + // Configure accelerometer + spi.write_register(Register::Ctrl1Xl as u8, + (AccelOdr::Hz104 as u8) | (AccelRange::G2 as u8))?; + + // Configure gyroscope + spi.write_register(Register::Ctrl2G as u8, + (GyroOdr::Hz104 as u8) | (GyroRange::Dps250 as u8))?; + + // Enable accelerometer and gyroscope + spi.write_register(Register::Ctrl4C as u8, 0x00)?; // Default settings + spi.write_register(Register::Ctrl5C as u8, 0x00)?; // Default settings + spi.write_register(Register::Ctrl6C as u8, 0x00)?; // Default settings + spi.write_register(Register::Ctrl7G as u8, 0x00)?; // Default settings + spi.write_register(Register::Ctrl8Xl as u8, 0x00)?; // Default settings + spi.write_register(Register::Ctrl9Xl as u8, 0x00)?; // Default settings + spi.write_register(Register::Ctrl10C as u8, 0x00)?; // Default settings + + // Wait for sensors to stabilize + thread::sleep(Duration::from_millis(50)); + + Ok(Lsm6dsv { + spi, + accel_range: AccelRange::G2, + gyro_range: GyroRange::Dps250, + }) + } + + /// Read raw accelerometer data + pub fn read_raw_accelerometer(&mut self) -> Result { + let data = self.spi.read_registers(Register::OutXlA as u8, 6)?; + + let scale = match self.accel_range { + AccelRange::G2 => Sensitivity::ACCEL_2G, + AccelRange::G4 => Sensitivity::ACCEL_4G, + AccelRange::G8 => Sensitivity::ACCEL_8G, + AccelRange::G16 => Sensitivity::ACCEL_16G, + }; + + let raw_x = LittleEndian::read_i16(&data[0..2]) as f32; + let raw_y = LittleEndian::read_i16(&data[2..4]) as f32; + let raw_z = LittleEndian::read_i16(&data[4..6]) as f32; + + // Convert from mg to m/s² (multiply by 9.80665/1000) + let g_to_ms2 = 9.80665 / 1000.0; + Ok(Vector3 { + x: raw_x * scale * g_to_ms2, + y: raw_y * scale * g_to_ms2, + z: raw_z * scale * g_to_ms2, + }) + } + + /// Read raw gyroscope data + pub fn read_raw_gyroscope(&mut self) -> Result { + let data = self.spi.read_registers(Register::OutXlG as u8, 6)?; + + let scale = match self.gyro_range { + GyroRange::Dps125 => Sensitivity::GYRO_125, + GyroRange::Dps250 => Sensitivity::GYRO_250, + GyroRange::Dps500 => Sensitivity::GYRO_500, + GyroRange::Dps1000 => Sensitivity::GYRO_1000, + GyroRange::Dps2000 => Sensitivity::GYRO_2000, + GyroRange::Dps4000 => Sensitivity::GYRO_4000, + }; + + let raw_x = LittleEndian::read_i16(&data[0..2]) as f32; + let raw_y = LittleEndian::read_i16(&data[2..4]) as f32; + let raw_z = LittleEndian::read_i16(&data[4..6]) as f32; + + // Convert from mdps to deg/s (divide by 1000) + Ok(Vector3 { + x: raw_x * scale / 1000.0, + y: raw_y * scale / 1000.0, + z: raw_z * scale / 1000.0, + }) + } + + /// Read temperature in Celsius + pub fn read_temperature(&mut self) -> Result { + let data = self.spi.read_registers(Register::OutTempL as u8, 2)?; + let raw_temp = LittleEndian::read_i16(&data[0..2]) as f32; + Ok(raw_temp * TEMP_SENSITIVITY + TEMP_OFFSET) + } + + /// Check if new data is available + pub fn is_data_ready(&mut self) -> Result { + let status = self.spi.read_register(Register::StatusReg as u8)?; + Ok((status & 0x03) != 0) // Check if either accel or gyro data is ready + } +} + +/// LSM6DSVReader runs a background thread to update sensor data continuously +pub struct Lsm6dsvReader { + data: Arc>, + command_tx: mpsc::Sender, + running: Arc>, +} + +/// Commands sent to the reading thread +#[derive(Debug)] +pub enum Lsm6dsvCommand { + SetAccelRange(AccelRange), + SetGyroRange(GyroRange), + SetAccelOdr(AccelOdr), + SetGyroOdr(GyroOdr), + Reset, + Stop, +} + +impl Lsm6dsvReader { + /// Create a new LSM6DSVReader + pub fn new(spi_device: &str) -> Result { + debug!("Initializing LSM6DSV Reader..."); + let data = Arc::new(RwLock::new(ImuData::default())); + let running = Arc::new(RwLock::new(true)); + let (command_tx, command_rx) = mpsc::channel(); + let reader = Lsm6dsvReader { + data: Arc::clone(&data), + command_tx, + running: Arc::clone(&running), + }; + reader.start_reading_thread(spi_device, command_rx)?; + Ok(reader) + } + + fn start_reading_thread( + &self, + spi_device: &str, + command_rx: mpsc::Receiver, + ) -> Result<(), ImuError> { + let data = Arc::clone(&self.data); + let running = Arc::clone(&self.running); + let spi_device = spi_device.to_string(); + let (tx, rx) = mpsc::channel(); + + thread::spawn(move || { + let init_result = Lsm6dsv::new(&spi_device); + if let Err(e) = init_result { + error!("Failed to initialize LSM6DSV: {}", e); + let _ = tx.send(Err(e)); + return; + } + let mut imu = init_result.unwrap(); + let _ = tx.send(Ok(())); + + while let Ok(guard) = running.read() { + if !*guard { + break; + } + + // Handle incoming commands + if let Ok(cmd) = command_rx.try_recv() { + match cmd { + Lsm6dsvCommand::SetAccelRange(range) => { + imu.accel_range = range; + // Update the control register + let range_val = range as u8; + let _ = imu.spi.read_register(Register::Ctrl1Xl as u8) + .and_then(|ctrl| { + imu.spi.write_register(Register::Ctrl1Xl as u8, (ctrl & 0x0F) | range_val) + }); + } + Lsm6dsvCommand::SetGyroRange(range) => { + imu.gyro_range = range; + // Update the control register + let range_val = range as u8; + let _ = imu.spi.read_register(Register::Ctrl2G as u8) + .and_then(|ctrl| { + imu.spi.write_register(Register::Ctrl2G as u8, (ctrl & 0x0F) | range_val) + }); + } + Lsm6dsvCommand::SetAccelOdr(odr) => { + // Update the control register + let odr_val = odr as u8; + let _ = imu.spi.read_register(Register::Ctrl1Xl as u8) + .and_then(|ctrl| { + imu.spi.write_register(Register::Ctrl1Xl as u8, (ctrl & 0xF0) | odr_val) + }); + } + Lsm6dsvCommand::SetGyroOdr(odr) => { + // Update the control register + let odr_val = odr as u8; + let _ = imu.spi.read_register(Register::Ctrl2G as u8) + .and_then(|ctrl| { + imu.spi.write_register(Register::Ctrl2G as u8, (ctrl & 0xF0) | odr_val) + }); + } + Lsm6dsvCommand::Reset => { + // Reinitialize if needed + imu = Lsm6dsv::new(&spi_device).unwrap(); + } + Lsm6dsvCommand::Stop => { + if let Ok(mut w) = running.write() { + *w = false; + } + break; + } + } + } + + // Read sensor data + let mut sensor_data = ImuData::default(); + + if let Ok(accel) = imu.read_raw_accelerometer() { + sensor_data.accelerometer = Some(accel); + } else { + warn!("Failed to read accelerometer"); + } + + if let Ok(gyro) = imu.read_raw_gyroscope() { + sensor_data.gyroscope = Some(gyro); + } else { + warn!("Failed to read gyroscope"); + } + + if let Ok(temp) = imu.read_temperature() { + sensor_data.temperature = Some(temp); + } + + if let Ok(mut shared) = data.write() { + *shared = sensor_data; + } + + thread::sleep(Duration::from_millis(10)); + } + }); + + rx.recv()? + } + + /// Reset the LSM6DSV sensor + pub fn reset(&self) -> Result<(), ImuError> { + self.command_tx.send(Lsm6dsvCommand::Reset)?; + Ok(()) + } + + /// Set accelerometer range + pub fn set_accel_range(&self, range: AccelRange) -> Result<(), ImuError> { + self.command_tx.send(Lsm6dsvCommand::SetAccelRange(range))?; + Ok(()) + } + + /// Set gyroscope range + pub fn set_gyro_range(&self, range: GyroRange) -> Result<(), ImuError> { + self.command_tx.send(Lsm6dsvCommand::SetGyroRange(range))?; + Ok(()) + } + + /// Set accelerometer output data rate + pub fn set_accel_odr(&self, odr: AccelOdr) -> Result<(), ImuError> { + self.command_tx.send(Lsm6dsvCommand::SetAccelOdr(odr))?; + Ok(()) + } + + /// Set gyroscope output data rate + pub fn set_gyro_odr(&self, odr: GyroOdr) -> Result<(), ImuError> { + self.command_tx.send(Lsm6dsvCommand::SetGyroOdr(odr))?; + Ok(()) + } +} + +impl ImuReader for Lsm6dsvReader { + /// Returns the most recent sensor data + fn get_data(&self) -> Result { + Ok(self.data.read().map(|data| *data)?) + } + + /// Stops the reading thread + fn stop(&self) -> Result<(), ImuError> { + self.command_tx.send(Lsm6dsvCommand::Stop)?; + Ok(()) + } +} + +impl Drop for Lsm6dsvReader { + fn drop(&mut self) { + let _ = self.stop(); + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_register_values() { + assert_eq!(Register::WhoAmI as u8, 0x0F); + assert_eq!(Register::Ctrl1Xl as u8, 0x10); + assert_eq!(Register::Ctrl2G as u8, 0x11); + assert_eq!(Constants::WHO_AM_I_VALUE, 0x70); + } + + #[test] + fn test_sensitivity_values() { + assert_eq!(Sensitivity::ACCEL_2G, 0.061); + assert_eq!(Sensitivity::GYRO_250, 8.75); + } +} + diff --git a/imu/drivers/lsm6dsv/src/registers.rs b/imu/drivers/lsm6dsv/src/registers.rs new file mode 100644 index 0000000..9737f0c --- /dev/null +++ b/imu/drivers/lsm6dsv/src/registers.rs @@ -0,0 +1,136 @@ +// LSM6DSV Register definitions and constants + +/// LSM6DSV Register addresses +#[repr(u8)] +pub enum Register { + // WHO_AM_I register + WhoAmI = 0x0F, + + // Control registers + Ctrl1Xl = 0x10, // Accelerometer control + Ctrl2G = 0x11, // Gyroscope control + Ctrl3C = 0x12, // Control register 3 + Ctrl4C = 0x13, // Control register 4 + Ctrl5C = 0x14, // Control register 5 + Ctrl6C = 0x15, // Control register 6 + Ctrl7G = 0x16, // Gyroscope control 2 + Ctrl8Xl = 0x17, // Accelerometer control 2 + Ctrl9Xl = 0x18, // Accelerometer control 3 + Ctrl10C = 0x19, // Control register 10 + + // Status registers + StatusReg = 0x1E, // Status register + + // Temperature register + OutTempL = 0x20, // Temperature output LSB + OutTempH = 0x21, // Temperature output MSB + + // Gyroscope output registers + OutXlG = 0x22, // Gyroscope X-axis LSB + OutXhG = 0x23, // Gyroscope X-axis MSB + OutYlG = 0x24, // Gyroscope Y-axis LSB + OutYhG = 0x25, // Gyroscope Y-axis MSB + OutZlG = 0x26, // Gyroscope Z-axis LSB + OutZhG = 0x27, // Gyroscope Z-axis MSB + + // Accelerometer output registers + OutXlA = 0x28, // Accelerometer X-axis LSB + OutXhA = 0x29, // Accelerometer X-axis MSB + OutYlA = 0x2A, // Accelerometer Y-axis LSB + OutYhA = 0x2B, // Accelerometer Y-axis MSB + OutZlA = 0x2C, // Accelerometer Z-axis LSB + OutZhA = 0x2D, // Accelerometer Z-axis MSB +} + +/// LSM6DSV Constants +pub mod Constants { + /// Expected WHO_AM_I value for LSM6DSV + pub const WHO_AM_I_VALUE: u8 = 0x70; + + /// Soft reset command + pub const SOFT_RESET_CMD: u8 = 0x01; + + /// Power-on command for accelerometer + pub const ACCEL_POWER_ON: u8 = 0x04; + + /// Power-on command for gyroscope + pub const GYRO_POWER_ON: u8 = 0x04; +} + +/// Accelerometer output data rate (ODR) settings +#[repr(u8)] +#[derive(Debug, Clone, Copy)] +pub enum AccelOdr { + Off = 0x00, + Hz12_5 = 0x01, + Hz26 = 0x02, + Hz52 = 0x03, + Hz104 = 0x04, + Hz208 = 0x05, + Hz416 = 0x06, + Hz833 = 0x07, + Hz1660 = 0x08, + Hz3330 = 0x09, + Hz6660 = 0x0A, +} + +/// Accelerometer full-scale range settings +#[repr(u8)] +#[derive(Debug, Clone, Copy)] +pub enum AccelRange { + G2 = 0x00, // ±2g + G4 = 0x02, // ±4g + G8 = 0x03, // ±8g + G16 = 0x01, // ±16g +} + +/// Gyroscope output data rate (ODR) settings +#[repr(u8)] +#[derive(Debug, Clone, Copy)] +pub enum GyroOdr { + Off = 0x00, + Hz12_5 = 0x01, + Hz26 = 0x02, + Hz52 = 0x03, + Hz104 = 0x04, + Hz208 = 0x05, + Hz416 = 0x06, + Hz833 = 0x07, + Hz1660 = 0x08, + Hz3330 = 0x09, + Hz6660 = 0x0A, +} + +/// Gyroscope full-scale range settings +#[repr(u8)] +#[derive(Debug, Clone, Copy)] +pub enum GyroRange { + Dps125 = 0x00, // ±125 dps + Dps250 = 0x01, // ±250 dps + Dps500 = 0x02, // ±500 dps + Dps1000 = 0x03, // ±1000 dps + Dps2000 = 0x04, // ±2000 dps + Dps4000 = 0x05, // ±4000 dps +} + +/// Sensitivity values for different ranges +pub mod Sensitivity { + // Accelerometer sensitivity (mg/LSB) + pub const ACCEL_2G: f32 = 0.061; // ±2g + pub const ACCEL_4G: f32 = 0.122; // ±4g + pub const ACCEL_8G: f32 = 0.244; // ±8g + pub const ACCEL_16G: f32 = 0.488; // ±16g + + // Gyroscope sensitivity (mdps/LSB) + pub const GYRO_125: f32 = 4.375; // ±125 dps + pub const GYRO_250: f32 = 8.75; // ±250 dps + pub const GYRO_500: f32 = 17.50; // ±500 dps + pub const GYRO_1000: f32 = 35.0; // ±1000 dps + pub const GYRO_2000: f32 = 70.0; // ±2000 dps + pub const GYRO_4000: f32 = 140.0; // ±4000 dps +} + +/// Temperature sensitivity (degC/LSB) +pub const TEMP_SENSITIVITY: f32 = 1.0 / 256.0; +pub const TEMP_OFFSET: f32 = 25.0; + diff --git a/imu/drivers/lsm6dsv/src/spi.rs b/imu/drivers/lsm6dsv/src/spi.rs new file mode 100644 index 0000000..3edefbe --- /dev/null +++ b/imu/drivers/lsm6dsv/src/spi.rs @@ -0,0 +1,83 @@ +// SPI communication layer for LSM6DSV + +use spidev::{Spidev, SpidevOptions, SpidevTransfer, SpiModeFlags}; +use imu_traits::ImuError; + +/// SPI communication wrapper for LSM6DSV +pub struct Lsm6dsvSpi { + spi: Spidev, +} + +impl Lsm6dsvSpi { + /// Initialize SPI communication with LSM6DSV + pub fn new(spi_device: &str) -> Result { + let mut spi = Spidev::open(spi_device) + .map_err(|e| ImuError::DeviceError(format!("Failed to open SPI device {}: {}", spi_device, e)))?; + + // Configure SPI mode and settings + let options = SpidevOptions::new() + .bits_per_word(8) + .max_speed_hz(10_000_000) // 10 MHz max speed for LSM6DSV + .mode(SpiModeFlags::SPI_MODE_0) // CPOL=0, CPHA=0 + .build(); + + spi.configure(&options) + .map_err(|e| ImuError::DeviceError(format!("Failed to configure SPI: {}", e)))?; + + Ok(Lsm6dsvSpi { spi }) + } + + /// Write a single byte to a register + pub fn write_register(&mut self, register: u8, value: u8) -> Result<(), ImuError> { + let tx_buf = [register & 0x7F, value]; // Clear MSB for write operation + let mut rx_buf = [0u8; 2]; + + let mut transfer = SpidevTransfer::read_write(&tx_buf, &mut rx_buf); + self.spi.transfer(&mut transfer) + .map_err(|e| ImuError::WriteError(format!("Failed to write register 0x{:02X}: {}", register, e)))?; + + Ok(()) + } + + /// Read a single byte from a register + pub fn read_register(&mut self, register: u8) -> Result { + let tx_buf = [register | 0x80, 0x00]; // Set MSB for read operation + let mut rx_buf = [0u8; 2]; + + let mut transfer = SpidevTransfer::read_write(&tx_buf, &mut rx_buf); + self.spi.transfer(&mut transfer) + .map_err(|e| ImuError::ReadError(format!("Failed to read register 0x{:02X}: {}", register, e)))?; + + Ok(rx_buf[1]) + } + + /// Read multiple bytes from consecutive registers + pub fn read_registers(&mut self, start_register: u8, count: usize) -> Result, ImuError> { + let mut tx_buf = vec![0u8; count + 1]; + let mut rx_buf = vec![0u8; count + 1]; + + tx_buf[0] = start_register | 0x80; // Set MSB for read operation + + let mut transfer = SpidevTransfer::read_write(&tx_buf, &mut rx_buf); + self.spi.transfer(&mut transfer) + .map_err(|e| ImuError::ReadError(format!("Failed to read registers starting at 0x{:02X}: {}", start_register, e)))?; + + Ok(rx_buf[1..].to_vec()) + } + + /// Write multiple bytes to consecutive registers + pub fn write_registers(&mut self, start_register: u8, values: &[u8]) -> Result<(), ImuError> { + let mut tx_buf = vec![0u8; values.len() + 1]; + let mut rx_buf = vec![0u8; values.len() + 1]; + + tx_buf[0] = start_register & 0x7F; // Clear MSB for write operation + tx_buf[1..].copy_from_slice(values); + + let mut transfer = SpidevTransfer::read_write(&tx_buf, &mut rx_buf); + self.spi.transfer(&mut transfer) + .map_err(|e| ImuError::WriteError(format!("Failed to write registers starting at 0x{:02X}: {}", start_register, e)))?; + + Ok(()) + } +} + diff --git a/imu/src/lib.rs b/imu/src/lib.rs index 5f69c73..f0054d7 100644 --- a/imu/src/lib.rs +++ b/imu/src/lib.rs @@ -4,6 +4,8 @@ pub use hexmove::HexmoveImuReader; pub use linux_bmi088::Bmi088Reader; #[cfg(target_os = "linux")] pub use linux_bno055::Bno055Reader; +#[cfg(target_os = "linux")] +pub use lsm6dsv::Lsm6dsvReader; pub use hiwonder::HiwonderReader; pub use hiwonder::Output as HiwonderOutput; diff --git a/tests/lsm6dsv/monitor_imu.py b/tests/lsm6dsv/monitor_imu.py new file mode 100644 index 0000000..2527a7b --- /dev/null +++ b/tests/lsm6dsv/monitor_imu.py @@ -0,0 +1,208 @@ +#!/usr/bin/env python3 +""" +Real-time IMU monitoring script +Continuously reads and displays the current state of the LSM6DSV IMU +""" + +import sys +import os +import time +import signal + +# Add the project root to the path so we can import the imu module +project_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +sys.path.insert(0, project_root) + +try: + from imu import create_lsm6dsvtr, Vector3, Quaternion + print("✓ Successfully imported LSM6DSV functions") +except ImportError as e: + print(f"✗ Failed to import LSM6DSV functions: {e}") + sys.exit(1) + +class IMUMonitor: + def __init__(self, spi_device="/dev/spidev1.0", update_rate=0.1): + """Initialize the IMU monitor + + Args: + spi_device: SPI device path (default: /dev/spidev1.0) + update_rate: Update rate in seconds (default: 0.1 = 10 Hz) + """ + self.spi_device = spi_device + self.update_rate = update_rate + self.reader = None + self.running = False + + def connect(self): + """Connect to the IMU""" + try: + print(f"Connecting to LSM6DSV on {self.spi_device}...") + self.reader = create_lsm6dsvtr(self.spi_device) + print("✓ Successfully connected to LSM6DSV") + return True + except Exception as e: + print(f"✗ Failed to connect to LSM6DSV: {e}") + return False + + def format_vector(self, vector, unit=""): + """Format a Vector3 for display""" + if vector is None: + return "N/A" + return f"x={vector.x:8.3f}, y={vector.y:8.3f}, z={vector.z:8.3f} {unit}" + + def format_quaternion(self, quat): + """Format a Quaternion for display""" + if quat is None: + return "N/A" + return f"w={quat.w:6.3f}, x={quat.x:6.3f}, y={quat.y:6.3f}, z={quat.z:6.3f}" + + def display_data(self, data): + """Display the IMU data in a formatted way""" + # Clear screen and move cursor to top + print("\033[2J\033[H", end="") + + print("=" * 80) + print(" LSM6DSV IMU Real-Time Monitor") + print("=" * 80) + print(f"Device: {self.spi_device} | Update Rate: {1/self.update_rate:.1f} Hz") + print("=" * 80) + + # Accelerometer data + if data.get('accelerometer'): + accel = data['accelerometer'] + print(f"Accelerometer (m/s²): {self.format_vector(accel, 'm/s²')}") + else: + print("Accelerometer: N/A") + + # Gyroscope data + if data.get('gyroscope'): + gyro = data['gyroscope'] + print(f"Gyroscope (deg/s): {self.format_vector(gyro, 'deg/s')}") + else: + print("Gyroscope: N/A") + + # Magnetometer data + if data.get('magnetometer'): + mag = data['magnetometer'] + print(f"Magnetometer (μT): {self.format_vector(mag, 'μT')}") + else: + print("Magnetometer: N/A") + + # Quaternion data + if data.get('quaternion'): + quat = data['quaternion'] + print(f"Quaternion: {self.format_quaternion(quat)}") + else: + print("Quaternion: N/A") + + # Euler angles + if data.get('euler'): + euler = data['euler'] + print(f"Euler Angles (deg): {self.format_vector(euler, 'deg')}") + else: + print("Euler Angles: N/A") + + # Linear acceleration + if data.get('linear_acceleration'): + lin_accel = data['linear_acceleration'] + print(f"Linear Accel (m/s²): {self.format_vector(lin_accel, 'm/s²')}") + else: + print("Linear Acceleration: N/A") + + # Gravity vector + if data.get('gravity'): + gravity = data['gravity'] + print(f"Gravity (m/s²): {self.format_vector(gravity, 'm/s²')}") + else: + print("Gravity: N/A") + + # Temperature + if data.get('temperature') is not None: + temp = data['temperature'] + print(f"Temperature: {temp:6.1f}°C") + else: + print("Temperature: N/A") + + # Calibration status + if data.get('calibration_status') is not None: + cal = data['calibration_status'] + print(f"Calibration Status: {cal}") + else: + print("Calibration Status: N/A") + + print("=" * 80) + print("Press Ctrl+C to stop monitoring") + print("=" * 80) + + def run(self): + """Run the continuous monitoring loop""" + if not self.connect(): + return + + self.running = True + + # Set up signal handler for graceful shutdown + def signal_handler(sig, frame): + print("\n\nShutting down gracefully...") + self.running = False + + signal.signal(signal.SIGINT, signal_handler) + + try: + while self.running: + try: + # Read data from IMU + data = self.reader.get_data() + self.display_data(data) + + # Wait for next update + time.sleep(self.update_rate) + + except KeyboardInterrupt: + break + except Exception as e: + print(f"\nError reading IMU data: {e}") + print("Retrying in 1 second...") + time.sleep(1) + + finally: + self.cleanup() + + def cleanup(self): + """Clean up resources""" + if self.reader: + try: + self.reader.stop() + print("✓ IMU reader stopped successfully") + except Exception as e: + print(f"Warning: Error stopping IMU reader: {e}") + +def main(): + """Main function""" + print("LSM6DSV IMU Real-Time Monitor") + print("=" * 40) + + # Parse command line arguments + spi_device = "/dev/spidev1.0" + update_rate = 0.1 # 10 Hz + + if len(sys.argv) > 1: + spi_device = sys.argv[1] + + if len(sys.argv) > 2: + try: + update_rate = float(sys.argv[2]) + except ValueError: + print("Error: Update rate must be a number") + sys.exit(1) + + print(f"Using SPI device: {spi_device}") + print(f"Update rate: {1/update_rate:.1f} Hz") + print() + + # Create and run monitor + monitor = IMUMonitor(spi_device, update_rate) + monitor.run() + +if __name__ == "__main__": + main() diff --git a/tests/lsm6dsv/test_lsm6dsv.py b/tests/lsm6dsv/test_lsm6dsv.py new file mode 100644 index 0000000..91d35cc --- /dev/null +++ b/tests/lsm6dsv/test_lsm6dsv.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +""" +Test script for LSM6DSV IMU driver +""" + +import sys +import os +import time + +# Add the project root to the path so we can import the imu module +project_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +sys.path.insert(0, project_root) + +try: + from imu import create_lsm6dsvtr, Vector3, Quaternion + print("✓ Successfully imported LSM6DSV functions") +except ImportError as e: + print(f"✗ Failed to import LSM6DSV functions: {e}") + sys.exit(1) + +def test_lsm6dsv_creation(): + """Test creating an LSM6DSV reader instance""" + try: + # Try to create an LSM6DSV reader (this will fail if SPI device doesn't exist) + reader = create_lsm6dsvtr("/dev/spidev1.0") + print("✓ Successfully created LSM6DSV reader") + return reader + except Exception as e: + print(f"✗ Failed to create LSM6DSV reader: {e}") + print(" This is expected if the SPI device doesn't exist or permissions are insufficient") + return None + +def test_data_reading(reader): + """Test reading data from the LSM6DSV""" + if reader is None: + print("⚠ Skipping data reading test (no reader available)") + return + + try: + # Try to read data + data = reader.get_data() + print("✓ Successfully read data from LSM6DSV") + + # Print available data + if data.get('accelerometer'): + accel = data['accelerometer'] + print(f" Accelerometer: x={accel.x:.3f}, y={accel.y:.3f}, z={accel.z:.3f} m/s²") + + if data.get('gyroscope'): + gyro = data['gyroscope'] + print(f" Gyroscope: x={gyro.x:.3f}, y={gyro.y:.3f}, z={gyro.z:.3f} deg/s") + + if data.get('temperature') is not None: + temp = data['temperature'] + print(f" Temperature: {temp:.1f}°C") + + except Exception as e: + print(f"✗ Failed to read data from LSM6DSV: {e}") + +def main(): + print("LSM6DSV IMU Driver Test") + print("=" * 30) + + # Test 1: Import functions + print("\n1. Testing imports...") + print("✓ All imports successful") + + # Test 2: Create reader + print("\n2. Testing reader creation...") + reader = test_lsm6dsv_creation() + + # Test 3: Read data + print("\n3. Testing data reading...") + test_data_reading(reader) + + # Test 4: Cleanup + if reader: + print("\n4. Testing cleanup...") + try: + reader.stop() + print("✓ Successfully stopped reader") + except Exception as e: + print(f"✗ Failed to stop reader: {e}") + + print("\n" + "=" * 30) + print("Test completed!") + +if __name__ == "__main__": + main()