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

Author: kelvinos2624

Drivers/imu/inc/icm42688.hpp

@@ -0,0 +1,109 @@
+// icm42688.hpp
+
+#ifndef ICM42688_HPP
+#define ICM42688_HPP
+
+#include "imu_driver.hpp"
+#include <stdint.h>
+
+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

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,137 @@
+// icm42688.cpp
+
+#include "icm42688.hpp"
+#include "stm32l5xx_hal_conf"
+#include "stm32l5xx_it.h"
+#include "spi.h"
+#include "gpio.h"
+#include <string.h>
+#include <stdio.h>
+#include <stdbool.h>
+
+#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, &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(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;
+}