From b316f42a2dc96240d5643b1571d1ff88f91c65d5 Mon Sep 17 00:00:00 2001 From: Adam Go Date: Sat, 5 Oct 2024 15:24:02 -0600 Subject: [PATCH 1/4] -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/4] - 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 149b80de5244d9fd96f922b902c9255a650d0973 Mon Sep 17 00:00:00 2001 From: Adam Go Date: Wed, 9 Oct 2024 18:28:01 -0600 Subject: [PATCH 3/4] - STILL UNTESTED changed register constants to constexpr --- Components/LIS3MDLTRDriver.h | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/Components/LIS3MDLTRDriver.h b/Components/LIS3MDLTRDriver.h index 2dc10b3..46a56a0 100644 --- a/Components/LIS3MDLTRDriver.h +++ b/Components/LIS3MDLTRDriver.h @@ -71,25 +71,25 @@ class LIS3MDLTR_Driver { // Constant addresses for registers in the LIS3MDLTR magnetometer namespace LIS3MDLTR_REG { - static const LIS3MDLTR_REGISTER_t WHO_AM_I = 0x0F; + constexpr 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; + constexpr LIS3MDLTR_REGISTER_t CTRL_REG1 = 0x20; + constexpr LIS3MDLTR_REGISTER_t CTRL_REG2 = 0x21; + constexpr LIS3MDLTR_REGISTER_t CTRL_REG3 = 0x22; + constexpr LIS3MDLTR_REGISTER_t CTRL_REG4 = 0x23; + constexpr LIS3MDLTR_REGISTER_t CTRL_REG5 = 0x24; - static const LIS3MDLTR_REGISTER_t STATUS_REG = 0x27; + constexpr LIS3MDLTR_REGISTER_t STATUS_REG = 0x27; - static const LIS3MDLTR_REGISTER_t OUT_TEMP_L = 0x2E; - static const LIS3MDLTR_REGISTER_t OUT_TEMP_H = 0x2F; + constexpr LIS3MDLTR_REGISTER_t OUT_TEMP_L = 0x2E; + constexpr 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; + constexpr LIS3MDLTR_REGISTER_t OUTX_L = 0x28; + constexpr LIS3MDLTR_REGISTER_t OUTX_H = 0x29; + constexpr LIS3MDLTR_REGISTER_t OUTY_L = 0x2A; + constexpr LIS3MDLTR_REGISTER_t OUTY_H = 0x2B; + constexpr LIS3MDLTR_REGISTER_t OUTZ_L = 0x2C; + constexpr LIS3MDLTR_REGISTER_t OUTZ_H = 0x2D; } From 01d629276135ea7dbfcd9ecb6675bf7617d16578 Mon Sep 17 00:00:00 2001 From: Local user Date: Sat, 26 Jul 2025 10:20:13 -0600 Subject: [PATCH 4/4] -renamed ReadAllSensorRegs to GetMeasurements -added GetTemp function --- Components/LIS3MDLTRDriver.cpp | 11 ++++++++++- Components/LIS3MDLTRDriver.h | 4 +++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/Components/LIS3MDLTRDriver.cpp b/Components/LIS3MDLTRDriver.cpp index 34cbd74..bc76d02 100644 --- a/Components/LIS3MDLTRDriver.cpp +++ b/Components/LIS3MDLTRDriver.cpp @@ -109,7 +109,7 @@ const LIS3MDLTR_DATA_t LIS3MDLTR_Driver::GetDataFromBuf(const uint8_t *buf, bool /* @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) { +void LIS3MDLTR_Driver::GetMeasurements(uint8_t* out) { GetMultipleRegisters(LIS3MDLTR_REG::OUTX_L, 8, out); } @@ -142,3 +142,12 @@ void LIS3MDLTR_Driver::CSHigh() { assert(initialized); HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_SET); } +/* @brief Gets the temperature. Must have temperature enabled with EnableTemp. + * @return Temperature in Celsius. + */ +uint16_t GetTemp() { + uint8_t tempbyte[2]; + GetMultipleRegisters(LIS3MDLTR_REG::OUT_TEMP_L,2,tempbyte); + uint16_t temp = (tempbyte[0])|(tempbyte[1]<<8); + return 25+temp/8; +} diff --git a/Components/LIS3MDLTRDriver.h b/Components/LIS3MDLTRDriver.h index 46a56a0..3b4cd31 100644 --- a/Components/LIS3MDLTRDriver.h +++ b/Components/LIS3MDLTRDriver.h @@ -45,11 +45,13 @@ class LIS3MDLTR_Driver { 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 GetMeasurements(uint8_t* out); void EnableTemp(); void DisableTemp(); + uint16_t GetTemp(); + LIS3MDLTR_Driver(const LIS3MDLTR_Driver&) = delete; LIS3MDLTR_Driver& operator=(const LIS3MDLTR_Driver&) = delete;