IMU ICM42688 Driver

Drivers/imu/inc/icm42688.hpp

@@ -0,0 +1,115 @@
+// icm42688.hpp
+
+#ifndef ICM42688_HPP
+#define ICM42688_HPP
+
+#include "imu_driver.hpp"
+#include <stdint.h>
+#include "stm32l5xx_hal.h"
+#include "stm32l5xx_hal_gpio.h"
+#include <stm32l552xx.h>
+
+#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 fssel -> full-scale selection for the gyro
+         * 
+        * @return none
+         */
+        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];
+
+        //Used to hold raw IMU data
+        int16_t raw_meas[RAW_MEAS_BUFFER_SIZE];
+
+        SPI_HandleTypeDef * SPI_HANDLE;
+        GPIO_TypeDef * CS_GPIO_PORT;
+        uint16_t CS_PIN;
+};
+
+#endif //ICM42688_HPP

Drivers/imu/inc/imu_driver.hpp

@@ -0,0 +1,33 @@
+// imu_driver.hpp
+
+#ifndef IMU_DRIVER_HPP
+#define IMU_DRIVER_HPP
+
+#include <stdint.h>
+
+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

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 <string.h>
+#include <stdio.h>
+
+//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) {
+    SPI_HANDLE = spi_handle;
+    CS_GPIO_PORT = cs_gpio_port;
+    CS_PIN = 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(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, 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_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_SPI_Transmit(SPI_HANDLE, tx_buf, sizeof(tx_buf), 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, &reg);
+
+    //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(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(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] / 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;
+}

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 ##

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")