├── .gitignore ├── extras ├── signal_path.png └── DS-000292-ICM-42605-v1.7.pdf ├── library.properties ├── keywords.txt ├── LICENSE ├── examples ├── Basic_I2C │ └── Basic_I2C.ino ├── Basic_SPI │ └── Basic_SPI.ino ├── Advanced_I2C │ └── Advanced_I2C.ino ├── FIFO_SPI │ └── FIFO_SPI.ino ├── Interrupt_SPI │ └── Interrupt_SPI.ino └── Stm32F722_SPI_Raw │ └── Stm32F722_SPI_Raw.ino ├── README.md └── src ├── registers.h ├── ICM42605.h └── ICM42605.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | -------------------------------------------------------------------------------- /extras/signal_path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/njh/arduino-ICM42605/master/extras/signal_path.png -------------------------------------------------------------------------------- /extras/DS-000292-ICM-42605-v1.7.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/njh/arduino-ICM42605/master/extras/DS-000292-ICM-42605-v1.7.pdf -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=ICM42605 2 | version=1.1.0 3 | author=Inhwan Wee , Parker Lusk , Dominic Clifton 4 | maintainer=Dominic Clifton 5 | sentence=Library for communicating with the ICM42605 six-axis Inertial Measurement Units (IMU). 6 | paragraph=This library supports both I2C and SPI communication with the ICM42605. Based on the ICM42688 library. 7 | category=Sensors 8 | url=https://github.com/hydra/ICM42605 9 | architectures=* 10 | includes=ICM42605.h 11 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ICM42605 KEYWORD1 2 | ICM42605FIFO KEYWORD1 3 | begin KEYWORD2 4 | setAccelFS KEYWORD2 5 | setGyroFS KEYWORD2 6 | setAccelODR KEYWORD2 7 | setGyroODR KEYWORD2 8 | setFilters KEYWORD2 9 | enableDataReadyInterrupt KEYWORD2 10 | disableDataReadyInterrupt KEYWORD2 11 | enableFifo KEYWORD2 12 | getAGT KEYWORD2 13 | accX KEYWORD2 14 | accY KEYWORD2 15 | accZ KEYWORD2 16 | gyrX KEYWORD2 17 | gyrY KEYWORD2 18 | gyrZ KEYWORD2 19 | temp KEYWORD2 20 | readFifo KEYWORD2 21 | getFifoAccelX KEYWORD2 22 | getFifoAccelY KEYWORD2 23 | getFifoAccelZ KEYWORD2 24 | getFifoGyroX KEYWORD2 25 | getFifoGyroY KEYWORD2 26 | getFifoGyroZ KEYWORD2 27 | getFifoTemp_C KEYWORD2 28 | calibrateGyro KEYWORD2 29 | getGyroBiasX KEYWORD2 30 | getGyroBiasY KEYWORD2 31 | getGyroBiasZ KEYWORD2 32 | setGyroBiasX KEYWORD2 33 | setGyroBiasY KEYWORD2 34 | setGyroBiasZ KEYWORD2 35 | calibrateAccel KEYWORD2 36 | getAccelBiasX KEYWORD2 37 | getAccelScaleFactorX KEYWORD2 38 | getAccelBiasY KEYWORD2 39 | getAccelScaleFactorY KEYWORD2 40 | getAccelBiasZ KEYWORD2 41 | getAccelScaleFactorZ KEYWORD2 42 | setAccelCalX KEYWORD2 43 | setAccelCalY KEYWORD2 44 | setAccelCalZ KEYWORD2 45 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Inhwan Wee 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /examples/Basic_I2C/Basic_I2C.ino: -------------------------------------------------------------------------------- 1 | #include "ICM42605.h" 2 | 3 | // an ICM42605 object with the ICM42605 sensor on I2C bus 0 with address 0x68 4 | ICM42605 IMU(Wire, 0x68); 5 | 6 | void setup() { 7 | // serial to display data 8 | Serial.begin(115200); 9 | while(!Serial) {} 10 | 11 | // start communication with IMU 12 | int status = IMU.begin(); 13 | if (status < 0) { 14 | Serial.println("IMU initialization unsuccessful"); 15 | Serial.println("Check IMU wiring or try cycling power"); 16 | Serial.print("Status: "); 17 | Serial.println(status); 18 | while(1) {} 19 | } 20 | Serial.println("ax,ay,az,gx,gy,gz,temp_C"); 21 | } 22 | 23 | void loop() { 24 | // read the sensor 25 | IMU.getAGT(); 26 | // display the data 27 | Serial.print(IMU.accX(),6); 28 | Serial.print("\t"); 29 | Serial.print(IMU.accY(),6); 30 | Serial.print("\t"); 31 | Serial.print(IMU.accZ(),6); 32 | Serial.print("\t"); 33 | Serial.print(IMU.gyrX(),6); 34 | Serial.print("\t"); 35 | Serial.print(IMU.gyrY(),6); 36 | Serial.print("\t"); 37 | Serial.print(IMU.gyrZ(),6); 38 | Serial.print("\t"); 39 | Serial.println(IMU.temp(),6); 40 | delay(100); 41 | } 42 | -------------------------------------------------------------------------------- /examples/Basic_SPI/Basic_SPI.ino: -------------------------------------------------------------------------------- 1 | #include "ICM42605.h" 2 | 3 | // an ICM42605 object with the ICM42605 sensor on SPI bus 0 and chip select pin 10 4 | ICM42605 IMU(SPI, 10); 5 | 6 | void setup() { 7 | // serial to display data 8 | Serial.begin(115200); 9 | while(!Serial) {} 10 | 11 | // start communication with IMU 12 | int status = IMU.begin(); 13 | if (status < 0) { 14 | Serial.println("IMU initialization unsuccessful"); 15 | Serial.println("Check IMU wiring or try cycling power"); 16 | Serial.print("Status: "); 17 | Serial.println(status); 18 | while(1) {} 19 | } 20 | Serial.println("ax,ay,az,gx,gy,gz,temp_C"); 21 | } 22 | 23 | void loop() { 24 | // read the sensor 25 | IMU.getAGT(); 26 | // display the data 27 | Serial.print(IMU.accX(),6); 28 | Serial.print("\t"); 29 | Serial.print(IMU.accY(),6); 30 | Serial.print("\t"); 31 | Serial.print(IMU.accZ(),6); 32 | Serial.print("\t"); 33 | Serial.print(IMU.gyrX(),6); 34 | Serial.print("\t"); 35 | Serial.print(IMU.gyrY(),6); 36 | Serial.print("\t"); 37 | Serial.print(IMU.gyrZ(),6); 38 | Serial.print("\t"); 39 | Serial.println(IMU.temp(),6); 40 | delay(100); 41 | } 42 | -------------------------------------------------------------------------------- /examples/Advanced_I2C/Advanced_I2C.ino: -------------------------------------------------------------------------------- 1 | #include "ICM42605.h" 2 | 3 | // an ICM42605 object with the ICM42605 sensor on I2C bus 0 with address 0x68 4 | ICM42605 IMU(Wire, 0x68); 5 | 6 | void setup() { 7 | // serial to display data 8 | Serial.begin(115200); 9 | while(!Serial) {} 10 | 11 | // start communication with IMU 12 | int status = IMU.begin(); 13 | if (status < 0) { 14 | Serial.println("IMU initialization unsuccessful"); 15 | Serial.println("Check IMU wiring or try cycling power"); 16 | Serial.print("Status: "); 17 | Serial.println(status); 18 | while(1) {} 19 | } 20 | 21 | // setting the accelerometer full scale range to +/-8G 22 | imu.setAccelFS(ICM42605::gpm8); 23 | // setting the gyroscope full scale range to +/-500 deg/s 24 | imu.setGyroFS(ICM42605::dps500); 25 | 26 | // set output data rate to 12.5 Hz 27 | imu.setAccelODR(ICM42605::odr12_5); 28 | imu.setGyroODR(ICM42605::odr12_5); 29 | 30 | Serial.println("ax,ay,az,gx,gy,gz,temp_C"); 31 | } 32 | 33 | void loop() { 34 | // read the sensor 35 | IMU.getAGT(); 36 | 37 | // display the data 38 | Serial.print(IMU.accX(),6); 39 | Serial.print("\t"); 40 | Serial.print(IMU.accY(),6); 41 | Serial.print("\t"); 42 | Serial.print(IMU.accZ(),6); 43 | Serial.print("\t"); 44 | Serial.print(IMU.gyrX(),6); 45 | Serial.print("\t"); 46 | Serial.print(IMU.gyrY(),6); 47 | Serial.print("\t"); 48 | Serial.print(IMU.gyrZ(),6); 49 | Serial.print("\t"); 50 | Serial.println(IMU.temp(),6); 51 | delay(20); 52 | } 53 | -------------------------------------------------------------------------------- /examples/FIFO_SPI/FIFO_SPI.ino: -------------------------------------------------------------------------------- 1 | #include "ICM42605.h" 2 | // TODO: Need to test this with the ICM42605 3 | 4 | // an ICM42605 object with the ICM42605 sensor on SPI bus 0 and chip select pin 10 5 | ICM42605_FIFO IMU(SPI,10); 6 | int status; 7 | 8 | // variables to hold FIFO data, these need to be large enough to hold the data 9 | double ax[100], ay[100], az[100]; 10 | size_t fifoSize; 11 | 12 | void setup() { 13 | // serial to display data 14 | Serial.begin(115200); 15 | while(!Serial) {} 16 | 17 | // start communication with IMU 18 | status = IMU.begin(); 19 | if (status < 0) { 20 | Serial.println("IMU initialization unsuccessful"); 21 | Serial.println("Check IMU wiring or try cycling power"); 22 | Serial.print("Status: "); 23 | Serial.println(status); 24 | while(1) {} 25 | } 26 | // setting DLPF bandwidth to 20 Hz 27 | IMU.setDlpfBandwidth(ICM42605::DLPF_BANDWIDTH_21HZ); 28 | // setting SRD to 19 for a 50 Hz update rate 29 | IMU.setSrd(19); 30 | // enabling the FIFO to record just the accelerometers 31 | IMU.enableFifo(true,false,false); 32 | // gather 50 samples of data 33 | delay(980); 34 | // read the fifo buffer from the IMU 35 | IMU.readFifo(); 36 | // get the X, Y, and Z accelerometer data and their size 37 | IMU.getFifoAccelX_mss(&fifoSize,ax); 38 | IMU.getFifoAccelY_mss(&fifoSize,ay); 39 | IMU.getFifoAccelZ_mss(&fifoSize,az); 40 | // print the data 41 | Serial.print("The FIFO buffer is "); 42 | Serial.print(fifoSize); 43 | Serial.println(" samples long."); 44 | for (size_t i=0; i < fifoSize; i++) { 45 | Serial.print(ax[i],6); 46 | Serial.print("\t"); 47 | Serial.print(ay[i],6); 48 | Serial.print("\t"); 49 | Serial.println(az[i],6); 50 | } 51 | } 52 | 53 | void loop() {} 54 | -------------------------------------------------------------------------------- /examples/Interrupt_SPI/Interrupt_SPI.ino: -------------------------------------------------------------------------------- 1 | #include "ICM42605.h" 2 | 3 | static const uint8_t CS_PIN = 10; 4 | static const uint8_t INT_PIN = 22; 5 | 6 | // an ICM42605 object with the ICM42605 sensor on SPI bus 0 and chip select pin CS_PIN 7 | ICM42605 imu(SPI, CS_PIN); 8 | 9 | void setImuFlag() { 10 | dataReady = true; 11 | } 12 | 13 | volatile bool dataReady = false; 14 | 15 | void setup() { 16 | // serial to display data 17 | Serial.begin(115200); 18 | while(!Serial) {} 19 | 20 | // start communication with IMU 21 | int status = imu.begin(); 22 | if (status < 0) { 23 | Serial.println("IMU initialization unsuccessful"); 24 | Serial.println("Check IMU wiring or try cycling power"); 25 | Serial.print("Status: "); 26 | Serial.println(status); 27 | while(1) {} 28 | } 29 | 30 | // attaching the interrupt to microcontroller pin INT_PIN 31 | pinMode(INT_PIN, INPUT); 32 | attachInterrupt(INT_PIN, setImuFlag, RISING); 33 | 34 | // set output data rate to 12.5 Hz 35 | imu.setAccelODR(ICM42605::odr12_5); 36 | imu.setGyroODR(ICM42605::odr12_5); 37 | 38 | // enabling the data ready interrupt 39 | imu.enableDataReadyInterrupt(); 40 | 41 | Serial.println("ax,ay,az,gx,gy,gz,temp_C"); 42 | } 43 | 44 | void loop() { 45 | if (!dataReady) return; 46 | 47 | dataReady = false; 48 | 49 | // read the sensor 50 | imu.getAGT(); 51 | 52 | // display the data 53 | Serial.print(imu.accX(),6); 54 | Serial.print("\t"); 55 | Serial.print(imu.accY(),6); 56 | Serial.print("\t"); 57 | Serial.print(imu.accZ(),6); 58 | Serial.print("\t"); 59 | Serial.print(imu.gyrX(),6); 60 | Serial.print("\t"); 61 | Serial.print(imu.gyrY(),6); 62 | Serial.print("\t"); 63 | Serial.print(imu.gyrZ(),6); 64 | Serial.print("\t"); 65 | Serial.println(imu.temp(),6); 66 | } 67 | -------------------------------------------------------------------------------- /examples/Stm32F722_SPI_Raw/Stm32F722_SPI_Raw.ino: -------------------------------------------------------------------------------- 1 | #include "ICM42605.h" 2 | 3 | static const uint8_t CS_PIN = PA4; 4 | static const uint8_t INT_PIN = PC4; 5 | 6 | static const uint8_t MOSI_PIN = PA7; 7 | static const uint8_t MISO_PIN = PA6; 8 | static const uint8_t SCLK_PIN = PA5; 9 | 10 | static SPIClass spi = SPIClass(MOSI_PIN, MISO_PIN, SCLK_PIN); 11 | 12 | // an ICM42605 object with the ICM42605 sensor on SPI bus 0 and chip select pin CS_PIN 13 | ICM42605 imu(spi, CS_PIN); 14 | 15 | void setImuFlag() { 16 | dataReady = true; 17 | } 18 | 19 | volatile bool dataReady = false; 20 | 21 | void setup() { 22 | // serial to display data 23 | Serial.begin(115200); 24 | while(!Serial) {} 25 | 26 | // start communication with IMU 27 | int status = imu.begin(); 28 | if (status < 0) { 29 | Serial.println("IMU initialization unsuccessful"); 30 | Serial.println("Check IMU wiring or try cycling power"); 31 | Serial.print("Status: "); 32 | Serial.println(status); 33 | while(1) {} 34 | } 35 | 36 | // attaching the interrupt to microcontroller pin INT_PIN 37 | pinMode(INT_PIN, INPUT); 38 | attachInterrupt(INT_PIN, setImuFlag, RISING); 39 | 40 | // set output data rate 41 | imu.setAccelODR(ICM42605::odr8k); 42 | imu.setGyroODR(ICM42605::odr8k); 43 | 44 | // enabling the data ready interrupt 45 | imu.enableDataReadyInterrupt(); 46 | } 47 | 48 | void loop() { 49 | if (!dataReady) return; 50 | 51 | dataReady = false; 52 | 53 | // read the sensor 54 | imu.getAGT(); 55 | 56 | // display the raw data 57 | Serial.print(imu.getAccelX_count()); 58 | Serial.print("\t"); 59 | Serial.print(imu.getAccelY_count()); 60 | Serial.print("\t"); 61 | Serial.print(imu.getAccelZ_count()); 62 | Serial.print("\t"); 63 | Serial.print(imu.getGyroX_count()); 64 | Serial.print("\t"); 65 | Serial.print(imu.getGyroY_count()); 66 | Serial.print("\t"); 67 | Serial.println(imu.getGyroZ_count()); 68 | } 69 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ICM42605 2 | Arduino library for communicating with the [ICM42605](https://invensense.tdk.com/products/motion-tracking/6-axis/icm-42605/) six-axis Inertial Measurement Units (IMU). 3 | 4 | It is based on the ICM42688 library, as the register interface between the two gyros is almost identical. 5 | 6 | 7 | 8 | # Description 9 | The InvenSense ICM42605 supports I2C, up to 400 kHz, and SPI communication, up to 1 MHz for register setup and 24 MHz for data reading. The following selectable full scale sensor ranges are available: 10 | 11 | | Gyroscope Full Scale Range | Accelerometer Full Scale Range | 12 | |----------------------------|--------------------------------| 13 | | +/- 15.6 (deg/s) | - | 14 | | +/- 31.2 (deg/s) | - | 15 | | +/- 62.5 (deg/s) | - | 16 | | +/- 125 (deg/s) | - | 17 | | +/- 250 (deg/s) | +/- 2 (g) | 18 | | +/- 500 (deg/s) | +/- 4 (g) | 19 | | +/- 1000 (deg/s) | +/- 8 (g) | 20 | | +/- 2000 (deg/s) | +/- 16 (g) | 21 | 22 | The ICM42605 samples the gyroscopes, and accelerometers with 16 bit analog to digital converters. It also features programmable digital filters, a precision clock, an embedded temperature sensor, programmable interrupts (including wake on motion), and a 2kB byte FIFO buffer. 23 | 24 | Refer to the ICM42688 library from which this library forked until the documentation is ported, proof-read and updated. 25 | 26 | # Wiring and Pullups 27 | 28 | Please refer to the datasheet. This library should work well for other breakout boards or embedded sensors, please refer to your vendor's pinout diagram. 29 | 30 | For best results ensure the power supply to the gyro is noise-free, when making custom PCBs follow the guidelines applicable to all MEMS devices 31 | when placing components. Review application notes from multiple vendors and then make your own decisions. e.g. do not place directly in-line between mounting holes, etc. 32 | 33 | ## I2C 34 | 35 | The ICM42605 pins should be connected as: 36 | * 3V3: this should be a 3.0V to 3.6V power source. 37 | * GND: ground. 38 | * INT1: (optional) used for the interrupt output setup in *enableDataReadyInterrupt* and *enableWakeOnMotion*. Connect to interruptable pin on microcontroller. 39 | * SDA: connect to SDA. 40 | * SCL: connect to SCL. 41 | 42 | 4.7 kOhm resistors should be used as pullups on SDA and SCL, these resistors should pullup with a 3.3V source. 43 | 44 | ## SPI 45 | 46 | The ICM42605 pins should be connected as: 47 | * 3V3: this should be a 3.0V to 3.6V power source. 48 | * GND: ground. 49 | * INT1: (optional) used for the interrupt output setup in *enableDataReadyInterrupt* and *enableWakeOnMotion*. Connect to interruptable pin on microcontroller. 50 | * SDI: connect to MOSI. 51 | * SCK: connect to SCK. 52 | * SDO: connect to MISO. 53 | * CS: connect to chip select pin. Pin 10 was used in the code snippets in this document and the included examples, but any digital I/O pin can be used. 54 | 55 | Some breakout boards, including the Embedded Masters breakout board, require slight modification to enable SPI. Please refer to your vendor's documentation. 56 | -------------------------------------------------------------------------------- /src/registers.h: -------------------------------------------------------------------------------- 1 | #ifndef ICM42605_REGISTERS_H_ 2 | #define ICM42605_REGISTERS_H_ 3 | 4 | #include 5 | 6 | namespace ICM42605reg { 7 | 8 | // Accesible from all user banks 9 | static constexpr uint8_t REG_BANK_SEL = 0x76; 10 | 11 | 12 | // User Bank 0 13 | static constexpr uint8_t UB0_REG_DEVICE_CONFIG = 0x11; 14 | // break 15 | static constexpr uint8_t UB0_REG_DRIVE_CONFIG = 0x13; 16 | static constexpr uint8_t UB0_REG_INT_CONFIG = 0x14; 17 | // break 18 | static constexpr uint8_t UB0_REG_FIFO_CONFIG = 0x16; 19 | // break 20 | static constexpr uint8_t UB0_REG_TEMP_DATA1 = 0x1D; 21 | static constexpr uint8_t UB0_REG_TEMP_DATA0 = 0x1E; 22 | static constexpr uint8_t UB0_REG_ACCEL_DATA_X1 = 0x1F; 23 | static constexpr uint8_t UB0_REG_ACCEL_DATA_X0 = 0x20; 24 | static constexpr uint8_t UB0_REG_ACCEL_DATA_Y1 = 0x21; 25 | static constexpr uint8_t UB0_REG_ACCEL_DATA_Y0 = 0x22; 26 | static constexpr uint8_t UB0_REG_ACCEL_DATA_Z1 = 0x23; 27 | static constexpr uint8_t UB0_REG_ACCEL_DATA_Z0 = 0x24; 28 | static constexpr uint8_t UB0_REG_GYRO_DATA_X1 = 0x25; 29 | static constexpr uint8_t UB0_REG_GYRO_DATA_X0 = 0x26; 30 | static constexpr uint8_t UB0_REG_GYRO_DATA_Y1 = 0x27; 31 | static constexpr uint8_t UB0_REG_GYRO_DATA_Y0 = 0x28; 32 | static constexpr uint8_t UB0_REG_GYRO_DATA_Z1 = 0x29; 33 | static constexpr uint8_t UB0_REG_GYRO_DATA_Z0 = 0x2A; 34 | static constexpr uint8_t UB0_REG_TMST_FSYNCH = 0x2B; 35 | static constexpr uint8_t UB0_REG_TMST_FSYNCL = 0x2C; 36 | static constexpr uint8_t UB0_REG_INT_STATUS = 0x2D; 37 | static constexpr uint8_t UB0_REG_FIFO_COUNTH = 0x2E; 38 | static constexpr uint8_t UB0_REG_FIFO_COUNTL = 0x2F; 39 | static constexpr uint8_t UB0_REG_FIFO_DATA = 0x30; 40 | static constexpr uint8_t UB0_REG_APEX_DATA0 = 0x31; 41 | static constexpr uint8_t UB0_REG_APEX_DATA1 = 0x32; 42 | static constexpr uint8_t UB0_REG_APEX_DATA2 = 0x33; 43 | static constexpr uint8_t UB0_REG_APEX_DATA3 = 0x34; 44 | static constexpr uint8_t UB0_REG_APEX_DATA4 = 0x35; 45 | static constexpr uint8_t UB0_REG_APEX_DATA5 = 0x36; 46 | static constexpr uint8_t UB0_REG_INT_STATUS2 = 0x37; 47 | static constexpr uint8_t UB0_REG_INT_STATUS3 = 0x38; 48 | // break 49 | static constexpr uint8_t UB0_REG_SIGNAL_PATH_RESET = 0x4B; 50 | static constexpr uint8_t UB0_REG_INTF_CONFIG0 = 0x4C; 51 | static constexpr uint8_t UB0_REG_INTF_CONFIG1 = 0x4D; 52 | static constexpr uint8_t UB0_REG_PWR_MGMT0 = 0x4E; 53 | static constexpr uint8_t UB0_REG_GYRO_CONFIG0 = 0x4F; 54 | static constexpr uint8_t UB0_REG_ACCEL_CONFIG0 = 0x50; 55 | static constexpr uint8_t UB0_REG_GYRO_CONFIG1 = 0x51; 56 | static constexpr uint8_t UB0_REG_GYRO_ACCEL_CONFIG0 = 0x52; 57 | static constexpr uint8_t UB0_REG_ACCEL_CONFIG1 = 0x53; 58 | static constexpr uint8_t UB0_REG_TMST_CONFIG = 0x54; 59 | // break 60 | static constexpr uint8_t UB0_REG_APEX_CONFIG0 = 0x56; 61 | static constexpr uint8_t UB0_REG_SMD_CONFIG = 0x57; 62 | // break 63 | static constexpr uint8_t UB0_REG_FIFO_CONFIG1 = 0x5F; 64 | static constexpr uint8_t UB0_REG_FIFO_CONFIG2 = 0x60; 65 | static constexpr uint8_t UB0_REG_FIFO_CONFIG3 = 0x61; 66 | static constexpr uint8_t UB0_REG_FSYNC_CONFIG = 0x62; 67 | static constexpr uint8_t UB0_REG_INT_CONFIG0 = 0x63; 68 | static constexpr uint8_t UB0_REG_INT_CONFIG1 = 0x64; 69 | static constexpr uint8_t UB0_REG_INT_SOURCE0 = 0x65; 70 | static constexpr uint8_t UB0_REG_INT_SOURCE1 = 0x66; 71 | // break 72 | static constexpr uint8_t UB0_REG_INT_SOURCE3 = 0x68; 73 | static constexpr uint8_t UB0_REG_INT_SOURCE4 = 0x69; 74 | // break 75 | static constexpr uint8_t UB0_REG_FIFO_LOST_PKT0 = 0x6C; 76 | static constexpr uint8_t UB0_REG_FIFO_LOST_PKT1 = 0x6D; 77 | // break 78 | static constexpr uint8_t UB0_REG_SELF_TEST_CONFIG = 0x70; 79 | // break 80 | static constexpr uint8_t UB0_REG_WHO_AM_I = 0x75; 81 | 82 | 83 | // User Bank 1 84 | static constexpr uint8_t UB1_REG_SENSOR_CONFIG0 = 0x03; 85 | // break 86 | static constexpr uint8_t UB1_REG_GYRO_CONFIG_STATIC2 = 0x0B; 87 | static constexpr uint8_t UB1_REG_GYRO_CONFIG_STATIC3 = 0x0C; 88 | static constexpr uint8_t UB1_REG_GYRO_CONFIG_STATIC4 = 0x0D; 89 | static constexpr uint8_t UB1_REG_GYRO_CONFIG_STATIC5 = 0x0E; 90 | static constexpr uint8_t UB1_REG_GYRO_CONFIG_STATIC6 = 0x0F; 91 | static constexpr uint8_t UB1_REG_GYRO_CONFIG_STATIC7 = 0x10; 92 | static constexpr uint8_t UB1_REG_GYRO_CONFIG_STATIC8 = 0x11; 93 | static constexpr uint8_t UB1_REG_GYRO_CONFIG_STATIC9 = 0x12; 94 | static constexpr uint8_t UB1_REG_GYRO_CONFIG_STATIC10 = 0x13; 95 | // break 96 | static constexpr uint8_t UB1_REG_XG_ST_DATA = 0x5F; 97 | static constexpr uint8_t UB1_REG_YG_ST_DATA = 0x60; 98 | static constexpr uint8_t UB1_REG_ZG_ST_DATA = 0x61; 99 | static constexpr uint8_t UB1_REG_TMSTVAL0 = 0x62; 100 | static constexpr uint8_t UB1_REG_TMSTVAL1 = 0x63; 101 | static constexpr uint8_t UB1_REG_TMSTVAL2 = 0x64; 102 | // break 103 | static constexpr uint8_t UB1_REG_INTF_CONFIG4 = 0x7A; 104 | static constexpr uint8_t UB1_REG_INTF_CONFIG5 = 0x7B; 105 | static constexpr uint8_t UB1_REG_INTF_CONFIG6 = 0x7C; 106 | 107 | 108 | // User Bank 2 109 | static constexpr uint8_t UB2_REG_ACCEL_CONFIG_STATIC2 = 0x03; 110 | static constexpr uint8_t UB2_REG_ACCEL_CONFIG_STATIC3 = 0x04; 111 | static constexpr uint8_t UB2_REG_ACCEL_CONFIG_STATIC4 = 0x05; 112 | // break 113 | static constexpr uint8_t UB2_REG_XA_ST_DATA = 0x3B; 114 | static constexpr uint8_t UB2_REG_YA_ST_DATA = 0x3C; 115 | static constexpr uint8_t UB2_REG_ZA_ST_DATA = 0x3D; 116 | 117 | 118 | // User Bank 4 119 | static constexpr uint8_t UB4_REG_APEX_CONFIG1 = 0x40; 120 | static constexpr uint8_t UB4_REG_APEX_CONFIG2 = 0x41; 121 | static constexpr uint8_t UB4_REG_APEX_CONFIG3 = 0x42; 122 | static constexpr uint8_t UB4_REG_APEX_CONFIG4 = 0x43; 123 | static constexpr uint8_t UB4_REG_APEX_CONFIG5 = 0x44; 124 | static constexpr uint8_t UB4_REG_APEX_CONFIG6 = 0x45; 125 | static constexpr uint8_t UB4_REG_APEX_CONFIG7 = 0x46; 126 | static constexpr uint8_t UB4_REG_APEX_CONFIG8 = 0x47; 127 | static constexpr uint8_t UB4_REG_APEX_CONFIG9 = 0x48; 128 | // break 129 | static constexpr uint8_t UB4_REG_ACCEL_WOM_X_THR = 0x4A; 130 | static constexpr uint8_t UB4_REG_ACCEL_WOM_Y_THR = 0x4B; 131 | static constexpr uint8_t UB4_REG_ACCEL_WOM_Z_THR = 0x4C; 132 | static constexpr uint8_t UB4_REG_INT_SOURCE6 = 0x4D; 133 | static constexpr uint8_t UB4_REG_INT_SOURCE7 = 0x4E; 134 | static constexpr uint8_t UB4_REG_INT_SOURCE8 = 0x4F; 135 | static constexpr uint8_t UB4_REG_INT_SOURCE9 = 0x50; 136 | static constexpr uint8_t UB4_REG_INT_SOURCE10 = 0x51; 137 | // break 138 | static constexpr uint8_t UB4_REG_OFFSET_USER0 = 0x77; 139 | static constexpr uint8_t UB4_REG_OFFSET_USER1 = 0x78; 140 | static constexpr uint8_t UB4_REG_OFFSET_USER2 = 0x79; 141 | static constexpr uint8_t UB4_REG_OFFSET_USER3 = 0x7A; 142 | static constexpr uint8_t UB4_REG_OFFSET_USER4 = 0x7B; 143 | static constexpr uint8_t UB4_REG_OFFSET_USER5 = 0x7C; 144 | static constexpr uint8_t UB4_REG_OFFSET_USER6 = 0x7D; 145 | static constexpr uint8_t UB4_REG_OFFSET_USER7 = 0x7E; 146 | static constexpr uint8_t UB4_REG_OFFSET_USER8 = 0x7F; 147 | 148 | } // ns ICM42605reg 149 | 150 | #endif // ICM42605_REGISTERS_H_ 151 | -------------------------------------------------------------------------------- /src/ICM42605.h: -------------------------------------------------------------------------------- 1 | #ifndef ICM42605_H 2 | #define ICM42605_H 3 | 4 | #include "Arduino.h" 5 | #include "Wire.h" // I2C library 6 | #include "SPI.h" // SPI library 7 | 8 | class ICM42605 9 | { 10 | public: 11 | 12 | enum GyroFS : uint8_t { 13 | dps2000 = 0x00, 14 | dps1000 = 0x01, 15 | dps500 = 0x02, 16 | dps250 = 0x03, 17 | dps125 = 0x04, 18 | dps62_5 = 0x05, 19 | dps31_25 = 0x06, 20 | dps15_625 = 0x07 21 | }; 22 | 23 | enum AccelFS : uint8_t { 24 | gpm16 = 0x00, 25 | gpm8 = 0x01, 26 | gpm4 = 0x02, 27 | gpm2 = 0x03 28 | }; 29 | 30 | enum ODR : uint8_t { 31 | odr32k = 0x01, // LN mode only 32 | odr16k = 0x02, // LN mode only 33 | odr8k = 0x03, // LN mode only 34 | odr4k = 0x04, // LN mode only 35 | odr2k = 0x05, // LN mode only 36 | odr1k = 0x06, // LN mode only 37 | odr200 = 0x07, 38 | odr100 = 0x08, 39 | odr50 = 0x09, 40 | odr25 = 0x0A, 41 | odr12_5 = 0x0B, 42 | odr6a25 = 0x0C, // LP mode only (accel only) 43 | odr3a125 = 0x0D, // LP mode only (accel only) 44 | odr1a5625 = 0x0E, // LP mode only (accel only) 45 | odr500 = 0x0F, 46 | }; 47 | 48 | /** 49 | * @brief Constructor for I2C communication 50 | * 51 | * @param bus I2C bus 52 | * @param[in] address Address of ICM 42605-p device 53 | */ 54 | ICM42605(TwoWire &bus, uint8_t address); 55 | 56 | /** 57 | * @brief Constructor for SPI communication 58 | * 59 | * @param bus SPI bus 60 | * @param[in] csPin Chip Select pin 61 | */ 62 | ICM42605(SPIClass &bus, uint8_t csPin, uint32_t SPI_HS_CLK=8000000); 63 | 64 | /** 65 | * @brief Initialize the device. 66 | * 67 | * @return ret < 0 if error 68 | */ 69 | int begin(); 70 | 71 | /** 72 | * @brief Sets the full scale range for the accelerometer 73 | * 74 | * @param[in] fssel Full scale selection 75 | * 76 | * @return ret < 0 if error 77 | */ 78 | int setAccelFS(AccelFS fssel); 79 | 80 | /** 81 | * @brief Sets the full scale range for the gyro 82 | * 83 | * @param[in] fssel Full scale selection 84 | * 85 | * @return ret < 0 if error 86 | */ 87 | int setGyroFS(GyroFS fssel); 88 | 89 | /** 90 | * @brief Set the ODR for accelerometer 91 | * 92 | * @param[in] odr Output data rate 93 | * 94 | * @return ret < 0 if error 95 | */ 96 | int setAccelODR(ODR odr); 97 | 98 | /** 99 | * @brief Set the ODR for gyro 100 | * 101 | * @param[in] odr Output data rate 102 | * 103 | * @return ret < 0 if error 104 | */ 105 | int setGyroODR(ODR odr); 106 | 107 | int setFilters(bool gyroFilters, bool accFilters); 108 | 109 | /** 110 | * @brief Enables the data ready interrupt. 111 | * 112 | * - routes UI data ready interrupt to INT1 113 | * - push-pull, pulsed, active HIGH interrupts 114 | * 115 | * @return ret < 0 if error 116 | */ 117 | int enableDataReadyInterrupt(); 118 | 119 | /** 120 | * @brief Masks the data ready interrupt 121 | * 122 | * @return ret < 0 if error 123 | */ 124 | int disableDataReadyInterrupt(); 125 | 126 | /** 127 | * @brief Transfers data from ICM 42605-p to microcontroller. 128 | * Must be called to access new measurements. 129 | * 130 | * @return ret < 0 if error 131 | */ 132 | int getAGT(); 133 | 134 | /** 135 | * @brief Get accelerometer data, per axis 136 | * 137 | * @return Acceleration in g's 138 | */ 139 | float accX() const { return _acc[0]; } 140 | float accY() const { return _acc[1]; } 141 | float accZ() const { return _acc[2]; } 142 | 143 | /** 144 | * @brief Get gyro data, per axis 145 | * 146 | * @return Angular velocity in dps 147 | */ 148 | float gyrX() const { return _gyr[0]; } 149 | float gyrY() const { return _gyr[1]; } 150 | float gyrZ() const { return _gyr[2]; } 151 | 152 | /** 153 | * @brief Get temperature of gyro die 154 | * 155 | * @return Temperature in Celsius 156 | */ 157 | float temp() const { return _t; } 158 | 159 | int16_t getAccelX_count(); 160 | int16_t getAccelY_count(); 161 | int16_t getAccelZ_count(); 162 | int16_t getGyroX_count(); 163 | int16_t getGyroY_count(); 164 | int16_t getGyroZ_count(); 165 | 166 | int calibrateGyro(); 167 | float getGyroBiasX(); 168 | float getGyroBiasY(); 169 | float getGyroBiasZ(); 170 | void setGyroBiasX(float bias); 171 | void setGyroBiasY(float bias); 172 | void setGyroBiasZ(float bias); 173 | int calibrateAccel(); 174 | float getAccelBiasX_mss(); 175 | float getAccelScaleFactorX(); 176 | float getAccelBiasY_mss(); 177 | float getAccelScaleFactorY(); 178 | float getAccelBiasZ_mss(); 179 | float getAccelScaleFactorZ(); 180 | void setAccelCalX(float bias,float scaleFactor); 181 | void setAccelCalY(float bias,float scaleFactor); 182 | void setAccelCalZ(float bias,float scaleFactor); 183 | protected: 184 | ///\brief I2C Communication 185 | uint8_t _address = 0; 186 | TwoWire *_i2c = {}; 187 | static constexpr uint32_t I2C_CLK = 400000; // 400 kHz 188 | size_t _numBytes = 0; // number of bytes received from I2C 189 | 190 | int16_t _rawMeas[7]; // temp, accel xyz, gyro xyz 191 | 192 | ///\brief SPI Communication 193 | SPIClass *_spi = {}; 194 | uint8_t _csPin = 0; 195 | bool _useSPI = false; 196 | bool _useSPIHS = false; 197 | static constexpr uint32_t SPI_LS_CLOCK = 1000000; // 1 MHz 198 | uint32_t SPI_HS_CLOCK = 8000000; // 8 MHz 199 | 200 | // buffer for reading from sensor 201 | uint8_t _buffer[15] = {}; 202 | 203 | // data buffer 204 | float _t = 0.0f; 205 | float _acc[3] = {}; 206 | float _gyr[3] = {}; 207 | 208 | ///\brief Full scale resolution factors 209 | float _accelScale = 0.0f; 210 | float _gyroScale = 0.0f; 211 | 212 | ///\brief Full scale selections 213 | AccelFS _accelFS; 214 | GyroFS _gyroFS; 215 | 216 | ///\brief Accel calibration 217 | float _accBD[3] = {}; 218 | float _accB[3] = {}; 219 | float _accS[3] = {1.0f, 1.0f, 1.0f}; 220 | float _accMax[3] = {}; 221 | float _accMin[3] = {}; 222 | 223 | ///\brief Gyro calibration 224 | float _gyroBD[3] = {}; 225 | float _gyrB[3] = {}; 226 | 227 | ///\brief Constants 228 | static constexpr uint8_t WHO_AM_I = 0x42; ///< expected value in UB0_REG_WHO_AM_I reg 229 | static constexpr int NUM_CALIB_SAMPLES = 1000; ///< for gyro/accel bias calib 230 | 231 | ///\brief Conversion formula to get temperature in Celsius (Sec 4.13) 232 | static constexpr float TEMP_DATA_REG_SCALE = 132.48f; 233 | static constexpr float TEMP_OFFSET = 25.0f; 234 | 235 | uint8_t _bank = 0; ///< current user bank 236 | 237 | const uint8_t FIFO_EN = 0x23; 238 | const uint8_t FIFO_TEMP_EN = 0x04; 239 | const uint8_t FIFO_GYRO = 0x02; 240 | const uint8_t FIFO_ACCEL = 0x01; 241 | // const uint8_t FIFO_COUNT = 0x2E; 242 | // const uint8_t FIFO_DATA = 0x30; 243 | 244 | // BANK 1 245 | // const uint8_t GYRO_CONFIG_STATIC2 = 0x0B; 246 | const uint8_t GYRO_NF_ENABLE = 0x00; 247 | const uint8_t GYRO_NF_DISABLE = 0x01; 248 | const uint8_t GYRO_AAF_ENABLE = 0x00; 249 | const uint8_t GYRO_AAF_DISABLE = 0x02; 250 | 251 | // BANK 2 252 | // const uint8_t ACCEL_CONFIG_STATIC2 = 0x03; 253 | const uint8_t ACCEL_AAF_ENABLE = 0x00; 254 | const uint8_t ACCEL_AAF_DISABLE = 0x01; 255 | 256 | // private functions 257 | int writeRegister(uint8_t subAddress, uint8_t data); 258 | int readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest); 259 | int setBank(uint8_t bank); 260 | 261 | /** 262 | * @brief Software reset of the device 263 | */ 264 | void reset(); 265 | 266 | /** 267 | * @brief Read the WHO_AM_I register 268 | * 269 | * @return Value of WHO_AM_I register 270 | */ 271 | uint8_t whoAmI(); 272 | }; 273 | 274 | class ICM42605_FIFO: public ICM42605 { 275 | public: 276 | using ICM42605::ICM42605; 277 | int enableFifo(bool accel,bool gyro,bool temp); 278 | int readFifo(); 279 | void getFifoAccelX_mss(size_t *size,float* data); 280 | void getFifoAccelY_mss(size_t *size,float* data); 281 | void getFifoAccelZ_mss(size_t *size,float* data); 282 | void getFifoGyroX(size_t *size,float* data); 283 | void getFifoGyroY(size_t *size,float* data); 284 | void getFifoGyroZ(size_t *size,float* data); 285 | void getFifoTemperature_C(size_t *size,float* data); 286 | protected: 287 | // fifo 288 | bool _enFifoAccel = false; 289 | bool _enFifoGyro = false; 290 | bool _enFifoTemp = false; 291 | size_t _fifoSize = 0; 292 | size_t _fifoFrameSize = 0; 293 | float _axFifo[85] = {}; 294 | float _ayFifo[85] = {}; 295 | float _azFifo[85] = {}; 296 | size_t _aSize = 0; 297 | float _gxFifo[85] = {}; 298 | float _gyFifo[85] = {}; 299 | float _gzFifo[85] = {}; 300 | size_t _gSize = 0; 301 | float _tFifo[256] = {}; 302 | size_t _tSize = 0; 303 | }; 304 | 305 | #endif // ICM42605_H 306 | -------------------------------------------------------------------------------- /src/ICM42605.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "ICM42605.h" 3 | #include "registers.h" 4 | 5 | using namespace ICM42605reg; 6 | 7 | /* ICM42605 object, input the I2C bus and address */ 8 | ICM42605::ICM42605(TwoWire &bus, uint8_t address) { 9 | _i2c = &bus; // I2C bus 10 | _address = address; // I2C address 11 | _useSPI = false; // set to use I2C 12 | } 13 | 14 | /* ICM42605 object, input the SPI bus and chip select pin */ 15 | ICM42605::ICM42605(SPIClass &bus, uint8_t csPin, uint32_t SPI_HS_CLK) { 16 | _spi = &bus; // SPI bus 17 | _csPin = csPin; // chip select pin 18 | _useSPI = true; // set to use SPI 19 | SPI_HS_CLOCK = SPI_HS_CLK; 20 | } 21 | 22 | /* starts communication with the ICM42605 */ 23 | int ICM42605::begin() { 24 | if( _useSPI ) { // using SPI for communication 25 | // use low speed SPI for register setting 26 | _useSPIHS = false; 27 | // setting CS pin to output 28 | pinMode(_csPin,OUTPUT); 29 | // setting CS pin high 30 | digitalWrite(_csPin,HIGH); 31 | // begin SPI communication 32 | _spi->begin(); 33 | } else { // using I2C for communication 34 | // starting the I2C bus 35 | _i2c->begin(); 36 | // setting the I2C clock 37 | _i2c->setClock(I2C_CLK); 38 | } 39 | 40 | // reset the ICM42605 41 | reset(); 42 | 43 | // check the WHO AM I byte 44 | if(whoAmI() != WHO_AM_I) { 45 | return -3; 46 | } 47 | 48 | // turn on accel and gyro in Low Noise (LN) Mode 49 | if(writeRegister(UB0_REG_PWR_MGMT0, 0x0F) < 0) { 50 | return -4; 51 | } 52 | 53 | // 16G is default -- do this to set up accel resolution scaling 54 | int ret = setAccelFS(gpm16); 55 | if (ret < 0) return ret; 56 | 57 | // 2000DPS is default -- do this to set up gyro resolution scaling 58 | ret = setGyroFS(dps2000); 59 | if (ret < 0) return ret; 60 | 61 | // // disable inner filters (Notch filter, Anti-alias filter, UI filter block) 62 | // if (setFilters(false, false) < 0) { 63 | // return -7; 64 | // } 65 | 66 | // estimate gyro bias 67 | if (calibrateGyro() < 0) { 68 | return -8; 69 | } 70 | // successful init, return 1 71 | return 1; 72 | } 73 | 74 | /* sets the accelerometer full scale range to values other than default */ 75 | int ICM42605::setAccelFS(AccelFS fssel) { 76 | // use low speed SPI for register setting 77 | _useSPIHS = false; 78 | 79 | setBank(0); 80 | 81 | // read current register value 82 | uint8_t reg; 83 | if (readRegisters(UB0_REG_ACCEL_CONFIG0, 1, ®) < 0) return -1; 84 | 85 | // only change FS_SEL in reg 86 | reg = (fssel << 5) | (reg & 0x1F); 87 | 88 | if (writeRegister(UB0_REG_ACCEL_CONFIG0, reg) < 0) return -2; 89 | 90 | _accelScale = static_cast(1 << (4 - fssel)) / 32768.0f; 91 | _accelFS = fssel; 92 | 93 | return 1; 94 | } 95 | 96 | /* sets the gyro full scale range to values other than default */ 97 | int ICM42605::setGyroFS(GyroFS fssel) { 98 | // use low speed SPI for register setting 99 | _useSPIHS = false; 100 | 101 | setBank(0); 102 | 103 | // read current register value 104 | uint8_t reg; 105 | if (readRegisters(UB0_REG_GYRO_CONFIG0, 1, ®) < 0) return -1; 106 | 107 | // only change FS_SEL in reg 108 | reg = (fssel << 5) | (reg & 0x1F); 109 | 110 | if (writeRegister(UB0_REG_GYRO_CONFIG0, reg) < 0) return -2; 111 | 112 | _gyroScale = (2000.0f / static_cast(1 << fssel)) / 32768.0f; 113 | _gyroFS = fssel; 114 | 115 | return 1; 116 | } 117 | 118 | int ICM42605::setAccelODR(ODR odr) { 119 | // use low speed SPI for register setting 120 | _useSPIHS = false; 121 | 122 | setBank(0); 123 | 124 | // read current register value 125 | uint8_t reg; 126 | if (readRegisters(UB0_REG_ACCEL_CONFIG0, 1, ®) < 0) return -1; 127 | 128 | // only change ODR in reg 129 | reg = odr | (reg & 0xF0); 130 | 131 | if (writeRegister(UB0_REG_ACCEL_CONFIG0, reg) < 0) return -2; 132 | 133 | return 1; 134 | } 135 | 136 | int ICM42605::setGyroODR(ODR odr) { 137 | // use low speed SPI for register setting 138 | _useSPIHS = false; 139 | 140 | setBank(0); 141 | 142 | // read current register value 143 | uint8_t reg; 144 | if (readRegisters(UB0_REG_GYRO_CONFIG0, 1, ®) < 0) return -1; 145 | 146 | // only change ODR in reg 147 | reg = odr | (reg & 0xF0); 148 | 149 | if (writeRegister(UB0_REG_GYRO_CONFIG0, reg) < 0) return -2; 150 | 151 | return 1; 152 | } 153 | 154 | int ICM42605::setFilters(bool gyroFilters, bool accFilters) { 155 | if (setBank(1) < 0) return -1; 156 | 157 | if (gyroFilters == true) { 158 | if (writeRegister(UB1_REG_GYRO_CONFIG_STATIC2, GYRO_NF_ENABLE | GYRO_AAF_ENABLE) < 0) { 159 | return -2; 160 | } 161 | } 162 | else { 163 | if (writeRegister(UB1_REG_GYRO_CONFIG_STATIC2, GYRO_NF_DISABLE | GYRO_AAF_DISABLE) < 0) { 164 | return -3; 165 | } 166 | } 167 | 168 | if (setBank(2) < 0) return -4; 169 | 170 | if (accFilters == true) { 171 | if (writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, ACCEL_AAF_ENABLE) < 0) { 172 | return -5; 173 | } 174 | } 175 | else { 176 | if (writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, ACCEL_AAF_DISABLE) < 0) { 177 | return -6; 178 | } 179 | } 180 | if (setBank(0) < 0) return -7; 181 | return 1; 182 | } 183 | 184 | int ICM42605::enableDataReadyInterrupt() { 185 | // use low speed SPI for register setting 186 | _useSPIHS = false; 187 | 188 | // push-pull, pulsed, active HIGH interrupts 189 | if (writeRegister(UB0_REG_INT_CONFIG, 0x18 | 0x03) < 0) return -1; 190 | 191 | // need to clear bit 4 to allow proper INT1 and INT2 operation 192 | uint8_t reg; 193 | if (readRegisters(UB0_REG_INT_CONFIG1, 1, ®) < 0) return -2; 194 | reg &= ~0x10; 195 | if (writeRegister(UB0_REG_INT_CONFIG1, reg) < 0) return -3; 196 | 197 | // route UI data ready interrupt to INT1 198 | if (writeRegister(UB0_REG_INT_SOURCE0, 0x18) < 0) return -4; 199 | 200 | return 1; 201 | } 202 | 203 | int ICM42605::disableDataReadyInterrupt() { 204 | // use low speed SPI for register setting 205 | _useSPIHS = false; 206 | 207 | // set pin 4 to return to reset value 208 | uint8_t reg; 209 | if (readRegisters(UB0_REG_INT_CONFIG1, 1, ®) < 0) return -1; 210 | reg |= 0x10; 211 | if (writeRegister(UB0_REG_INT_CONFIG1, reg) < 0) return -2; 212 | 213 | // return reg to reset value 214 | if (writeRegister(UB0_REG_INT_SOURCE0, 0x10) < 0) return -3; 215 | 216 | return 1; 217 | } 218 | 219 | /* reads the most current data from ICM42605 and stores in buffer */ 220 | int ICM42605::getAGT() { 221 | _useSPIHS = true; // use the high speed SPI for data readout 222 | // grab the data from the ICM42605 223 | if (readRegisters(UB0_REG_TEMP_DATA1, 14, _buffer) < 0) return -1; 224 | 225 | // combine bytes into 16 bit values 226 | for (size_t i=0; i<7; i++) { 227 | _rawMeas[i] = ((int16_t)_buffer[i*2] << 8) | _buffer[i*2+1]; 228 | } 229 | 230 | _t = (static_cast(_rawMeas[0]) / TEMP_DATA_REG_SCALE) + TEMP_OFFSET; 231 | 232 | _acc[0] = ((_rawMeas[1] * _accelScale) - _accB[0]) * _accS[0]; 233 | _acc[1] = ((_rawMeas[2] * _accelScale) - _accB[1]) * _accS[1]; 234 | _acc[2] = ((_rawMeas[3] * _accelScale) - _accB[2]) * _accS[2]; 235 | 236 | _gyr[0] = (_rawMeas[4] * _gyroScale) - _gyrB[0]; 237 | _gyr[1] = (_rawMeas[5] * _gyroScale) - _gyrB[1]; 238 | _gyr[2] = (_rawMeas[6] * _gyroScale) - _gyrB[2]; 239 | 240 | return 1; 241 | } 242 | 243 | /* configures and enables the FIFO buffer */ 244 | int ICM42605_FIFO::enableFifo(bool accel,bool gyro,bool temp) { 245 | // use low speed SPI for register setting 246 | _useSPIHS = false; 247 | if(writeRegister(FIFO_EN,(accel*FIFO_ACCEL)|(gyro*FIFO_GYRO)|(temp*FIFO_TEMP_EN)) < 0) { 248 | return -2; 249 | } 250 | _enFifoAccel = accel; 251 | _enFifoGyro = gyro; 252 | _enFifoTemp = temp; 253 | _fifoFrameSize = accel*6 + gyro*6 + temp*2; 254 | return 1; 255 | } 256 | 257 | /* reads data from the ICM42605 FIFO and stores in buffer */ 258 | int ICM42605_FIFO::readFifo() { 259 | _useSPIHS = true; // use the high speed SPI for data readout 260 | // get the fifo size 261 | readRegisters(UB0_REG_FIFO_COUNTH, 2, _buffer); 262 | _fifoSize = (((uint16_t) (_buffer[0]&0x0F)) <<8) + (((uint16_t) _buffer[1])); 263 | // read and parse the buffer 264 | for (size_t i=0; i < _fifoSize/_fifoFrameSize; i++) { 265 | // grab the data from the ICM42605 266 | if (readRegisters(UB0_REG_FIFO_DATA, _fifoFrameSize, _buffer) < 0) { 267 | return -1; 268 | } 269 | if (_enFifoAccel) { 270 | // combine into 16 bit values 271 | int16_t rawMeas[3]; 272 | rawMeas[0] = (((int16_t)_buffer[0]) << 8) | _buffer[1]; 273 | rawMeas[1] = (((int16_t)_buffer[2]) << 8) | _buffer[3]; 274 | rawMeas[2] = (((int16_t)_buffer[4]) << 8) | _buffer[5]; 275 | // transform and convert to float values 276 | _axFifo[i] = ((rawMeas[0] * _accelScale) - _accB[0]) * _accS[0]; 277 | _ayFifo[i] = ((rawMeas[1] * _accelScale) - _accB[1]) * _accS[1]; 278 | _azFifo[i] = ((rawMeas[2] * _accelScale) - _accB[2]) * _accS[2]; 279 | _aSize = _fifoSize / _fifoFrameSize; 280 | } 281 | if (_enFifoTemp) { 282 | // combine into 16 bit values 283 | int16_t rawMeas = (((int16_t)_buffer[0 + _enFifoAccel*6]) << 8) | _buffer[1 + _enFifoAccel*6]; 284 | // transform and convert to float values 285 | _tFifo[i] = (static_cast(rawMeas) / TEMP_DATA_REG_SCALE) + TEMP_OFFSET; 286 | _tSize = _fifoSize/_fifoFrameSize; 287 | } 288 | if (_enFifoGyro) { 289 | // combine into 16 bit values 290 | int16_t rawMeas[3]; 291 | rawMeas[0] = (((int16_t)_buffer[0 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[1 + _enFifoAccel*6 + _enFifoTemp*2]; 292 | rawMeas[1] = (((int16_t)_buffer[2 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[3 + _enFifoAccel*6 + _enFifoTemp*2]; 293 | rawMeas[2] = (((int16_t)_buffer[4 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[5 + _enFifoAccel*6 + _enFifoTemp*2]; 294 | // transform and convert to float values 295 | _gxFifo[i] = (rawMeas[0] * _gyroScale) - _gyrB[0]; 296 | _gyFifo[i] = (rawMeas[1] * _gyroScale) - _gyrB[1]; 297 | _gzFifo[i] = (rawMeas[2] * _gyroScale) - _gyrB[2]; 298 | _gSize = _fifoSize/_fifoFrameSize; 299 | } 300 | } 301 | return 1; 302 | } 303 | 304 | /* returns the accelerometer FIFO size and data in the x direction, m/s/s */ 305 | void ICM42605_FIFO::getFifoAccelX_mss(size_t *size,float* data) { 306 | *size = _aSize; 307 | memcpy(data,_axFifo,_aSize*sizeof(float)); 308 | } 309 | 310 | /* returns the accelerometer FIFO size and data in the y direction, m/s/s */ 311 | void ICM42605_FIFO::getFifoAccelY_mss(size_t *size,float* data) { 312 | *size = _aSize; 313 | memcpy(data,_ayFifo,_aSize*sizeof(float)); 314 | } 315 | 316 | /* returns the accelerometer FIFO size and data in the z direction, m/s/s */ 317 | void ICM42605_FIFO::getFifoAccelZ_mss(size_t *size,float* data) { 318 | *size = _aSize; 319 | memcpy(data,_azFifo,_aSize*sizeof(float)); 320 | } 321 | 322 | /* returns the gyroscope FIFO size and data in the x direction, dps */ 323 | void ICM42605_FIFO::getFifoGyroX(size_t *size,float* data) { 324 | *size = _gSize; 325 | memcpy(data,_gxFifo,_gSize*sizeof(float)); 326 | } 327 | 328 | /* returns the gyroscope FIFO size and data in the y direction, dps */ 329 | void ICM42605_FIFO::getFifoGyroY(size_t *size,float* data) { 330 | *size = _gSize; 331 | memcpy(data,_gyFifo,_gSize*sizeof(float)); 332 | } 333 | 334 | /* returns the gyroscope FIFO size and data in the z direction, dps */ 335 | void ICM42605_FIFO::getFifoGyroZ(size_t *size,float* data) { 336 | *size = _gSize; 337 | memcpy(data,_gzFifo,_gSize*sizeof(float)); 338 | } 339 | 340 | /* returns the die temperature FIFO size and data, C */ 341 | void ICM42605_FIFO::getFifoTemperature_C(size_t *size,float* data) { 342 | *size = _tSize; 343 | memcpy(data,_tFifo,_tSize*sizeof(float)); 344 | } 345 | 346 | /* estimates the gyro biases */ 347 | int ICM42605::calibrateGyro() { 348 | // set at a lower range (more resolution) since IMU not moving 349 | const GyroFS current_fssel = _gyroFS; 350 | if (setGyroFS(dps250) < 0) return -1; 351 | 352 | // take samples and find bias 353 | _gyroBD[0] = 0; 354 | _gyroBD[1] = 0; 355 | _gyroBD[2] = 0; 356 | for (size_t i=0; i < NUM_CALIB_SAMPLES; i++) { 357 | getAGT(); 358 | _gyroBD[0] += (gyrX() + _gyrB[0]) / NUM_CALIB_SAMPLES; 359 | _gyroBD[1] += (gyrY() + _gyrB[1]) / NUM_CALIB_SAMPLES; 360 | _gyroBD[2] += (gyrZ() + _gyrB[2]) / NUM_CALIB_SAMPLES; 361 | delay(1); 362 | } 363 | _gyrB[0] = _gyroBD[0]; 364 | _gyrB[1] = _gyroBD[1]; 365 | _gyrB[2] = _gyroBD[2]; 366 | 367 | // recover the full scale setting 368 | if (setGyroFS(current_fssel) < 0) return -4; 369 | return 1; 370 | } 371 | 372 | /* returns the gyro bias in the X direction, dps */ 373 | float ICM42605::getGyroBiasX() { 374 | return _gyrB[0]; 375 | } 376 | 377 | /* returns the gyro bias in the Y direction, dps */ 378 | float ICM42605::getGyroBiasY() { 379 | return _gyrB[1]; 380 | } 381 | 382 | /* returns the gyro bias in the Z direction, dps */ 383 | float ICM42605::getGyroBiasZ() { 384 | return _gyrB[2]; 385 | } 386 | 387 | /* sets the gyro bias in the X direction to bias, dps */ 388 | void ICM42605::setGyroBiasX(float bias) { 389 | _gyrB[0] = bias; 390 | } 391 | 392 | /* sets the gyro bias in the Y direction to bias, dps */ 393 | void ICM42605::setGyroBiasY(float bias) { 394 | _gyrB[1] = bias; 395 | } 396 | 397 | /* sets the gyro bias in the Z direction to bias, dps */ 398 | void ICM42605::setGyroBiasZ(float bias) { 399 | _gyrB[2] = bias; 400 | } 401 | 402 | /* finds bias and scale factor calibration for the accelerometer, 403 | this should be run for each axis in each direction (6 total) to find 404 | the min and max values along each */ 405 | int ICM42605::calibrateAccel() { 406 | // set at a lower range (more resolution) since IMU not moving 407 | const AccelFS current_fssel = _accelFS; 408 | if (setAccelFS(gpm2) < 0) return -1; 409 | 410 | // take samples and find min / max 411 | _accBD[0] = 0; 412 | _accBD[1] = 0; 413 | _accBD[2] = 0; 414 | for (size_t i=0; i < NUM_CALIB_SAMPLES; i++) { 415 | getAGT(); 416 | _accBD[0] += (accX()/_accS[0] + _accB[0]) / NUM_CALIB_SAMPLES; 417 | _accBD[1] += (accY()/_accS[1] + _accB[1]) / NUM_CALIB_SAMPLES; 418 | _accBD[2] += (accZ()/_accS[2] + _accB[2]) / NUM_CALIB_SAMPLES; 419 | delay(1); 420 | } 421 | if (_accBD[0] > 0.9f) { 422 | _accMax[0] = _accBD[0]; 423 | } 424 | if (_accBD[1] > 0.9f) { 425 | _accMax[1] = _accBD[1]; 426 | } 427 | if (_accBD[2] > 0.9f) { 428 | _accMax[2] = _accBD[2]; 429 | } 430 | if (_accBD[0] < -0.9f) { 431 | _accMin[0] = _accBD[0]; 432 | } 433 | if (_accBD[1] < -0.9f) { 434 | _accMin[1] = _accBD[1]; 435 | } 436 | if (_accBD[2] < -0.9f) { 437 | _accMin[2] = _accBD[2]; 438 | } 439 | 440 | // find bias and scale factor 441 | if ((abs(_accMin[0]) > 0.9f) && (abs(_accMax[0]) > 0.9f)) { 442 | _accB[0] = (_accMin[0] + _accMax[0]) / 2.0f; 443 | _accS[0] = 1/((abs(_accMin[0]) + abs(_accMax[0])) / 2.0f); 444 | } 445 | if ((abs(_accMin[1]) > 0.9f) && (abs(_accMax[1]) > 0.9f)) { 446 | _accB[1] = (_accMin[1] + _accMax[1]) / 2.0f; 447 | _accS[1] = 1/((abs(_accMin[1]) + abs(_accMax[1])) / 2.0f); 448 | } 449 | if ((abs(_accMin[2]) > 0.9f) && (abs(_accMax[2]) > 0.9f)) { 450 | _accB[2] = (_accMin[2] + _accMax[2]) / 2.0f; 451 | _accS[2] = 1/((abs(_accMin[2]) + abs(_accMax[2])) / 2.0f); 452 | } 453 | 454 | // recover the full scale setting 455 | if (setAccelFS(current_fssel) < 0) return -4; 456 | return 1; 457 | } 458 | 459 | /* returns the accelerometer bias in the X direction, m/s/s */ 460 | float ICM42605::getAccelBiasX_mss() { 461 | return _accB[0]; 462 | } 463 | 464 | /* returns the accelerometer scale factor in the X direction */ 465 | float ICM42605::getAccelScaleFactorX() { 466 | return _accS[0]; 467 | } 468 | 469 | /* returns the accelerometer bias in the Y direction, m/s/s */ 470 | float ICM42605::getAccelBiasY_mss() { 471 | return _accB[1]; 472 | } 473 | 474 | /* returns the accelerometer scale factor in the Y direction */ 475 | float ICM42605::getAccelScaleFactorY() { 476 | return _accS[1]; 477 | } 478 | 479 | /* returns the accelerometer bias in the Z direction, m/s/s */ 480 | float ICM42605::getAccelBiasZ_mss() { 481 | return _accB[2]; 482 | } 483 | 484 | /* returns the accelerometer scale factor in the Z direction */ 485 | float ICM42605::getAccelScaleFactorZ() { 486 | return _accS[2]; 487 | } 488 | 489 | /* sets the accelerometer bias (m/s/s) and scale factor in the X direction */ 490 | void ICM42605::setAccelCalX(float bias,float scaleFactor) { 491 | _accB[0] = bias; 492 | _accS[0] = scaleFactor; 493 | } 494 | 495 | /* sets the accelerometer bias (m/s/s) and scale factor in the Y direction */ 496 | void ICM42605::setAccelCalY(float bias,float scaleFactor) { 497 | _accB[1] = bias; 498 | _accS[1] = scaleFactor; 499 | } 500 | 501 | /* sets the accelerometer bias (m/s/s) and scale factor in the Z direction */ 502 | void ICM42605::setAccelCalZ(float bias,float scaleFactor) { 503 | _accB[2] = bias; 504 | _accS[2] = scaleFactor; 505 | } 506 | 507 | /* returns the accelerometer measurement in the x direction, raw 16-bit integer */ 508 | int16_t ICM42605::getAccelX_count() 509 | { 510 | return _rawMeas[1]; 511 | } 512 | 513 | /* returns the accelerometer measurement in the y direction, raw 16-bit integer */ 514 | int16_t ICM42605::getAccelY_count() 515 | { 516 | return _rawMeas[2]; 517 | } 518 | 519 | /* returns the accelerometer measurement in the z direction, raw 16-bit integer */ 520 | int16_t ICM42605::getAccelZ_count() 521 | { 522 | return _rawMeas[3]; 523 | } 524 | 525 | /* returns the gyroscople measurement in the x direction, raw 16-bit integer */ 526 | int16_t ICM42605::getGyroX_count() 527 | { 528 | return _rawMeas[4]; 529 | } 530 | 531 | /* returns the gyroscople measurement in the y direction, raw 16-bit integer */ 532 | int16_t ICM42605::getGyroY_count() 533 | { 534 | return _rawMeas[5]; 535 | } 536 | 537 | /* returns the gyroscople measurement in the z direction, raw 16-bit integer */ 538 | int16_t ICM42605::getGyroZ_count() 539 | { 540 | return _rawMeas[6]; 541 | } 542 | 543 | 544 | /* writes a byte to ICM42605 register given a register address and data */ 545 | int ICM42605::writeRegister(uint8_t subAddress, uint8_t data) { 546 | /* write data to device */ 547 | if( _useSPI ) { 548 | _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction 549 | digitalWrite(_csPin,LOW); // select the ICM42605 chip 550 | _spi->transfer(subAddress); // write the register address 551 | _spi->transfer(data); // write the data 552 | digitalWrite(_csPin,HIGH); // deselect the ICM42605 chip 553 | _spi->endTransaction(); // end the transaction 554 | } 555 | else{ 556 | _i2c->beginTransmission(_address); // open the device 557 | _i2c->write(subAddress); // write the register address 558 | _i2c->write(data); // write the data 559 | _i2c->endTransmission(); 560 | } 561 | 562 | delay(10); 563 | 564 | /* read back the register */ 565 | readRegisters(subAddress, 1, _buffer); 566 | /* check the read back register against the written register */ 567 | if(_buffer[0] == data) { 568 | return 1; 569 | } 570 | else{ 571 | return -1; 572 | } 573 | } 574 | 575 | /* reads registers from ICM42605 given a starting register address, number of bytes, and a pointer to store data */ 576 | int ICM42605::readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest) { 577 | if( _useSPI ) { 578 | // begin the transaction 579 | if(_useSPIHS) { 580 | _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); 581 | } 582 | else{ 583 | _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); 584 | } 585 | digitalWrite(_csPin,LOW); // select the ICM42605 chip 586 | _spi->transfer(subAddress | 0x80); // specify the starting register address 587 | for(uint8_t i = 0; i < count; i++) { 588 | dest[i] = _spi->transfer(0x00); // read the data 589 | } 590 | digitalWrite(_csPin,HIGH); // deselect the ICM42605 chip 591 | _spi->endTransaction(); // end the transaction 592 | return 1; 593 | } 594 | else{ 595 | _i2c->beginTransmission(_address); // open the device 596 | _i2c->write(subAddress); // specify the starting register address 597 | _i2c->endTransmission(false); 598 | _numBytes = _i2c->requestFrom(_address, count); // specify the number of bytes to receive 599 | if (_numBytes == count) { 600 | for(uint8_t i = 0; i < count; i++) { 601 | dest[i] = _i2c->read(); 602 | } 603 | return 1; 604 | } else { 605 | return -1; 606 | } 607 | } 608 | } 609 | 610 | int ICM42605::setBank(uint8_t bank) { 611 | // if we are already on this bank, bail 612 | if (_bank == bank) return 1; 613 | 614 | _bank = bank; 615 | 616 | return writeRegister(REG_BANK_SEL, bank); 617 | } 618 | 619 | void ICM42605::reset() { 620 | setBank(0); 621 | 622 | writeRegister(UB0_REG_DEVICE_CONFIG, 0x01); 623 | 624 | // wait for ICM42605 to come back up 625 | delay(1); 626 | } 627 | 628 | /* gets the ICM42605 WHO_AM_I register value */ 629 | uint8_t ICM42605::whoAmI() { 630 | setBank(0); 631 | 632 | // read the WHO AM I register 633 | if (readRegisters(UB0_REG_WHO_AM_I, 1, _buffer) < 0) { 634 | return -1; 635 | } 636 | // return the register value 637 | return _buffer[0]; 638 | } 639 | --------------------------------------------------------------------------------