diff --git a/Drivers/imu/inc/icm42688.hpp b/Drivers/imu/inc/icm42688.hpp new file mode 100644 index 00000000..ebce197f --- /dev/null +++ b/Drivers/imu/inc/icm42688.hpp @@ -0,0 +1,115 @@ +// icm42688.hpp + +#ifndef ICM42688_HPP +#define ICM42688_HPP + +#include "imu_driver.hpp" +#include +#include "stm32l5xx_hal.h" +#include "stm32l5xx_hal_gpio.h" +#include + +#define GYRO_CALIBRATION_DATA_BUFFER_SIZE 14 //Size of buffer to store raw data from IMU, as there are 14 registers to be read from +#define RAW_MEAS_BUFFER_SIZE 7 //Size of buffer to store raw measurements from IMU (3 accel, 3 gyrop, 1 temp) + +class ICM42688 : public IMUDriver { + public: + ICM42688(SPI_HandleTypeDef * spi_handle, GPIO_TypeDef * cs_gpio_port, uint16_t cs_pin); + + /** + * Sets up the IMU's initial starting conditions by setting + * the cs pin and calling reset(), setLowNoiseMode(), and calibrate() + * + * @return none + */ + void beginMeasuring() override; + + /** + * Calibrates the gyroscope. Algorithm adapted from https://github.com/finani/ICM42688/tree/master/src + * + * @return none + */ + void calibrate() override; + + /** + * Retrieve and process IMU accelerometer and gyroscope data into m/s and deg/s + * + * @param data_buffer -> array used to store and process raw data from IMU + * + * @return IMUData_t structure consisting of accelerometer and gyroscope data in m/s and deg/s + */ + IMUData_t getResult(uint8_t * data_buffer) override; + + private: + /** + * Communicate with IMU via SPI to read raw data + * + * @param sub_address -> memory address of starting byte to be retrieved + * @param num_bytes_to_retrieve -> number of bytes to be retrieved + * @param destination -> array to be populated with raw data + * + * @return none + */ + void readRegister(uint8_t sub_address, uint8_t num_bytes_to_retrieve, uint8_t * destination); + + /** + * Communicate with IMU via SPI to write data onto IMU + * + * @param sub_address -> memory address of byte to be written on + * @param data_to_imu -> data to be written onto IMU + * + * @return none + */ + void writeRegister(uint8_t sub_address, uint8_t data_to_imu); + + /** + * Resets the IMU's device configurations. Necessary to initilize IMU + * + * @return none + */ + void reset(); + + /** + * Sets the IMU's power management settings to low noise mode. Necessary to initilaize IMU + * + * @return none + */ + void setLowNoiseMode(); + + /** + * Sets the IMU register address bank to a value of 0 - 4 + * + * @param bank -> Bank number (0 - 4) + * + * @return none + */ + void setBank(uint8_t bank); + + /** + * Configures the full-scale range of the gyroscope on the IMU. Adapted from https://github.com/finani/ICM42688/tree/master/src + * + * @param fs_sel -> full-scale selection for the gyro + * + * @return none + */ + void setGyroFS(uint8_t fs_sel); + + //Variables used in gyro calibration + float gyroScale_ = 0; //gyro scale factor + uint8_t currentFSSel_ = 0; //current full-scale selection for the gyro + uint8_t gyroFS_ = 0; + float gyroBD_[3] = {0, 0, 0}; + float gyrB_[3] = {0, 0, 0}; + float gyr_[3] = {0, 0, 0}; + uint8_t gyro_buffer_[GYRO_CALIBRATION_DATA_BUFFER_SIZE]; + int16_t raw_meas_gyro_[RAW_MEAS_BUFFER_SIZE]; + + //Used to hold raw IMU data + int16_t raw_meas_[RAW_MEAS_BUFFER_SIZE]; + + SPI_HandleTypeDef * spiHandle_; + GPIO_TypeDef * csGpioPort_; + uint16_t csPin_; +}; + +#endif //ICM42688_HPP diff --git a/Drivers/imu/inc/imu_driver.hpp b/Drivers/imu/inc/imu_driver.hpp new file mode 100644 index 00000000..529e0210 --- /dev/null +++ b/Drivers/imu/inc/imu_driver.hpp @@ -0,0 +1,33 @@ +// imu_driver.hpp + +#ifndef IMU_DRIVER_HPP +#define IMU_DRIVER_HPP + +#include + +struct IMUData_t { + float gyrx, gyry, gyrz; + float accx, accy, accz; +}; + +class IMUDriver { + public: + /** + * Initializes the IMU for sampling + */ + virtual void beginMeasuring() = 0; + + /** + * Calibrates IMU gyro sensor + */ + virtual void calibrate() = 0; + + /** + * Retrieves newest data stored by IMU + */ + virtual IMUData_t getResult(uint8_t * data_buffer) = 0; + + virtual ~IMUDriver() {} +}; + +#endif //IMU_DRIVER_HPP diff --git a/Drivers/imu/src/icm42688.cpp b/Drivers/imu/src/icm42688.cpp new file mode 100644 index 00000000..2e41a913 --- /dev/null +++ b/Drivers/imu/src/icm42688.cpp @@ -0,0 +1,174 @@ +// icm42688.cpp + +#include "icm42688.hpp" +#include "stm32l5xx_hal.h" +#include "stm32l5xx_hal_gpio.h" +#include "stm32l5xx_hal_spi.h" +#include "spi.h" +#include "gpio.h" +#include +#include + +//Register Addresses +#define REG_BANK_SEL 0x76 +#define UB0_REG_DEVICE_CONFIG 0x11 +#define UB0_REG_PWR_MGMT0 0x4E +#define UB0_REG_TEMP_DATA1 0x1D + +#define BIT_READ 0x80 //Read bit mask + +#define NUM_GYRO_SAMPLES 1000 //Number of samples to take for calibration + +//Scale Factors (refer to page 11-12 of https://product.tdk.com/system/files/dam/doc/product/sensor/mortion-inertial/imu/data_sheet/ds-000347-icm-42688-p-v1.6.pdf) +#define GYRO_SENSITIVITY_2000DPS 16.4 //Currently in Primary Use +#define GYRO_SENSITIVITY_1000DPS 32.8 +#define GYRO_SENSITIVITY_500DPS 65.5 +#define GYRO_SENSITIVITY_250DPS 131 +#define GYRO_SENSITIVITY_125DPS 262 +#define GYRO_SENSITIVITY_62_5DPS 524.3 +#define GYRO_SENSITIVITY_31_25DPS 1048.6 +#define GYRO_SENSITIVITY_15_625PS 2097.2 + +#define ACCEL_SENSITIVITY_2G 16384 +#define ACCEL_SENSITIVITY_4G 8192 +#define ACCEL_SENSITIVITY_8G 4096 +#define ACCEL_SENSITIVITY_16G 2048 //Currently in Primary Use + +ICM42688::ICM42688(SPI_HandleTypeDef * spi_handle, GPIO_TypeDef * cs_gpio_port, uint16_t cs_pin) { + spiHandle_ = spi_handle; + csGpioPort_ = cs_gpio_port; + csPin_ = cs_pin; +} + +void ICM42688::readRegister(uint8_t sub_address, uint8_t num_bytes_to_retrieve, uint8_t * destination) { + //Set read bit for register address + uint8_t tx = sub_address | BIT_READ; + + //Dummy transmit and receive buffers + uint8_t dummy_tx[num_bytes_to_retrieve]; + uint8_t dummy_rx; + + //Initialize values to 0 + memset(dummy_tx, 0, num_bytes_to_retrieve * sizeof(dummy_tx[0])); + + HAL_GPIO_WritePin(csGpioPort_, csPin_, GPIO_PIN_RESET); + + HAL_SPI_TransmitReceive(spiHandle_, &tx, &dummy_rx, 1, HAL_MAX_DELAY); + HAL_SPI_TransmitReceive(spiHandle_, dummy_tx, destination, num_bytes_to_retrieve, HAL_MAX_DELAY); + + HAL_GPIO_WritePin(csGpioPort_, csPin_, GPIO_PIN_SET); +} + +void ICM42688::writeRegister(uint8_t sub_address, uint8_t data_to_imu) { + //Prepare transmit buffer + uint8_t tx_buf[2] = {sub_address, data_to_imu}; + + HAL_GPIO_WritePin(csGpioPort_, csPin_, GPIO_PIN_RESET); + + HAL_SPI_Transmit(spiHandle_, tx_buf, sizeof(tx_buf), HAL_MAX_DELAY); + + HAL_GPIO_WritePin(csGpioPort_, csPin_, GPIO_PIN_SET); +} + +void ICM42688::beginMeasuring() { + HAL_GPIO_WritePin(csGpioPort_, csPin_, GPIO_PIN_SET); + reset(); + setLowNoiseMode(); + calibrate(); +} + +void ICM42688::setBank(uint8_t bank) { + writeRegister(REG_BANK_SEL, bank); //bank should always be 0 for acc and gyr data +} + +void ICM42688::reset() { + setBank(0); + writeRegister(UB0_REG_DEVICE_CONFIG, 0x01); + HAL_Delay(1); +} + +void ICM42688::setLowNoiseMode() { + writeRegister(UB0_REG_PWR_MGMT0, 0x0F); +} + +void ICM42688::setGyroFS(uint8_t fs_sel) { + uint8_t reg; + + setBank(0); + + //Read current register value + readRegister(0x4F, 1, ®); + + //Only change FS_SEL in reg + reg = (fs_sel << 5) | (reg & 0x1F); + + writeRegister(0x4F, reg); + + gyroScale_ = (2000.0f / (float)(1 << fs_sel)) / 32768.0f; + gyroFS_ = fs_sel; +} + +void ICM42688::calibrate() { + //Set at a lower range (more resolution since IMU not moving) + const uint8_t currentFSSel_ = gyroFS_; + setGyroFS(GYRO_SENSITIVITY_250DPS); //Set to 250 dps + + //Take samples and find bias + gyroBD_[0] = 0; + gyroBD_[1] = 0; + gyroBD_[2] = 0; + + for (size_t i = 0; i < NUM_GYRO_SAMPLES; i++) { + readRegister(UB0_REG_TEMP_DATA1, 14, gyro_buffer_); + + //Combine raw bytes into 16-bit values + for (size_t j = 0; j < 7; j++) { + raw_meas_gyro_[j] = ((int16_t)gyro_buffer_[j * 2] << 8) | gyro_buffer_[j * 2 + 1]; + } + + //Process gyro data + for (size_t k = 0; k < 3; k++) { + gyr_[k] = (float)raw_meas_gyro_[k + 3] / GYRO_SENSITIVITY_250DPS; + } + + /* + Calculate bias by collecting samples and considering pre-existing bias + For each iteration, add the existing bias with the new bias and divide by the sample size + to get an average bias over a specified number of gyro samples + */ + gyroBD_[0] += (gyr_[0] + gyrB_[0]) / NUM_GYRO_SAMPLES; + gyroBD_[1] += (gyr_[1] + gyrB_[1]) / NUM_GYRO_SAMPLES; + gyroBD_[2] += (gyr_[2] + gyrB_[2]) / NUM_GYRO_SAMPLES; + + HAL_Delay(1); + } + + gyrB_[0] = gyroBD_[0]; + gyrB_[1] = gyroBD_[1]; + gyrB_[2] = gyroBD_[2]; + + //Recover the full scale setting + setGyroFS(currentFSSel_); +} + +IMUData_t ICM42688::getResult(uint8_t * data_buffer) { + //Collect Data + readRegister(UB0_REG_TEMP_DATA1, 14, data_buffer); + + //Formatting raw data + for (size_t i = 0; i < 3; i++) { + raw_meas_[i] = ((int16_t)data_buffer[i * 2] << 8) | data_buffer[i * 2 + 1]; + } + + IMUData_t IMUData {}; + + IMUData.accx = (float)raw_meas_[1] / ACCEL_SENSITIVITY_16G; + IMUData.accy = (float)raw_meas_[2] / ACCEL_SENSITIVITY_16G; + IMUData.accz = (float)raw_meas_[3] / ACCEL_SENSITIVITY_16G; + + IMUData.gyrx = (float)raw_meas_[4] / GYRO_SENSITIVITY_2000DPS; + IMUData.gyry = (float)raw_meas_[5] / GYRO_SENSITIVITY_2000DPS; + IMUData.gyrz = (float)raw_meas_[6] / GYRO_SENSITIVITY_2000DPS; + + return IMUData; +} \ No newline at end of file diff --git a/Tools/Firmware/CMakeLists.txt b/Tools/Firmware/CMakeLists.txt index 57afd5a2..14419750 100644 --- a/Tools/Firmware/CMakeLists.txt +++ b/Tools/Firmware/CMakeLists.txt @@ -33,6 +33,7 @@ include_directories( ${ROOT_DIR}/Drivers/common/circular_buffer/inc ${ROOT_DIR}/Drivers/common/drivers_config/inc ${ROOT_DIR}/Drivers/common/dma_uart_device/inc + ${ROOT_DIR}/Drivers/imu/inc ${ROOT_DIR}/Drivers/iwdg_driver/inc ${ROOT_DIR}/Drivers/motor_channel/inc ${ROOT_DIR}/Drivers/rc_receiver/inc @@ -114,6 +115,8 @@ set(DRIVERS_IWDGDriver_DIR ${ROOT_DIR}/Drivers/iwdg_driver/src) set(DRIVERS_IWDGDriver_CXX_SOURCES "${DRIVERS_IWDGDriver_DIR}/*.cpp") set(DRIVERS_SDCard_DIR ${ROOT_DIR}/Drivers/sd_card/src) set(DRIVERS_SDCard_C_SOURCES "${DRIVERS_SDCard_DIR}/*.c") +set(DRIVERS_IMU_DIR ${ROOT_DIR}/Drivers/imu/src) +set(DRIVERS_IMU_CXX_SOURCES "${DRIVERS_IMU_DIR}/*.cpp") ## Actually find the sources, NOTE: if you add a new source above, add it here! ## @@ -141,7 +144,8 @@ file(GLOB_RECURSE CXX_SOURCES ${HAL_CORE_CXX_SOURCES} ${DRIVERS_Common_CXX_SOURCES} ${DRIVERS_CXX_SOURCES} ${DRIVERS_IWDGDriver_CXX_SOURCES} - ${DRIVERS_MotorChannel_CXX_SOURCES}) + ${DRIVERS_MotorChannel_CXX_SOURCES} + ${DRIVERS_IMU_CXX_SOURCES}) message("C++ Sources: ${CXX_SOURCES}") ## Find the startup and linker script ## diff --git a/Tools/Testing/CMakeLists.txt b/Tools/Testing/CMakeLists.txt index 15769215..0d2e67c6 100644 --- a/Tools/Testing/CMakeLists.txt +++ b/Tools/Testing/CMakeLists.txt @@ -93,6 +93,9 @@ set(CIRCULAR_BUFFER_SRC "${DRIVERS_FOLDER}/common/circular_buffer/src") set(DRIVERS_CONFIG_INC "${DRIVERS_FOLDER}/Drivers/common/drivers_config/inc") +set(IMU_INC "${DRIVERS_FOLDER}/IMU/inc") +set(IMU_SRC "${DRIVERS_FOLDER}/IMU/src") + set(MOTORCHANNEL_MOCK_INC "${ROOT_DIR}/Testing/Mocks/Inc") set(MOTORCHANNEL_MOCK_SRC "${ROOT_DIR}/Testing/Mocks/Src")