diff --git a/Components/LSM6DO32Driver.cpp b/Components/LSM6DO32Driver.cpp new file mode 100644 index 0000000..3d7448e --- /dev/null +++ b/Components/LSM6DO32Driver.cpp @@ -0,0 +1,187 @@ +/* + * 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 result = HAL_SPI_Transmit(hspi, data, 2, 1000); + CSHigh(); + return result == 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}; // first byte is dummy byte + CSLow(); + HAL_SPI_TransmitReceive(hspi, data, incoming, 2, 1000); // second incoming byte is response + 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::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); + } +} + +/* @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::ConvertRawMeasurementToStruct(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::ReadSensors(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..4158a82 --- /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 ReadSensors(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 ConvertRawMeasurementToStruct(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 { + + constexpr LSM6DSO32_REGISTER_t FIFO_CTRL2 = 0x08; + constexpr LSM6DSO32_REGISTER_t FIFO_CTRL3 = 0x09; + constexpr LSM6DSO32_REGISTER_t FIFO_CTRL4 = 0x0A; + + constexpr LSM6DSO32_REGISTER_t WHO_AM_I = 0x0F; + + constexpr LSM6DSO32_REGISTER_t CTRL1_XL = 0x10; + constexpr LSM6DSO32_REGISTER_t CTRL2_G = 0x11; + + constexpr LSM6DSO32_REGISTER_t STATUS_REG = 0x1E; + + constexpr LSM6DSO32_REGISTER_t OUT_TEMP_L = 0x20; + constexpr LSM6DSO32_REGISTER_t OUT_TEMP_H = 0x21; + + 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; + + 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; + + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_TAG = 0x78; + + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_X_L = 0x79; + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_X_H = 0x7A; + + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Y_L = 0x7B; + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Y_H = 0x7C; + + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Z_L = 0x7D; + constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Z_H = 0x7E; + + +} + + + +#endif /* LSM6DO32DRIVER_H_ */