From 9757b1337b9882c3787106b75a54845154cc10c7 Mon Sep 17 00:00:00 2001 From: kelvinos2624 Date: Tue, 24 Sep 2024 21:28:41 -0400 Subject: [PATCH 1/5] IMU Driver for ICM42688 Created cpp and header files for the ICM42688. Code has been structured from the tested code found at https://github.com/UWARG/efs-kitchen-sinks/blob/main/icm42688.zip --- Drivers/imu/inc/icm42688.hpp | 109 ++++++++++++++++++++++++++ Drivers/imu/inc/imu_driver.hpp | 33 ++++++++ Drivers/imu/src/icm42688.cpp | 137 +++++++++++++++++++++++++++++++++ 3 files changed, 279 insertions(+) create mode 100644 Drivers/imu/inc/icm42688.hpp create mode 100644 Drivers/imu/inc/imu_driver.hpp create mode 100644 Drivers/imu/src/icm42688.cpp diff --git a/Drivers/imu/inc/icm42688.hpp b/Drivers/imu/inc/icm42688.hpp new file mode 100644 index 0000000..94f544c --- /dev/null +++ b/Drivers/imu/inc/icm42688.hpp @@ -0,0 +1,109 @@ +// icm42688.hpp + +#ifndef ICM42688_HPP +#define ICM42688_HPP + +#include "imu_driver.hpp" +#include + +class ICM42688 : public IMUDriver { + public: + /** + * 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; + + /** + * Communicate with IMU via SPI to read raw data + * + * @param sub_address -> memory address of starting byte to be retrieved + * @param count -> number of bytes to be retrieved + * @param dest -> array to be populated with raw data + * + * @return none + */ + void readRegister(uint8_t sub_address, uint8_t count, uint8_t * dest); + + /** + * Communicate with IMU via SPI to write data onto IMU + * + * @param sub_address -> memory address of byte to be written on + * @param data -> data to be written onto IMU + * + * @return none + */ + void writeRegister(uint8_t sub_address, uint8_t data); + + /** + * 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 fssel -> full-scale selection for the gyro + * + * @return none + */ + void setGyroFS(uint8_t fssel); + + private: + //Constants (can be found in ICM42688 documentation https://www.cdiweb.com/datasheets/invensense/ds-000347-icm-42688-p-v1.2.pdf) + const uint8_t REG_BANK_SEL = 0x76; + const uint8_t UB0_REG_DEVICE_CONFIG = 0x11; + const uint8_t UB0_REG_PWR_MGMT0 = 0x4E; + const uint8_t UB0_REG_TEMP_DATA1 = 0x1D; + + //Variables used in gyro calibration + float gyro_scale = 0; + uint8_t current_fssel = 0; + 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[14]; + int16_t raw_meas_gyro[7]; + + //Used to hold raw IMU data + int16_t raw_meas[7]; +}; + +#endif //ICM42688_HPP diff --git a/Drivers/imu/inc/imu_driver.hpp b/Drivers/imu/inc/imu_driver.hpp new file mode 100644 index 0000000..529e021 --- /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 0000000..9306dc6 --- /dev/null +++ b/Drivers/imu/src/icm42688.cpp @@ -0,0 +1,137 @@ +// icm42688.cpp + +#include "icm42688.hpp" +#include "stm32l5xx_hal_conf" +#include "stm32l5xx_it.h" +#include "spi.h" +#include "gpio.h" +#include +#include +#include + +#define CS_GPIO_PORT GPIOA +#define CS_PIN GPIO_PIN_4 + +void ICM42688::readRegister(uint8_t sub_address, uint8_t count, uint8_t * dest) { + //Set read bit for register address + uint8_t tx = sub_address | 0x80; + + //Dummy transmit and receive buffers + uint8_t dummy_tx[count]; + uint8_t dummy_rx; + + //Initialize values to 0 + memset(dummy_tx, 0, count * sizeof(dummy_tx[0])); + + HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_RESET); + + HAL_SPI_TransmitReceive(&hspi1, &tx, &dummy_rx, 1, HAL_MAX_DELAY); + HAL_SPI_TransmitReceive(&hspi1, dummy_tx, dest, count, HAL_MAX_DELAY); + + HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_SET); +} + +void ICM42688::writeRegister(uint8_t sub_address, uint8_t data) { + //Prepare transmit buffer + uint8_t tx_buf[2] = {sub_address, data}; + + HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_RESET); + + HAL_SPI_Transmit(&hspi1, tx_buf, 2, HAL_MAX_DELAY); + + HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_SET); +} + +void ICM42688::beginMeasuring() { + HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, 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 fssel) { + uint8_t reg; + + setBank(0); + + //Read current register value + readRegister(0x4F, 1, ®); + + //Only change FS_SEL in reg + reg = (fssel << 5) | (reg & 0x1F); + + writeRegister(0x4F, reg); + + gyro_scale = (2000.0f / (float)(1 << fssel)) / 32768.0f; + gyroFS = fssel; +} + +void ICM42688::calibrate() { + //Set at a lower range (more resolution since IMU not moving) + const uint8_t current_fssel = gyroFS; + setGyroFS(0x03); //Set to 250 dps + + //Take samples and find bias + gyroBD[0] = 0; + gyroBD[1] = 0; + gyroBD[2] = 0; + + for (size_t i = 0; i < 3; i++) { + getResult(gyro_buffer); + for (size_t i = 0; i < 7; i++) { + raw_meas_gyro[i] = ((int16_t)gyro_buffer[i * 2] << 8) | gyro_buffer[i * 2 + 1]; + } + for (size_t i = 0; i < 3; i++) { + gyr[i] = (float)raw_meas_gyro[i + 3] / 16.4; + } + + gyroBD[0] += (gyr[0] + gyrB[0]) / 1000; + gyroBD[1] += (gyr[1] + gyrB[1]) / 1000; + gyroBD[2] += (gyr[2] + gyrB[2]) / 1000; + + HAL_Delay(1); + } + + gyrB[0] = gyroBD[0]; + gyrB[1] = gyroBD[1]; + gyrB[2] = gyroBD[2]; + + //Recover the full scale setting + setGyroFS(current_fssel); +} + +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] / 2048.0; //Sensitivity Scale Factor in LSB/g + IMUData.accy = (float)raw_meas[2] / 2048.0; //2048.0 Corresponds with +/- 16g + IMUData.accz = (float)raw_meas[3] / 2048.0; + + IMUData.gyrx = (float)raw_meas[4] / 16.4; //Sensitivity Scale Factor LSB/dps + IMUData.gyry = (float)raw_meas[5] / 16.4; //16.4 Corresponds with +/- 2000dps + IMUData.gyrz = (float)raw_meas[6] / 16.4; + + return IMUData; +} \ No newline at end of file From 67f8b4010c85943ce2c72e670c67a0456f0b4275 Mon Sep 17 00:00:00 2001 From: kelvinos2624 Date: Tue, 8 Oct 2024 22:23:46 -0400 Subject: [PATCH 2/5] Code Clean Up and Restructuring - Implemented a constructor to take in SPI_HANDLE, GPIO PORT, and CS_PIN - Moved some methods to private for abstraction - Used new defines to reduce magic numbers --- Drivers/imu/inc/icm42688.hpp | 20 +++++++++++--- Drivers/imu/src/icm42688.cpp | 49 +++++++++++++++++++++++++---------- Tools/Firmware/CMakeLists.txt | 3 +++ 3 files changed, 55 insertions(+), 17 deletions(-) diff --git a/Drivers/imu/inc/icm42688.hpp b/Drivers/imu/inc/icm42688.hpp index 94f544c..88f76b6 100644 --- a/Drivers/imu/inc/icm42688.hpp +++ b/Drivers/imu/inc/icm42688.hpp @@ -5,9 +5,17 @@ #include "imu_driver.hpp" #include +#include "stm32f4xx_hal.h" +#include "stm32f4xx_hal_gpio.h" + +#define GYRO_CALIBRATION_DATA_BUFFER_SIZE 14 +#define GYRO_CALIBRATION_RAW_DATA_BUFFER_SIZE 7 +#define RAW_DATA_BUFFER_SIZE 7 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() @@ -32,6 +40,7 @@ class ICM42688 : public IMUDriver { */ IMUData_t getResult(uint8_t * data_buffer) override; + private: /** * Communicate with IMU via SPI to read raw data * @@ -85,7 +94,6 @@ class ICM42688 : public IMUDriver { */ void setGyroFS(uint8_t fssel); - private: //Constants (can be found in ICM42688 documentation https://www.cdiweb.com/datasheets/invensense/ds-000347-icm-42688-p-v1.2.pdf) const uint8_t REG_BANK_SEL = 0x76; const uint8_t UB0_REG_DEVICE_CONFIG = 0x11; @@ -99,11 +107,15 @@ class ICM42688 : public IMUDriver { float gyroBD[3] = {0, 0, 0}; float gyrB[3] = {0, 0, 0}; float gyr[3] = {0, 0, 0}; - uint8_t gyro_buffer[14]; - int16_t raw_meas_gyro[7]; + uint8_t gyro_buffer[GYRO_CALIBRATION_DATA_BUFFER_SIZE]; + int16_t raw_meas_gyro[GYRO_CALIBRATION_RAW_DATA_BUFFER_SIZE]; //Used to hold raw IMU data - int16_t raw_meas[7]; + int16_t raw_meas[RAW_DATA_BUFFER_SIZE]; + + SPI_HandleTypeDef * SPI_HANDLE; + GPIO_TypeDef * CS_GPIO_PORT; + uint16_t CS_PIN; }; #endif //ICM42688_HPP diff --git a/Drivers/imu/src/icm42688.cpp b/Drivers/imu/src/icm42688.cpp index 9306dc6..890f9b4 100644 --- a/Drivers/imu/src/icm42688.cpp +++ b/Drivers/imu/src/icm42688.cpp @@ -1,7 +1,7 @@ // icm42688.cpp #include "icm42688.hpp" -#include "stm32l5xx_hal_conf" +#include "stm32l5xx_hal_conf.h" #include "stm32l5xx_it.h" #include "spi.h" #include "gpio.h" @@ -9,12 +9,35 @@ #include #include -#define CS_GPIO_PORT GPIOA -#define CS_PIN GPIO_PIN_4 +//#define CS_GPIO_PORT GPIOA +//#define CS_PIN GPIO_PIN_4 + +#define REG_BANK_SEL 0x76; +#define UB0_REG_DEVICE_CONFIG 0x11; +#define UB0_REG_PWR_MGMT0 0x4E; +#define UB0_REG_TEMP_DATA1 0x1D; + +#define READ_BIT 0x80 + +//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 void ICM42688::readRegister(uint8_t sub_address, uint8_t count, uint8_t * dest) { //Set read bit for register address - uint8_t tx = sub_address | 0x80; + uint8_t tx = sub_address | READ_BIT; //Dummy transmit and receive buffers uint8_t dummy_tx[count]; @@ -25,8 +48,8 @@ void ICM42688::readRegister(uint8_t sub_address, uint8_t count, uint8_t * dest) HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_RESET); - HAL_SPI_TransmitReceive(&hspi1, &tx, &dummy_rx, 1, HAL_MAX_DELAY); - HAL_SPI_TransmitReceive(&hspi1, dummy_tx, dest, count, HAL_MAX_DELAY); + HAL_SPI_TransmitReceive(SPI_HANDLE, &tx, &dummy_rx, 1, HAL_MAX_DELAY); + HAL_SPI_TransmitReceive(SPI_HANDLE, dummy_tx, dest, count, HAL_MAX_DELAY); HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_SET); } @@ -37,7 +60,7 @@ void ICM42688::writeRegister(uint8_t sub_address, uint8_t data) { HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_RESET); - HAL_SPI_Transmit(&hspi1, tx_buf, 2, HAL_MAX_DELAY); + HAL_SPI_Transmit(SPI_HANDLE, tx_buf, sizeof(tx_buf), HAL_MAX_DELAY); HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_SET); } @@ -125,13 +148,13 @@ IMUData_t ICM42688::getResult(uint8_t * data_buffer) { IMUData_t IMUData {}; - IMUData.accx = (float)raw_meas[1] / 2048.0; //Sensitivity Scale Factor in LSB/g - IMUData.accy = (float)raw_meas[2] / 2048.0; //2048.0 Corresponds with +/- 16g - IMUData.accz = (float)raw_meas[3] / 2048.0; + 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] / 16.4; //Sensitivity Scale Factor LSB/dps - IMUData.gyry = (float)raw_meas[5] / 16.4; //16.4 Corresponds with +/- 2000dps - IMUData.gyrz = (float)raw_meas[6] / 16.4; + 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 57afd5a..d7a080d 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! ## From df4485983d0c1c399ea496444b722e4f3ad99528 Mon Sep 17 00:00:00 2001 From: kelvinos2624 Date: Sun, 27 Oct 2024 17:44:37 -0400 Subject: [PATCH 3/5] Fixed compilation issues Altered cmakelist to support compilation --- Drivers/imu/inc/icm42688.hpp | 5 +++-- Drivers/imu/src/icm42688.cpp | 27 +++++++++++++++------------ Tools/Firmware/CMakeLists.txt | 3 ++- Tools/Testing/CMakeLists.txt | 3 +++ 4 files changed, 23 insertions(+), 15 deletions(-) diff --git a/Drivers/imu/inc/icm42688.hpp b/Drivers/imu/inc/icm42688.hpp index 88f76b6..0f0d1ef 100644 --- a/Drivers/imu/inc/icm42688.hpp +++ b/Drivers/imu/inc/icm42688.hpp @@ -5,8 +5,9 @@ #include "imu_driver.hpp" #include -#include "stm32f4xx_hal.h" -#include "stm32f4xx_hal_gpio.h" +#include "stm32l5xx_hal.h" +#include "stm32l5xx_hal_gpio.h" +#include #define GYRO_CALIBRATION_DATA_BUFFER_SIZE 14 #define GYRO_CALIBRATION_RAW_DATA_BUFFER_SIZE 7 diff --git a/Drivers/imu/src/icm42688.cpp b/Drivers/imu/src/icm42688.cpp index 890f9b4..3310508 100644 --- a/Drivers/imu/src/icm42688.cpp +++ b/Drivers/imu/src/icm42688.cpp @@ -1,23 +1,20 @@ // icm42688.cpp #include "icm42688.hpp" -#include "stm32l5xx_hal_conf.h" -#include "stm32l5xx_it.h" +#include "stm32l5xx_hal.h" +#include "stm32l5xx_hal_gpio.h" +#include "stm32l5xx_hal_spi.h" #include "spi.h" #include "gpio.h" #include #include -#include -//#define CS_GPIO_PORT GPIOA -//#define CS_PIN GPIO_PIN_4 +#define REG_BANK_SEL 0x76 +#define UB0_REG_DEVICE_CONFIG 0x11 +#define UB0_REG_PWR_MGMT0 0x4E +#define UB0_REG_TEMP_DATA1 0x1D -#define REG_BANK_SEL 0x76; -#define UB0_REG_DEVICE_CONFIG 0x11; -#define UB0_REG_PWR_MGMT0 0x4E; -#define UB0_REG_TEMP_DATA1 0x1D; - -#define READ_BIT 0x80 +#define BIT_READ 0x80 //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) @@ -35,9 +32,15 @@ #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) { + SPI_HANDLE = spi_handle; + CS_GPIO_PORT = cs_gpio_port; + CS_PIN = cs_pin; +} + void ICM42688::readRegister(uint8_t sub_address, uint8_t count, uint8_t * dest) { //Set read bit for register address - uint8_t tx = sub_address | READ_BIT; + uint8_t tx = sub_address | BIT_READ; //Dummy transmit and receive buffers uint8_t dummy_tx[count]; diff --git a/Tools/Firmware/CMakeLists.txt b/Tools/Firmware/CMakeLists.txt index d7a080d..1441975 100644 --- a/Tools/Firmware/CMakeLists.txt +++ b/Tools/Firmware/CMakeLists.txt @@ -144,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 1576921..0d2e67c 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") From f1b30cb7386aa0fe07aedb687c2bb9d1b994c379 Mon Sep 17 00:00:00 2001 From: kelvinos2624 Date: Tue, 5 Nov 2024 22:05:37 -0500 Subject: [PATCH 4/5] IMU ICM42688 Driver Implementation IMU Driver restructured with a virtual class header file, actual driver header file, and driver code. Implemented code to calibrate, read, and process data from a 6-axis IMU --- Drivers/imu/inc/icm42688.hpp | 29 +++++++++------------- Drivers/imu/src/icm42688.cpp | 47 ++++++++++++++++++++++-------------- 2 files changed, 40 insertions(+), 36 deletions(-) diff --git a/Drivers/imu/inc/icm42688.hpp b/Drivers/imu/inc/icm42688.hpp index 0f0d1ef..a58d829 100644 --- a/Drivers/imu/inc/icm42688.hpp +++ b/Drivers/imu/inc/icm42688.hpp @@ -9,9 +9,8 @@ #include "stm32l5xx_hal_gpio.h" #include -#define GYRO_CALIBRATION_DATA_BUFFER_SIZE 14 -#define GYRO_CALIBRATION_RAW_DATA_BUFFER_SIZE 7 -#define RAW_DATA_BUFFER_SIZE 7 +#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: @@ -46,22 +45,22 @@ class ICM42688 : public IMUDriver { * Communicate with IMU via SPI to read raw data * * @param sub_address -> memory address of starting byte to be retrieved - * @param count -> number of bytes to be retrieved - * @param dest -> array to be populated with raw data + * @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 count, uint8_t * dest); + 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 -> data to be written onto IMU + * @param data_to_imu -> data to be written onto IMU * * @return none */ - void writeRegister(uint8_t sub_address, uint8_t data); + void writeRegister(uint8_t sub_address, uint8_t data_to_imu); /** * Resets the IMU's device configurations. Necessary to initilize IMU @@ -95,24 +94,18 @@ class ICM42688 : public IMUDriver { */ void setGyroFS(uint8_t fssel); - //Constants (can be found in ICM42688 documentation https://www.cdiweb.com/datasheets/invensense/ds-000347-icm-42688-p-v1.2.pdf) - const uint8_t REG_BANK_SEL = 0x76; - const uint8_t UB0_REG_DEVICE_CONFIG = 0x11; - const uint8_t UB0_REG_PWR_MGMT0 = 0x4E; - const uint8_t UB0_REG_TEMP_DATA1 = 0x1D; - //Variables used in gyro calibration - float gyro_scale = 0; - uint8_t current_fssel = 0; + float gyro_scale = 0; //gyro scale factor + uint8_t current_fssel = 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[GYRO_CALIBRATION_RAW_DATA_BUFFER_SIZE]; + int16_t raw_meas_gyro[RAW_MEAS_BUFFER_SIZE]; //Used to hold raw IMU data - int16_t raw_meas[RAW_DATA_BUFFER_SIZE]; + int16_t raw_meas[RAW_MEAS_BUFFER_SIZE]; SPI_HandleTypeDef * SPI_HANDLE; GPIO_TypeDef * CS_GPIO_PORT; diff --git a/Drivers/imu/src/icm42688.cpp b/Drivers/imu/src/icm42688.cpp index 3310508..69e8ea3 100644 --- a/Drivers/imu/src/icm42688.cpp +++ b/Drivers/imu/src/icm42688.cpp @@ -9,15 +9,17 @@ #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 +#define BIT_READ 0x80 //Read bit mask -//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 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 @@ -38,28 +40,28 @@ ICM42688::ICM42688(SPI_HandleTypeDef * spi_handle, GPIO_TypeDef * cs_gpio_port, CS_PIN = cs_pin; } -void ICM42688::readRegister(uint8_t sub_address, uint8_t count, uint8_t * dest) { +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[count]; + uint8_t dummy_tx[num_bytes_to_retrieve]; uint8_t dummy_rx; //Initialize values to 0 - memset(dummy_tx, 0, count * sizeof(dummy_tx[0])); + memset(dummy_tx, 0, num_bytes_to_retrieve * sizeof(dummy_tx[0])); HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_RESET); HAL_SPI_TransmitReceive(SPI_HANDLE, &tx, &dummy_rx, 1, HAL_MAX_DELAY); - HAL_SPI_TransmitReceive(SPI_HANDLE, dummy_tx, dest, count, HAL_MAX_DELAY); + HAL_SPI_TransmitReceive(SPI_HANDLE, dummy_tx, destination, num_bytes_to_retrieve, HAL_MAX_DELAY); HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_SET); } -void ICM42688::writeRegister(uint8_t sub_address, uint8_t data) { +void ICM42688::writeRegister(uint8_t sub_address, uint8_t data_to_imu) { //Prepare transmit buffer - uint8_t tx_buf[2] = {sub_address, data}; + uint8_t tx_buf[2] = {sub_address, data_to_imu}; HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_RESET); @@ -109,25 +111,34 @@ void ICM42688::setGyroFS(uint8_t fssel) { void ICM42688::calibrate() { //Set at a lower range (more resolution since IMU not moving) const uint8_t current_fssel = gyroFS; - setGyroFS(0x03); //Set to 250 dps + 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 < 3; i++) { - getResult(gyro_buffer); - for (size_t i = 0; i < 7; i++) { - raw_meas_gyro[i] = ((int16_t)gyro_buffer[i * 2] << 8) | gyro_buffer[i * 2 + 1]; + 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]; } - for (size_t i = 0; i < 3; i++) { - gyr[i] = (float)raw_meas_gyro[i + 3] / 16.4; + + //Process gyro data + for (size_t k = 0; k < 3; k++) { + gyr[k] = (float)raw_meas_gyro[k + 3] / GYRO_SENSITIVITY_250DPS; } - gyroBD[0] += (gyr[0] + gyrB[0]) / 1000; - gyroBD[1] += (gyr[1] + gyrB[1]) / 1000; - gyroBD[2] += (gyr[2] + gyrB[2]) / 1000; + /* + 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); } From 11d848cb97730e861065150699f6eca1ca816147 Mon Sep 17 00:00:00 2001 From: kelvinos2624 Date: Wed, 6 Nov 2024 20:42:12 -0500 Subject: [PATCH 5/5] Variable naming conventions Modified member variable names to align with the WARG naming convention --- Drivers/imu/inc/icm42688.hpp | 26 +++++++------- Drivers/imu/src/icm42688.cpp | 68 ++++++++++++++++++------------------ 2 files changed, 47 insertions(+), 47 deletions(-) diff --git a/Drivers/imu/inc/icm42688.hpp b/Drivers/imu/inc/icm42688.hpp index a58d829..7a56821 100644 --- a/Drivers/imu/inc/icm42688.hpp +++ b/Drivers/imu/inc/icm42688.hpp @@ -14,7 +14,7 @@ class ICM42688 : public IMUDriver { public: - ICM42688(SPI_HandleTypeDef * SPI_HANDLE, GPIO_TypeDef * CS_GPIO_PORT, uint16_t CS_PIN); + ICM42688(SPI_HandleTypeDef * spi_handle, GPIO_TypeDef * cs_gpio_port, uint16_t cs_pin); /** * Sets up the IMU's initial starting conditions by setting @@ -95,21 +95,21 @@ class ICM42688 : public IMUDriver { void setGyroFS(uint8_t fssel); //Variables used in gyro calibration - float gyro_scale = 0; //gyro scale factor - uint8_t current_fssel = 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]; + 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]; + int16_t raw_meas_[RAW_MEAS_BUFFER_SIZE]; - SPI_HandleTypeDef * SPI_HANDLE; - GPIO_TypeDef * CS_GPIO_PORT; - uint16_t CS_PIN; + SPI_HandleTypeDef * spiHandle_; + GPIO_TypeDef * csGpioPort_; + uint16_t csPin_; }; #endif //ICM42688_HPP diff --git a/Drivers/imu/src/icm42688.cpp b/Drivers/imu/src/icm42688.cpp index 69e8ea3..0ff4067 100644 --- a/Drivers/imu/src/icm42688.cpp +++ b/Drivers/imu/src/icm42688.cpp @@ -35,9 +35,9 @@ #define ACCEL_SENSITIVITY_16G 2048 //Currently in Primary Use ICM42688::ICM42688(SPI_HandleTypeDef * spi_handle, GPIO_TypeDef * cs_gpio_port, uint16_t cs_pin) { - SPI_HANDLE = spi_handle; - CS_GPIO_PORT = cs_gpio_port; - CS_PIN = 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) { @@ -51,27 +51,27 @@ void ICM42688::readRegister(uint8_t sub_address, uint8_t num_bytes_to_retrieve, //Initialize values to 0 memset(dummy_tx, 0, num_bytes_to_retrieve * sizeof(dummy_tx[0])); - HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_RESET); + HAL_GPIO_WritePin(csGpioPort_, csPin_, GPIO_PIN_RESET); - HAL_SPI_TransmitReceive(SPI_HANDLE, &tx, &dummy_rx, 1, HAL_MAX_DELAY); - HAL_SPI_TransmitReceive(SPI_HANDLE, dummy_tx, destination, num_bytes_to_retrieve, HAL_MAX_DELAY); + 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(CS_GPIO_PORT, CS_PIN, GPIO_PIN_SET); + 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(CS_GPIO_PORT, CS_PIN, GPIO_PIN_RESET); + HAL_GPIO_WritePin(csGpioPort_, csPin_, GPIO_PIN_RESET); - HAL_SPI_Transmit(SPI_HANDLE, tx_buf, sizeof(tx_buf), HAL_MAX_DELAY); + HAL_SPI_Transmit(spiHandle_, tx_buf, sizeof(tx_buf), HAL_MAX_DELAY); - HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_SET); + HAL_GPIO_WritePin(csGpioPort_, csPin_, GPIO_PIN_SET); } void ICM42688::beginMeasuring() { - HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, GPIO_PIN_SET); + HAL_GPIO_WritePin(csGpioPort_, csPin_, GPIO_PIN_SET); reset(); setLowNoiseMode(); calibrate(); @@ -104,31 +104,31 @@ void ICM42688::setGyroFS(uint8_t fssel) { writeRegister(0x4F, reg); - gyro_scale = (2000.0f / (float)(1 << fssel)) / 32768.0f; - gyroFS = fssel; + gyroScale_ = (2000.0f / (float)(1 << fssel)) / 32768.0f; + gyroFS_ = fssel; } void ICM42688::calibrate() { //Set at a lower range (more resolution since IMU not moving) - const uint8_t current_fssel = gyroFS; + 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; + 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); + 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]; + 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; + gyr_[k] = (float)raw_meas_gyro_[k + 3] / GYRO_SENSITIVITY_250DPS; } /* @@ -136,19 +136,19 @@ void ICM42688::calibrate() { 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; + 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]; + gyrB_[0] = gyroBD_[0]; + gyrB_[1] = gyroBD_[1]; + gyrB_[2] = gyroBD_[2]; //Recover the full scale setting - setGyroFS(current_fssel); + setGyroFS(currentFSSel_); } IMUData_t ICM42688::getResult(uint8_t * data_buffer) { @@ -157,18 +157,18 @@ IMUData_t ICM42688::getResult(uint8_t * 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]; + 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.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; + 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