├── LICENSE ├── README.md ├── examples ├── Advanced_I2C │ └── Advanced_I2C.ino └── Basic_I2C │ └── Basic_I2C.ino ├── extras ├── AK09916C.pdf └── ICM-20948-v1.3.pdf ├── keywords.txt ├── library.properties └── src ├── ICM20948.cpp └── ICM20948.h /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 David Törnqvist 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Arduino driver for ICM-20948 2 | Arduino library for i2c communication with the 9-DOF IMU from TDK, ICM-20948. All output data is given in SI-units and all sensor data is aligned with the acc/gyro-coordinate system (see [data sheet](http://www.invensense.com/wp-content/uploads/2016/06/DS-000189-ICM-20948-v1.3.pdf)). 3 | 4 | I have developed this library for my [ICM-20948 breakout board](https://github.com/dtornqvist/icm-20948-breakout). 5 | 6 | ## Note 7 | The library is in beta phase and have not been tested thoroughly. 8 | 9 | ## Credits 10 | The library is heavily inspired by the Bolder Flight [MPU-9250 library](https://github.com/bolderflight/MPU9250). 11 | -------------------------------------------------------------------------------- /examples/Advanced_I2C/Advanced_I2C.ino: -------------------------------------------------------------------------------- 1 | #include "ICM20948.h" 2 | 3 | ICM20948 IMU(Wire, 0x69); // an ICM20948 object with the ICM-20948 sensor on I2C bus 0 with address 0x69 4 | int status; 5 | 6 | bool dataAvailable = false; 7 | int dataTime = 0; 8 | int lastDataTime = 0; 9 | 10 | void setup() { 11 | // serial to display data 12 | Serial.begin(115200); 13 | while(!Serial) {} 14 | 15 | // start communication with IMU 16 | status = IMU.begin(); 17 | Serial.print("status = "); 18 | Serial.println(status); 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 | 27 | IMU.configAccel(ICM20948::ACCEL_RANGE_16G, ICM20948::ACCEL_DLPF_BANDWIDTH_50HZ); 28 | IMU.configGyro(ICM20948::GYRO_RANGE_2000DPS, ICM20948::GYRO_DLPF_BANDWIDTH_51HZ); 29 | IMU.setGyroSrd(113); // Output data rate is 1125/(1 + srd) Hz 30 | IMU.setAccelSrd(113); 31 | IMU.enableDataReadyInterrupt(); 32 | pinMode(1, INPUT); 33 | attachInterrupt(1, imuReady, RISING); 34 | } 35 | 36 | void imuReady() { 37 | dataTime = micros(); 38 | dataAvailable = true; 39 | } 40 | 41 | void loop() { 42 | if (dataAvailable) { 43 | dataAvailable = false; 44 | int timeDiff = dataTime - lastDataTime; 45 | lastDataTime = dataTime; 46 | IMU.readSensor(); 47 | // display the data 48 | Serial.print(dataTime); 49 | Serial.print("\t"); 50 | Serial.print(timeDiff); 51 | Serial.print("\t"); 52 | Serial.print(IMU.getAccelX_mss(),6); 53 | Serial.print("\t"); 54 | Serial.print(IMU.getAccelY_mss(),6); 55 | Serial.print("\t"); 56 | Serial.print(IMU.getAccelZ_mss(),6); 57 | Serial.print("\t"); 58 | Serial.print(IMU.getGyroX_rads(),6); 59 | Serial.print("\t"); 60 | Serial.print(IMU.getGyroY_rads(),6); 61 | Serial.print("\t"); 62 | Serial.print(IMU.getGyroZ_rads(),6); 63 | Serial.print("\t"); 64 | Serial.print(IMU.getMagX_uT(),6); 65 | Serial.print("\t"); 66 | Serial.print(IMU.getMagY_uT(),6); 67 | Serial.print("\t"); 68 | Serial.print(IMU.getMagZ_uT(),6); 69 | Serial.print("\t"); 70 | Serial.println(IMU.getTemperature_C(),6); 71 | } 72 | 73 | } 74 | -------------------------------------------------------------------------------- /examples/Basic_I2C/Basic_I2C.ino: -------------------------------------------------------------------------------- 1 | #include "ICM20948.h" 2 | 3 | // an ICM20948 object with the ICM-20948 sensor on I2C bus 0 with address 0x69 4 | ICM20948 IMU(Wire, 0x69); 5 | int status; 6 | 7 | void setup() { 8 | // serial to display data 9 | Serial.begin(115200); 10 | while(!Serial) {} 11 | 12 | // start communication with IMU 13 | status = IMU.begin(); 14 | Serial.print("status = "); 15 | Serial.println(status); 16 | if (status < 0) { 17 | Serial.println("IMU initialization unsuccessful"); 18 | Serial.println("Check IMU wiring or try cycling power"); 19 | Serial.print("Status: "); 20 | Serial.println(status); 21 | while(1) {} 22 | } 23 | } 24 | 25 | void loop() { 26 | // read the sensor 27 | IMU.readSensor(); 28 | // display the data 29 | Serial.print(IMU.getAccelX_mss(),6); 30 | Serial.print("\t"); 31 | Serial.print(IMU.getAccelY_mss(),6); 32 | Serial.print("\t"); 33 | Serial.print(IMU.getAccelZ_mss(),6); 34 | Serial.print("\t"); 35 | Serial.print(IMU.getGyroX_rads(),6); 36 | Serial.print("\t"); 37 | Serial.print(IMU.getGyroY_rads(),6); 38 | Serial.print("\t"); 39 | Serial.print(IMU.getGyroZ_rads(),6); 40 | Serial.print("\t"); 41 | Serial.print(IMU.getMagX_uT(),6); 42 | Serial.print("\t"); 43 | Serial.print(IMU.getMagY_uT(),6); 44 | Serial.print("\t"); 45 | Serial.print(IMU.getMagZ_uT(),6); 46 | Serial.print("\t"); 47 | Serial.println(IMU.getTemperature_C(),6); 48 | 49 | delay(100); 50 | } 51 | -------------------------------------------------------------------------------- /extras/AK09916C.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dtornqvist/icm-20948-arduino-library/e6fc8ac9ecee01e8c1b64161bdac642fd16a9802/extras/AK09916C.pdf -------------------------------------------------------------------------------- /extras/ICM-20948-v1.3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dtornqvist/icm-20948-arduino-library/e6fc8ac9ecee01e8c1b64161bdac642fd16a9802/extras/ICM-20948-v1.3.pdf -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ICM20948 KEYWORD1 2 | begin KEYWORD2 3 | setAccelRange KEYWORD2 4 | setGyroRange KEYWORD2 5 | setDlpfBandwidth KEYWORD2 6 | setSrd KEYWORD2 7 | enableDataReadyInterrupt KEYWORD2 8 | disableDataReadyInterrupt KEYWORD2 9 | enableWakeOnMotion KEYWORD2 10 | enableFifo KEYWORD2 11 | readSensor KEYWORD2 12 | getAccelX_mss KEYWORD2 13 | getAccelY_mss KEYWORD2 14 | getAccelZ_mss KEYWORD2 15 | getGyroX_rads KEYWORD2 16 | getGyroY_rads KEYWORD2 17 | getGyroZ_rads KEYWORD2 18 | getMagX_uT KEYWORD2 19 | getMagY_uT KEYWORD2 20 | getMagZ_uT KEYWORD2 21 | getTemp_C KEYWORD2 -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=ICM20948 2 | version=0.1.0 3 | author=David Törnqvist 4 | maintainer=David Törnqvist 5 | sentence=Library for communicating with the ICM-20948 nine-axis Inertial Measurement Units (IMU). 6 | paragraph=This library supports both I2C communication with the ICM-20948. 7 | category=Sensors 8 | url=https://github.com/dtornqvist/icm-20948-arduino-library 9 | architectures=* 10 | includes=ICM20948.h -------------------------------------------------------------------------------- /src/ICM20948.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ICM20948.cpp 3 | 4 | Copyright (c) 2019 David Törnqvist 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #include "Arduino.h" 26 | #include "ICM20948.h" 27 | 28 | /* ICM20948 object, input the I2C bus and address */ 29 | ICM20948::ICM20948(TwoWire &bus, uint8_t address) { 30 | _i2c = &bus; // I2C bus 31 | _address = address; // I2C address 32 | } 33 | 34 | /* starts communication with the ICM-20948 */ 35 | int ICM20948::begin() { 36 | _i2c->begin(); // starting the I2C bus 37 | _i2c->setClock(_i2cRate); // setting the I2C clock 38 | 39 | if (changeUserBank(USER_BANK_0, true) < 0) { // Make sure that the user bank selection is in sync 40 | return -1; 41 | } 42 | if (selectAutoClockSource() < 0) { // TODO: Why set clock source here? It is resetted anyway... 43 | return -1; 44 | } 45 | // enable I2C master mode 46 | if(enableI2cMaster() < 0){ 47 | return -2; 48 | } 49 | if (powerDownMag() < 0) { 50 | return -3; 51 | } 52 | reset(); // reset the ICM20948. Don't check return value as a reset clears the register and can't be verified. 53 | delay(1); // wait for ICM-20948 to come back up 54 | resetMag(); // Don't check return value as a reset clears the register and can't be verified. 55 | if (selectAutoClockSource() < 0) { 56 | return -6; 57 | } 58 | if (whoAmI() != ICM20948_WHO_AM_I) { 59 | return -7; 60 | } 61 | if (enableAccelGyro() < 0) { 62 | return -8; 63 | } 64 | if (configAccel(ACCEL_RANGE_16G, ACCEL_DLPF_BANDWIDTH_246HZ) < 0) { 65 | return -9; 66 | } 67 | if (configGyro(GYRO_RANGE_2000DPS, GYRO_DLPF_BANDWIDTH_197HZ) < 0) { 68 | return -10; 69 | } 70 | if (setGyroSrd(0) < 0) { 71 | return -11; 72 | } 73 | if (setAccelSrd(0) < 0) { 74 | return -12; 75 | } 76 | if(enableI2cMaster() < 0) { 77 | return -13; 78 | } 79 | if(whoAmIMag() != MAG_AK09916_WHO_AM_I ) { 80 | return -14; 81 | } 82 | if(configMag() < 0){ 83 | return -18; 84 | } 85 | if(selectAutoClockSource() < 0) { // TODO: Why do this again here? 86 | return -19; 87 | } 88 | readMagRegisters(MAG_HXL, MAG_DATA_LENGTH, _buffer); // instruct the ICM20948 to get data from the magnetometer at the sample rate 89 | return 1; 90 | } 91 | 92 | int ICM20948::enableI2cMaster() { 93 | if (changeUserBank(USER_BANK_0) < 0) { 94 | return -1; 95 | } 96 | if (writeRegister(UB0_USER_CTRL, UB0_USER_CTRL_I2C_MST_EN) < 0) { 97 | return -2; 98 | } 99 | if (changeUserBank(USER_BANK_3) < 0) { 100 | return -3; 101 | } 102 | if(writeRegister(UB3_I2C_MST_CTRL, UB3_I2C_MST_CTRL_CLK_400KHZ) < 0){ 103 | return -4; 104 | } 105 | return 1; 106 | } 107 | 108 | /* enables the data ready interrupt */ 109 | int ICM20948::enableDataReadyInterrupt() { 110 | if (changeUserBank(USER_BANK_0) < 0) { 111 | return -1; 112 | } 113 | if (writeRegister(UB0_INT_PIN_CFG, UB0_INT_PIN_CFG_HIGH_50US) < 0) { // setup interrupt, 50 us pulse 114 | return -2; 115 | } 116 | if (writeRegister(UB0_INT_ENABLE_1, UB0_INT_ENABLE_1_RAW_RDY_EN) < 0) { // set to data ready 117 | return -3; 118 | } 119 | return 1; 120 | } 121 | 122 | /* disables the data ready interrupt */ 123 | int ICM20948::disableDataReadyInterrupt() { 124 | if (changeUserBank(USER_BANK_0) < 0) { 125 | return -1; 126 | } 127 | if (writeRegister(UB0_INT_ENABLE_1, UB0_INT_ENABLE_1_DIS) < 0) { // disable interrupt 128 | return -1; 129 | } 130 | return 1; 131 | } 132 | 133 | int ICM20948::reset() { 134 | if (changeUserBank(USER_BANK_0) < 0) { 135 | return -1; 136 | } 137 | if (writeRegister(UB0_PWR_MGMNT_1, UB0_PWR_MGMNT_1_DEV_RESET) < 0) { 138 | return -2; 139 | } 140 | return 1; 141 | } 142 | 143 | int ICM20948::selectAutoClockSource() { 144 | if (changeUserBank(USER_BANK_0) < 0 || writeRegister(UB0_PWR_MGMNT_1, UB0_PWR_MGMNT_1_CLOCK_SEL_AUTO) < 0) { 145 | return -1; 146 | } 147 | return 1; 148 | } 149 | 150 | int ICM20948::enableAccelGyro() { 151 | if (changeUserBank(USER_BANK_0) < 0 || writeRegister(UB0_PWR_MGMNT_2, UB0_PWR_MGMNT_2_SEN_ENABLE) < 0) { 152 | return -1; 153 | } 154 | return 1; 155 | } 156 | 157 | int ICM20948::configAccel(AccelRange range, AccelDlpfBandwidth bandwidth) { 158 | if (changeUserBank(USER_BANK_2) < 0) { 159 | return -1; 160 | } 161 | uint8_t accelRangeRegValue = 0x00; 162 | float accelScale = 0.0f; 163 | switch(range) { 164 | case ACCEL_RANGE_2G: { 165 | accelRangeRegValue = UB2_ACCEL_CONFIG_FS_SEL_2G; 166 | accelScale = G * 2.0f/accRawScaling; // setting the accel scale to 2G 167 | break; 168 | } 169 | case ACCEL_RANGE_4G: { 170 | accelRangeRegValue = UB2_ACCEL_CONFIG_FS_SEL_4G; 171 | accelScale = G * 4.0f/accRawScaling; // setting the accel scale to 4G 172 | break; 173 | } 174 | case ACCEL_RANGE_8G: { 175 | accelRangeRegValue = UB2_ACCEL_CONFIG_FS_SEL_8G; 176 | accelScale = G * 8.0f/accRawScaling; // setting the accel scale to 8G 177 | break; 178 | } 179 | case ACCEL_RANGE_16G: { 180 | accelRangeRegValue = UB2_ACCEL_CONFIG_FS_SEL_16G; 181 | accelScale = G * 16.0f/accRawScaling; // setting the accel scale to 16G 182 | break; 183 | } 184 | } 185 | uint8_t dlpfRegValue = 0x00; 186 | switch(bandwidth) { 187 | case ACCEL_DLPF_BANDWIDTH_1209HZ: dlpfRegValue = UB2_ACCEL_CONFIG_DLPFCFG_1209HZ; break; 188 | case ACCEL_DLPF_BANDWIDTH_246HZ: dlpfRegValue = UB2_ACCEL_CONFIG_DLPFCFG_246HZ; break; 189 | case ACCEL_DLPF_BANDWIDTH_111HZ: dlpfRegValue = UB2_ACCEL_CONFIG_DLPFCFG_111HZ; break; 190 | case ACCEL_DLPF_BANDWIDTH_50HZ: dlpfRegValue = UB2_ACCEL_CONFIG_DLPFCFG_50HZ; break; 191 | case ACCEL_DLPF_BANDWIDTH_24HZ: dlpfRegValue = UB2_ACCEL_CONFIG_DLPFCFG_24HZ; break; 192 | case ACCEL_DLPF_BANDWIDTH_12HZ: dlpfRegValue = UB2_ACCEL_CONFIG_DLPFCFG_12HZ; break; 193 | case ACCEL_DLPF_BANDWIDTH_6HZ: dlpfRegValue = UB2_ACCEL_CONFIG_DLPFCFG_6HZ; break; 194 | case ACCEL_DLPF_BANDWIDTH_473HZ: dlpfRegValue = UB2_ACCEL_CONFIG_DLPFCFG_473HZ; break; 195 | } 196 | if (writeRegister(UB2_ACCEL_CONFIG, accelRangeRegValue | dlpfRegValue) < 0) { 197 | return -1; 198 | } 199 | _accelScale = accelScale; 200 | _accelRange = range; 201 | _accelBandwidth = bandwidth; 202 | return 1; 203 | } 204 | 205 | /* sets the gyro full scale range to values other than default */ 206 | int ICM20948::configGyro(GyroRange range, GyroDlpfBandwidth bandwidth) { 207 | if (changeUserBank(USER_BANK_2) < 0) { 208 | return -1; 209 | } 210 | uint8_t gyroConfigRegValue = 0x00; 211 | float gyroScale = 0x00; 212 | switch(range) { 213 | case GYRO_RANGE_250DPS: { 214 | gyroConfigRegValue = UB2_GYRO_CONFIG_1_FS_SEL_250DPS; 215 | gyroScale = 250.0f/gyroRawScaling * _d2r; // setting the gyro scale to 250DPS 216 | break; 217 | } 218 | case GYRO_RANGE_500DPS: { 219 | gyroConfigRegValue = UB2_GYRO_CONFIG_1_FS_SEL_500DPS; 220 | gyroScale = 500.0f/gyroRawScaling * _d2r; // setting the gyro scale to 500DPS 221 | break; 222 | } 223 | case GYRO_RANGE_1000DPS: { 224 | gyroConfigRegValue = UB2_GYRO_CONFIG_1_FS_SEL_1000DPS; 225 | gyroScale = 1000.0f/gyroRawScaling * _d2r; // setting the gyro scale to 1000DPS 226 | break; 227 | } 228 | case GYRO_RANGE_2000DPS: { 229 | gyroConfigRegValue = UB2_GYRO_CONFIG_1_FS_SEL_2000DPS; 230 | gyroScale = 2000.0f/gyroRawScaling * _d2r; // setting the gyro scale to 2000DPS 231 | break; 232 | } 233 | } 234 | uint8_t dlpfRegValue = 0x00; 235 | switch(bandwidth) { 236 | case GYRO_DLPF_BANDWIDTH_12106HZ: dlpfRegValue = UB2_GYRO_CONFIG_1_DLPFCFG_12106HZ; break; 237 | case GYRO_DLPF_BANDWIDTH_197HZ: dlpfRegValue = UB2_GYRO_CONFIG_1_DLPFCFG_197HZ; break; 238 | case GYRO_DLPF_BANDWIDTH_152HZ: dlpfRegValue = UB2_GYRO_CONFIG_1_DLPFCFG_152HZ; break; 239 | case GYRO_DLPF_BANDWIDTH_120HZ: dlpfRegValue = UB2_GYRO_CONFIG_1_DLPFCFG_120HZ; break; 240 | case GYRO_DLPF_BANDWIDTH_51HZ: dlpfRegValue = UB2_GYRO_CONFIG_1_DLPFCFG_51HZ; break; 241 | case GYRO_DLPF_BANDWIDTH_24HZ: dlpfRegValue = UB2_GYRO_CONFIG_1_DLPFCFG_24HZ; break; 242 | case GYRO_DLPF_BANDWIDTH_12HZ: dlpfRegValue = UB2_GYRO_CONFIG_1_DLPFCFG_12HZ; break; 243 | case GYRO_DLPF_BANDWIDTH_6HZ: dlpfRegValue = UB2_GYRO_CONFIG_1_DLPFCFG_6HZ; break; 244 | case GYRO_DLPF_BANDWIDTH_361HZ: dlpfRegValue = UB2_GYRO_CONFIG_1_DLPFCFG_361HZ; break; 245 | } 246 | if (writeRegister(UB2_GYRO_CONFIG_1, gyroConfigRegValue | dlpfRegValue) < 0) { 247 | return -1; 248 | } 249 | _gyroScale = gyroScale; 250 | _gyroRange = range; 251 | _gyroBandwidth = bandwidth; 252 | return 1; 253 | } 254 | 255 | int ICM20948::configMag() { // TODO: Add possibility to use other modes 256 | if (writeMagRegister(MAG_CNTL2, MAG_CNTL2_MODE_100HZ) < 0) { 257 | return -1; 258 | } 259 | return 1; 260 | } 261 | 262 | int ICM20948::setGyroSrd(uint8_t srd) { 263 | if (changeUserBank(USER_BANK_2) < 0 || writeRegister(UB2_GYRO_SMPLRT_DIV, srd) < 0) { 264 | return -1; 265 | } 266 | _gyroSrd = srd; 267 | return 1; 268 | } 269 | 270 | int ICM20948::setAccelSrd(uint16_t srd) { 271 | if (changeUserBank(USER_BANK_2) < 0) { 272 | return -1; 273 | } 274 | uint8_t srdHigh = srd >> 8 & 0x0F; // Only last 4 bits can be set 275 | if (writeRegister(UB2_ACCEL_SMPLRT_DIV_1, srdHigh) < 0) { 276 | return -1; 277 | } 278 | uint8_t srdLow = srd & 0x0F; // Only last 4 bits can be set 279 | if (writeRegister(UB2_ACCEL_SMPLRT_DIV_2, srdLow) < 0) { 280 | return -1; 281 | } 282 | _accelSrd = srd; 283 | return 1; 284 | } 285 | 286 | /* reads the most current data from MPU9250 and stores in buffer */ 287 | int ICM20948::readSensor() { 288 | if (changeUserBank(USER_BANK_0) < 0) { 289 | return -1; 290 | } 291 | if (readRegisters(UB0_ACCEL_XOUT_H, 20, _buffer) < 0) { 292 | return -1; 293 | } 294 | // combine into 16 bit values 295 | _axcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1]; 296 | _aycounts = (((int16_t)_buffer[2]) << 8) | _buffer[3]; 297 | _azcounts = (((int16_t)_buffer[4]) << 8) | _buffer[5]; 298 | _gxcounts = (((int16_t)_buffer[6]) << 8) | _buffer[7]; 299 | _gycounts = (((int16_t)_buffer[8]) << 8) | _buffer[9]; 300 | _gzcounts = (((int16_t)_buffer[10]) << 8) | _buffer[11]; 301 | _tcounts = (((int16_t)_buffer[12]) << 8) | _buffer[13]; 302 | _hxcounts = (((int16_t)_buffer[15]) << 8) | _buffer[14]; 303 | _hycounts = (((int16_t)_buffer[17]) << 8) | _buffer[16]; 304 | _hzcounts = (((int16_t)_buffer[19]) << 8) | _buffer[18]; 305 | // transform and convert to float values 306 | _ax = (((float)_axcounts * _accelScale) - _axb)*_axs; 307 | _ay = (((float)_aycounts * _accelScale) - _ayb)*_ays; 308 | _az = (((float)_azcounts * _accelScale) - _azb)*_azs; 309 | _gx = ((float)_gxcounts * _gyroScale) - _gxb; 310 | _gy = ((float)_gycounts * _gyroScale) - _gyb; 311 | _gz = ((float)_gzcounts * _gyroScale) - _gzb; 312 | _t = ((((float) _tcounts) - _tempOffset)/_tempScale) + _tempOffset; 313 | _hx = (((float)(tX[0]*_hxcounts + tX[1]*_hycounts + tX[2]*_hzcounts) * _magScale) - _hxb)*_hxs; 314 | _hy = (((float)(tY[0]*_hxcounts + tY[1]*_hycounts + tY[2]*_hzcounts) * _magScale) - _hyb)*_hys; 315 | _hz = (((float)(tZ[0]*_hxcounts + tZ[1]*_hycounts + tZ[2]*_hzcounts) * _magScale) - _hzb)*_hzs; 316 | return 1; 317 | } 318 | 319 | /* returns the accelerometer measurement in the x direction, m/s/s */ 320 | float ICM20948::getAccelX_mss() { 321 | return _ax; 322 | } 323 | 324 | /* returns the accelerometer measurement in the y direction, m/s/s */ 325 | float ICM20948::getAccelY_mss() { 326 | return _ay; 327 | } 328 | 329 | /* returns the accelerometer measurement in the z direction, m/s/s */ 330 | float ICM20948::getAccelZ_mss() { 331 | return _az; 332 | } 333 | 334 | /* returns the gyroscope measurement in the x direction, rad/s */ 335 | float ICM20948::getGyroX_rads() { 336 | return _gx; 337 | } 338 | 339 | /* returns the gyroscope measurement in the y direction, rad/s */ 340 | float ICM20948::getGyroY_rads() { 341 | return _gy; 342 | } 343 | 344 | /* returns the gyroscope measurement in the z direction, rad/s */ 345 | float ICM20948::getGyroZ_rads() { 346 | return _gz; 347 | } 348 | 349 | /* returns the magnetometer measurement in the x direction, uT */ 350 | float ICM20948::getMagX_uT() { 351 | return _hx; 352 | } 353 | 354 | /* returns the magnetometer measurement in the y direction, uT */ 355 | float ICM20948::getMagY_uT() { 356 | return _hy; 357 | } 358 | 359 | /* returns the magnetometer measurement in the z direction, uT */ 360 | float ICM20948::getMagZ_uT() { 361 | return _hz; 362 | } 363 | 364 | /* returns the die temperature, C */ 365 | float ICM20948::getTemperature_C() { 366 | return _t; 367 | } 368 | 369 | /* gets the WHO_AM_I register value, expected to be 0xEA */ 370 | int ICM20948::whoAmI() { 371 | if (changeUserBank(USER_BANK_0) < 0) { 372 | return -1; 373 | } 374 | // read the WHO AM I register 375 | if (readRegisters(UB0_WHO_AM_I, 1, _buffer) < 0) { 376 | return -1; 377 | } 378 | // return the register value 379 | return _buffer[0]; 380 | } 381 | 382 | int ICM20948::whoAmIMag() { 383 | if (readMagRegisters(MAG_WHO_AM_I, 2, _buffer) < 0) { 384 | return -1; 385 | } 386 | return (_buffer[0] << 8) + _buffer[1]; 387 | } 388 | 389 | int ICM20948::powerDownMag() { 390 | if (writeMagRegister(MAG_CNTL2, MAG_CNTL2_POWER_DOWN) < 0) { 391 | return -1; 392 | } 393 | return 1; 394 | } 395 | 396 | int ICM20948::resetMag() { 397 | if (writeMagRegister(MAG_CNTL3, MAG_CNTL3_RESET) < 0) { 398 | return -1; 399 | } 400 | return 1; 401 | } 402 | 403 | int ICM20948::changeUserBank(UserBank userBank) { 404 | return changeUserBank(userBank, false); 405 | } 406 | 407 | int ICM20948::changeUserBank(UserBank userBank, bool force) { 408 | if (!force && userBank == _currentUserBank) { 409 | return 2; // No need to change 410 | } 411 | uint8_t userBankRegValue = 0x00; 412 | switch(userBank) { 413 | case USER_BANK_0: { 414 | userBankRegValue = REG_BANK_SEL_USER_BANK_0; 415 | break; 416 | } 417 | case USER_BANK_1: { 418 | userBankRegValue = REG_BANK_SEL_USER_BANK_1; 419 | break; 420 | } 421 | case USER_BANK_2: { 422 | userBankRegValue = REG_BANK_SEL_USER_BANK_2; 423 | break; 424 | } 425 | case USER_BANK_3: { 426 | userBankRegValue = REG_BANK_SEL_USER_BANK_3; 427 | break; 428 | } 429 | } 430 | if (writeRegister(REG_BANK_SEL, userBankRegValue) < 0) { 431 | return -1; 432 | } 433 | _currentUserBank = userBank; 434 | return 1; 435 | } 436 | 437 | int ICM20948::writeMagRegister(uint8_t subAddress, uint8_t data) { 438 | if (changeUserBank(USER_BANK_3) < 0) { 439 | return -1; 440 | } 441 | if (writeRegister(UB3_I2C_SLV0_ADDR, MAG_AK09916_I2C_ADDR) < 0) { 442 | return -2; 443 | } 444 | // set the register to the desired magnetometer sub address 445 | if (writeRegister(UB3_I2C_SLV0_REG, subAddress) < 0) { 446 | return -3; 447 | } 448 | // store the data for write 449 | if (writeRegister(UB3_I2C_SLV0_DO, data) < 0) { 450 | return -4; 451 | } 452 | // enable I2C and send 1 byte 453 | if (writeRegister(UB3_I2C_SLV0_CTRL, UB3_I2C_SLV0_CTRL_EN | (uint8_t)1) < 0) { 454 | return -5; 455 | } 456 | // read the register and confirm 457 | if (readMagRegisters(subAddress, 1, _buffer) < 0) { 458 | return -6; 459 | } 460 | if(_buffer[0] != data) { 461 | return -7; 462 | } 463 | return 1; 464 | } 465 | 466 | int ICM20948::readMagRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest) { 467 | if (changeUserBank(USER_BANK_3) < 0) { 468 | return -1; 469 | } 470 | if (writeRegister(UB3_I2C_SLV0_ADDR, MAG_AK09916_I2C_ADDR | UB3_I2C_SLV0_ADDR_READ_FLAG) < 0) { 471 | return -2; 472 | } 473 | // set the register to the desired magnetometer sub address 474 | if (writeRegister(UB3_I2C_SLV0_REG, subAddress) < 0) { 475 | return -3; 476 | } 477 | // enable I2C and request the bytes 478 | if (writeRegister(UB3_I2C_SLV0_CTRL, UB3_I2C_SLV0_CTRL_EN | count) < 0) { 479 | return -4; 480 | } 481 | delay(1); // takes some time for these registers to fill 482 | // read the bytes off the ICM-20948 EXT_SLV_SENS_DATA registers 483 | if (changeUserBank(USER_BANK_0) < 0) { 484 | return -5; 485 | } 486 | _status = readRegisters(UB0_EXT_SLV_SENS_DATA_00, count, dest); 487 | return _status; 488 | } 489 | 490 | /* writes a byte to ICM20948 register given a register address and data */ 491 | int ICM20948::writeRegister(uint8_t subAddress, uint8_t data) { 492 | /* write data to device */ 493 | _i2c->beginTransmission(_address); // open the device 494 | _i2c->write(subAddress); // write the register address 495 | _i2c->write(data); // write the data 496 | _i2c->endTransmission(); 497 | 498 | delay(10); 499 | 500 | /* read back the register */ 501 | readRegisters(subAddress, 1, _buffer); 502 | /* check the read back register against the written register */ 503 | if(_buffer[0] == data) { 504 | return 1; 505 | } 506 | else{ 507 | return -1; 508 | } 509 | } 510 | 511 | /* reads registers from ICM20948 given a starting register address, number of bytes, and a pointer to store data */ 512 | int ICM20948::readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest) { 513 | _i2c->beginTransmission(_address); // open the device 514 | _i2c->write(subAddress); // specify the starting register address 515 | _i2c->endTransmission(false); 516 | _numBytes = _i2c->requestFrom(_address, count); // specify the number of bytes to receive 517 | if (_numBytes == count) { 518 | for(uint8_t i = 0; i < count; i++){ 519 | dest[i] = _i2c->read(); 520 | } 521 | return 1; 522 | } else { 523 | return -1; 524 | } 525 | } -------------------------------------------------------------------------------- /src/ICM20948.h: -------------------------------------------------------------------------------- 1 | /* 2 | ICM20948.h 3 | 4 | Copyright (c) 2019 David Törnqvist 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #ifndef ICM20948_h 26 | #define ICM20948_h 27 | 28 | #include "Arduino.h" 29 | #include "Wire.h" // I2C library 30 | 31 | class ICM20948 { 32 | public: 33 | enum GyroRange 34 | { 35 | GYRO_RANGE_250DPS, 36 | GYRO_RANGE_500DPS, 37 | GYRO_RANGE_1000DPS, 38 | GYRO_RANGE_2000DPS 39 | }; 40 | enum AccelRange 41 | { 42 | ACCEL_RANGE_2G, 43 | ACCEL_RANGE_4G, 44 | ACCEL_RANGE_8G, 45 | ACCEL_RANGE_16G 46 | }; 47 | enum AccelDlpfBandwidth 48 | { 49 | ACCEL_DLPF_BANDWIDTH_1209HZ, 50 | ACCEL_DLPF_BANDWIDTH_246HZ, 51 | ACCEL_DLPF_BANDWIDTH_111HZ, 52 | ACCEL_DLPF_BANDWIDTH_50HZ, 53 | ACCEL_DLPF_BANDWIDTH_24HZ, 54 | ACCEL_DLPF_BANDWIDTH_12HZ, 55 | ACCEL_DLPF_BANDWIDTH_6HZ, 56 | ACCEL_DLPF_BANDWIDTH_473HZ 57 | }; 58 | enum GyroDlpfBandwidth 59 | { 60 | GYRO_DLPF_BANDWIDTH_12106HZ, 61 | GYRO_DLPF_BANDWIDTH_197HZ, 62 | GYRO_DLPF_BANDWIDTH_152HZ, 63 | GYRO_DLPF_BANDWIDTH_120HZ, 64 | GYRO_DLPF_BANDWIDTH_51HZ, 65 | GYRO_DLPF_BANDWIDTH_24HZ, 66 | GYRO_DLPF_BANDWIDTH_12HZ, 67 | GYRO_DLPF_BANDWIDTH_6HZ, 68 | GYRO_DLPF_BANDWIDTH_361HZ 69 | }; 70 | enum LpAccelOdr 71 | { 72 | LP_ACCEL_ODR_0_24HZ = 0, 73 | LP_ACCEL_ODR_0_49HZ = 1, 74 | LP_ACCEL_ODR_0_98HZ = 2, 75 | LP_ACCEL_ODR_1_95HZ = 3, 76 | LP_ACCEL_ODR_3_91HZ = 4, 77 | LP_ACCEL_ODR_7_81HZ = 5, 78 | LP_ACCEL_ODR_15_63HZ = 6, 79 | LP_ACCEL_ODR_31_25HZ = 7, 80 | LP_ACCEL_ODR_62_50HZ = 8, 81 | LP_ACCEL_ODR_125HZ = 9, 82 | LP_ACCEL_ODR_250HZ = 10, 83 | LP_ACCEL_ODR_500HZ = 11 84 | }; 85 | enum UserBank 86 | { 87 | USER_BANK_0, 88 | USER_BANK_1, 89 | USER_BANK_2, 90 | USER_BANK_3, 91 | }; 92 | ICM20948(TwoWire &bus, uint8_t address); 93 | int begin(); 94 | int configAccel(AccelRange range, AccelDlpfBandwidth bandwidth); 95 | int configGyro(GyroRange range, GyroDlpfBandwidth bandwidth); 96 | int configMag(); 97 | int setGyroSrd(uint8_t srd); 98 | int setAccelSrd(uint16_t srd); 99 | int enableDataReadyInterrupt(); 100 | int disableDataReadyInterrupt(); 101 | int readSensor(); 102 | float getAccelX_mss(); 103 | float getAccelY_mss(); 104 | float getAccelZ_mss(); 105 | float getGyroX_rads(); 106 | float getGyroY_rads(); 107 | float getGyroZ_rads(); 108 | float getMagX_uT(); 109 | float getMagY_uT(); 110 | float getMagZ_uT(); 111 | float getTemperature_C(); 112 | protected: 113 | // i2c 114 | uint8_t _address; 115 | TwoWire *_i2c; 116 | const uint32_t _i2cRate = 400000; // 400 kHz 117 | size_t _numBytes; // number of bytes received from I2C 118 | // track success of interacting with sensor 119 | int _status; 120 | // buffer for reading from sensor 121 | uint8_t _buffer[21]; 122 | // data counts 123 | int16_t _axcounts,_aycounts,_azcounts; 124 | int16_t _gxcounts,_gycounts,_gzcounts; 125 | int16_t _hxcounts,_hycounts,_hzcounts; 126 | int16_t _tcounts; 127 | // data buffer 128 | float _ax, _ay, _az; 129 | float _gx, _gy, _gz; 130 | float _hx, _hy, _hz; 131 | float _t; 132 | // wake on motion 133 | uint8_t _womThreshold; 134 | // scale factors 135 | float _accelScale; 136 | float _gyroScale; 137 | const float _tempScale = 333.87f; 138 | const float _tempOffset = 21.0f; 139 | // configuration 140 | AccelRange _accelRange; 141 | GyroRange _gyroRange; 142 | AccelDlpfBandwidth _accelBandwidth; 143 | GyroDlpfBandwidth _gyroBandwidth; 144 | UserBank _currentUserBank = USER_BANK_0; 145 | uint8_t _gyroSrd; 146 | uint16_t _accelSrd; 147 | // gyro bias estimation 148 | size_t _numSamples = 100; 149 | double _gxbD, _gybD, _gzbD; 150 | float _gxb, _gyb, _gzb; 151 | // accel bias and scale factor estimation 152 | double _axbD, _aybD, _azbD; 153 | float _axmax, _aymax, _azmax; 154 | float _axmin, _aymin, _azmin; 155 | float _axb, _ayb, _azb; 156 | float _axs = 1.0f; 157 | float _ays = 1.0f; 158 | float _azs = 1.0f; 159 | // magnetometer bias and scale factor estimation 160 | uint16_t _maxCounts = 1000; 161 | float _deltaThresh = 0.3f; 162 | uint8_t _coeff = 8; 163 | uint16_t _counter; 164 | float _framedelta, _delta; 165 | float _hxfilt, _hyfilt, _hzfilt; 166 | float _hxmax, _hymax, _hzmax; 167 | float _hxmin, _hymin, _hzmin; 168 | float _hxb, _hyb, _hzb; 169 | float _hxs = 1.0f; 170 | float _hys = 1.0f; 171 | float _hzs = 1.0f; 172 | float _avgs; 173 | 174 | // transformation matrix 175 | /* transform the magnetometer values to match the coordinate system of the IMU */ 176 | const int16_t tX[3] = {1, 0, 0}; 177 | const int16_t tY[3] = {0, -1, 0}; 178 | const int16_t tZ[3] = {0, 0, -1}; 179 | // constants 180 | const float G = 9.807f; 181 | const float _d2r = 3.14159265359f/180.0f; 182 | 183 | const float accRawScaling = 32767.5f; // =(2^16-1)/2 16 bit representation of acc value to cover +/- range 184 | const float gyroRawScaling = 32767.5f; // =(2^16-1)/2 16 bit representation of gyro value to cover +/- range 185 | const float magRawScaling = 32767.5f; // =(2^16-1)/2 16 bit representation of gyro value to cover +/- range 186 | 187 | const float _magScale = 4912.0f / magRawScaling; // micro Tesla, measurement range is +/- 4912 uT. 188 | 189 | const uint8_t ICM20948_WHO_AM_I = 0xEA; 190 | 191 | // ICM20948 registers 192 | // User bank 0 193 | const uint8_t UB0_WHO_AM_I = 0x00; 194 | const uint8_t UB0_USER_CTRL = 0x03; 195 | const uint8_t UB0_USER_CTRL_I2C_MST_EN = 0x20; 196 | 197 | const uint8_t UB0_PWR_MGMNT_1 = 0x06; 198 | const uint8_t UB0_PWR_MGMNT_1_CLOCK_SEL_AUTO = 0x01; 199 | const uint8_t UB0_PWR_MGMNT_1_DEV_RESET = 0x80; 200 | 201 | const uint8_t UB0_PWR_MGMNT_2 = 0x07; 202 | const uint8_t UB0_PWR_MGMNT_2_SEN_ENABLE = 0x00; 203 | 204 | const uint8_t UB0_INT_PIN_CFG = 0x0F; 205 | const uint8_t UB0_INT_PIN_CFG_HIGH_50US = 0x00; 206 | 207 | const uint8_t UB0_INT_ENABLE_1 = 0x11; 208 | const uint8_t UB0_INT_ENABLE_1_RAW_RDY_EN = 0x01; 209 | const uint8_t UB0_INT_ENABLE_1_DIS = 0x00; 210 | 211 | 212 | const uint8_t UB0_ACCEL_XOUT_H = 0x2D; 213 | 214 | const uint8_t UB0_EXT_SLV_SENS_DATA_00 = 0x3B; 215 | 216 | // User bank 2 217 | const uint8_t UB2_GYRO_SMPLRT_DIV = 0x00; 218 | 219 | const uint8_t UB2_GYRO_CONFIG_1 = 0x01; 220 | const uint8_t UB2_GYRO_CONFIG_1_FS_SEL_250DPS = 0x00; 221 | const uint8_t UB2_GYRO_CONFIG_1_FS_SEL_500DPS = 0x02; 222 | const uint8_t UB2_GYRO_CONFIG_1_FS_SEL_1000DPS = 0x04; 223 | const uint8_t UB2_GYRO_CONFIG_1_FS_SEL_2000DPS = 0x06; 224 | const uint8_t UB2_GYRO_CONFIG_1_DLPFCFG_12106HZ = 0x00; 225 | const uint8_t UB2_GYRO_CONFIG_1_DLPFCFG_197HZ = 0x00 | 0x01; 226 | const uint8_t UB2_GYRO_CONFIG_1_DLPFCFG_152HZ = 0b00001000 | 0x01; 227 | const uint8_t UB2_GYRO_CONFIG_1_DLPFCFG_120HZ = 0b00010000 | 0x01; 228 | const uint8_t UB2_GYRO_CONFIG_1_DLPFCFG_51HZ = 0b00011000 | 0x01; 229 | const uint8_t UB2_GYRO_CONFIG_1_DLPFCFG_24HZ = 0b00100000 | 0x01; 230 | const uint8_t UB2_GYRO_CONFIG_1_DLPFCFG_12HZ = 0b00101000 | 0x01; 231 | const uint8_t UB2_GYRO_CONFIG_1_DLPFCFG_6HZ = 0b00110000 | 0x01; 232 | const uint8_t UB2_GYRO_CONFIG_1_DLPFCFG_361HZ = 0b00111000 | 0x01; 233 | 234 | const uint8_t UB2_ACCEL_SMPLRT_DIV_1 = 0x10; 235 | const uint8_t UB2_ACCEL_SMPLRT_DIV_2 = 0x11; 236 | 237 | const uint8_t UB2_ACCEL_CONFIG = 0x14; 238 | const uint8_t UB2_ACCEL_CONFIG_FS_SEL_2G = 0x00; 239 | const uint8_t UB2_ACCEL_CONFIG_FS_SEL_4G = 0x02; 240 | const uint8_t UB2_ACCEL_CONFIG_FS_SEL_8G = 0x04; 241 | const uint8_t UB2_ACCEL_CONFIG_FS_SEL_16G = 0x06; 242 | const uint8_t UB2_ACCEL_CONFIG_DLPFCFG_1209HZ = 0x00; 243 | const uint8_t UB2_ACCEL_CONFIG_DLPFCFG_246HZ = 0x00 | 0x01; 244 | const uint8_t UB2_ACCEL_CONFIG_DLPFCFG_111HZ = 0b00010000 | 0x01; 245 | const uint8_t UB2_ACCEL_CONFIG_DLPFCFG_50HZ = 0b00011000 | 0x01; 246 | const uint8_t UB2_ACCEL_CONFIG_DLPFCFG_24HZ = 0b00100000 | 0x01; 247 | const uint8_t UB2_ACCEL_CONFIG_DLPFCFG_12HZ = 0b00101000 | 0x01; 248 | const uint8_t UB2_ACCEL_CONFIG_DLPFCFG_6HZ = 0b00110000 | 0x01; 249 | const uint8_t UB2_ACCEL_CONFIG_DLPFCFG_473HZ = 0b00111000 | 0x01; 250 | 251 | // User bank 3 252 | const uint8_t UB3_I2C_MST_CTRL = 0x01; 253 | const uint8_t UB3_I2C_MST_CTRL_CLK_400KHZ = 0x07; // Gives 345.6kHz and is recommended to achieve max 400kHz 254 | 255 | const uint8_t UB3_I2C_SLV0_ADDR = 0x03; 256 | const uint8_t UB3_I2C_SLV0_ADDR_READ_FLAG = 0x80; 257 | 258 | const uint8_t UB3_I2C_SLV0_REG = 0x04; 259 | 260 | const uint8_t UB3_I2C_SLV0_CTRL = 0x05; 261 | const uint8_t UB3_I2C_SLV0_CTRL_EN = 0x80; 262 | 263 | const uint8_t UB3_I2C_SLV0_DO = 0x06; 264 | 265 | // Common to all user banks 266 | const uint8_t REG_BANK_SEL = 0x7F; 267 | const uint8_t REG_BANK_SEL_USER_BANK_0 = 0x00; 268 | const uint8_t REG_BANK_SEL_USER_BANK_1 = 0x10; 269 | const uint8_t REG_BANK_SEL_USER_BANK_2 = 0x20; 270 | const uint8_t REG_BANK_SEL_USER_BANK_3 = 0x30; 271 | 272 | // Magnetometer constants 273 | const uint8_t MAG_AK09916_I2C_ADDR = 0x0C; 274 | const uint16_t MAG_AK09916_WHO_AM_I = 0x4809; 275 | const uint8_t MAG_DATA_LENGTH = 8; // Bytes 276 | 277 | // Magnetometer (AK09916) registers 278 | const uint8_t MAG_WHO_AM_I = 0x00; 279 | 280 | const uint8_t MAG_HXL = 0x11; 281 | 282 | const uint8_t MAG_CNTL2 = 0x31; 283 | const uint8_t MAG_CNTL2_POWER_DOWN = 0x00; 284 | const uint8_t MAG_CNTL2_MODE_10HZ = 0x02; 285 | const uint8_t MAG_CNTL2_MODE_50HZ = 0x06; 286 | const uint8_t MAG_CNTL2_MODE_100HZ = 0x08; 287 | 288 | const uint8_t MAG_CNTL3 = 0x32; 289 | const uint8_t MAG_CNTL3_RESET = 0x01; 290 | 291 | // private functions 292 | int enableI2cMaster(); 293 | int selectAutoClockSource(); 294 | int enableAccelGyro(); 295 | int reset(); 296 | int changeUserBank(UserBank userBank); 297 | int changeUserBank(UserBank userBank, bool force); 298 | int writeRegister(uint8_t subAddress, uint8_t data); 299 | int readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest); 300 | int writeMagRegister(uint8_t subAddress, uint8_t data); 301 | int readMagRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest); 302 | int whoAmI(); 303 | int whoAmIMag(); 304 | int powerDownMag(); 305 | int resetMag(); 306 | }; 307 | 308 | #endif --------------------------------------------------------------------------------