From b316f42a2dc96240d5643b1571d1ff88f91c65d5 Mon Sep 17 00:00:00 2001 From: Adam Go Date: Sat, 5 Oct 2024 15:24:02 -0600 Subject: [PATCH 1/6] -UNTESTED singleton driver for the ICM20948 9-axis IMU --- Components/ICM20948_Driver.cpp | 223 +++++++++++++++++++++++++++++++++ Components/ICM20948_Driver.h | 140 +++++++++++++++++++++ 2 files changed, 363 insertions(+) create mode 100644 Components/ICM20948_Driver.cpp create mode 100644 Components/ICM20948_Driver.h diff --git a/Components/ICM20948_Driver.cpp b/Components/ICM20948_Driver.cpp new file mode 100644 index 0000000..ca62a2e --- /dev/null +++ b/Components/ICM20948_Driver.cpp @@ -0,0 +1,223 @@ +/* + * IMUDriver.cpp + * + * Created on: Aug 31, 2024 + * Author: goada + */ + +#include + +/* @brief Initialize the driver. Must be called before any other functions can be used. + * @param hspi_ Pointer to the SPI handle + * @param cs_gpio_ GPIO port for the chip select pin (GPIOA, GPIOB ...) + * @param cs_pin_ Pin number for the chip select + */ +void IMUDriver::Init(SPI_HandleTypeDef* hspi_, GPIO_TypeDef* cs_gpio_, uint16_t cs_pin_) { + hspi = hspi_; + initialized = true; + SetCSPin(cs_gpio_, cs_pin_); + CSHigh(); + + uint8_t ID = GetRegister(ICM20948_REG::WHO_AM_I); + if(ID != ICM20948_ID) { + // couldn't get chip ID + initialized = false; + return; + } + + // The ICM20948 contains an AK09916 magnetometer, which is on the same chip but is separate and + // communicates internally with the rest of the module via I2C. The module itself communicates + // with the microcontroller via SPI + + SetRegister(ICM20948_REG::USER_CTL, 0b00110000); // lock to SPI mode, and enable I2C master for the magnetometer + + SetRegister(ICM20948_REG::I2C_MST_CTRL, 0b00000011); // set I2C clock frequency + + SetRegister(ICM20948_REG::PWR_MGMT_1, 0b00000001); // wake from sleep + + SetRegister(ICM20948_REG::FIFO_EN_2, 0b00011111); // enable accel, gyro and temp FIFO. + + // can either use FIFO to get accel/gyro/(temp) or read most recent from registers directly + + MagRegWrite(AK09916_REG::CNTL2, 0b00010); // set mag to continuous mode + + SetMagReadLocation(0x01, 1); + uint8_t magID = GetRegister(ICM20948_REG::EXT_SLV_SENS_DATA_00); + if(magID != 0b00001001) { + // couldn't get magnetometer ID + initialized = false; + return; + } + + SetMagReadLocation(AK09916_REG::MEAS, 8); // prepare mag for reading + + +} + +/* @brief Sets a single 8-bit register. + * @param reg The register to set. Constants contained in ICM20948_REG namespace. + * @param val Value to set the register to. + * @return Success + */ +bool IMUDriver::SetRegister(ICM20948_REGISTER_t reg, uint8_t val) { + assert(initialized); + if(reg.bank <= 0b11) { + SwitchBank(reg.bank); + } + uint8_t data[2] = {(uint8_t)(0b00000000 | (reg.addr&0x7F)),val}; + CSLow(); + HAL_StatusTypeDef r = HAL_SPI_Transmit(hspi, data, 2, 1000); + CSHigh(); + return r == HAL_OK; + +} + +/* @brief Gets a single 8-bit register. + * @param reg The register to get. Constants contained in ICM20948_REG namespace. + * @return Value read from the register. + */ +uint8_t IMUDriver::GetRegister(ICM20948_REGISTER_t reg) { + assert(initialized); + if(reg.bank <= 0b11) { + SwitchBank(reg.bank); + } + uint8_t data[2] = {(uint8_t)(0b10000000 | reg.addr),SPI_DUMMY_BYTE}; + uint8_t incoming[2] = {0,0}; + CSLow(); + HAL_SPI_TransmitReceive(hspi, data, incoming, 2, 1000); + CSHigh(); + return incoming[1]; +} + +/* @brief Reads multiple successive registers in a row. + * @param startreg The register that the readings should start at. + * @param numBytes Number of bytes to read. + * @param out Address of buffer to receive data. Must be numBytes long. + */ +void IMUDriver::GetMultipleRegisters(ICM20948_REGISTER_t startreg, int numBytes, + uint8_t *out) { + assert(initialized); + if(startreg.bank <= 0b11) { + SwitchBank(startreg.bank); + } + uint8_t transmit[numBytes+1] = {SPI_DUMMY_BYTE}; + transmit[0] = (uint8_t)(0b10000000 | startreg.addr); + + CSLow(); + HAL_SPI_TransmitReceive(hspi, transmit, out, numBytes+1, 1000); + CSHigh(); +} + +/* @brief Repeatedly reads the top byte of the FIFO. + * @param numReads Number of repetitive reads to perform. + * @param out Buffer to receive data in. Must be numReads long. + */ +void IMUDriver::ReadFIFO(int numReads, uint8_t *out) { + assert(initialized); + for(int i = 0; i < numReads; i++) { + out[i] = GetRegister(ICM20948_REG::FIFO_RW); + } +} + +/* @brief Sets the mem location that the magnetometer will read from\ + * @param addr The memorhy location + * @param len Number of bytes to read + */ +void IMUDriver::SetMagReadLocation(uint8_t addr, uint8_t len) { + assert(initialized); + SetRegister(ICM20948_REG::I2C_SLV0_ADDR, MAGNETOMETER_ID | 0b10000000); + SetRegister(ICM20948_REG::I2C_SLV0_REG, addr); + SetRegister(ICM20948_REG::I2C_SLV0_CTRL, 0b10000000 | (len & 0x7f)); +} + +/* @brief Writes an 8-bit register in the magnetometer + * @param addr Address of register + * @param val Value to write + */ +void IMUDriver::MagRegWrite(uint8_t addr, uint8_t val) { + assert(initialized); + SetRegister(ICM20948_REG::I2C_SLV0_ADDR, MAGNETOMETER_ID); + SetRegister(ICM20948_REG::I2C_SLV0_REG, addr); + SetRegister(ICM20948_REG::I2C_SLV0_DO, val); +} + +/* @brief Extract IMU data from a raw byte buffer (such as from ReadAllSensorRegs) into a struct. + * @param buf Input buffer containing raw data. + * @param accel Buffer includes accel data + * @param gyro Buffer includes gyroscope data + * @param temp Buffer includes temperature data + * @param mag Buffer includes magnetometer data + * @return Struct containing extracted data + */ +const ICM20948_DATA_t IMUDriver::GetDataFromBuf(const uint8_t *buf, bool accel, bool gyro, bool temp, bool mag) { + ICM20948_DATA_t out; + size_t i = 0; + + // Accel gyro and temp are big-endian + if(accel) { + out.accel.x = (buf[i ] << 8) | buf[i+1]; + out.accel.y = (buf[i+2] << 8) | buf[i+3]; + out.accel.z = (buf[i+4] << 8) | buf[i+5]; + i += 6; + } + + if(gyro) { + + out.gyro.x = (buf[i ] << 8) | buf[i+1]; + out.gyro.y = (buf[i+2] << 8) | buf[i+3]; + out.gyro.z = (buf[i+4] << 8) | buf[i+5]; + i += 6; + } + + if(temp) { + out.temp = (buf[i] << 8) | buf[i+1]; + i += 2; + } + + // Magnetometer is little-endian + if(mag) { + + out.mag.x = (buf[i+1] << 8) | buf[i]; + out.mag.y = (buf[i+3] << 8) | buf[i+2]; + out.mag.z = (buf[i+5] << 8) | buf[i+4]; + } + + return out; + +} + +/* @brief Reads all 20 sensor registers in order. + * @param out Raw bytes read from registers. Must be 20 bytes long + */ +void IMUDriver::ReadAllSensorRegs(uint8_t* out) { + GetMultipleRegisters(ICM20948_REG::ACCEL_XOUT_H, 20, out); +} + +/* @brief Switches between register banks 0-3. + * @param bank Bank to switch to. Should be from 0-3. + * @return Success + */ +bool IMUDriver::SwitchBank(uint8_t bank) { + return SetRegister(ICM20948_REG::BANK, (bank & 0b11) << 4); +} + +IMUDriver::IMUDriver() { +} + +IMUDriver::~IMUDriver() { +} + +void IMUDriver::CSLow() { + assert(initialized); + HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_RESET); +} + +void IMUDriver::SetCSPin(GPIO_TypeDef* gpio, uint16_t pin) { + cs_gpio = gpio; + cs_pin = pin; +} + +void IMUDriver::CSHigh() { + assert(initialized); + HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_SET); +} diff --git a/Components/ICM20948_Driver.h b/Components/ICM20948_Driver.h new file mode 100644 index 0000000..cdbd097 --- /dev/null +++ b/Components/ICM20948_Driver.h @@ -0,0 +1,140 @@ +/* + * IMUDriver.h + * + * Created on: Aug 31, 2024 + * Author: goada + */ + +#ifndef ICM20948_DRIVER_H_ +#define ICM20948_DRIVER_H_ + +#include "stm32h7xx.h" + +constexpr uint8_t SPI_DUMMY_BYTE = 0x00; +constexpr uint8_t ICM20948_ID = 0xEA; +constexpr uint8_t MAGNETOMETER_ID = 0x0C; + +struct ICM20948_REGISTER_t { + uint8_t bank; + uint8_t addr; +}; + +struct ACCEL_t { + int16_t x; + int16_t y; + int16_t z; +}; + +struct GYRO_t { + int16_t x; + int16_t y; + int16_t z; +}; + +struct MAG_t { + int16_t x; + int16_t y; + int16_t z; +}; + +struct ICM20948_DATA_t { + ACCEL_t accel; + GYRO_t gyro; + int16_t temp; + MAG_t mag; +}; + +/* @brief Singleton SPI driver for the ICM20948 IMU. + * Must call Init before using any other functions. + * Max SPI frequency 7MHz. + */ +class IMUDriver { +public: + IMUDriver(); + ~IMUDriver(); + + static IMUDriver& Inst() { + static IMUDriver inst; + return inst; + } + + void Init(SPI_HandleTypeDef* hspi, GPIO_TypeDef* cs_gpio_, uint16_t cs_pin_); + + bool SetRegister(ICM20948_REGISTER_t reg, uint8_t val); + uint8_t GetRegister(ICM20948_REGISTER_t reg); + void GetMultipleRegisters(ICM20948_REGISTER_t startreg, int numBytes, uint8_t* out); + void ReadAllSensorRegs(uint8_t* out); + void ReadFIFO(int numReads, uint8_t* out); + + void SetMagReadLocation(uint8_t addr, uint8_t len); + void MagRegWrite(uint8_t addr, uint8_t val); + + IMUDriver(const IMUDriver&) = delete; + IMUDriver& operator=(const IMUDriver&) = delete; + + const ICM20948_DATA_t GetDataFromBuf(const uint8_t *buf, bool accel = true, bool gyro = true, bool temp = true, bool mag = true); + + void SetCSPin(GPIO_TypeDef* gpio, uint16_t pin); + +private: + bool initialized = false; + SPI_HandleTypeDef* hspi = nullptr; + GPIO_TypeDef* cs_gpio; + uint16_t cs_pin; + + bool SwitchBank(uint8_t bank); + + void CSLow(); + void CSHigh(); + +}; + + + +// Constants for registers in the ICM20948 IMU +// First byte is the bank in which it is located +// (0xff means it is located in the same place in every bank. Only used by the bank switch register) +// Second byte is address within bank +namespace ICM20948_REG { + + static const ICM20948_REGISTER_t BANK = {0xff,0x7f}; + static const ICM20948_REGISTER_t WHO_AM_I = {0,0x00}; + static const ICM20948_REGISTER_t USER_CTL = {0,0x03}; + static const ICM20948_REGISTER_t FIFO_EN_2 = {0,0x67}; + static const ICM20948_REGISTER_t PWR_MGMT_1 = {0,0x06}; + + static const ICM20948_REGISTER_t ACCEL_XOUT_H = {0,0x2d}; + static const ICM20948_REGISTER_t ACCEL_XOUT_L = {0,0x2e}; + static const ICM20948_REGISTER_t ACCEL_YOUT_H = {0,0x2f}; + static const ICM20948_REGISTER_t ACCEL_YOUT_L = {0,0x30}; + static const ICM20948_REGISTER_t ACCEL_ZOUT_H = {0,0x31}; + static const ICM20948_REGISTER_t ACCEL_ZOUT_L = {0,0x32}; + + static const ICM20948_REGISTER_t GYRO_XOUT_H = {0,0x33}; + static const ICM20948_REGISTER_t GYRO_XOUT_L = {0,0x34}; + static const ICM20948_REGISTER_t GYRO_YOUT_H = {0,0x35}; + static const ICM20948_REGISTER_t GYRO_YOUT_L = {0,0x36}; + static const ICM20948_REGISTER_t GYRO_ZOUT_H = {0,0x37}; + static const ICM20948_REGISTER_t GYRO_ZOUT_L = {0,0x38}; + + static const ICM20948_REGISTER_t TEMP_OUT_H = {0,0x39}; + static const ICM20948_REGISTER_t TEMP_OUT_L = {0,0x3a}; + + static const ICM20948_REGISTER_t FIFO_RW = {0,0x72}; + + static const ICM20948_REGISTER_t I2C_MST_CTRL = {3,0x01}; + static const ICM20948_REGISTER_t I2C_SLV0_ADDR = {3,0x03}; + static const ICM20948_REGISTER_t I2C_SLV0_REG = {3,0x04}; + static const ICM20948_REGISTER_t I2C_SLV0_CTRL = {3,0x05}; + static const ICM20948_REGISTER_t I2C_SLV0_DO = {3,0x06}; + + static const ICM20948_REGISTER_t EXT_SLV_SENS_DATA_00 = {0,0x3b}; +} + +// Constants for registers in the AK09916 magnetometer in the IMU +namespace AK09916_REG { + static const uint8_t CNTL2 = 0x31; + static const uint8_t MEAS = 0x11; +} + +#endif /* ICM20948_DRIVER_H_ */ From 3d19b17b412119f5f777ec5d05fda99d1ec18234 Mon Sep 17 00:00:00 2001 From: Adam Go Date: Sat, 5 Oct 2024 18:00:22 -0600 Subject: [PATCH 2/6] - UNTESTED LIS3MDLTR magnetometer driver --- Components/ICM20948_Driver.cpp | 223 --------------------------------- Components/ICM20948_Driver.h | 140 --------------------- Components/LIS3MDLTRDriver.cpp | 144 +++++++++++++++++++++ Components/LIS3MDLTRDriver.h | 96 ++++++++++++++ 4 files changed, 240 insertions(+), 363 deletions(-) delete mode 100644 Components/ICM20948_Driver.cpp delete mode 100644 Components/ICM20948_Driver.h create mode 100644 Components/LIS3MDLTRDriver.cpp create mode 100644 Components/LIS3MDLTRDriver.h diff --git a/Components/ICM20948_Driver.cpp b/Components/ICM20948_Driver.cpp deleted file mode 100644 index ca62a2e..0000000 --- a/Components/ICM20948_Driver.cpp +++ /dev/null @@ -1,223 +0,0 @@ -/* - * IMUDriver.cpp - * - * Created on: Aug 31, 2024 - * Author: goada - */ - -#include - -/* @brief Initialize the driver. Must be called before any other functions can be used. - * @param hspi_ Pointer to the SPI handle - * @param cs_gpio_ GPIO port for the chip select pin (GPIOA, GPIOB ...) - * @param cs_pin_ Pin number for the chip select - */ -void IMUDriver::Init(SPI_HandleTypeDef* hspi_, GPIO_TypeDef* cs_gpio_, uint16_t cs_pin_) { - hspi = hspi_; - initialized = true; - SetCSPin(cs_gpio_, cs_pin_); - CSHigh(); - - uint8_t ID = GetRegister(ICM20948_REG::WHO_AM_I); - if(ID != ICM20948_ID) { - // couldn't get chip ID - initialized = false; - return; - } - - // The ICM20948 contains an AK09916 magnetometer, which is on the same chip but is separate and - // communicates internally with the rest of the module via I2C. The module itself communicates - // with the microcontroller via SPI - - SetRegister(ICM20948_REG::USER_CTL, 0b00110000); // lock to SPI mode, and enable I2C master for the magnetometer - - SetRegister(ICM20948_REG::I2C_MST_CTRL, 0b00000011); // set I2C clock frequency - - SetRegister(ICM20948_REG::PWR_MGMT_1, 0b00000001); // wake from sleep - - SetRegister(ICM20948_REG::FIFO_EN_2, 0b00011111); // enable accel, gyro and temp FIFO. - - // can either use FIFO to get accel/gyro/(temp) or read most recent from registers directly - - MagRegWrite(AK09916_REG::CNTL2, 0b00010); // set mag to continuous mode - - SetMagReadLocation(0x01, 1); - uint8_t magID = GetRegister(ICM20948_REG::EXT_SLV_SENS_DATA_00); - if(magID != 0b00001001) { - // couldn't get magnetometer ID - initialized = false; - return; - } - - SetMagReadLocation(AK09916_REG::MEAS, 8); // prepare mag for reading - - -} - -/* @brief Sets a single 8-bit register. - * @param reg The register to set. Constants contained in ICM20948_REG namespace. - * @param val Value to set the register to. - * @return Success - */ -bool IMUDriver::SetRegister(ICM20948_REGISTER_t reg, uint8_t val) { - assert(initialized); - if(reg.bank <= 0b11) { - SwitchBank(reg.bank); - } - uint8_t data[2] = {(uint8_t)(0b00000000 | (reg.addr&0x7F)),val}; - CSLow(); - HAL_StatusTypeDef r = HAL_SPI_Transmit(hspi, data, 2, 1000); - CSHigh(); - return r == HAL_OK; - -} - -/* @brief Gets a single 8-bit register. - * @param reg The register to get. Constants contained in ICM20948_REG namespace. - * @return Value read from the register. - */ -uint8_t IMUDriver::GetRegister(ICM20948_REGISTER_t reg) { - assert(initialized); - if(reg.bank <= 0b11) { - SwitchBank(reg.bank); - } - uint8_t data[2] = {(uint8_t)(0b10000000 | reg.addr),SPI_DUMMY_BYTE}; - uint8_t incoming[2] = {0,0}; - CSLow(); - HAL_SPI_TransmitReceive(hspi, data, incoming, 2, 1000); - CSHigh(); - return incoming[1]; -} - -/* @brief Reads multiple successive registers in a row. - * @param startreg The register that the readings should start at. - * @param numBytes Number of bytes to read. - * @param out Address of buffer to receive data. Must be numBytes long. - */ -void IMUDriver::GetMultipleRegisters(ICM20948_REGISTER_t startreg, int numBytes, - uint8_t *out) { - assert(initialized); - if(startreg.bank <= 0b11) { - SwitchBank(startreg.bank); - } - uint8_t transmit[numBytes+1] = {SPI_DUMMY_BYTE}; - transmit[0] = (uint8_t)(0b10000000 | startreg.addr); - - CSLow(); - HAL_SPI_TransmitReceive(hspi, transmit, out, numBytes+1, 1000); - CSHigh(); -} - -/* @brief Repeatedly reads the top byte of the FIFO. - * @param numReads Number of repetitive reads to perform. - * @param out Buffer to receive data in. Must be numReads long. - */ -void IMUDriver::ReadFIFO(int numReads, uint8_t *out) { - assert(initialized); - for(int i = 0; i < numReads; i++) { - out[i] = GetRegister(ICM20948_REG::FIFO_RW); - } -} - -/* @brief Sets the mem location that the magnetometer will read from\ - * @param addr The memorhy location - * @param len Number of bytes to read - */ -void IMUDriver::SetMagReadLocation(uint8_t addr, uint8_t len) { - assert(initialized); - SetRegister(ICM20948_REG::I2C_SLV0_ADDR, MAGNETOMETER_ID | 0b10000000); - SetRegister(ICM20948_REG::I2C_SLV0_REG, addr); - SetRegister(ICM20948_REG::I2C_SLV0_CTRL, 0b10000000 | (len & 0x7f)); -} - -/* @brief Writes an 8-bit register in the magnetometer - * @param addr Address of register - * @param val Value to write - */ -void IMUDriver::MagRegWrite(uint8_t addr, uint8_t val) { - assert(initialized); - SetRegister(ICM20948_REG::I2C_SLV0_ADDR, MAGNETOMETER_ID); - SetRegister(ICM20948_REG::I2C_SLV0_REG, addr); - SetRegister(ICM20948_REG::I2C_SLV0_DO, val); -} - -/* @brief Extract IMU data from a raw byte buffer (such as from ReadAllSensorRegs) into a struct. - * @param buf Input buffer containing raw data. - * @param accel Buffer includes accel data - * @param gyro Buffer includes gyroscope data - * @param temp Buffer includes temperature data - * @param mag Buffer includes magnetometer data - * @return Struct containing extracted data - */ -const ICM20948_DATA_t IMUDriver::GetDataFromBuf(const uint8_t *buf, bool accel, bool gyro, bool temp, bool mag) { - ICM20948_DATA_t out; - size_t i = 0; - - // Accel gyro and temp are big-endian - if(accel) { - out.accel.x = (buf[i ] << 8) | buf[i+1]; - out.accel.y = (buf[i+2] << 8) | buf[i+3]; - out.accel.z = (buf[i+4] << 8) | buf[i+5]; - i += 6; - } - - if(gyro) { - - out.gyro.x = (buf[i ] << 8) | buf[i+1]; - out.gyro.y = (buf[i+2] << 8) | buf[i+3]; - out.gyro.z = (buf[i+4] << 8) | buf[i+5]; - i += 6; - } - - if(temp) { - out.temp = (buf[i] << 8) | buf[i+1]; - i += 2; - } - - // Magnetometer is little-endian - if(mag) { - - out.mag.x = (buf[i+1] << 8) | buf[i]; - out.mag.y = (buf[i+3] << 8) | buf[i+2]; - out.mag.z = (buf[i+5] << 8) | buf[i+4]; - } - - return out; - -} - -/* @brief Reads all 20 sensor registers in order. - * @param out Raw bytes read from registers. Must be 20 bytes long - */ -void IMUDriver::ReadAllSensorRegs(uint8_t* out) { - GetMultipleRegisters(ICM20948_REG::ACCEL_XOUT_H, 20, out); -} - -/* @brief Switches between register banks 0-3. - * @param bank Bank to switch to. Should be from 0-3. - * @return Success - */ -bool IMUDriver::SwitchBank(uint8_t bank) { - return SetRegister(ICM20948_REG::BANK, (bank & 0b11) << 4); -} - -IMUDriver::IMUDriver() { -} - -IMUDriver::~IMUDriver() { -} - -void IMUDriver::CSLow() { - assert(initialized); - HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_RESET); -} - -void IMUDriver::SetCSPin(GPIO_TypeDef* gpio, uint16_t pin) { - cs_gpio = gpio; - cs_pin = pin; -} - -void IMUDriver::CSHigh() { - assert(initialized); - HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_SET); -} diff --git a/Components/ICM20948_Driver.h b/Components/ICM20948_Driver.h deleted file mode 100644 index cdbd097..0000000 --- a/Components/ICM20948_Driver.h +++ /dev/null @@ -1,140 +0,0 @@ -/* - * IMUDriver.h - * - * Created on: Aug 31, 2024 - * Author: goada - */ - -#ifndef ICM20948_DRIVER_H_ -#define ICM20948_DRIVER_H_ - -#include "stm32h7xx.h" - -constexpr uint8_t SPI_DUMMY_BYTE = 0x00; -constexpr uint8_t ICM20948_ID = 0xEA; -constexpr uint8_t MAGNETOMETER_ID = 0x0C; - -struct ICM20948_REGISTER_t { - uint8_t bank; - uint8_t addr; -}; - -struct ACCEL_t { - int16_t x; - int16_t y; - int16_t z; -}; - -struct GYRO_t { - int16_t x; - int16_t y; - int16_t z; -}; - -struct MAG_t { - int16_t x; - int16_t y; - int16_t z; -}; - -struct ICM20948_DATA_t { - ACCEL_t accel; - GYRO_t gyro; - int16_t temp; - MAG_t mag; -}; - -/* @brief Singleton SPI driver for the ICM20948 IMU. - * Must call Init before using any other functions. - * Max SPI frequency 7MHz. - */ -class IMUDriver { -public: - IMUDriver(); - ~IMUDriver(); - - static IMUDriver& Inst() { - static IMUDriver inst; - return inst; - } - - void Init(SPI_HandleTypeDef* hspi, GPIO_TypeDef* cs_gpio_, uint16_t cs_pin_); - - bool SetRegister(ICM20948_REGISTER_t reg, uint8_t val); - uint8_t GetRegister(ICM20948_REGISTER_t reg); - void GetMultipleRegisters(ICM20948_REGISTER_t startreg, int numBytes, uint8_t* out); - void ReadAllSensorRegs(uint8_t* out); - void ReadFIFO(int numReads, uint8_t* out); - - void SetMagReadLocation(uint8_t addr, uint8_t len); - void MagRegWrite(uint8_t addr, uint8_t val); - - IMUDriver(const IMUDriver&) = delete; - IMUDriver& operator=(const IMUDriver&) = delete; - - const ICM20948_DATA_t GetDataFromBuf(const uint8_t *buf, bool accel = true, bool gyro = true, bool temp = true, bool mag = true); - - void SetCSPin(GPIO_TypeDef* gpio, uint16_t pin); - -private: - bool initialized = false; - SPI_HandleTypeDef* hspi = nullptr; - GPIO_TypeDef* cs_gpio; - uint16_t cs_pin; - - bool SwitchBank(uint8_t bank); - - void CSLow(); - void CSHigh(); - -}; - - - -// Constants for registers in the ICM20948 IMU -// First byte is the bank in which it is located -// (0xff means it is located in the same place in every bank. Only used by the bank switch register) -// Second byte is address within bank -namespace ICM20948_REG { - - static const ICM20948_REGISTER_t BANK = {0xff,0x7f}; - static const ICM20948_REGISTER_t WHO_AM_I = {0,0x00}; - static const ICM20948_REGISTER_t USER_CTL = {0,0x03}; - static const ICM20948_REGISTER_t FIFO_EN_2 = {0,0x67}; - static const ICM20948_REGISTER_t PWR_MGMT_1 = {0,0x06}; - - static const ICM20948_REGISTER_t ACCEL_XOUT_H = {0,0x2d}; - static const ICM20948_REGISTER_t ACCEL_XOUT_L = {0,0x2e}; - static const ICM20948_REGISTER_t ACCEL_YOUT_H = {0,0x2f}; - static const ICM20948_REGISTER_t ACCEL_YOUT_L = {0,0x30}; - static const ICM20948_REGISTER_t ACCEL_ZOUT_H = {0,0x31}; - static const ICM20948_REGISTER_t ACCEL_ZOUT_L = {0,0x32}; - - static const ICM20948_REGISTER_t GYRO_XOUT_H = {0,0x33}; - static const ICM20948_REGISTER_t GYRO_XOUT_L = {0,0x34}; - static const ICM20948_REGISTER_t GYRO_YOUT_H = {0,0x35}; - static const ICM20948_REGISTER_t GYRO_YOUT_L = {0,0x36}; - static const ICM20948_REGISTER_t GYRO_ZOUT_H = {0,0x37}; - static const ICM20948_REGISTER_t GYRO_ZOUT_L = {0,0x38}; - - static const ICM20948_REGISTER_t TEMP_OUT_H = {0,0x39}; - static const ICM20948_REGISTER_t TEMP_OUT_L = {0,0x3a}; - - static const ICM20948_REGISTER_t FIFO_RW = {0,0x72}; - - static const ICM20948_REGISTER_t I2C_MST_CTRL = {3,0x01}; - static const ICM20948_REGISTER_t I2C_SLV0_ADDR = {3,0x03}; - static const ICM20948_REGISTER_t I2C_SLV0_REG = {3,0x04}; - static const ICM20948_REGISTER_t I2C_SLV0_CTRL = {3,0x05}; - static const ICM20948_REGISTER_t I2C_SLV0_DO = {3,0x06}; - - static const ICM20948_REGISTER_t EXT_SLV_SENS_DATA_00 = {0,0x3b}; -} - -// Constants for registers in the AK09916 magnetometer in the IMU -namespace AK09916_REG { - static const uint8_t CNTL2 = 0x31; - static const uint8_t MEAS = 0x11; -} - -#endif /* ICM20948_DRIVER_H_ */ diff --git a/Components/LIS3MDLTRDriver.cpp b/Components/LIS3MDLTRDriver.cpp new file mode 100644 index 0000000..34cbd74 --- /dev/null +++ b/Components/LIS3MDLTRDriver.cpp @@ -0,0 +1,144 @@ +/* + * IMUDriver.cpp + * + * Created on: Aug 31, 2024 + * Author: goada + */ + +#include + +/* @brief Initialize the driver. Must be called before any other functions can be used. + * @param hspi_ Pointer to the SPI handle + * @param cs_gpio_ GPIO port for the chip select pin (GPIOA, GPIOB ...) + * @param cs_pin_ Pin number for the chip select + */ +void LIS3MDLTR_Driver::Init(SPI_HandleTypeDef* hspi_, GPIO_TypeDef* cs_gpio_, uint16_t cs_pin_) { + hspi = hspi_; + initialized = true; + SetCSPin(cs_gpio_, cs_pin_); + CSHigh(); + + uint8_t ID = GetRegister(LIS3MDLTR_REG::WHO_AM_I); + if(ID != LIS3MDLTR_ID) { + // couldn't get chip ID + initialized = false; + return; + } + // Enable temp and max speed + SetRegister(LIS3MDLTR_REG::CTRL_REG1, 0b11111110); + + // Leave power-down mode + SetRegister(LIS3MDLTR_REG::CTRL_REG3, 0b00000000); + + // High-performance z axis + SetRegister(LIS3MDLTR_REG::CTRL_REG4, 0b00001100); + +} + +/* @brief Sets a single 8-bit register. + * @param reg The register to set. Constants contained in ICM20948_REG namespace. + * @param val Value to set the register to. + * @return Success + */ +bool LIS3MDLTR_Driver::SetRegister(LIS3MDLTR_REGISTER_t reg, uint8_t val) { + assert(initialized); + + uint8_t data[2] = {(uint8_t)(0b00000000 | (reg&0x3F)),val}; + CSLow(); + HAL_StatusTypeDef r = HAL_SPI_Transmit(hspi, data, 2, 1000); + CSHigh(); + return r == HAL_OK; + +} + +/* @brief Gets a single 8-bit register. + * @param reg The register to get. Constants contained in ICM20948_REG namespace. + * @return Value read from the register. + */ +uint8_t LIS3MDLTR_Driver::GetRegister(LIS3MDLTR_REGISTER_t reg) { + assert(initialized); + uint8_t data[2] = {(uint8_t)(0b10000000 | (reg&0x3F)),SPI_DUMMY_BYTE}; + uint8_t incoming[2] = {0,0}; + CSLow(); + HAL_SPI_TransmitReceive(hspi, data, incoming, 2, 1000); + CSHigh(); + return incoming[1]; +} + +/* @brief Reads multiple successive registers in a row. + * @param startreg The register that the readings should start at. + * @param numBytes Number of bytes to read. + * @param out Address of buffer to receive data. Must be numBytes long. + */ +void LIS3MDLTR_Driver::GetMultipleRegisters(LIS3MDLTR_REGISTER_t startreg, int numBytes, + uint8_t *out) { + assert(initialized); + + uint8_t transmit[numBytes+1] = {SPI_DUMMY_BYTE}; + transmit[0] = (uint8_t)(0b11000000 | (startreg&0x3f)); + + CSLow(); + HAL_SPI_TransmitReceive(hspi, transmit, out, numBytes+1, 1000); + CSHigh(); +} + + +/* @brief Extract IMU data from a raw byte buffer (such as from ReadAllSensorRegs) into a struct. + * @param temp Buffer includes temperature data + * @return Struct containing extracted data + */ +const LIS3MDLTR_DATA_t LIS3MDLTR_Driver::GetDataFromBuf(const uint8_t *buf, bool mag, bool temp) { + LIS3MDLTR_DATA_t out; + size_t i = 0; + + //both little-endian + if(mag) { + out.mag.x = (buf[i]) | (buf[1+1]<<8); + out.mag.y = (buf[i+2]) | (buf[1+3]<<8); + out.mag.z = (buf[i+4]) | (buf[1+5]<<8); + } + if(temp) { + out.temp = (buf[i]) | (buf[i+1]<<8); + i += 2; + } + + return out; + +} + +/* @brief Reads all 8 sensor registers in order. (mag, temp) + * @param out Raw bytes read from registers. Must be 8 bytes long + */ +void LIS3MDLTR_Driver::ReadAllSensorRegs(uint8_t* out) { + GetMultipleRegisters(LIS3MDLTR_REG::OUTX_L, 8, out); +} + + +LIS3MDLTR_Driver::LIS3MDLTR_Driver() { +} + +LIS3MDLTR_Driver::~LIS3MDLTR_Driver() { +} + +void LIS3MDLTR_Driver::CSLow() { + assert(initialized); + HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_RESET); +} + +void LIS3MDLTR_Driver::SetCSPin(GPIO_TypeDef* gpio, uint16_t pin) { + cs_gpio = gpio; + cs_pin = pin; +} + +void LIS3MDLTR_Driver::EnableTemp() { + SetRegister(LIS3MDLTR_REG::CTRL_REG1, GetRegister(LIS3MDLTR_REG::CTRL_REG1) | 0b10000000); +} + +void LIS3MDLTR_Driver::DisableTemp() { + SetRegister(LIS3MDLTR_REG::CTRL_REG1, GetRegister(LIS3MDLTR_REG::CTRL_REG1) & 0b01111111); +} + +void LIS3MDLTR_Driver::CSHigh() { + assert(initialized); + HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_SET); +} diff --git a/Components/LIS3MDLTRDriver.h b/Components/LIS3MDLTRDriver.h new file mode 100644 index 0000000..2dc10b3 --- /dev/null +++ b/Components/LIS3MDLTRDriver.h @@ -0,0 +1,96 @@ +/* + * IMUDriver.h + * + * Created on: Aug 31, 2024 + * Author: goada + */ + +#ifndef LIS3MDLTRDRIVER_H_ +#define LIS3MDLTRDRIVER_H_ + +#include "stm32h7xx.h" + +constexpr uint8_t SPI_DUMMY_BYTE = 0x00; +constexpr uint8_t LIS3MDLTR_ID = 0b00111101; + +typedef uint8_t LIS3MDLTR_REGISTER_t; + +struct MAG_t { + int16_t x; + int16_t y; + int16_t z; +}; + +struct LIS3MDLTR_DATA_t { + MAG_t mag; + int16_t temp; +}; + +/* @brief Singleton SPI driver for the LIS3MDLTR magnetometer. + * Must call Init before using any other functions besides SetCSPin. + * SPI max clock speed 10MHz. + */ +class LIS3MDLTR_Driver { +public: + LIS3MDLTR_Driver(); + ~LIS3MDLTR_Driver(); + + static LIS3MDLTR_Driver& Inst() { + static LIS3MDLTR_Driver inst; + return inst; + } + + void Init(SPI_HandleTypeDef* hspi, GPIO_TypeDef* cs_gpio_, uint16_t cs_pin_); + + bool SetRegister(LIS3MDLTR_REGISTER_t reg, uint8_t val); + uint8_t GetRegister(LIS3MDLTR_REGISTER_t reg); + void GetMultipleRegisters(LIS3MDLTR_REGISTER_t startreg, int numBytes, uint8_t* out); + void ReadAllSensorRegs(uint8_t* out); + + void EnableTemp(); + void DisableTemp(); + + LIS3MDLTR_Driver(const LIS3MDLTR_Driver&) = delete; + LIS3MDLTR_Driver& operator=(const LIS3MDLTR_Driver&) = delete; + + const LIS3MDLTR_DATA_t GetDataFromBuf(const uint8_t *buf, bool mag = true, bool temp = true); + + void SetCSPin(GPIO_TypeDef* gpio, uint16_t pin); + +private: + bool initialized = false; + SPI_HandleTypeDef* hspi = nullptr; + GPIO_TypeDef* cs_gpio; + uint16_t cs_pin; + + void CSLow(); + void CSHigh(); + +}; + +// Constant addresses for registers in the LIS3MDLTR magnetometer +namespace LIS3MDLTR_REG { + + static const LIS3MDLTR_REGISTER_t WHO_AM_I = 0x0F; + + static const LIS3MDLTR_REGISTER_t CTRL_REG1 = 0x20; + static const LIS3MDLTR_REGISTER_t CTRL_REG2 = 0x21; + static const LIS3MDLTR_REGISTER_t CTRL_REG3 = 0x22; + static const LIS3MDLTR_REGISTER_t CTRL_REG4 = 0x23; + static const LIS3MDLTR_REGISTER_t CTRL_REG5 = 0x24; + + static const LIS3MDLTR_REGISTER_t STATUS_REG = 0x27; + + static const LIS3MDLTR_REGISTER_t OUT_TEMP_L = 0x2E; + static const LIS3MDLTR_REGISTER_t OUT_TEMP_H = 0x2F; + + static const LIS3MDLTR_REGISTER_t OUTX_L = 0x28; + static const LIS3MDLTR_REGISTER_t OUTX_H = 0x29; + static const LIS3MDLTR_REGISTER_t OUTY_L = 0x2A; + static const LIS3MDLTR_REGISTER_t OUTY_H = 0x2B; + static const LIS3MDLTR_REGISTER_t OUTZ_L = 0x2C; + static const LIS3MDLTR_REGISTER_t OUTZ_H = 0x2D; + +} + +#endif /* LIS3MDLTRDRIVER_H_ */ From daa750dfaf269422d2ab9d62e85405cce9c31483 Mon Sep 17 00:00:00 2001 From: Adam Go Date: Sat, 5 Oct 2024 18:03:20 -0600 Subject: [PATCH 3/6] - UNTESTED LSM6DO32 6-axis IMU driver --- Components/LIS3MDLTRDriver.cpp | 144 ------------------------- Components/LIS3MDLTRDriver.h | 96 ----------------- Components/LSM6DO32Driver.cpp | 188 +++++++++++++++++++++++++++++++++ Components/LSM6DO32Driver.h | 162 ++++++++++++++++++++++++++++ 4 files changed, 350 insertions(+), 240 deletions(-) delete mode 100644 Components/LIS3MDLTRDriver.cpp delete mode 100644 Components/LIS3MDLTRDriver.h create mode 100644 Components/LSM6DO32Driver.cpp create mode 100644 Components/LSM6DO32Driver.h diff --git a/Components/LIS3MDLTRDriver.cpp b/Components/LIS3MDLTRDriver.cpp deleted file mode 100644 index 34cbd74..0000000 --- a/Components/LIS3MDLTRDriver.cpp +++ /dev/null @@ -1,144 +0,0 @@ -/* - * IMUDriver.cpp - * - * Created on: Aug 31, 2024 - * Author: goada - */ - -#include - -/* @brief Initialize the driver. Must be called before any other functions can be used. - * @param hspi_ Pointer to the SPI handle - * @param cs_gpio_ GPIO port for the chip select pin (GPIOA, GPIOB ...) - * @param cs_pin_ Pin number for the chip select - */ -void LIS3MDLTR_Driver::Init(SPI_HandleTypeDef* hspi_, GPIO_TypeDef* cs_gpio_, uint16_t cs_pin_) { - hspi = hspi_; - initialized = true; - SetCSPin(cs_gpio_, cs_pin_); - CSHigh(); - - uint8_t ID = GetRegister(LIS3MDLTR_REG::WHO_AM_I); - if(ID != LIS3MDLTR_ID) { - // couldn't get chip ID - initialized = false; - return; - } - // Enable temp and max speed - SetRegister(LIS3MDLTR_REG::CTRL_REG1, 0b11111110); - - // Leave power-down mode - SetRegister(LIS3MDLTR_REG::CTRL_REG3, 0b00000000); - - // High-performance z axis - SetRegister(LIS3MDLTR_REG::CTRL_REG4, 0b00001100); - -} - -/* @brief Sets a single 8-bit register. - * @param reg The register to set. Constants contained in ICM20948_REG namespace. - * @param val Value to set the register to. - * @return Success - */ -bool LIS3MDLTR_Driver::SetRegister(LIS3MDLTR_REGISTER_t reg, uint8_t val) { - assert(initialized); - - uint8_t data[2] = {(uint8_t)(0b00000000 | (reg&0x3F)),val}; - CSLow(); - HAL_StatusTypeDef r = HAL_SPI_Transmit(hspi, data, 2, 1000); - CSHigh(); - return r == HAL_OK; - -} - -/* @brief Gets a single 8-bit register. - * @param reg The register to get. Constants contained in ICM20948_REG namespace. - * @return Value read from the register. - */ -uint8_t LIS3MDLTR_Driver::GetRegister(LIS3MDLTR_REGISTER_t reg) { - assert(initialized); - uint8_t data[2] = {(uint8_t)(0b10000000 | (reg&0x3F)),SPI_DUMMY_BYTE}; - uint8_t incoming[2] = {0,0}; - CSLow(); - HAL_SPI_TransmitReceive(hspi, data, incoming, 2, 1000); - CSHigh(); - return incoming[1]; -} - -/* @brief Reads multiple successive registers in a row. - * @param startreg The register that the readings should start at. - * @param numBytes Number of bytes to read. - * @param out Address of buffer to receive data. Must be numBytes long. - */ -void LIS3MDLTR_Driver::GetMultipleRegisters(LIS3MDLTR_REGISTER_t startreg, int numBytes, - uint8_t *out) { - assert(initialized); - - uint8_t transmit[numBytes+1] = {SPI_DUMMY_BYTE}; - transmit[0] = (uint8_t)(0b11000000 | (startreg&0x3f)); - - CSLow(); - HAL_SPI_TransmitReceive(hspi, transmit, out, numBytes+1, 1000); - CSHigh(); -} - - -/* @brief Extract IMU data from a raw byte buffer (such as from ReadAllSensorRegs) into a struct. - * @param temp Buffer includes temperature data - * @return Struct containing extracted data - */ -const LIS3MDLTR_DATA_t LIS3MDLTR_Driver::GetDataFromBuf(const uint8_t *buf, bool mag, bool temp) { - LIS3MDLTR_DATA_t out; - size_t i = 0; - - //both little-endian - if(mag) { - out.mag.x = (buf[i]) | (buf[1+1]<<8); - out.mag.y = (buf[i+2]) | (buf[1+3]<<8); - out.mag.z = (buf[i+4]) | (buf[1+5]<<8); - } - if(temp) { - out.temp = (buf[i]) | (buf[i+1]<<8); - i += 2; - } - - return out; - -} - -/* @brief Reads all 8 sensor registers in order. (mag, temp) - * @param out Raw bytes read from registers. Must be 8 bytes long - */ -void LIS3MDLTR_Driver::ReadAllSensorRegs(uint8_t* out) { - GetMultipleRegisters(LIS3MDLTR_REG::OUTX_L, 8, out); -} - - -LIS3MDLTR_Driver::LIS3MDLTR_Driver() { -} - -LIS3MDLTR_Driver::~LIS3MDLTR_Driver() { -} - -void LIS3MDLTR_Driver::CSLow() { - assert(initialized); - HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_RESET); -} - -void LIS3MDLTR_Driver::SetCSPin(GPIO_TypeDef* gpio, uint16_t pin) { - cs_gpio = gpio; - cs_pin = pin; -} - -void LIS3MDLTR_Driver::EnableTemp() { - SetRegister(LIS3MDLTR_REG::CTRL_REG1, GetRegister(LIS3MDLTR_REG::CTRL_REG1) | 0b10000000); -} - -void LIS3MDLTR_Driver::DisableTemp() { - SetRegister(LIS3MDLTR_REG::CTRL_REG1, GetRegister(LIS3MDLTR_REG::CTRL_REG1) & 0b01111111); -} - -void LIS3MDLTR_Driver::CSHigh() { - assert(initialized); - HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_SET); -} diff --git a/Components/LIS3MDLTRDriver.h b/Components/LIS3MDLTRDriver.h deleted file mode 100644 index 2dc10b3..0000000 --- a/Components/LIS3MDLTRDriver.h +++ /dev/null @@ -1,96 +0,0 @@ -/* - * IMUDriver.h - * - * Created on: Aug 31, 2024 - * Author: goada - */ - -#ifndef LIS3MDLTRDRIVER_H_ -#define LIS3MDLTRDRIVER_H_ - -#include "stm32h7xx.h" - -constexpr uint8_t SPI_DUMMY_BYTE = 0x00; -constexpr uint8_t LIS3MDLTR_ID = 0b00111101; - -typedef uint8_t LIS3MDLTR_REGISTER_t; - -struct MAG_t { - int16_t x; - int16_t y; - int16_t z; -}; - -struct LIS3MDLTR_DATA_t { - MAG_t mag; - int16_t temp; -}; - -/* @brief Singleton SPI driver for the LIS3MDLTR magnetometer. - * Must call Init before using any other functions besides SetCSPin. - * SPI max clock speed 10MHz. - */ -class LIS3MDLTR_Driver { -public: - LIS3MDLTR_Driver(); - ~LIS3MDLTR_Driver(); - - static LIS3MDLTR_Driver& Inst() { - static LIS3MDLTR_Driver inst; - return inst; - } - - void Init(SPI_HandleTypeDef* hspi, GPIO_TypeDef* cs_gpio_, uint16_t cs_pin_); - - bool SetRegister(LIS3MDLTR_REGISTER_t reg, uint8_t val); - uint8_t GetRegister(LIS3MDLTR_REGISTER_t reg); - void GetMultipleRegisters(LIS3MDLTR_REGISTER_t startreg, int numBytes, uint8_t* out); - void ReadAllSensorRegs(uint8_t* out); - - void EnableTemp(); - void DisableTemp(); - - LIS3MDLTR_Driver(const LIS3MDLTR_Driver&) = delete; - LIS3MDLTR_Driver& operator=(const LIS3MDLTR_Driver&) = delete; - - const LIS3MDLTR_DATA_t GetDataFromBuf(const uint8_t *buf, bool mag = true, bool temp = true); - - void SetCSPin(GPIO_TypeDef* gpio, uint16_t pin); - -private: - bool initialized = false; - SPI_HandleTypeDef* hspi = nullptr; - GPIO_TypeDef* cs_gpio; - uint16_t cs_pin; - - void CSLow(); - void CSHigh(); - -}; - -// Constant addresses for registers in the LIS3MDLTR magnetometer -namespace LIS3MDLTR_REG { - - static const LIS3MDLTR_REGISTER_t WHO_AM_I = 0x0F; - - static const LIS3MDLTR_REGISTER_t CTRL_REG1 = 0x20; - static const LIS3MDLTR_REGISTER_t CTRL_REG2 = 0x21; - static const LIS3MDLTR_REGISTER_t CTRL_REG3 = 0x22; - static const LIS3MDLTR_REGISTER_t CTRL_REG4 = 0x23; - static const LIS3MDLTR_REGISTER_t CTRL_REG5 = 0x24; - - static const LIS3MDLTR_REGISTER_t STATUS_REG = 0x27; - - static const LIS3MDLTR_REGISTER_t OUT_TEMP_L = 0x2E; - static const LIS3MDLTR_REGISTER_t OUT_TEMP_H = 0x2F; - - static const LIS3MDLTR_REGISTER_t OUTX_L = 0x28; - static const LIS3MDLTR_REGISTER_t OUTX_H = 0x29; - static const LIS3MDLTR_REGISTER_t OUTY_L = 0x2A; - static const LIS3MDLTR_REGISTER_t OUTY_H = 0x2B; - static const LIS3MDLTR_REGISTER_t OUTZ_L = 0x2C; - static const LIS3MDLTR_REGISTER_t OUTZ_H = 0x2D; - -} - -#endif /* LIS3MDLTRDRIVER_H_ */ diff --git a/Components/LSM6DO32Driver.cpp b/Components/LSM6DO32Driver.cpp new file mode 100644 index 0000000..ce39241 --- /dev/null +++ b/Components/LSM6DO32Driver.cpp @@ -0,0 +1,188 @@ +/* + * IMUDriver.cpp + * + * Created on: Aug 31, 2024 + * Author: goada + */ + +#include + +/* @brief Initialize the driver. Must be called before any other functions can be used. + * @param hspi_ Pointer to the SPI handle + * @param cs_gpio_ GPIO port for the chip select pin (GPIOA, GPIOB ...) + * @param cs_pin_ Pin number for the chip select + */ +void LSM6DO32_Driver::Init(SPI_HandleTypeDef* hspi_, GPIO_TypeDef* cs_gpio_, uint16_t cs_pin_) { + hspi = hspi_; + initialized = true; + SetCSPin(cs_gpio_, cs_pin_); + CSHigh(); + + uint8_t ID = GetRegister(LSM6DSO32_REG::WHO_AM_I); + if(ID != LSM6DSO32_ID) { + // couldn't get chip ID + initialized = false; + return; + } + + + + +} + +/* @brief Sets a single 8-bit register. + * @param reg The register to set. Constants contained in ICM20948_REG namespace. + * @param val Value to set the register to. + * @return Success + */ +bool LSM6DO32_Driver::SetRegister(LSM6DSO32_REGISTER_t reg, uint8_t val) { + assert(initialized); + + uint8_t data[2] = {(uint8_t)(0b00000000 | (reg&0x7F)),val}; + CSLow(); + HAL_StatusTypeDef r = HAL_SPI_Transmit(hspi, data, 2, 1000); + CSHigh(); + return r == HAL_OK; + +} + +/* @brief Gets a single 8-bit register. + * @param reg The register to get. Constants contained in ICM20948_REG namespace. + * @return Value read from the register. + */ +uint8_t LSM6DO32_Driver::GetRegister(LSM6DSO32_REGISTER_t reg) { + assert(initialized); + uint8_t data[2] = {(uint8_t)(0b10000000 | (reg&0x7F)),SPI_DUMMY_BYTE}; + uint8_t incoming[2] = {0,0}; + CSLow(); + HAL_SPI_TransmitReceive(hspi, data, incoming, 2, 1000); + CSHigh(); + return incoming[1]; +} + +/* @brief Reads multiple successive registers in a row. + * @param startreg The register that the readings should start at. + * @param numBytes Number of bytes to read. + * @param out Address of buffer to receive data. Must be numBytes long. + */ +void LSM6DO32_Driver::GetMultipleRegisters(LSM6DSO32_REGISTER_t startreg, int numBytes, + uint8_t *out) { + assert(initialized); + + uint8_t transmit[numBytes+1] = {SPI_DUMMY_BYTE}; + transmit[0] = (uint8_t)(0b10000000 | startreg); + + CSLow(); + HAL_SPI_TransmitReceive(hspi, transmit, out, numBytes+1, 1000); + CSHigh(); +} + +/* @brief Repeatedly reads the top two bytes of each of the x, y, and z FIFOs in little-endian format. + * @param numReads Number of repetitive reads to perform. + * @param out Buffer to receive data in. Must be numReads*6 long. + */ +void LSM6DO32_Driver::ReadFIFOs(int numReads, uint8_t *out) { + assert(initialized); + for(int i = 0; i < numReads; i++) { + GetMultipleRegisters(LSM6DSO32_REG::FIFO_DATA_OUT_X_L, 6, out+i*6); + } +} + +/* @brief Extract IMU data from a raw byte buffer (such as from ReadAllSensorRegs) into a struct. + * @param buf Input buffer containing raw data. + * @param accel Buffer includes accel data + * @param gyro Buffer includes gyroscope data + * @param temp Buffer includes temperature data + * @return Struct containing extracted data + */ +const LSM6DSO32_DATA_t LSM6DO32_Driver::GetDataFromBuf(const uint8_t *buf, bool accel, bool gyro, bool temp) { + LSM6DSO32_DATA_t out; + size_t i = 0; + + // Accel gyro and temp are little-endian + if(temp) { + out.temp = (buf[i]) | (buf[i+1]<<8); + i += 2; + } + + if(gyro) { + + out.gyro.x = (buf[i ]) | (buf[i+1] << 8); + out.gyro.y = (buf[i+2]) | (buf[i+3] << 8); + out.gyro.z = (buf[i+4]) | (buf[i+5] << 8); + i += 6; + } + + if(accel) { + out.accel.x = (buf[i ]) | ((buf[i+1]) << 8); + out.accel.y = (buf[i+2]) | ((buf[i+3]) << 8); + out.accel.z = (buf[i+4]) | ((buf[i+5]) << 8); + i += 6; + } + + return out; + +} + +/* @brief Reads all 14 sensor registers in order. (temp, gyro, accel) + * @param out Raw bytes read from registers. Must be 14 bytes long + */ +void LSM6DO32_Driver::ReadAllSensorRegs(uint8_t* out) { + GetMultipleRegisters(LSM6DSO32_REG::OUT_TEMP_L, 14, out); +} + + +LSM6DO32_Driver::LSM6DO32_Driver() { +} + +LSM6DO32_Driver::~LSM6DO32_Driver() { +} + +void LSM6DO32_Driver::CSLow() { + assert(initialized); + HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_RESET); +} + +void LSM6DO32_Driver::SetCSPin(GPIO_TypeDef* gpio, uint16_t pin) { + cs_gpio = gpio; + cs_pin = pin; +} + +void LSM6DO32_Driver::CSHigh() { + assert(initialized); + HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_SET); +} + +/* @brief Set the output data rate of the accelerometer + * @param speed Output speed + */ +void LSM6DO32_Driver::SetAccelODR(LSM6D032_SAMPLE_SPEED speed) { + assert(initialized); + SetRegister(LSM6DSO32_REG::CTRL1_XL, (GetRegister(LSM6DSO32_REG::CTRL1_XL) & 0b00001111) | (speed<<4)); +} + +/* @brief Set the output data rate of the gyroscope + * @param speed Output speed + */ +void LSM6DO32_Driver::SetGyroODR(LSM6D032_SAMPLE_SPEED speed) { + assert(initialized); + SetRegister(LSM6DSO32_REG::CTRL2_G, (GetRegister(LSM6DSO32_REG::CTRL2_G) & 0b00001111) | (speed<<4)); +} + +/* @brief Set the full-scale range of the accelerometer + * @param scale Range, in plus/minus G + */ +void LSM6DO32_Driver::SetAccelFullScaleRange(LSM6D032_ACCEL_SCALE_SELECT scale) { + assert(initialized); + SetRegister(LSM6DSO32_REG::CTRL1_XL, (GetRegister(LSM6DSO32_REG::CTRL1_XL) & 0b11110011) | (scale<<2)); +} + +/* @brief Set the full-scale range of the gyroscope + * @param scale Range, in plus/minus dps + */ +void LSM6DO32_Driver::SetGyroFullScaleRange(LSM6D032_GYRO_SCALE_SELECT scale) { + assert(initialized); + SetRegister(LSM6DSO32_REG::CTRL2_G, (GetRegister(LSM6DSO32_REG::CTRL2_G) & 0b11110011) | (scale<<2)); + +} + diff --git a/Components/LSM6DO32Driver.h b/Components/LSM6DO32Driver.h new file mode 100644 index 0000000..91465fa --- /dev/null +++ b/Components/LSM6DO32Driver.h @@ -0,0 +1,162 @@ +/* + * IMUDriver.h + * + * Created on: Aug 31, 2024 + * Author: goada + */ + +#ifndef LSM6DO32DRIVER_H_ +#define LSM6DO32DRIVER_H_ + +#include "stm32h7xx.h" + +constexpr uint8_t SPI_DUMMY_BYTE = 0x00; +constexpr uint8_t LSM6DSO32_ID = 0x6C; + +typedef uint8_t LSM6DSO32_REGISTER_t; + +struct ACCEL_t { + int16_t x; + int16_t y; + int16_t z; +}; + +struct GYRO_t { + int16_t x; + int16_t y; + int16_t z; +}; + +struct LSM6DSO32_DATA_t { + ACCEL_t accel; + GYRO_t gyro; + int16_t temp; +}; + +/* @brief Singleton SPI driver for the 6-axis LSM6DSO32 IMU. + * Must call Init before using any other functions besides SetCSPin. + * SPI max clock speed 10MHz. + */ +class LSM6DO32_Driver { +public: + LSM6DO32_Driver(); + ~LSM6DO32_Driver(); + + static LSM6DO32_Driver& Inst() { + static LSM6DO32_Driver inst; + return inst; + } + enum LSM6D032_SAMPLE_SPEED { + FREQ_12p5_HZ=0b0001, + FREQ_26_HZ=0b0010, + FREQ_52_HZ=0b0011, + FREQ_104_HZ=0b0100, + FREQ_208_HZ=0b0101, + FREQ_416_HZ=0b0110, + FREQ_833_HZ=0b0111, + FREQ_1660_HZ=0b1000, + FREQ_3330_HZ=0b1001, + FREQ_6660_HZ=0b1010 + }; + + enum LSM6D032_ACCEL_SCALE_SELECT { + SCALE_4g = 0b00, + SCALE_32g = 0b01, + SCALE_8g = 0b10, + SCALE_16g = 0b11 + }; + + enum LSM6D032_GYRO_SCALE_SELECT { + SCALE_250dps = 0b00, + SCALE_500dps = 0b01, + SCALE_1000dps = 0b10, + SCALE_2000dps = 0b11 + }; + + + void Init(SPI_HandleTypeDef* hspi, GPIO_TypeDef* cs_gpio_, uint16_t cs_pin_); + + bool SetRegister(LSM6DSO32_REGISTER_t reg, uint8_t val); + uint8_t GetRegister(LSM6DSO32_REGISTER_t reg); + void GetMultipleRegisters(LSM6DSO32_REGISTER_t startreg, int numBytes, uint8_t* out); + void ReadAllSensorRegs(uint8_t* out); + void ReadFIFOs(int numReads, uint8_t* out); + + void SetAccelODR(LSM6D032_SAMPLE_SPEED speed); + void SetGyroODR(LSM6D032_SAMPLE_SPEED speed); + void SetAccelFullScaleRange(LSM6D032_ACCEL_SCALE_SELECT scale); + void SetGyroFullScaleRange(LSM6D032_GYRO_SCALE_SELECT scale); + + LSM6DO32_Driver(const LSM6DO32_Driver&) = delete; + LSM6DO32_Driver& operator=(const LSM6DO32_Driver&) = delete; + + const LSM6DSO32_DATA_t GetDataFromBuf(const uint8_t *buf, bool accel = true, bool gyro = true, bool temp = true); + + void SetCSPin(GPIO_TypeDef* gpio, uint16_t pin); + +private: + bool initialized = false; + SPI_HandleTypeDef* hspi = nullptr; + GPIO_TypeDef* cs_gpio; + uint16_t cs_pin; + + bool SwitchBank(uint8_t bank); + + void CSLow(); + void CSHigh(); + +}; + + + +// Constants for registers in the LSM6DSO32 IMU +// First byte is the bank in which it is located +// (0xff means it is located in the same place in every bank. Only used by the bank switch register) +// Second byte is address within bank +namespace LSM6DSO32_REG { + + static const LSM6DSO32_REGISTER_t FIFO_CTRL2 = 0x08; + static const LSM6DSO32_REGISTER_t FIFO_CTRL3 = 0x09; + static const LSM6DSO32_REGISTER_t FIFO_CTRL4 = 0x0A; + + static const LSM6DSO32_REGISTER_t WHO_AM_I = 0x0F; + + static const LSM6DSO32_REGISTER_t CTRL1_XL = 0x10; + static const LSM6DSO32_REGISTER_t CTRL2_G = 0x11; + + static const LSM6DSO32_REGISTER_t STATUS_REG = 0x1E; + + static const LSM6DSO32_REGISTER_t OUT_TEMP_L = 0x20; + static const LSM6DSO32_REGISTER_t OUT_TEMP_H = 0x21; + + static const LSM6DSO32_REGISTER_t OUTX_L_G = 0x22; + static const LSM6DSO32_REGISTER_t OUTX_H_G = 0x23; + static const LSM6DSO32_REGISTER_t OUTY_L_G = 0x24; + static const LSM6DSO32_REGISTER_t OUTY_H_G = 0x25; + static const LSM6DSO32_REGISTER_t OUTZ_L_G = 0x26; + static const LSM6DSO32_REGISTER_t OUTZ_H_G = 0x27; + + static const LSM6DSO32_REGISTER_t OUTX_L_A = 0x28; + static const LSM6DSO32_REGISTER_t OUTX_H_A = 0x29; + static const LSM6DSO32_REGISTER_t OUTY_L_A = 0x2A; + static const LSM6DSO32_REGISTER_t OUTY_H_A = 0x2B; + static const LSM6DSO32_REGISTER_t OUTZ_L_A = 0x2C; + static const LSM6DSO32_REGISTER_t OUTZ_H_A = 0x2D; + + static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_TAG = 0x78; + + static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_X_L = 0x79; + static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_X_H = 0x7A; + + static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Y_L = 0x7B; + static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Y_H = 0x7C; + + static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Z_L = 0x7D; + static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Z_H = 0x7E; + + +} + + + +#endif /* LSM6DO32DRIVER_H_ */ From e8a45d64af720853758811a8223fe1d509ff0d50 Mon Sep 17 00:00:00 2001 From: Adam Go Date: Wed, 9 Oct 2024 18:21:38 -0600 Subject: [PATCH 4/6] - STILL UNTESTED changed register constants to constexpr --- Components/LSM6DO32Driver.h | 56 ++++++++++++++++++------------------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/Components/LSM6DO32Driver.h b/Components/LSM6DO32Driver.h index 91465fa..7d5c161 100644 --- a/Components/LSM6DO32Driver.h +++ b/Components/LSM6DO32Driver.h @@ -115,44 +115,44 @@ class LSM6DO32_Driver { // Second byte is address within bank namespace LSM6DSO32_REG { - static const LSM6DSO32_REGISTER_t FIFO_CTRL2 = 0x08; - static const LSM6DSO32_REGISTER_t FIFO_CTRL3 = 0x09; - static const LSM6DSO32_REGISTER_t FIFO_CTRL4 = 0x0A; + constexpr LSM6DSO32_REGISTER_t FIFO_CTRL2 = 0x08; + constexpr LSM6DSO32_REGISTER_t FIFO_CTRL3 = 0x09; + constexpr LSM6DSO32_REGISTER_t FIFO_CTRL4 = 0x0A; - static const LSM6DSO32_REGISTER_t WHO_AM_I = 0x0F; + constexpr LSM6DSO32_REGISTER_t WHO_AM_I = 0x0F; - static const LSM6DSO32_REGISTER_t CTRL1_XL = 0x10; - static const LSM6DSO32_REGISTER_t CTRL2_G = 0x11; + constexpr LSM6DSO32_REGISTER_t CTRL1_XL = 0x10; + constexpr LSM6DSO32_REGISTER_t CTRL2_G = 0x11; - static const LSM6DSO32_REGISTER_t STATUS_REG = 0x1E; + constexpr LSM6DSO32_REGISTER_t STATUS_REG = 0x1E; - static const LSM6DSO32_REGISTER_t OUT_TEMP_L = 0x20; - static const LSM6DSO32_REGISTER_t OUT_TEMP_H = 0x21; + constexpr LSM6DSO32_REGISTER_t OUT_TEMP_L = 0x20; + constexpr LSM6DSO32_REGISTER_t OUT_TEMP_H = 0x21; - static const LSM6DSO32_REGISTER_t OUTX_L_G = 0x22; - static const LSM6DSO32_REGISTER_t OUTX_H_G = 0x23; - static const LSM6DSO32_REGISTER_t OUTY_L_G = 0x24; - static const LSM6DSO32_REGISTER_t OUTY_H_G = 0x25; - static const LSM6DSO32_REGISTER_t OUTZ_L_G = 0x26; - static const LSM6DSO32_REGISTER_t OUTZ_H_G = 0x27; + constexpr LSM6DSO32_REGISTER_t OUTX_L_G = 0x22; + constexpr LSM6DSO32_REGISTER_t OUTX_H_G = 0x23; + constexpr LSM6DSO32_REGISTER_t OUTY_L_G = 0x24; + constexpr LSM6DSO32_REGISTER_t OUTY_H_G = 0x25; + constexpr LSM6DSO32_REGISTER_t OUTZ_L_G = 0x26; + constexpr LSM6DSO32_REGISTER_t OUTZ_H_G = 0x27; - static const LSM6DSO32_REGISTER_t OUTX_L_A = 0x28; - static const LSM6DSO32_REGISTER_t OUTX_H_A = 0x29; - static const LSM6DSO32_REGISTER_t OUTY_L_A = 0x2A; - static const LSM6DSO32_REGISTER_t OUTY_H_A = 0x2B; - static const LSM6DSO32_REGISTER_t OUTZ_L_A = 0x2C; - static const LSM6DSO32_REGISTER_t OUTZ_H_A = 0x2D; + constexpr LSM6DSO32_REGISTER_t OUTX_L_A = 0x28; + constexpr LSM6DSO32_REGISTER_t OUTX_H_A = 0x29; + constexpr LSM6DSO32_REGISTER_t OUTY_L_A = 0x2A; + constexpr LSM6DSO32_REGISTER_t OUTY_H_A = 0x2B; + constexpr LSM6DSO32_REGISTER_t OUTZ_L_A = 0x2C; + constexpr LSM6DSO32_REGISTER_t OUTZ_H_A = 0x2D; - static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_TAG = 0x78; + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_TAG = 0x78; - static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_X_L = 0x79; - static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_X_H = 0x7A; + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_X_L = 0x79; + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_X_H = 0x7A; - static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Y_L = 0x7B; - static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Y_H = 0x7C; + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Y_L = 0x7B; + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Y_H = 0x7C; - static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Z_L = 0x7D; - static const LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Z_H = 0x7E; + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Z_L = 0x7D; + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Z_H = 0x7E; } From 9eb120304d67883995160982478c281745b9cdb3 Mon Sep 17 00:00:00 2001 From: Andrey D <44914169+AndreyDiDev@users.noreply.github.com> Date: Sat, 19 Jul 2025 16:04:51 -0600 Subject: [PATCH 5/6] Update LSM6DO32Driver.cpp minor improvements --- Components/LSM6DO32Driver.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/Components/LSM6DO32Driver.cpp b/Components/LSM6DO32Driver.cpp index ce39241..62514d4 100644 --- a/Components/LSM6DO32Driver.cpp +++ b/Components/LSM6DO32Driver.cpp @@ -25,9 +25,6 @@ void LSM6DO32_Driver::Init(SPI_HandleTypeDef* hspi_, GPIO_TypeDef* cs_gpio_, uin return; } - - - } /* @brief Sets a single 8-bit register. @@ -40,9 +37,9 @@ bool LSM6DO32_Driver::SetRegister(LSM6DSO32_REGISTER_t reg, uint8_t val) { uint8_t data[2] = {(uint8_t)(0b00000000 | (reg&0x7F)),val}; CSLow(); - HAL_StatusTypeDef r = HAL_SPI_Transmit(hspi, data, 2, 1000); + HAL_StatusTypeDef result = HAL_SPI_Transmit(hspi, data, 2, 1000); CSHigh(); - return r == HAL_OK; + return result == HAL_OK; } @@ -53,9 +50,9 @@ bool LSM6DO32_Driver::SetRegister(LSM6DSO32_REGISTER_t reg, uint8_t val) { uint8_t LSM6DO32_Driver::GetRegister(LSM6DSO32_REGISTER_t reg) { assert(initialized); uint8_t data[2] = {(uint8_t)(0b10000000 | (reg&0x7F)),SPI_DUMMY_BYTE}; - uint8_t incoming[2] = {0,0}; + uint8_t incoming[2] = {0,0}; // first byte is dummy byte CSLow(); - HAL_SPI_TransmitReceive(hspi, data, incoming, 2, 1000); + HAL_SPI_TransmitReceive(hspi, data, incoming, 2, 1000); // second incoming byte is response CSHigh(); return incoming[1]; } @@ -81,8 +78,10 @@ void LSM6DO32_Driver::GetMultipleRegisters(LSM6DSO32_REGISTER_t startreg, int nu * @param numReads Number of repetitive reads to perform. * @param out Buffer to receive data in. Must be numReads*6 long. */ -void LSM6DO32_Driver::ReadFIFOs(int numReads, uint8_t *out) { +void LSM6DO32_Driver::SampleFIFOs(int numReads, uint8_t *out, size_t outBufferSize) { assert(initialized); + assert(outBufferSize >= (size_t)numReads * 6); + for(int i = 0; i < numReads; i++) { GetMultipleRegisters(LSM6DSO32_REG::FIFO_DATA_OUT_X_L, 6, out+i*6); } From 2b3b28c7d90f2bfb7161539ab7b554734c8e8723 Mon Sep 17 00:00:00 2001 From: Local user Date: Sat, 26 Jul 2025 10:29:30 -0600 Subject: [PATCH 6/6] -renamed reading functions --- Components/LSM6DO32Driver.cpp | 4 ++-- Components/LSM6DO32Driver.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Components/LSM6DO32Driver.cpp b/Components/LSM6DO32Driver.cpp index 62514d4..3d7448e 100644 --- a/Components/LSM6DO32Driver.cpp +++ b/Components/LSM6DO32Driver.cpp @@ -94,7 +94,7 @@ void LSM6DO32_Driver::SampleFIFOs(int numReads, uint8_t *out, size_t outBufferSi * @param temp Buffer includes temperature data * @return Struct containing extracted data */ -const LSM6DSO32_DATA_t LSM6DO32_Driver::GetDataFromBuf(const uint8_t *buf, bool accel, bool gyro, bool temp) { +const LSM6DSO32_DATA_t LSM6DO32_Driver::ConvertRawMeasurementToStruct(const uint8_t *buf, bool accel, bool gyro, bool temp) { LSM6DSO32_DATA_t out; size_t i = 0; @@ -126,7 +126,7 @@ const LSM6DSO32_DATA_t LSM6DO32_Driver::GetDataFromBuf(const uint8_t *buf, bool /* @brief Reads all 14 sensor registers in order. (temp, gyro, accel) * @param out Raw bytes read from registers. Must be 14 bytes long */ -void LSM6DO32_Driver::ReadAllSensorRegs(uint8_t* out) { +void LSM6DO32_Driver::ReadSensors(uint8_t* out) { GetMultipleRegisters(LSM6DSO32_REG::OUT_TEMP_L, 14, out); } diff --git a/Components/LSM6DO32Driver.h b/Components/LSM6DO32Driver.h index 7d5c161..4158a82 100644 --- a/Components/LSM6DO32Driver.h +++ b/Components/LSM6DO32Driver.h @@ -79,7 +79,7 @@ class LSM6DO32_Driver { bool SetRegister(LSM6DSO32_REGISTER_t reg, uint8_t val); uint8_t GetRegister(LSM6DSO32_REGISTER_t reg); void GetMultipleRegisters(LSM6DSO32_REGISTER_t startreg, int numBytes, uint8_t* out); - void ReadAllSensorRegs(uint8_t* out); + void ReadSensors(uint8_t* out); void ReadFIFOs(int numReads, uint8_t* out); void SetAccelODR(LSM6D032_SAMPLE_SPEED speed); @@ -90,7 +90,7 @@ class LSM6DO32_Driver { LSM6DO32_Driver(const LSM6DO32_Driver&) = delete; LSM6DO32_Driver& operator=(const LSM6DO32_Driver&) = delete; - const LSM6DSO32_DATA_t GetDataFromBuf(const uint8_t *buf, bool accel = true, bool gyro = true, bool temp = true); + const LSM6DSO32_DATA_t ConvertRawMeasurementToStruct(const uint8_t *buf, bool accel = true, bool gyro = true, bool temp = true); void SetCSPin(GPIO_TypeDef* gpio, uint16_t pin);