├── .gitattributes ├── ICM20948 ├── Arduino-ICM20948.cpp ├── Arduino-ICM20948.h ├── DataConverter.c ├── Icm20948.h ├── Icm20948Augmented.c ├── Icm20948Augmented.h ├── Icm20948AuxCompassAkm.c ├── Icm20948AuxCompassAkm.h ├── Icm20948AuxTransport.c ├── Icm20948AuxTransport.h ├── Icm20948DataBaseControl.c ├── Icm20948DataBaseControl.h ├── Icm20948DataBaseDriver.c ├── Icm20948DataBaseDriver.h ├── Icm20948DataConverter.c ├── Icm20948DataConverter.h ├── Icm20948Defs.h ├── Icm20948Dmp3Driver.c ├── Icm20948Dmp3Driver.h ├── Icm20948LoadFirmware.c ├── Icm20948LoadFirmware.h ├── Icm20948MPUFifoControl.c ├── Icm20948MPUFifoControl.h ├── Icm20948SelfTest.c ├── Icm20948SelfTest.h ├── Icm20948Serif.h ├── Icm20948Setup.c ├── Icm20948Setup.h ├── Icm20948Transport.c ├── Icm20948Transport.h ├── InvBool.h ├── InvError.h ├── InvExport.h ├── Message.c ├── Message.h ├── SensorTypes.h ├── dataconverter.h ├── examples │ └── ICM20948_Arduino_DMP.ino └── icm20948_img.dmp3a.h └── README.md /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /ICM20948/Arduino-ICM20948.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | Includes 3 | *************************************************************************/ 4 | 5 | #include "Arduino-ICM20948.h" 6 | #include 7 | #include 8 | #include 9 | 10 | // InvenSense drivers and utils 11 | #include "Icm20948.h" 12 | #include "SensorTypes.h" 13 | #include "Icm20948MPUFifoControl.h" 14 | 15 | /************************************************************************* 16 | Variables 17 | *************************************************************************/ 18 | 19 | int chipSelectPin; 20 | int com_speed; 21 | 22 | uint8_t I2C_Address = 0x69; 23 | 24 | float gyro[3]; 25 | bool gyro_data_ready = false; 26 | 27 | float accel[3]; 28 | bool accel_data_ready = false; 29 | 30 | float mag[3]; 31 | bool mag_data_ready = false; 32 | 33 | float grav[3]; 34 | bool grav_data_ready = false; 35 | 36 | float lAccel[3]; 37 | bool linearAccel_data_ready = false; 38 | 39 | float quat6[4]; 40 | bool quat6_data_ready = false; 41 | 42 | float euler6[3]; 43 | bool euler6_data_ready = false; 44 | 45 | float quat9[4]; 46 | bool quat9_data_ready = false; 47 | 48 | float euler9[3]; 49 | bool euler9_data_ready = false; 50 | 51 | int har; 52 | bool har_data_ready = false; 53 | 54 | unsigned long steps; 55 | bool steps_data_ready = false; 56 | 57 | /************************************************************************* 58 | HAL Functions for Arduino 59 | *************************************************************************/ 60 | int spi_master_read_register(uint8_t reg, uint8_t* rbuffer, uint32_t rlen) 61 | { 62 | //return spi_master_transfer_rx(NULL, reg, rbuffer, rlen); 63 | 64 | SPI.beginTransaction(SPISettings(com_speed, MSBFIRST, SPI_MODE0)); 65 | SPI.transfer(0x00); 66 | SPI.endTransaction(); 67 | 68 | digitalWrite(chipSelectPin, LOW); 69 | SPI.beginTransaction(SPISettings(com_speed, MSBFIRST, SPI_MODE0)); 70 | SPI.transfer(((reg & 0x7F) | 0x80)); 71 | for (uint32_t indi = 0; indi < rlen; indi++) 72 | { 73 | *(rbuffer + indi) = SPI.transfer(0x00); 74 | } 75 | SPI.endTransaction(); 76 | digitalWrite(chipSelectPin, HIGH); 77 | 78 | return 0; 79 | } 80 | 81 | int spi_master_write_register(uint8_t reg, const uint8_t* wbuffer, uint32_t wlen) 82 | { 83 | //return spi_master_transfer_tx(NULL, reg, wbuffer, wlen); 84 | 85 | SPI.beginTransaction(SPISettings(com_speed, MSBFIRST, SPI_MODE0)); 86 | SPI.transfer(0x00); 87 | SPI.endTransaction(); 88 | 89 | digitalWrite(chipSelectPin, LOW); 90 | SPI.beginTransaction(SPISettings(com_speed, MSBFIRST, SPI_MODE0)); 91 | SPI.transfer((reg & 0x7F) | 0x00); 92 | for (uint32_t indi = 0; indi < wlen; indi++) 93 | { 94 | SPI.transfer(*(wbuffer + indi)); 95 | } 96 | SPI.endTransaction(); 97 | digitalWrite(chipSelectPin, HIGH); 98 | 99 | return 0; 100 | } 101 | 102 | int i2c_master_write_register(uint8_t address, uint8_t reg, uint32_t len, const uint8_t *data) 103 | { 104 | if (address != 0x69) 105 | { 106 | 107 | Serial.print("Odd address:"); 108 | Serial.println(address); 109 | } 110 | //Serial.print("write address "); 111 | //Serial.println(address); 112 | //Serial.print("register "); 113 | //Serial.println(reg); 114 | //Serial.print("length = "); 115 | //Serial.println(len); 116 | Wire.beginTransmission(address); 117 | Wire.write(reg); 118 | Wire.write(data, len); 119 | Wire.endTransmission(); 120 | return 0; 121 | } 122 | 123 | int i2c_master_read_register(uint8_t address, uint8_t reg, uint32_t len, uint8_t *buff) 124 | { 125 | if (address != 0x69) 126 | { 127 | 128 | Serial.print("Odd read address:"); 129 | Serial.println(address); 130 | } 131 | //Serial.print("read address "); 132 | //Serial.println(address); 133 | //Serial.print("register "); 134 | //Serial.println(reg); 135 | //Serial.print("length = "); 136 | //Serial.println(len); 137 | 138 | Wire.beginTransmission(address); 139 | Wire.write(reg); 140 | Wire.endTransmission(false); // Send repeated start 141 | 142 | uint32_t offset = 0; 143 | uint32_t num_received = Wire.requestFrom(address, len); 144 | //Serial.print("received = "); 145 | //Serial.println(num_received); 146 | //Serial.println(buff[0]); 147 | if (num_received == len) 148 | { 149 | for (uint8_t i = 0; i < len; i++) 150 | { 151 | buff[i] = Wire.read(); 152 | } 153 | return 0; 154 | } 155 | else 156 | { 157 | return -1; 158 | } 159 | } 160 | 161 | 162 | /************************************************************************* 163 | Invensense Variables 164 | *************************************************************************/ 165 | 166 | inv_icm20948_t icm_device; 167 | int rc = 0; 168 | static const uint8_t EXPECTED_WHOAMI[] = { 0xEA }; /* WHOAMI value for ICM20948 or derivative */ 169 | #define AK0991x_DEFAULT_I2C_ADDR 0x0C 170 | #define AK0991x_SECONDARY_I2C_ADDR 0x0E /* The secondary I2C address for AK0991x Magnetometers */ 171 | 172 | #define ICM_I2C_ADDR_REVA 0x68 /* I2C slave address for INV device on Rev A board */ 173 | #define ICM_I2C_ADDR_REVB 0x69 /* I2C slave address for INV device on Rev B board */ 174 | 175 | #define AD0_VAL 1 // The value of the last bit of the I2C address. 176 | 177 | 178 | #define THREE_AXES 3 179 | static int unscaled_bias[THREE_AXES * 2]; 180 | 181 | static const float cfg_mounting_matrix[9] = { 182 | 1.f, 0, 0, 183 | 0, 1.f, 0, 184 | 0, 0, 1.f 185 | }; 186 | 187 | int32_t cfg_acc_fsr = 4; // Default = +/- 4g. Valid ranges: 2, 4, 8, 16 188 | int32_t cfg_gyr_fsr = 2000; // Default = +/- 2000dps. Valid ranges: 250, 500, 1000, 2000 189 | 190 | static const uint8_t dmp3_image[] = { 191 | #include "icm20948_img.dmp3a.h" 192 | }; 193 | 194 | /************************************************************************* 195 | Invensense Functions 196 | *************************************************************************/ 197 | 198 | void check_rc(int rc, const char * msg_context) 199 | { 200 | if (rc < 0) { 201 | Serial.println("ICM20948 ERROR!"); 202 | while (1); 203 | } 204 | } 205 | 206 | int load_dmp3(void) 207 | { 208 | int rc = 0; 209 | rc = inv_icm20948_load(&icm_device, dmp3_image, sizeof(dmp3_image)); 210 | return rc; 211 | } 212 | 213 | void inv_icm20948_sleep_us(int us) 214 | { 215 | delayMicroseconds(us); 216 | } 217 | 218 | void inv_icm20948_sleep(int ms) 219 | { 220 | delay(ms); 221 | } 222 | 223 | uint64_t inv_icm20948_get_time_us(void) 224 | { 225 | return micros(); 226 | } 227 | 228 | 229 | void initiliaze_SPI(void) 230 | { 231 | pinMode(chipSelectPin, OUTPUT); 232 | digitalWrite(chipSelectPin, HIGH); 233 | SPI.begin(); 234 | 235 | SPI.beginTransaction(SPISettings(com_speed, MSBFIRST, SPI_MODE0)); 236 | SPI.transfer(0x00); 237 | SPI.endTransaction(); 238 | } 239 | void initiliaze_I2C(void) 240 | { 241 | Wire.begin(); 242 | Wire.setClock(com_speed); 243 | } 244 | 245 | bool is_interface_SPI = false; 246 | void set_comm_interface(ArduinoICM20948Settings settings) 247 | { 248 | chipSelectPin = settings.cs_pin; 249 | is_interface_SPI = settings.is_SPI; 250 | if (is_interface_SPI) 251 | { 252 | com_speed = settings.spi_speed; 253 | initiliaze_SPI(); 254 | } 255 | else 256 | { 257 | com_speed = settings.i2c_speed; 258 | //initiliaze_I2C(); 259 | } 260 | 261 | } 262 | inv_bool_t interface_is_SPI(void) 263 | { 264 | return is_interface_SPI; 265 | } 266 | 267 | //--------------------------------------------------------------------- 268 | int idd_io_hal_read_reg(void *context, uint8_t reg, uint8_t *rbuffer, uint32_t rlen) 269 | { 270 | if (interface_is_SPI()) 271 | { 272 | return spi_master_read_register(reg, rbuffer, rlen); 273 | } 274 | return i2c_master_read_register(I2C_Address, reg, rlen, rbuffer); 275 | } 276 | 277 | //--------------------------------------------------------------------- 278 | 279 | int idd_io_hal_write_reg(void *context, uint8_t reg, const uint8_t *wbuffer, uint32_t wlen) 280 | { 281 | if (interface_is_SPI()) 282 | { 283 | return spi_master_write_register(reg, wbuffer, wlen); 284 | } 285 | return i2c_master_write_register(I2C_Address, reg, wlen, wbuffer); 286 | } 287 | 288 | static void icm20948_apply_mounting_matrix(void) 289 | { 290 | int ii; 291 | 292 | for (ii = 0; ii < INV_ICM20948_SENSOR_MAX; ii++) { 293 | inv_icm20948_set_matrix(&icm_device, cfg_mounting_matrix, (inv_icm20948_sensor)ii); 294 | } 295 | } 296 | 297 | static void icm20948_set_fsr(void) 298 | { 299 | inv_icm20948_set_fsr(&icm_device, INV_ICM20948_SENSOR_RAW_ACCELEROMETER, (const void *)&cfg_acc_fsr); 300 | inv_icm20948_set_fsr(&icm_device, INV_ICM20948_SENSOR_ACCELEROMETER, (const void *)&cfg_acc_fsr); 301 | inv_icm20948_set_fsr(&icm_device, INV_ICM20948_SENSOR_RAW_GYROSCOPE, (const void *)&cfg_gyr_fsr); 302 | inv_icm20948_set_fsr(&icm_device, INV_ICM20948_SENSOR_GYROSCOPE, (const void *)&cfg_gyr_fsr); 303 | inv_icm20948_set_fsr(&icm_device, INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED, (const void *)&cfg_gyr_fsr); 304 | } 305 | 306 | int icm20948_sensor_setup(void) 307 | { 308 | 309 | int rc; 310 | uint8_t i, whoami = 0xff; 311 | 312 | inv_icm20948_soft_reset(&icm_device); 313 | 314 | // Get whoami number 315 | rc = inv_icm20948_get_whoami(&icm_device, &whoami); 316 | 317 | // Check if WHOAMI value corresponds to any value from EXPECTED_WHOAMI array 318 | for (i = 0; i < sizeof(EXPECTED_WHOAMI) / sizeof(EXPECTED_WHOAMI[0]); ++i) { 319 | 320 | if (whoami == EXPECTED_WHOAMI[i]) { 321 | break; 322 | } 323 | } 324 | 325 | if (i == sizeof(EXPECTED_WHOAMI) / sizeof(EXPECTED_WHOAMI[0])) { 326 | Serial.print("Bad WHOAMI value = 0x"); 327 | Serial.println(whoami, HEX); 328 | return rc; 329 | } 330 | 331 | // Setup accel and gyro mounting matrix and associated angle for current board 332 | inv_icm20948_init_matrix(&icm_device); 333 | 334 | // set default power mode 335 | rc = inv_icm20948_initialize(&icm_device, dmp3_image, sizeof(dmp3_image)); 336 | if (rc != 0) { 337 | Serial.println("Icm20948 Initialization failed."); 338 | return rc; 339 | } 340 | 341 | // Configure and initialize the ICM20948 for normal use 342 | 343 | // Initialize auxiliary sensors 344 | inv_icm20948_register_aux_compass( &icm_device, INV_ICM20948_COMPASS_ID_AK09916, AK0991x_DEFAULT_I2C_ADDR); 345 | rc = inv_icm20948_initialize_auxiliary(&icm_device); 346 | if (rc == -1) { 347 | Serial.println("Compass not detected..."); 348 | } 349 | 350 | icm20948_apply_mounting_matrix(); 351 | 352 | icm20948_set_fsr(); 353 | 354 | // re-initialize base state structure 355 | inv_icm20948_init_structure(&icm_device); 356 | 357 | return 0; 358 | } 359 | 360 | static uint8_t icm20948_get_grv_accuracy(void) 361 | { 362 | uint8_t accel_accuracy; 363 | uint8_t gyro_accuracy; 364 | 365 | accel_accuracy = (uint8_t)inv_icm20948_get_accel_accuracy(); 366 | gyro_accuracy = (uint8_t)inv_icm20948_get_gyro_accuracy(); 367 | return (min(accel_accuracy, gyro_accuracy)); 368 | } 369 | 370 | static uint8_t convert_to_generic_ids[INV_ICM20948_SENSOR_MAX] = { 371 | INV_SENSOR_TYPE_ACCELEROMETER, 372 | INV_SENSOR_TYPE_GYROSCOPE, 373 | INV_SENSOR_TYPE_RAW_ACCELEROMETER, 374 | INV_SENSOR_TYPE_RAW_GYROSCOPE, 375 | INV_SENSOR_TYPE_UNCAL_MAGNETOMETER, 376 | INV_SENSOR_TYPE_UNCAL_GYROSCOPE, 377 | INV_SENSOR_TYPE_BAC, 378 | INV_SENSOR_TYPE_STEP_DETECTOR, 379 | INV_SENSOR_TYPE_STEP_COUNTER, 380 | INV_SENSOR_TYPE_GAME_ROTATION_VECTOR, 381 | INV_SENSOR_TYPE_ROTATION_VECTOR, 382 | INV_SENSOR_TYPE_GEOMAG_ROTATION_VECTOR, 383 | INV_SENSOR_TYPE_MAGNETOMETER, 384 | INV_SENSOR_TYPE_SMD, 385 | INV_SENSOR_TYPE_PICK_UP_GESTURE, 386 | INV_SENSOR_TYPE_TILT_DETECTOR, 387 | INV_SENSOR_TYPE_GRAVITY, 388 | INV_SENSOR_TYPE_LINEAR_ACCELERATION, 389 | INV_SENSOR_TYPE_ORIENTATION, 390 | INV_SENSOR_TYPE_B2S 391 | }; 392 | 393 | void build_sensor_event_data(void * context, enum inv_icm20948_sensor sensortype, uint64_t timestamp, const void * data, const void *arg) 394 | { 395 | float raw_bias_data[6]; 396 | inv_sensor_event_t event; 397 | (void)context; 398 | uint8_t sensor_id = convert_to_generic_ids[sensortype]; 399 | 400 | memset((void *)&event, 0, sizeof(event)); 401 | event.sensor = sensor_id; 402 | event.timestamp = timestamp; 403 | switch (sensor_id) 404 | { 405 | case INV_SENSOR_TYPE_UNCAL_GYROSCOPE: 406 | memcpy(raw_bias_data, data, sizeof(raw_bias_data)); 407 | memcpy(event.data.gyr.vect, &raw_bias_data[0], sizeof(event.data.gyr.vect)); 408 | memcpy(event.data.gyr.bias, &raw_bias_data[3], sizeof(event.data.gyr.bias)); 409 | memcpy(&(event.data.gyr.accuracy_flag), arg, sizeof(event.data.gyr.accuracy_flag)); 410 | break; 411 | case INV_SENSOR_TYPE_UNCAL_MAGNETOMETER: 412 | memcpy(raw_bias_data, data, sizeof(raw_bias_data)); 413 | memcpy(event.data.mag.vect, &raw_bias_data[0], sizeof(event.data.mag.vect)); 414 | memcpy(event.data.mag.bias, &raw_bias_data[3], sizeof(event.data.mag.bias)); 415 | memcpy(&(event.data.gyr.accuracy_flag), arg, sizeof(event.data.gyr.accuracy_flag)); 416 | break; 417 | case INV_SENSOR_TYPE_GYROSCOPE: 418 | memcpy(event.data.gyr.vect, data, sizeof(event.data.gyr.vect)); 419 | memcpy(&(event.data.gyr.accuracy_flag), arg, sizeof(event.data.gyr.accuracy_flag)); 420 | 421 | // WE WANT THIS 422 | gyro[0] = event.data.gyr.vect[0]; 423 | gyro[1] = event.data.gyr.vect[1]; 424 | gyro[2] = event.data.gyr.vect[2]; 425 | gyro_data_ready = true; 426 | break; 427 | 428 | case INV_SENSOR_TYPE_GRAVITY: 429 | memcpy(event.data.grav.vect, data, sizeof(event.data.grav.vect)); 430 | event.data.grav.accuracy_flag = inv_icm20948_get_accel_accuracy(); 431 | 432 | grav[0] = event.data.grav.vect[0]; 433 | grav[1] = event.data.grav.vect[1]; 434 | grav[2] = event.data.grav.vect[2]; 435 | grav_data_ready = true; 436 | //add gravity 437 | break; 438 | case INV_SENSOR_TYPE_LINEAR_ACCELERATION: 439 | memcpy(event.data.linAcc.vect, data, sizeof(event.data.linAcc.vect)); 440 | memcpy(&(event.data.linAcc.accuracy_flag), arg, sizeof(event.data.linAcc.accuracy_flag)); 441 | 442 | // WE WANT THIS 443 | lAccel[0] = event.data.linAcc.vect[0]; 444 | lAccel[1] = event.data.linAcc.vect[1]; 445 | lAccel[2] = event.data.linAcc.vect[2]; 446 | linearAccel_data_ready = true; 447 | break; 448 | //add linear acceleration 449 | case INV_SENSOR_TYPE_ACCELEROMETER: 450 | memcpy(event.data.acc.vect, data, sizeof(event.data.acc.vect)); 451 | memcpy(&(event.data.acc.accuracy_flag), arg, sizeof(event.data.acc.accuracy_flag)); 452 | 453 | // WE WANT THIS 454 | accel[0] = event.data.acc.vect[0]; 455 | accel[1] = event.data.acc.vect[1]; 456 | accel[2] = event.data.acc.vect[2]; 457 | accel_data_ready = true; 458 | break; 459 | 460 | case INV_SENSOR_TYPE_MAGNETOMETER: 461 | memcpy(event.data.mag.vect, data, sizeof(event.data.mag.vect)); 462 | memcpy(&(event.data.mag.accuracy_flag), arg, sizeof(event.data.mag.accuracy_flag)); 463 | 464 | // WE WANT THIS 465 | mag[0] = event.data.mag.vect[0]; 466 | mag[1] = event.data.mag.vect[1]; 467 | mag[2] = event.data.mag.vect[2]; 468 | mag_data_ready = true; 469 | break; 470 | 471 | case INV_SENSOR_TYPE_GEOMAG_ROTATION_VECTOR: 472 | case INV_SENSOR_TYPE_ROTATION_VECTOR: 473 | memcpy(&(event.data.quaternion9DOF.accuracy), arg, sizeof(event.data.quaternion9DOF.accuracy)); 474 | memcpy(event.data.quaternion9DOF.quat, data, sizeof(event.data.quaternion9DOF.quat)); 475 | quat9[0] = event.data.quaternion9DOF.quat[0]; 476 | quat9[1] = event.data.quaternion9DOF.quat[1]; 477 | quat9[2] = event.data.quaternion9DOF.quat[2]; 478 | quat9[3] = event.data.quaternion9DOF.quat[3]; 479 | quat9_data_ready = true; 480 | euler9_data_ready = true; 481 | break; 482 | case INV_SENSOR_TYPE_GAME_ROTATION_VECTOR: 483 | memcpy(event.data.quaternion6DOF.quat, data, sizeof(event.data.quaternion6DOF.quat)); 484 | event.data.quaternion6DOF.accuracy_flag = icm20948_get_grv_accuracy(); 485 | 486 | // WE WANT THIS 487 | quat6[0] = event.data.quaternion6DOF.quat[0]; 488 | quat6[1] = event.data.quaternion6DOF.quat[1]; 489 | quat6[2] = event.data.quaternion6DOF.quat[2]; 490 | quat6[3] = event.data.quaternion6DOF.quat[3]; 491 | quat6_data_ready = true; 492 | euler6_data_ready = true; 493 | break; 494 | 495 | case INV_SENSOR_TYPE_BAC: 496 | memcpy(&(event.data.bac.event), data, sizeof(event.data.bac.event)); 497 | 498 | har = event.data.bac.event; 499 | har_data_ready = true; 500 | break; 501 | case INV_SENSOR_TYPE_PICK_UP_GESTURE: 502 | case INV_SENSOR_TYPE_TILT_DETECTOR: 503 | case INV_SENSOR_TYPE_STEP_DETECTOR: 504 | case INV_SENSOR_TYPE_SMD: 505 | event.data.event = true; 506 | break; 507 | case INV_SENSOR_TYPE_B2S: 508 | event.data.event = true; 509 | memcpy(&(event.data.b2s.direction), data, sizeof(event.data.b2s.direction)); 510 | break; 511 | case INV_SENSOR_TYPE_STEP_COUNTER: 512 | memcpy(&(event.data.step.count), data, sizeof(event.data.step.count)); 513 | 514 | steps = event.data.step.count; 515 | steps_data_ready = true; 516 | break; 517 | case INV_SENSOR_TYPE_ORIENTATION: 518 | //we just want to copy x,y,z from orientation data 519 | memcpy(&(event.data.orientation), data, 3 * sizeof(float)); 520 | break; 521 | case INV_SENSOR_TYPE_RAW_ACCELEROMETER: 522 | case INV_SENSOR_TYPE_RAW_GYROSCOPE: 523 | memcpy(event.data.raw3d.vect, data, sizeof(event.data.raw3d.vect)); 524 | break; 525 | default: 526 | return; 527 | } 528 | } 529 | 530 | static enum inv_icm20948_sensor idd_sensortype_conversion(int sensor) 531 | { 532 | switch (sensor) 533 | { 534 | case INV_SENSOR_TYPE_RAW_ACCELEROMETER: 535 | return INV_ICM20948_SENSOR_RAW_ACCELEROMETER; 536 | case INV_SENSOR_TYPE_RAW_GYROSCOPE: 537 | return INV_ICM20948_SENSOR_RAW_GYROSCOPE; 538 | case INV_SENSOR_TYPE_ACCELEROMETER: 539 | return INV_ICM20948_SENSOR_ACCELEROMETER; 540 | case INV_SENSOR_TYPE_GYROSCOPE: 541 | return INV_ICM20948_SENSOR_GYROSCOPE; 542 | case INV_SENSOR_TYPE_UNCAL_MAGNETOMETER: 543 | return INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED; 544 | case INV_SENSOR_TYPE_UNCAL_GYROSCOPE: 545 | return INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED; 546 | case INV_SENSOR_TYPE_BAC: 547 | return INV_ICM20948_SENSOR_ACTIVITY_CLASSIFICATON; 548 | case INV_SENSOR_TYPE_STEP_DETECTOR: 549 | return INV_ICM20948_SENSOR_STEP_DETECTOR; 550 | case INV_SENSOR_TYPE_STEP_COUNTER: 551 | return INV_ICM20948_SENSOR_STEP_COUNTER; 552 | case INV_SENSOR_TYPE_GAME_ROTATION_VECTOR: 553 | return INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR; 554 | case INV_SENSOR_TYPE_ROTATION_VECTOR: 555 | return INV_ICM20948_SENSOR_ROTATION_VECTOR; 556 | case INV_SENSOR_TYPE_GEOMAG_ROTATION_VECTOR: 557 | return INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR; 558 | case INV_SENSOR_TYPE_MAGNETOMETER: 559 | return INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD; 560 | case INV_SENSOR_TYPE_SMD: 561 | return INV_ICM20948_SENSOR_WAKEUP_SIGNIFICANT_MOTION; 562 | case INV_SENSOR_TYPE_PICK_UP_GESTURE: 563 | return INV_ICM20948_SENSOR_FLIP_PICKUP; 564 | case INV_SENSOR_TYPE_TILT_DETECTOR: 565 | return INV_ICM20948_SENSOR_WAKEUP_TILT_DETECTOR; 566 | case INV_SENSOR_TYPE_GRAVITY: 567 | return INV_ICM20948_SENSOR_GRAVITY; 568 | case INV_SENSOR_TYPE_LINEAR_ACCELERATION: 569 | return INV_ICM20948_SENSOR_LINEAR_ACCELERATION; 570 | case INV_SENSOR_TYPE_ORIENTATION: 571 | return INV_ICM20948_SENSOR_ORIENTATION; 572 | case INV_SENSOR_TYPE_B2S: 573 | return INV_ICM20948_SENSOR_B2S; 574 | default: 575 | return INV_ICM20948_SENSOR_MAX; 576 | } 577 | } 578 | 579 | /************************************************************************* 580 | Class Functions 581 | *************************************************************************/ 582 | 583 | ArduinoICM20948::ArduinoICM20948() 584 | { 585 | } 586 | 587 | void ArduinoICM20948::init(ArduinoICM20948Settings settings) 588 | { 589 | set_comm_interface(settings); 590 | Serial.println("Initializing ICM-20948..."); 591 | 592 | // Initialize icm20948 serif structure 593 | struct inv_icm20948_serif icm20948_serif; 594 | icm20948_serif.context = 0; // no need 595 | icm20948_serif.read_reg = idd_io_hal_read_reg; 596 | icm20948_serif.write_reg = idd_io_hal_write_reg; 597 | icm20948_serif.max_read = 1024 * 16; // maximum number of bytes allowed per serial read 598 | icm20948_serif.max_write = 1024 * 16; // maximum number of bytes allowed per serial write 599 | icm20948_serif.is_spi = interface_is_SPI(); 600 | 601 | // Reset icm20948 driver states 602 | inv_icm20948_reset_states(&icm_device, &icm20948_serif); 603 | inv_icm20948_register_aux_compass(&icm_device, INV_ICM20948_COMPASS_ID_AK09916, AK0991x_DEFAULT_I2C_ADDR); 604 | 605 | // Setup the icm20948 device 606 | rc = icm20948_sensor_setup(); 607 | 608 | if (icm_device.selftest_done && !icm_device.offset_done) 609 | { 610 | // If we've run self test and not already set the offset. 611 | inv_icm20948_set_offset(&icm_device, unscaled_bias); 612 | icm_device.offset_done = 1; 613 | } 614 | 615 | // Now that Icm20948 device is initialized, we can proceed with DMP image loading 616 | // This step is mandatory as DMP image is not stored in non volatile memory 617 | rc += load_dmp3(); 618 | check_rc(rc, "Error sensor_setup/DMP loading."); 619 | 620 | // Set mode 621 | inv_icm20948_set_lowpower_or_highperformance(&icm_device, settings.mode); 622 | 623 | // Set frequency 624 | rc = inv_icm20948_set_sensor_period(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_GYROSCOPE), 1000 / settings.gyroscope_frequency); 625 | rc = inv_icm20948_set_sensor_period(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_ACCELEROMETER), 1000 / settings.accelerometer_frequency); 626 | rc = inv_icm20948_set_sensor_period(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_MAGNETOMETER), 1000 / settings.magnetometer_frequency); 627 | rc = inv_icm20948_set_sensor_period(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_GAME_ROTATION_VECTOR), 1000 / settings.quaternion6_frequency); 628 | rc = inv_icm20948_set_sensor_period(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_ROTATION_VECTOR), 1000 / settings.quaternion9_frequency); 629 | rc = inv_icm20948_set_sensor_period(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_GRAVITY), 1000 / settings.gravity_frequency); 630 | rc = inv_icm20948_set_sensor_period(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_LINEAR_ACCELERATION), 1000 / settings.linearAcceleration_frequency); 631 | rc = inv_icm20948_set_sensor_period(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_BAC), 1000 / settings.har_frequency); 632 | rc = inv_icm20948_set_sensor_period(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_STEP_COUNTER), 1000 / settings.steps_frequency); 633 | 634 | 635 | 636 | // Enable / disable 637 | rc = inv_icm20948_enable_sensor(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_GYROSCOPE), settings.enable_gyroscope); 638 | rc = inv_icm20948_enable_sensor(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_ACCELEROMETER), settings.enable_accelerometer); 639 | rc = inv_icm20948_enable_sensor(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_MAGNETOMETER), settings.enable_magnetometer); 640 | rc = inv_icm20948_enable_sensor(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_GAME_ROTATION_VECTOR), settings.enable_quaternion6); 641 | rc = inv_icm20948_enable_sensor(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_ROTATION_VECTOR), settings.enable_quaternion9); 642 | rc = inv_icm20948_enable_sensor(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_GRAVITY), settings.enable_gravity); 643 | rc = inv_icm20948_enable_sensor(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_LINEAR_ACCELERATION), settings.enable_linearAcceleration); 644 | rc = inv_icm20948_enable_sensor(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_BAC), settings.enable_har); 645 | rc = inv_icm20948_enable_sensor(&icm_device, idd_sensortype_conversion(INV_SENSOR_TYPE_STEP_COUNTER), settings.enable_steps); 646 | } 647 | 648 | 649 | 650 | void ArduinoICM20948::task() 651 | { 652 | inv_icm20948_poll_sensor(&icm_device, (void*)0, build_sensor_event_data); 653 | } 654 | 655 | bool ArduinoICM20948::gyroDataIsReady() 656 | { 657 | return gyro_data_ready; 658 | } 659 | 660 | bool ArduinoICM20948::accelDataIsReady() 661 | { 662 | return accel_data_ready; 663 | } 664 | 665 | 666 | bool ArduinoICM20948::magDataIsReady() 667 | { 668 | return mag_data_ready; 669 | } 670 | 671 | bool ArduinoICM20948::linearAccelDataIsReady() 672 | { 673 | return linearAccel_data_ready; 674 | } 675 | 676 | bool ArduinoICM20948::gravDataIsReady() 677 | { 678 | return grav_data_ready; 679 | } 680 | 681 | bool ArduinoICM20948::quat6DataIsReady() 682 | { 683 | return quat6_data_ready; 684 | } 685 | 686 | bool ArduinoICM20948::euler6DataIsReady() 687 | { 688 | return euler6_data_ready; 689 | } 690 | 691 | bool ArduinoICM20948::quat9DataIsReady() 692 | { 693 | return quat9_data_ready; 694 | } 695 | 696 | bool ArduinoICM20948::euler9DataIsReady() 697 | { 698 | return euler9_data_ready; 699 | } 700 | 701 | bool ArduinoICM20948::harDataIsReady() 702 | { 703 | return har_data_ready; 704 | } 705 | 706 | bool ArduinoICM20948::stepsDataIsReady() 707 | { 708 | return steps_data_ready; 709 | } 710 | 711 | void ArduinoICM20948::readGyroData(float *x, float *y, float *z) 712 | { 713 | *x = gyro[0]; 714 | *y = gyro[1]; 715 | *z = gyro[2]; 716 | gyro_data_ready = false; 717 | } 718 | 719 | void ArduinoICM20948::readAccelData(float *x, float *y, float *z) 720 | { 721 | *x = accel[0]; 722 | *y = accel[1]; 723 | *z = accel[2]; 724 | accel_data_ready = false; 725 | } 726 | 727 | void ArduinoICM20948::readMagData(float *x, float *y, float *z) 728 | { 729 | *x = mag[0]; 730 | *y = mag[1]; 731 | *z = mag[2]; 732 | mag_data_ready = false; 733 | } 734 | 735 | void ArduinoICM20948::readLinearAccelData(float* x, float* y, float* z) 736 | { 737 | *x = lAccel[0]; 738 | *y = lAccel[1]; 739 | *z = lAccel[2]; 740 | linearAccel_data_ready = false; 741 | } 742 | 743 | void ArduinoICM20948::readGravData(float* x, float* y, float* z) 744 | { 745 | *x = grav[0]; 746 | *y = grav[1]; 747 | *z = grav[2]; 748 | grav_data_ready = false; 749 | } 750 | 751 | void ArduinoICM20948::readQuat6Data(float *w, float *x, float *y, float *z) 752 | { 753 | *w = quat6[0]; 754 | *x = quat6[1]; 755 | *y = quat6[2]; 756 | *z = quat6[3]; 757 | quat6_data_ready = false; 758 | } 759 | 760 | void ArduinoICM20948::readEuler6Data(float *roll, float *pitch, float *yaw) 761 | { 762 | *roll = (atan2f(quat6[0]*quat6[1] + quat6[2]*quat6[3], 0.5f - quat6[1]*quat6[1] - quat6[2]*quat6[2]))* 57.29578f; 763 | *pitch = (asinf(-2.0f * (quat6[1]*quat6[3] - quat6[0]*quat6[2])))* 57.29578f; 764 | *yaw = (atan2f(quat6[1]*quat6[2] + quat6[0]*quat6[3], 0.5f - quat6[2]*quat6[2] - quat6[3]*quat6[3]))* 57.29578f + 180.0f; 765 | euler6_data_ready = false; 766 | } 767 | 768 | void ArduinoICM20948::readQuat9Data(float* w, float* x, float* y, float* z) 769 | { 770 | *w = quat9[0]; 771 | *x = quat9[1]; 772 | *y = quat9[2]; 773 | *z = quat9[3]; 774 | quat9_data_ready = false; 775 | } 776 | 777 | void ArduinoICM20948::readEuler9Data(float* roll, float* pitch, float* yaw) 778 | { 779 | *roll = (atan2f(quat9[0] * quat9[1] + quat9[2] * quat9[3], 0.5f - quat9[1] * quat9[1] - quat9[2] * quat9[2])) * 57.29578f; 780 | *pitch = (asinf(-2.0f * (quat9[1] * quat9[3] - quat9[0] * quat9[2]))) * 57.29578f; 781 | *yaw = (atan2f(quat9[1] * quat9[2] + quat9[0] * quat9[3], 0.5f - quat9[2] * quat9[2] - quat9[3] * quat9[3])) * 57.29578f + 180.0f; 782 | euler9_data_ready = false; 783 | } 784 | 785 | void ArduinoICM20948::readHarData(char* activity) 786 | { 787 | 788 | char temp = 'n'; 789 | switch (har) 790 | { 791 | case 1: 792 | temp = 'd'; 793 | break; 794 | case 2: 795 | temp = 'w'; 796 | break; 797 | case 3: 798 | temp = 'r'; 799 | break; 800 | case 4: 801 | temp = 'b'; 802 | break; 803 | case 5: 804 | temp = 't'; 805 | break; 806 | case 6: 807 | temp = 's'; 808 | break; 809 | } 810 | *activity = temp; 811 | har_data_ready = false; 812 | } 813 | 814 | void ArduinoICM20948::readStepsData(unsigned long* step_count) 815 | { 816 | *step_count = steps; 817 | steps_data_ready = false; 818 | } 819 | -------------------------------------------------------------------------------- /ICM20948/Arduino-ICM20948.h: -------------------------------------------------------------------------------- 1 | #ifndef __Arduino_ICM20948_H__ 2 | #define __Arduino_ICM20948_H__ 3 | 4 | /************************************************************************* 5 | Defines 6 | *************************************************************************/ 7 | 8 | typedef struct { 9 | int i2c_speed; 10 | bool is_SPI; 11 | int cs_pin; 12 | int spi_speed; 13 | int mode; 14 | bool enable_gyroscope; 15 | bool enable_accelerometer; 16 | bool enable_magnetometer; 17 | bool enable_gravity; 18 | bool enable_linearAcceleration; 19 | bool enable_quaternion6; 20 | bool enable_quaternion9; 21 | bool enable_har; 22 | bool enable_steps; 23 | int gyroscope_frequency; 24 | int accelerometer_frequency; 25 | int magnetometer_frequency; 26 | int gravity_frequency; 27 | int linearAcceleration_frequency; 28 | int quaternion6_frequency; 29 | int quaternion9_frequency; 30 | int har_frequency; 31 | int steps_frequency; 32 | 33 | } ArduinoICM20948Settings; 34 | 35 | /************************************************************************* 36 | Class 37 | *************************************************************************/ 38 | 39 | class ArduinoICM20948 40 | { 41 | public: 42 | 43 | ArduinoICM20948(); 44 | 45 | //void init(TwoWire *theWire = &Wire, JTICM20948Settings settings); 46 | void init(ArduinoICM20948Settings settings); 47 | void task(); 48 | 49 | bool gyroDataIsReady(); 50 | bool accelDataIsReady(); 51 | bool magDataIsReady(); 52 | bool gravDataIsReady(); 53 | bool linearAccelDataIsReady(); 54 | bool quat6DataIsReady(); 55 | bool euler6DataIsReady(); 56 | bool quat9DataIsReady(); 57 | bool euler9DataIsReady(); 58 | bool harDataIsReady(); 59 | bool stepsDataIsReady(); 60 | 61 | void readGyroData(float *x, float *y, float *z); 62 | void readAccelData(float *x, float *y, float *z); 63 | void readMagData(float *x, float *y, float *z); 64 | void readGravData(float* x, float* y, float* z); 65 | void readLinearAccelData(float* x, float* y, float* z); 66 | void readQuat6Data(float *w, float *x, float *y, float *z); 67 | void readEuler6Data(float *roll, float *pitch, float *yaw); 68 | void readQuat9Data(float* w, float* x, float* y, float* z); 69 | void readEuler9Data(float* roll, float* pitch, float* yaw); 70 | void readHarData(char* activity); 71 | void readStepsData(unsigned long* steps_count); 72 | }; 73 | 74 | 75 | #endif -------------------------------------------------------------------------------- /ICM20948/DataConverter.c: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | #include "DataConverter.h" 25 | 26 | uint8_t * inv_dc_int32_to_little8(int32_t x, uint8_t * little8) 27 | { 28 | little8[3] = (uint8_t)((x >> 24) & 0xff); 29 | little8[2] = (uint8_t)((x >> 16) & 0xff); 30 | little8[1] = (uint8_t)((x >> 8) & 0xff); 31 | little8[0] = (uint8_t)(x & 0xff); 32 | 33 | return little8; 34 | } 35 | 36 | uint8_t * inv_dc_int16_to_little8(int16_t x, uint8_t * little8) 37 | { 38 | little8[0] = (uint8_t)(x & 0xff); 39 | little8[1] = (uint8_t)((x >> 8) & 0xff); 40 | 41 | return little8; 42 | } 43 | 44 | uint8_t * inv_dc_int32_to_big8(int32_t x, uint8_t * big8) 45 | { 46 | big8[0] = (uint8_t)((x >> 24) & 0xff); 47 | big8[1] = (uint8_t)((x >> 16) & 0xff); 48 | big8[2] = (uint8_t)((x >> 8) & 0xff); 49 | big8[3] = (uint8_t)(x & 0xff); 50 | 51 | return big8; 52 | } 53 | 54 | uint8_t * inv_dc_int16_to_big8(int16_t x, uint8_t * big8) 55 | { 56 | big8[0] = (uint8_t)((x >> 8) & 0xff); 57 | big8[1] = (uint8_t)(x & 0xff); 58 | 59 | return big8; 60 | } 61 | 62 | int32_t inv_dc_little8_to_int32(const uint8_t * little8) 63 | { 64 | int32_t x = 0; 65 | 66 | x |= ((int32_t)little8[3] << 24); 67 | x |= ((int32_t)little8[2] << 16); 68 | x |= ((int32_t)little8[1] << 8); 69 | x |= ((int32_t)little8[0]); 70 | 71 | return x; 72 | } 73 | 74 | int16_t inv_dc_big16_to_int16(uint8_t * data) 75 | { 76 | int16_t result; 77 | 78 | result = (*data << 8); 79 | data++; 80 | result |= *data; 81 | 82 | return result; 83 | } 84 | 85 | int16_t inv_dc_le_to_int16(const uint8_t * little8) 86 | { 87 | uint16_t x = 0; 88 | 89 | x |= ((uint16_t)little8[0]); 90 | x |= ((uint16_t)little8[1] << 8); 91 | 92 | return (int16_t)x; 93 | } 94 | 95 | void inv_dc_sfix32_to_float(const int32_t * in, uint32_t len, uint8_t qx, float * out) 96 | { 97 | uint8_t i; 98 | 99 | for(i = 0; i < len; ++i) { 100 | out[i] = (float)in[i] / (1 << qx); 101 | } 102 | } 103 | 104 | void inv_dc_float_to_sfix32(const float * in, uint32_t len, uint8_t qx, int32_t * out) 105 | { 106 | uint8_t i; 107 | 108 | for(i = 0; i < len; ++i) { 109 | out[i] = (int32_t)((in[i] * (1 << qx)) + ((in[i] >= 0) - 0.5f)); 110 | } 111 | } 112 | -------------------------------------------------------------------------------- /ICM20948/Icm20948.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | /** @defgroup DriverIcm20948 Icm20948 driver 25 | * @brief Low-level driver for ICM20948 devices 26 | * @ingroup Drivers 27 | * @{ 28 | */ 29 | 30 | #ifndef _INV_ICM20948_H_ 31 | #define _INV_ICM20948_H_ 32 | 33 | #include "InvExport.h" 34 | #include "InvBool.h" 35 | #include "InvError.h" 36 | 37 | 38 | #include "Icm20948Setup.h" 39 | #include "Icm20948Serif.h" 40 | #include "Icm20948Transport.h" 41 | #include "Icm20948DataConverter.h" 42 | #include "Icm20948AuxCompassAkm.h" 43 | #include "Icm20948SelfTest.h" 44 | 45 | 46 | #include 47 | #include 48 | #include 49 | 50 | #ifdef __cplusplus 51 | extern "C" { 52 | #endif 53 | 54 | /** @brief States for the secondary device 55 | */ 56 | typedef enum inv_icm20948_compass_state 57 | { 58 | INV_ICM20948_COMPASS_RESET = 0, 59 | INV_ICM20948_COMPASS_INITED, 60 | INV_ICM20948_COMPASS_SETUP, 61 | }inv_icm20948_compass_state_t; 62 | 63 | /** @brief ICM20948 driver states definition 64 | */ 65 | typedef struct sensor_type_icm20948{ 66 | uint64_t odr_applied_us; 67 | uint64_t odr_us; 68 | }sensor_type_icm20948_t; 69 | 70 | typedef enum { 71 | CHIP_LOW_NOISE_ICM20948, 72 | CHIP_LOW_POWER_ICM20948, 73 | }chip_lp_ln_mode_icm20948_t; 74 | 75 | typedef struct inv_icm20948 { 76 | struct inv_icm20948_serif serif; 77 | /** @brief struct for the base_driver : this contains the Mems information */ 78 | struct base_driver_t 79 | { 80 | unsigned char wake_state; 81 | chip_lp_ln_mode_icm20948_t chip_lp_ln_mode; 82 | unsigned char pwr_mgmt_1; 83 | unsigned char pwr_mgmt_2; 84 | unsigned char user_ctrl; 85 | unsigned char gyro_div; 86 | unsigned short secondary_div; 87 | short accel_div; 88 | unsigned char gyro_averaging; 89 | unsigned char accel_averaging; 90 | uint8_t gyro_fullscale; 91 | uint8_t accel_fullscale; 92 | uint8_t lp_en_support:1; 93 | uint8_t firmware_loaded:1; 94 | uint8_t serial_interface; 95 | uint8_t timebase_correction_pll; 96 | }base_state; 97 | /* secondary device support */ 98 | struct inv_icm20948_secondary_states { 99 | struct inv_icm20948_secondary_reg { 100 | uint16_t addr; 101 | uint16_t reg; 102 | uint16_t ctrl; 103 | uint16_t d0; 104 | } slv_reg[4]; 105 | unsigned char sSavedI2cOdr; 106 | /* compass support */ 107 | uint8_t compass_sens[3]; 108 | long final_matrix[9]; 109 | const int16_t *st_upper; 110 | const int16_t *st_lower; 111 | int scale; 112 | uint8_t dmp_on; 113 | uint8_t secondary_resume_compass_state; 114 | uint8_t mode_reg_addr; 115 | int compass_chip_addr; 116 | int compass_slave_id; 117 | inv_icm20948_compass_state_t compass_state; 118 | } secondary_state; 119 | /* self test */ 120 | uint8_t selftest_done; 121 | uint8_t offset_done; 122 | uint8_t gyro_st_data[3]; 123 | uint8_t accel_st_data[3]; 124 | /* mpu fifo control */ 125 | struct fifo_info_t 126 | { 127 | int fifoError; 128 | unsigned char fifo_overflow; 129 | } fifo_info; 130 | /* interface mapping */ 131 | unsigned long sStepCounterToBeSubtracted; 132 | unsigned long sOldSteps; 133 | /* data converter */ 134 | long s_quat_chip_to_body[4]; 135 | /* base driver */ 136 | uint8_t sAllowLpEn; 137 | uint8_t s_compass_available; 138 | uint8_t s_proximity_available; 139 | /* base sensor ctrl*/ 140 | unsigned short inv_dmp_odr_dividers[37];//INV_SENSOR_NUM_MAX /!\ if the size change 141 | unsigned short inv_dmp_odr_delays[37];//INV_SENSOR_NUM_MAX /!\ if the size change 142 | unsigned short bac_on; // indicates if ANDROID_SENSOR_ACTIVITY_CLASSIFICATON is on 143 | unsigned short pickup; 144 | unsigned short bac_status; 145 | unsigned short b2s_status; 146 | unsigned short flip_pickup_status; 147 | unsigned short inv_sensor_control; 148 | unsigned short inv_sensor_control2; 149 | unsigned long inv_androidSensorsOn_mask[2] ;// Each bit corresponds to a sensor being on 150 | unsigned short inv_androidSensorsOdr_boundaries[51][2];//GENERAL_SENSORS_MAX /!\ if the size change 151 | unsigned char sGmrvIsOn; // indicates if GMRV was requested to be ON by end-user. Once this variable is set, it is either GRV or GMRV which is enabled internally 152 | unsigned short lLastHwSmplrtDividerAcc; 153 | unsigned short lLastHwSmplrtDividerGyr; 154 | unsigned char sBatchMode; 155 | uint8_t header2_count; 156 | char mems_put_to_sleep; 157 | unsigned short smd_status; 158 | unsigned short ped_int_status; 159 | unsigned short bac_request; 160 | uint8_t go_back_lp_when_odr_low; // set to 1 when we forced a switch from LP to LN mode to be able to reach 1kHz ODR, so we will need to go back to LP mode ASAP 161 | unsigned short odr_acc_ms; // ODR in ms requested for ANDROID_SENSOR_ACCELEROMETER 162 | //unsigned short odr_acc_wom_ms; // ODR in ms requested for ANDROID_SENSOR_WOM when using ACC 163 | unsigned short odr_racc_ms; // ODR in ms requested for ANDROID_SENSOR_RAW_ACCELEROMETER 164 | unsigned short odr_gyr_ms; // ODR in ms requested for ANDROID_SENSOR_GYROSCOPE_UNCALIBRATED 165 | unsigned short odr_rgyr_ms; // ODR in ms requested for ANDROID_SENSOR_RAW_GYROSCOPE 166 | int bias[9];// dmp bias [0-2]:acc,[3-5]:gyr,[6-8]:mag 167 | /* Icm20948Fifo usage */ 168 | signed char mounting_matrix[9]; 169 | signed char mounting_matrix_secondary_compass[9]; 170 | long soft_iron_matrix[9]; 171 | uint8_t skip_sample[INV_ICM20948_SENSOR_MAX+1]; 172 | uint64_t timestamp[INV_ICM20948_SENSOR_MAX+1]; 173 | uint8_t sFirstBatch[INV_ICM20948_SENSOR_MAX+1]; 174 | sensor_type_icm20948_t sensorlist[INV_ICM20948_SENSOR_MAX+1]; 175 | unsigned short saved_count; 176 | /* Icm20948Transport*/ 177 | unsigned char reg; 178 | unsigned char lastBank; 179 | unsigned char lLastBankSelected; 180 | /* augmented sensors*/ 181 | unsigned short sGravityOdrMs; 182 | unsigned short sGrvOdrMs; 183 | unsigned short sLinAccOdrMs; 184 | unsigned short sGravityWuOdrMs; 185 | unsigned short sGrvWuOdrMs; 186 | unsigned short sLinAccWuOdrMs; 187 | unsigned short sRvOdrMs; 188 | unsigned short sOriOdrMs; 189 | unsigned short sRvWuOdrMs; 190 | unsigned short sOriWuOdrMs; 191 | /* Icm20649Setup */ 192 | short set_accuracy; 193 | int new_accuracy; 194 | } inv_icm20948_t; 195 | 196 | /** @brief ICM20948 driver states singleton declaration 197 | * Because of Low-level driver limitation only one insance of the driver is allowed 198 | */ 199 | extern struct inv_icm20948 * icm20948_instance; 200 | 201 | /** @brief Hook for low-level system sleep() function to be implemented by upper layer 202 | * @param[in] ms number of millisecond the calling thread should sleep 203 | */ 204 | extern void inv_icm20948_sleep_us(int us); 205 | 206 | /** @brief Hook for low-level system time() function to be implemented by upper layer 207 | * @return monotonic timestamp in us 208 | */ 209 | extern uint64_t inv_icm20948_get_time_us(void); 210 | 211 | /** @brief Reset and initialize driver states 212 | * @param[in] s handle to driver states structure 213 | */ 214 | static inline void inv_icm20948_reset_states(struct inv_icm20948 * s, 215 | const struct inv_icm20948_serif * serif) 216 | { 217 | assert(icm20948_instance == 0); 218 | 219 | memset(s, 0, sizeof(*s)); 220 | s->serif = *serif; 221 | icm20948_instance = s; 222 | } 223 | 224 | #ifdef __cplusplus 225 | } 226 | #endif 227 | 228 | #endif /* _INV_ICM20948_H_ */ 229 | 230 | /** @} */ 231 | -------------------------------------------------------------------------------- /ICM20948/Icm20948Augmented.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948Augmented.c -------------------------------------------------------------------------------- /ICM20948/Icm20948Augmented.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948Augmented.h -------------------------------------------------------------------------------- /ICM20948/Icm20948AuxCompassAkm.c: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright © 2014-2015 InvenSense Inc. Portions Copyright © 2014-2015 Movea. All rights reserved. 4 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 5 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and 6 | * other intellectual property rights laws. 7 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 8 | * and any use, reproduction, disclosure or distribution of the Software without an express license 9 | * agreement from InvenSense is strictly prohibited. 10 | * ________________________________________________________________________________________________________ 11 | */ 12 | 13 | #include "Icm20948.h" 14 | 15 | #include "Icm20948AuxCompassAkm.h" 16 | 17 | #include "Icm20948Defs.h" 18 | #include "Icm20948DataConverter.h" 19 | #include "Icm20948AuxTransport.h" 20 | #include "Icm20948Dmp3Driver.h" 21 | 22 | /* AKM definitions */ 23 | #define REG_AKM_ID 0x00 24 | #define REG_AKM_INFO 0x01 25 | #define REG_AKM_STATUS 0x02 26 | #define REG_AKM_MEASURE_DATA 0x03 27 | #define REG_AKM_MODE 0x0A 28 | #define REG_AKM_ST_CTRL 0x0C 29 | #define REG_AKM_SENSITIVITY 0x10 30 | #define REG_AKM8963_CNTL1 0x0A 31 | 32 | #if (MEMS_CHIP == HW_ICM20648) 33 | /* AK09911 register definition */ 34 | #define REG_AK09911_DMP_READ 0x3 35 | #define REG_AK09911_STATUS1 0x10 36 | #define REG_AK09911_CNTL2 0x31 37 | #define REG_AK09911_SENSITIVITY 0x60 38 | #define REG_AK09911_MEASURE_DATA 0x11 39 | 40 | /* AK09912 register definition */ 41 | #define REG_AK09912_DMP_READ 0x3 42 | #define REG_AK09912_STATUS1 0x10 43 | #define REG_AK09912_CNTL1 0x30 44 | #define REG_AK09912_CNTL2 0x31 45 | #define REG_AK09912_SENSITIVITY 0x60 46 | #define REG_AK09912_MEASURE_DATA 0x11 47 | #endif 48 | 49 | /* AK09916 register definition */ 50 | #define REG_AK09916_DMP_READ 0x3 51 | #define REG_AK09916_STATUS1 0x10 52 | #define REG_AK09916_STATUS2 0x18 53 | #define REG_AK09916_CNTL2 0x31 54 | #define REG_AK09916_CNTL3 0x32 55 | #define REG_AK09916_MEASURE_DATA 0x11 56 | #define REG_AK09916_TEST 0x33 57 | 58 | //-- REG WIA 59 | #define DATA_AKM_ID 0x48 60 | //-- REG CNTL2 61 | #define DATA_AKM_MODE_PD 0x00 62 | #define DATA_AKM_MODE_SM 0x01 63 | #define DATA_AKM_MODE_ST 0x08 64 | #define DATA_AK09911_MODE_ST 0x10 65 | #define DATA_AK09912_MODE_ST 0x10 66 | #define DATA_AK09916_MODE_ST 0x10 67 | #define DATA_AKM_MODE_FR 0x0F 68 | #define DATA_AK09911_MODE_FR 0x1F 69 | #define DATA_AK09912_MODE_FR 0x1F 70 | // AK09916 doesn't support Fuse ROM access 71 | #define DATA_AKM_SELF_TEST 0x40 72 | //-- REG Status 1 73 | #define DATA_AKM_DRDY 0x01 74 | #define DATA_AKM9916_DOR 0x01 75 | #define DATA_AKM8963_BIT 0x10 76 | 77 | #if (MEMS_CHIP == HW_ICM20648) 78 | /* 0.3 uT * (1 << 30) */ 79 | #define DATA_AKM8975_SCALE 322122547 80 | /* 0.6 uT * (1 << 30) */ 81 | #define DATA_AKM8972_SCALE 644245094 82 | /* 0.6 uT * (1 << 30) */ 83 | #define DATA_AKM8963_SCALE0 644245094 84 | /* 0.6 uT * (1 << 30) */ 85 | #define DATA_AK09911_SCALE 644245094 86 | /* 0.15 uT * (1 << 30) */ 87 | #define DATA_AK09912_SCALE 161061273 88 | #endif 89 | /* 0.15 uT * (1 << 30) */ 90 | #define DATA_AKM8963_SCALE1 161061273 91 | /* 0.15 uT * (1 << 30) */ 92 | #define DATA_AK09916_SCALE 161061273 93 | 94 | #define DATA_AKM8963_SCALE_SHIFT 4 95 | #define DATA_AKM_MIN_READ_TIME (9 * NSEC_PER_MSEC) 96 | 97 | /* AK09912C NSF */ 98 | /* 0:disable, 1:Low, 2:Middle, 3:High */ 99 | #define DATA_AK9912_NSF 1 100 | #define DATA_AK9912_NSF_SHIFT 5 101 | 102 | #define DEF_ST_COMPASS_WAIT_MIN (10 * 1000) 103 | #define DEF_ST_COMPASS_WAIT_MAX (15 * 1000) 104 | #define DEF_ST_COMPASS_TRY_TIMES 10 105 | #define DEF_ST_COMPASS_8963_SHIFT 2 106 | #define DEF_ST_COMPASS_9916_SHIFT 2 107 | #define X 0 108 | #define Y 1 109 | #define Z 2 110 | 111 | /* milliseconds between each access */ 112 | #define AKM_RATE_SCALE 10 113 | 114 | #define DATA_AKM_99_BYTES_DMP 10 115 | #define DATA_AKM_89_BYTES_DMP 9 116 | 117 | #if (MEMS_CHIP == HW_ICM20648) 118 | static const short AKM8975_ST_Lower[3] = {-100, -100, -1000}; 119 | static const short AKM8975_ST_Upper[3] = {100, 100, -300}; 120 | 121 | static const short AKM8972_ST_Lower[3] = {-50, -50, -500}; 122 | static const short AKM8972_ST_Upper[3] = {50, 50, -100}; 123 | 124 | static const short AKM8963_ST_Lower[3] = {-200, -200, -3200}; 125 | static const short AKM8963_ST_Upper[3] = {200, 200, -800}; 126 | 127 | static const short AK09911_ST_Lower[3] = {-30, -30, -400}; 128 | static const short AK09911_ST_Upper[3] = {30, 30, -50}; 129 | 130 | static const short AK09912_ST_Lower[3] = {-200, -200, -1600}; 131 | static const short AK09912_ST_Upper[3] = {200, 200, -400}; 132 | #endif 133 | 134 | static const short AK09916_ST_Lower[3] = {-200, -200, -1000}; 135 | static const short AK09916_ST_Upper[3] = {200, 200, -200}; 136 | 137 | void inv_icm20948_register_aux_compass(struct inv_icm20948 * s, 138 | enum inv_icm20948_compass_id compass_id, uint8_t compass_i2c_addr) 139 | { 140 | switch(compass_id) { 141 | case INV_ICM20948_COMPASS_ID_AK09911: 142 | s->secondary_state.compass_slave_id = HW_AK09911; 143 | s->secondary_state.compass_chip_addr = compass_i2c_addr; 144 | s->secondary_state.compass_state = INV_ICM20948_COMPASS_INITED; 145 | /* initialise mounting matrix of compass to identity akm9911 */ 146 | s->mounting_matrix_secondary_compass[0] = -1 ; 147 | s->mounting_matrix_secondary_compass[4] = -1; 148 | s->mounting_matrix_secondary_compass[8] = 1; 149 | break; 150 | case INV_ICM20948_COMPASS_ID_AK09912: 151 | s->secondary_state.compass_slave_id = HW_AK09912; 152 | s->secondary_state.compass_chip_addr = compass_i2c_addr; 153 | s->secondary_state.compass_state = INV_ICM20948_COMPASS_INITED; 154 | /* initialise mounting matrix of compass to identity akm9912 */ 155 | s->mounting_matrix_secondary_compass[0] = 1 ; 156 | s->mounting_matrix_secondary_compass[4] = 1; 157 | s->mounting_matrix_secondary_compass[8] = 1; 158 | break; 159 | case INV_ICM20948_COMPASS_ID_AK08963: 160 | s->secondary_state.compass_slave_id = HW_AK8963; 161 | s->secondary_state.compass_chip_addr = compass_i2c_addr; 162 | s->secondary_state.compass_state = INV_ICM20948_COMPASS_INITED; 163 | /* initialise mounting matrix of compass to identity akm8963 */ 164 | s->mounting_matrix_secondary_compass[0] = 1; 165 | s->mounting_matrix_secondary_compass[4] = 1; 166 | s->mounting_matrix_secondary_compass[8] = 1; 167 | break; 168 | case INV_ICM20948_COMPASS_ID_AK09916: 169 | s->secondary_state.compass_slave_id = HW_AK09916; 170 | s->secondary_state.compass_chip_addr = compass_i2c_addr; 171 | s->secondary_state.compass_state = INV_ICM20948_COMPASS_INITED; 172 | /* initialise mounting matrix of compass to identity akm9916 */ 173 | s->mounting_matrix_secondary_compass[0] = 1 ; 174 | s->mounting_matrix_secondary_compass[4] = -1; 175 | s->mounting_matrix_secondary_compass[8] = -1; 176 | break; 177 | default: 178 | s->secondary_state.compass_slave_id = 0; 179 | s->secondary_state.compass_chip_addr = 0; 180 | s->secondary_state.compass_state = INV_ICM20948_COMPASS_RESET; 181 | } 182 | } 183 | 184 | /* 185 | * inv_icm20948_setup_compass_akm() - Configure akm series compass. 186 | */ 187 | int inv_icm20948_setup_compass_akm(struct inv_icm20948 * s) 188 | { 189 | int result; 190 | unsigned char data[4]; 191 | #if (MEMS_CHIP != HW_ICM20948) 192 | uint8_t sens, cmd; 193 | #endif 194 | //reset variable to initial values 195 | memset(s->secondary_state.final_matrix, 0, sizeof(s->secondary_state.final_matrix)); 196 | memset(s->secondary_state.compass_sens, 0, sizeof(s->secondary_state.compass_sens)); 197 | s->secondary_state.scale = 0; 198 | s->secondary_state.dmp_on = 1; 199 | s->secondary_state.secondary_resume_compass_state = 0; 200 | 201 | /* Read WHOAMI through I2C SLV for compass */ 202 | result = inv_icm20948_execute_read_secondary(s, COMPASS_I2C_SLV_READ, s->secondary_state.compass_chip_addr, REG_AKM_ID, 1, data); 203 | if (result) { 204 | // inv_log("Read secondary error: Compass.\r\n"); 205 | return result; 206 | } 207 | if (data[0] != DATA_AKM_ID) { 208 | // inv_log("Compass not found!!\r\n"); 209 | return -1; 210 | } 211 | // inv_log("Compass found.\r\n"); 212 | 213 | /* setup upper and lower limit of self-test */ 214 | #if (MEMS_CHIP == HW_ICM20948) 215 | s->secondary_state.st_upper = AK09916_ST_Upper; 216 | s->secondary_state.st_lower = AK09916_ST_Lower; 217 | #else 218 | if (HW_AK8975 == s->secondary_state.compass_slave_id) { 219 | s->secondary_state.st_upper = AKM8975_ST_Upper; 220 | s->secondary_state.st_lower = AKM8975_ST_Lower; 221 | } else if (HW_AK8972 == s->secondary_state.compass_slave_id) { 222 | s->secondary_state.st_upper = AKM8972_ST_Upper; 223 | s->secondary_state.st_lower = AKM8972_ST_Lower; 224 | } else if (HW_AK8963 == s->secondary_state.compass_slave_id) { 225 | s->secondary_state.st_upper = AKM8963_ST_Upper; 226 | s->secondary_state.st_lower = AKM8963_ST_Lower; 227 | } else if (HW_AK09911 == s->secondary_state.compass_slave_id) { 228 | s->secondary_state.st_upper = AK09911_ST_Upper; 229 | s->secondary_state.st_lower = AK09911_ST_Lower; 230 | } else if (HW_AK09912 == s->secondary_state.compass_slave_id) { 231 | s->secondary_state.st_upper = AK09912_ST_Upper; 232 | s->secondary_state.st_lower = AK09912_ST_Lower; 233 | } else if (HW_AK09916 == s->secondary_state.compass_slave_id) { 234 | s->secondary_state.st_upper = AK09916_ST_Upper; 235 | s->secondary_state.st_lower = AK09916_ST_Lower; 236 | } else { 237 | return -1; 238 | } 239 | #endif 240 | 241 | 242 | #if (MEMS_CHIP == HW_ICM20948) 243 | /* Read conf and configure compass through I2C SLV for compass and subsequent channel */ 244 | s->secondary_state.mode_reg_addr = REG_AK09916_CNTL2; 245 | // no sensitivity adjustment value 246 | s->secondary_state.compass_sens[0] = 128; 247 | s->secondary_state.compass_sens[1] = 128; 248 | s->secondary_state.compass_sens[2] = 128; 249 | #else 250 | /* Read conf and configure compass through I2C SLV for compass and subsequent channel */ 251 | if (HW_AK09916 == s->secondary_state.compass_slave_id) { 252 | s->secondary_state.mode_reg_addr = REG_AK09916_CNTL2; 253 | // no sensitivity adjustment value 254 | s->secondary_state.compass_sens[0] = 128; 255 | s->secondary_state.compass_sens[1] = 128; 256 | s->secondary_state.compass_sens[2] = 128; 257 | } 258 | else { 259 | // Fuse ROM access not possible for ak9916 260 | /* set AKM to Fuse ROM access mode */ 261 | if (HW_AK09911 == s->secondary_state.compass_slave_id) { 262 | s->secondary_state.mode_reg_addr = REG_AK09911_CNTL2; 263 | sens = REG_AK09911_SENSITIVITY; 264 | cmd = DATA_AK09911_MODE_FR; 265 | } else if (HW_AK09912 == s->secondary_state.compass_slave_id) { 266 | s->secondary_state.mode_reg_addr = REG_AK09912_CNTL2; 267 | sens = REG_AK09912_SENSITIVITY; 268 | cmd = DATA_AK09912_MODE_FR; 269 | } else { 270 | s->secondary_state.mode_reg_addr = REG_AKM_MODE; 271 | sens = REG_AKM_SENSITIVITY; 272 | cmd = DATA_AKM_MODE_FR; 273 | } 274 | 275 | result = inv_icm20948_read_secondary(s, COMPASS_I2C_SLV_READ, s->secondary_state.compass_chip_addr, sens, THREE_AXES); 276 | if (result) 277 | return result; 278 | // activate FUSE_ROM mode to CNTL2 279 | result = inv_icm20948_execute_write_secondary(s, COMPASS_I2C_SLV_WRITE, s->secondary_state.compass_chip_addr, 280 | s->secondary_state.mode_reg_addr, cmd); 281 | 282 | if (result) 283 | return result; 284 | // read sensitivity 285 | result = inv_icm20948_read_mems_reg(s, REG_EXT_SLV_SENS_DATA_00, THREE_AXES, s->secondary_state.compass_sens); 286 | if (result) 287 | return result; 288 | } 289 | //aply noise suppression filter (only available for 9912) 290 | if (HW_AK09912 == s->secondary_state.compass_slave_id) { 291 | result = inv_icm20948_execute_write_secondary(s, COMPASS_I2C_SLV_WRITE, s->secondary_state.compass_chip_addr, REG_AK09912_CNTL1, 292 | DATA_AK9912_NSF << DATA_AK9912_NSF_SHIFT); 293 | if (result) 294 | return result; 295 | } 296 | #endif 297 | /* Set compass in power down through I2C SLV for compass */ 298 | result = inv_icm20948_execute_write_secondary(s, COMPASS_I2C_SLV_WRITE, s->secondary_state.compass_chip_addr, s->secondary_state.mode_reg_addr, DATA_AKM_MODE_PD); 299 | if (result) 300 | return result; 301 | 302 | s->secondary_state.secondary_resume_compass_state = 1; 303 | s->secondary_state.compass_state = INV_ICM20948_COMPASS_SETUP; 304 | return inv_icm20948_suspend_akm(s); 305 | } 306 | 307 | int inv_icm20948_check_akm_self_test(struct inv_icm20948 * s) 308 | { 309 | int result; 310 | unsigned char data[6], mode, addr; 311 | unsigned char counter; 312 | short x, y, z; 313 | unsigned char *sens; 314 | int shift; 315 | unsigned char slv_ctrl[2]; 316 | unsigned char odr_cfg; 317 | #if (MEMS_CHIP != HW_ICM20948) 318 | unsigned char cntl; 319 | #endif 320 | addr = s->secondary_state.compass_chip_addr; 321 | sens = s->secondary_state.compass_sens; 322 | 323 | /* back up registers */ 324 | /* SLV0_CTRL */ 325 | result = inv_icm20948_read_mems_reg(s, REG_I2C_SLV0_CTRL, 1, &slv_ctrl[0]); 326 | if (result) 327 | return result; 328 | result = inv_icm20948_write_single_mems_reg(s, REG_I2C_SLV0_CTRL, 0); 329 | if (result) 330 | return result; 331 | /* SLV1_CTRL */ 332 | result = inv_icm20948_read_mems_reg(s, REG_I2C_SLV1_CTRL, 1, &slv_ctrl[1]); 333 | if (result) 334 | return result; 335 | result = inv_icm20948_write_single_mems_reg(s, REG_I2C_SLV1_CTRL, 0); 336 | if (result) 337 | return result; 338 | /* I2C_MST ODR */ 339 | result = inv_icm20948_read_mems_reg(s, REG_I2C_MST_ODR_CONFIG, 1, &odr_cfg); 340 | if (result) 341 | return result; 342 | result = inv_icm20948_write_single_mems_reg(s, REG_I2C_MST_ODR_CONFIG, 0); 343 | if (result) 344 | return result; 345 | 346 | #if (MEMS_CHIP == HW_ICM20948) 347 | mode = REG_AK09916_CNTL2; 348 | #else 349 | if (HW_AK09911 == s->secondary_state.compass_slave_id) 350 | mode = REG_AK09911_CNTL2; 351 | else if (HW_AK09912 == s->secondary_state.compass_slave_id) 352 | mode = REG_AK09912_CNTL2; 353 | else if (HW_AK09916 == s->secondary_state.compass_slave_id) 354 | mode = REG_AK09916_CNTL2; 355 | else 356 | mode = REG_AKM_MODE; 357 | #endif 358 | /* set to power down mode */ 359 | result = inv_icm20948_execute_write_secondary(s, 0, addr, mode, DATA_AKM_MODE_PD); 360 | if (result) 361 | goto AKM_fail; 362 | 363 | /* write 1 to ASTC register */ 364 | if ((HW_AK09911 != s->secondary_state.compass_slave_id) && 365 | (HW_AK09912 != s->secondary_state.compass_slave_id)) { 366 | result = inv_icm20948_execute_write_secondary(s, 0, addr, REG_AKM_ST_CTRL, DATA_AKM_SELF_TEST); 367 | if (result) 368 | goto AKM_fail; 369 | } 370 | #if (MEMS_CHIP == HW_ICM20948) 371 | result = inv_icm20948_execute_write_secondary(s, 0, addr, mode, DATA_AK09916_MODE_ST); 372 | #else 373 | /* set self test mode */ 374 | if (HW_AK09911 == s->secondary_state.compass_slave_id) 375 | result = inv_icm20948_execute_write_secondary(s, 0, addr, mode, DATA_AK09911_MODE_ST); 376 | else if (HW_AK09912 == s->secondary_state.compass_slave_id) 377 | result = inv_icm20948_execute_write_secondary(s, 0, addr, mode, DATA_AK09912_MODE_ST); 378 | else if (HW_AK09916 == s->secondary_state.compass_slave_id) 379 | result = inv_icm20948_execute_write_secondary(s, 0, addr, mode, DATA_AK09916_MODE_ST); 380 | else 381 | result = inv_icm20948_execute_write_secondary(s, 0, addr, mode, DATA_AKM_MODE_ST); 382 | #endif 383 | if (result) 384 | goto AKM_fail; 385 | counter = DEF_ST_COMPASS_TRY_TIMES; 386 | while (counter > 0) { 387 | // usleep_range(DEF_ST_COMPASS_WAIT_MIN, DEF_ST_COMPASS_WAIT_MAX); 388 | inv_icm20948_sleep_us(15000); 389 | 390 | #if (MEMS_CHIP == HW_ICM20948) 391 | result = inv_icm20948_execute_read_secondary(s, 0, addr, REG_AK09916_STATUS1, 1, data); 392 | #else 393 | if (HW_AK09911 == s->secondary_state.compass_slave_id) 394 | result = inv_icm20948_execute_read_secondary(s, 0, addr, REG_AK09911_STATUS1, 1, data); 395 | else if (HW_AK09912 == s->secondary_state.compass_slave_id) 396 | result = inv_icm20948_execute_read_secondary(s, 0, addr, REG_AK09912_STATUS1, 1, data); 397 | else if (HW_AK09916 == s->secondary_state.compass_slave_id) 398 | result = inv_icm20948_execute_read_secondary(s, 0, addr, REG_AK09916_STATUS1, 1, data); 399 | else 400 | result = inv_icm20948_execute_read_secondary(s, 0, addr, REG_AKM_STATUS, 1, data); 401 | #endif 402 | if (result) 403 | goto AKM_fail; 404 | if ((data[0] & DATA_AKM_DRDY) == 0) 405 | counter--; 406 | else 407 | counter = 0; 408 | } 409 | if ((data[0] & DATA_AKM_DRDY) == 0) { 410 | result = -1; 411 | goto AKM_fail; 412 | } 413 | #if (MEMS_CHIP == HW_ICM20948) 414 | result = inv_icm20948_execute_read_secondary(s, 0, addr, REG_AK09916_MEASURE_DATA, BYTES_PER_SENSOR, data); 415 | #else 416 | if (HW_AK09911 == s->secondary_state.compass_slave_id) { 417 | result = inv_icm20948_execute_read_secondary(s, 0, addr, REG_AK09911_MEASURE_DATA, BYTES_PER_SENSOR, data); 418 | } else if (HW_AK09912 == s->secondary_state.compass_slave_id) { 419 | result = inv_icm20948_execute_read_secondary(s, 0, addr, REG_AK09912_MEASURE_DATA, BYTES_PER_SENSOR, data); 420 | } else if (HW_AK09916 == s->secondary_state.compass_slave_id) { 421 | result = inv_icm20948_execute_read_secondary(s, 0, addr, REG_AK09916_MEASURE_DATA, BYTES_PER_SENSOR, data); 422 | } else { 423 | result = inv_icm20948_execute_read_secondary(s, 0, addr, REG_AKM_MEASURE_DATA, BYTES_PER_SENSOR, data); 424 | } 425 | #endif 426 | if (result) 427 | goto AKM_fail; 428 | 429 | x = ((short)data[1])<<8|data[0]; 430 | y = ((short)data[3])<<8|data[2]; 431 | z = ((short)data[5])<<8|data[4]; 432 | 433 | if (HW_AK09911 == s->secondary_state.compass_slave_id) 434 | shift = 7; 435 | else 436 | shift = 8; 437 | x = ((x * (sens[0] + 128)) >> shift); 438 | y = ((y * (sens[1] + 128)) >> shift); 439 | z = ((z * (sens[2] + 128)) >> shift); 440 | #if (MEMS_CHIP == HW_ICM20648) 441 | if (HW_AK8963 == s->secondary_state.compass_slave_id) { 442 | result = inv_icm20948_execute_read_secondary(s, 0, addr, REG_AKM8963_CNTL1, 1, &cntl); 443 | if (result) 444 | goto AKM_fail; 445 | if (0 == (cntl & DATA_AKM8963_BIT)) { 446 | x <<= DEF_ST_COMPASS_8963_SHIFT; 447 | y <<= DEF_ST_COMPASS_8963_SHIFT; 448 | z <<= DEF_ST_COMPASS_8963_SHIFT; 449 | } 450 | } 451 | #endif 452 | 453 | result = -1; 454 | if (x > s->secondary_state.st_upper[0] || x < s->secondary_state.st_lower[0]) 455 | goto AKM_fail; 456 | if (y > s->secondary_state.st_upper[1] || y < s->secondary_state.st_lower[1]) 457 | goto AKM_fail; 458 | if (z > s->secondary_state.st_upper[2] || z < s->secondary_state.st_lower[2]) 459 | goto AKM_fail; 460 | result = 0; 461 | AKM_fail: 462 | /*write 0 to ASTC register */ 463 | if ((HW_AK09911 != s->secondary_state.compass_slave_id) && 464 | (HW_AK09912 != s->secondary_state.compass_slave_id) && 465 | (HW_AK09916 != s->secondary_state.compass_slave_id)) { 466 | result |= inv_icm20948_execute_write_secondary(s, 0, addr, REG_AKM_ST_CTRL, 0); 467 | } 468 | /*set to power down mode */ 469 | result |= inv_icm20948_execute_write_secondary(s, 0, addr, mode, DATA_AKM_MODE_PD); 470 | 471 | return result; 472 | } 473 | 474 | /* 475 | * inv_icm20948_write_akm_scale() - Configure the akm scale range. 476 | */ 477 | int inv_icm20948_write_akm_scale(struct inv_icm20948 * s, int data) 478 | { 479 | char d, en; 480 | int result; 481 | 482 | if (HW_AK8963 != s->secondary_state.compass_slave_id) 483 | return 0; 484 | en = !!data; 485 | if (s->secondary_state.scale == en) 486 | return 0; 487 | d = (DATA_AKM_MODE_SM | (en << DATA_AKM8963_SCALE_SHIFT)); 488 | 489 | result = inv_icm20948_write_single_mems_reg(s, REG_I2C_SLV1_DO, d); 490 | if (result) 491 | return result; 492 | 493 | s->secondary_state.scale = en; 494 | 495 | return 0; 496 | } 497 | 498 | /* 499 | * inv_icm20948_read_akm_scale() - show AKM scale. 500 | */ 501 | int inv_icm20948_read_akm_scale(struct inv_icm20948 * s, int *scale) 502 | { 503 | #if (MEMS_CHIP == HW_ICM20948) 504 | (void)s; 505 | *scale = DATA_AK09916_SCALE; 506 | #else 507 | if (HW_AK8975 == s->secondary_state.compass_slave_id) 508 | *scale = DATA_AKM8975_SCALE; 509 | else if (HW_AK8972 == s->secondary_state.compass_slave_id) 510 | *scale = DATA_AKM8972_SCALE; 511 | else if (HW_AK8963 == s->secondary_state.compass_slave_id) 512 | if (s->secondary_state.scale) 513 | *scale = DATA_AKM8963_SCALE1; 514 | else 515 | *scale = DATA_AKM8963_SCALE0; 516 | else if (HW_AK09911 == s->secondary_state.compass_slave_id) 517 | *scale = DATA_AK09911_SCALE; 518 | else if (HW_AK09912 == s->secondary_state.compass_slave_id) 519 | *scale = DATA_AK09912_SCALE; 520 | else if (HW_AK09916 == s->secondary_state.compass_slave_id) 521 | *scale = DATA_AK09916_SCALE; 522 | else 523 | return -1; 524 | #endif 525 | return 0; 526 | } 527 | 528 | int inv_icm20948_suspend_akm(struct inv_icm20948 * s) 529 | { 530 | int result; 531 | 532 | if (!s->secondary_state.secondary_resume_compass_state) 533 | return 0; 534 | 535 | /* slave 0 is disabled */ 536 | result = inv_icm20948_secondary_stop_channel(s, COMPASS_I2C_SLV_READ); 537 | /* slave 1 is disabled */ 538 | result |= inv_icm20948_secondary_stop_channel(s, COMPASS_I2C_SLV_WRITE); 539 | if (result) 540 | return result; 541 | 542 | // Switch off I2C Interface as compass is alone 543 | result |= inv_icm20948_secondary_disable_i2c(s); 544 | 545 | s->secondary_state.secondary_resume_compass_state = 0; 546 | 547 | return result; 548 | } 549 | 550 | int inv_icm20948_resume_akm(struct inv_icm20948 * s) 551 | { 552 | int result; 553 | uint8_t reg_addr, bytes; 554 | unsigned char lDataToWrite; 555 | 556 | if (s->secondary_state.secondary_resume_compass_state) 557 | return 0; 558 | 559 | /* slave 0 is used to read data from compass */ 560 | /*read mode */ 561 | #if (MEMS_CHIP == HW_ICM20948) 562 | if (s->secondary_state.dmp_on) { 563 | reg_addr = REG_AK09916_DMP_READ; 564 | bytes = DATA_AKM_99_BYTES_DMP; 565 | } else { 566 | reg_addr = REG_AK09916_STATUS1; 567 | bytes = DATA_AKM_99_BYTES_DMP - 1; 568 | } 569 | #else 570 | /* AKM status register address is 1 */ 571 | if (HW_AK09911 == s->secondary_state.compass_slave_id) { 572 | if (s->secondary_state.dmp_on) { 573 | reg_addr = REG_AK09911_DMP_READ; 574 | bytes = DATA_AKM_99_BYTES_DMP; 575 | } else { 576 | reg_addr = REG_AK09911_STATUS1; 577 | bytes = DATA_AKM_99_BYTES_DMP - 1; 578 | } 579 | } else if (HW_AK09912 == s->secondary_state.compass_slave_id) { 580 | if (s->secondary_state.dmp_on) { 581 | reg_addr = REG_AK09912_DMP_READ; 582 | bytes = DATA_AKM_99_BYTES_DMP; 583 | } else { 584 | reg_addr = REG_AK09912_STATUS1; 585 | bytes = DATA_AKM_99_BYTES_DMP - 1; 586 | } 587 | } else if (HW_AK09916 == s->secondary_state.compass_slave_id) { 588 | if (s->secondary_state.dmp_on) { 589 | reg_addr = REG_AK09916_DMP_READ; 590 | bytes = DATA_AKM_99_BYTES_DMP; 591 | } else { 592 | reg_addr = REG_AK09916_STATUS1; 593 | bytes = DATA_AKM_99_BYTES_DMP - 1; 594 | } 595 | } else { 596 | if (s->secondary_state.dmp_on) { 597 | reg_addr = REG_AKM_INFO; 598 | bytes = DATA_AKM_89_BYTES_DMP; 599 | } else { 600 | reg_addr = REG_AKM_STATUS; 601 | bytes = DATA_AKM_89_BYTES_DMP - 1; 602 | } 603 | } 604 | #endif 605 | /* slave 0 is enabled, read 10 or 8 bytes from here depending on compass type, swap bytes to feed DMP */ 606 | result = inv_icm20948_read_secondary(s, COMPASS_I2C_SLV_READ, s->secondary_state.compass_chip_addr, reg_addr, INV_MPU_BIT_GRP | INV_MPU_BIT_BYTE_SW | bytes); 607 | if (result) 608 | return result; 609 | #if (MEMS_CHIP == HW_ICM20948) 610 | lDataToWrite = DATA_AKM_MODE_SM; 611 | #else 612 | /* slave 1 is used to write one-shot accquisition configuration to compass */ 613 | /* output data for slave 1 is fixed, single measure mode */ 614 | s->secondary_state.scale = 1; 615 | if (HW_AK8975 == s->secondary_state.compass_slave_id) { 616 | lDataToWrite = DATA_AKM_MODE_SM; 617 | } else if (HW_AK8972 == s->secondary_state.compass_slave_id) { 618 | lDataToWrite = DATA_AKM_MODE_SM; 619 | } else if (HW_AK8963 == s->secondary_state.compass_slave_id) { 620 | lDataToWrite = DATA_AKM_MODE_SM | 621 | (s->secondary_state.scale << DATA_AKM8963_SCALE_SHIFT); 622 | } else if (HW_AK09911 == s->secondary_state.compass_slave_id) { 623 | lDataToWrite = DATA_AKM_MODE_SM; 624 | } else if (HW_AK09912 == s->secondary_state.compass_slave_id) { 625 | lDataToWrite = DATA_AKM_MODE_SM; 626 | } else if (HW_AK09916 == s->secondary_state.compass_slave_id) { 627 | lDataToWrite = DATA_AKM_MODE_SM; 628 | } else { 629 | return -1; 630 | } 631 | #endif 632 | result = inv_icm20948_write_secondary(s, COMPASS_I2C_SLV_WRITE, s->secondary_state.compass_chip_addr, s->secondary_state.mode_reg_addr, lDataToWrite); 633 | if (result) 634 | return result; 635 | 636 | result |= inv_icm20948_secondary_enable_i2c(s); 637 | 638 | s->secondary_state.secondary_resume_compass_state = 1; 639 | 640 | return result; 641 | } 642 | 643 | char inv_icm20948_compass_getstate(struct inv_icm20948 * s) 644 | { 645 | return s->secondary_state.secondary_resume_compass_state; 646 | } 647 | 648 | int inv_icm20948_compass_isconnected(struct inv_icm20948 * s) 649 | { 650 | if(s->secondary_state.compass_state == INV_ICM20948_COMPASS_SETUP) { 651 | return 1; 652 | } else { 653 | return 0; 654 | } 655 | } 656 | 657 | /** 658 | * @brief Set up the soft-iron matrix for compass in DMP. 659 | * @param[in] Accel/Gyro mounting matrix 660 | * @param[in] Compass mounting matrix 661 | * @return 0 if successful. 662 | */ 663 | 664 | int inv_icm20948_compass_dmp_cal(struct inv_icm20948 * s, const signed char *m, const signed char *compass_m) 665 | { 666 | int8_t trans[NINE_ELEM]; 667 | int tmp_m[NINE_ELEM]; 668 | int i, j, k; 669 | int sens[THREE_AXES]; 670 | int scale; 671 | int shift; 672 | int current_compass_matrix[NINE_ELEM]; 673 | 674 | for (i = 0; i < THREE_AXES; i++) 675 | for (j = 0; j < THREE_AXES; j++) 676 | trans[THREE_AXES * j + i] = m[THREE_AXES * i + j]; 677 | 678 | switch (s->secondary_state.compass_slave_id) 679 | { 680 | #if (MEMS_CHIP == HW_ICM20648) 681 | case HW_AK8972: 682 | scale = DATA_AKM8972_SCALE; 683 | shift = AK89XX_SHIFT; 684 | break; 685 | case HW_AK8975: 686 | scale = DATA_AKM8975_SCALE; 687 | shift = AK89XX_SHIFT; 688 | break; 689 | case HW_AK8963: 690 | scale = DATA_AKM8963_SCALE1; 691 | shift = AK89XX_SHIFT; 692 | break; 693 | case HW_AK09911: 694 | scale = DATA_AK09911_SCALE; 695 | shift = AK99XX_SHIFT; 696 | break; 697 | case HW_AK09912: 698 | scale = DATA_AK09912_SCALE; 699 | shift = AK89XX_SHIFT; 700 | break; 701 | #else 702 | case HW_AK09916: 703 | scale = DATA_AK09916_SCALE; 704 | shift = AK89XX_SHIFT; 705 | break; 706 | #endif 707 | default: 708 | scale = DATA_AKM8963_SCALE1; 709 | shift = AK89XX_SHIFT; 710 | break; 711 | } 712 | 713 | for (i = 0; i < THREE_AXES; i++) { 714 | sens[i] = s->secondary_state.compass_sens[i] + 128; 715 | sens[i] = inv_icm20948_convert_mult_q30_fxp(sens[i] << shift, scale); 716 | } 717 | for (i = 0; i < NINE_ELEM; i++) { 718 | current_compass_matrix[i] = compass_m[i] * sens[i % THREE_AXES]; 719 | tmp_m[i] = 0; 720 | } 721 | 722 | for (i = 0; i < THREE_AXES; i++) { 723 | for (j = 0; j < THREE_AXES; j++) { 724 | s->secondary_state.final_matrix[i * THREE_AXES + j] = 0; 725 | for (k = 0; k < THREE_AXES; k++) 726 | s->secondary_state.final_matrix[i * THREE_AXES + j] += 727 | inv_icm20948_convert_mult_q30_fxp(s->soft_iron_matrix[i * THREE_AXES + k], 728 | current_compass_matrix[j + k * THREE_AXES]); 729 | } 730 | } 731 | 732 | for (i = 0; i < THREE_AXES; i++) 733 | for (j = 0; j < THREE_AXES; j++) 734 | for (k = 0; k < THREE_AXES; k++) 735 | tmp_m[THREE_AXES * i + j] += 736 | trans[THREE_AXES * i + k] * 737 | s->secondary_state.final_matrix[THREE_AXES * k + j]; 738 | 739 | return dmp_icm20948_set_compass_matrix(s, tmp_m); 740 | } 741 | 742 | /** 743 | * @brief Apply mounting matrix and scaling to raw compass data. 744 | * @param[in] Raw compass data 745 | * @param[in] Compensated compass data 746 | * @return 0 if successful. 747 | */ 748 | 749 | int inv_icm20948_apply_raw_compass_matrix(struct inv_icm20948 * s, short *raw_data, long *compensated_out) 750 | { 751 | int i, j; 752 | long long tmp; 753 | 754 | for (i = 0; i < THREE_AXES; i++) { 755 | tmp = 0; 756 | for (j = 0; j < THREE_AXES; j++) 757 | tmp += 758 | (long long)s->secondary_state.final_matrix[i * THREE_AXES + j] * (((int)raw_data[j]) << 16); 759 | compensated_out[i] = (long)(tmp >> 30); 760 | } 761 | 762 | return 0; 763 | } 764 | 765 | -------------------------------------------------------------------------------- /ICM20948/Icm20948AuxCompassAkm.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright © 2014-2015 InvenSense Inc. Portions Copyright © 2014-2015 Movea. All rights reserved. 4 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 5 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and 6 | * other intellectual property rights laws. 7 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 8 | * and any use, reproduction, disclosure or distribution of the Software without an express license 9 | * agreement from InvenSense is strictly prohibited. 10 | * ________________________________________________________________________________________________________ 11 | */ 12 | /** @defgroup inv_icm20948_slave_compass inv_slave_compass 13 | @ingroup SmartSensor_driver 14 | @{ 15 | */ 16 | #ifndef INV_ICM20948_SLAVE_COMPASS_H_SDFWQN__ 17 | #define INV_ICM20948_SLAVE_COMPASS_H_SDFWQN__ 18 | 19 | #define CPASS_MTX_00 (23 * 16) 20 | #define CPASS_MTX_01 (23 * 16 + 4) 21 | #define CPASS_MTX_02 (23 * 16 + 8) 22 | #define CPASS_MTX_10 (23 * 16 + 12) 23 | #define CPASS_MTX_11 (24 * 16) 24 | #define CPASS_MTX_12 (24 * 16 + 4) 25 | #define CPASS_MTX_20 (24 * 16 + 8) 26 | #define CPASS_MTX_21 (24 * 16 + 12) 27 | #define CPASS_MTX_22 (25 * 16) 28 | 29 | #ifdef __cplusplus 30 | extern "C" { 31 | #endif 32 | 33 | /* forward declaration */ 34 | struct inv_icm20948; 35 | 36 | /** @brief Supported auxiliary compass identifer 37 | */ 38 | enum inv_icm20948_compass_id { 39 | INV_ICM20948_COMPASS_ID_NONE = 0, /**< no compass */ 40 | INV_ICM20948_COMPASS_ID_AK09911, /**< AKM AK09911 */ 41 | INV_ICM20948_COMPASS_ID_AK09912, /**< AKM AK09912 */ 42 | INV_ICM20948_COMPASS_ID_AK09916, /**< AKM AK09916 */ 43 | INV_ICM20948_COMPASS_ID_AK08963, /**< AKM AK08963 */ 44 | }; 45 | 46 | /** @brief Register AUX compass 47 | * 48 | * Will only set internal states and won't perform any transaction on the bus. 49 | * Must be called before inv_icm20948_initialize(). 50 | * 51 | * @param[in] compass_id Compass ID 52 | * @param[in] compass_i2c_addr Compass I2C address 53 | * @return 0 on success, negative value on error 54 | */ 55 | void INV_EXPORT inv_icm20948_register_aux_compass(struct inv_icm20948 * s, 56 | enum inv_icm20948_compass_id compass_id, uint8_t compass_i2c_addr); 57 | 58 | /** @brief Initializes the compass 59 | * @return 0 in case of success, -1 for any error 60 | */ 61 | int INV_EXPORT inv_icm20948_setup_compass_akm(struct inv_icm20948 * s); 62 | 63 | /** @brief Self test for the compass 64 | * @return 0 in case of success, -1 for any error 65 | */ 66 | int INV_EXPORT inv_icm20948_check_akm_self_test(struct inv_icm20948 * s); 67 | 68 | /** @brief Changes the scale of the compass 69 | * @param[in] data new scale for the compass 70 | * @return 0 in case of success, -1 for any error 71 | */ 72 | int INV_EXPORT inv_icm20948_write_akm_scale(struct inv_icm20948 * s, int data); 73 | 74 | /** @brief Reads the scale of the compass 75 | * @param[out] scale pointer to recuperate the scale 76 | * @return 0 in case of success, -1 for any error 77 | */ 78 | int INV_EXPORT inv_icm20948_read_akm_scale(struct inv_icm20948 * s, int *scale); 79 | 80 | /** @brief Stops the compass 81 | * @return 0 in case of success, -1 for any error 82 | */ 83 | int INV_EXPORT inv_icm20948_suspend_akm(struct inv_icm20948 * s); 84 | 85 | /** @brief Starts the compass 86 | * @return 0 in case of success, -1 for any error 87 | */ 88 | int INV_EXPORT inv_icm20948_resume_akm(struct inv_icm20948 * s); 89 | 90 | /** @brief Get compass power status 91 | * @return 1 in case compass is enabled, 0 if not started 92 | */ 93 | char INV_EXPORT inv_icm20948_compass_getstate(struct inv_icm20948 * s); 94 | 95 | /** @brief detects if the compass is connected 96 | * @return 1 if the compass is connected, 0 otherwise 97 | */ 98 | int INV_EXPORT inv_icm20948_compass_isconnected(struct inv_icm20948 * s); 99 | 100 | /** @brief Calibrates the data 101 | * @param[in] m pointer to the raw compass data 102 | * @param[out] compass_m pointer to the calibrated compass data 103 | * @return 0 in case of success, -1 for any error 104 | */ 105 | int INV_EXPORT inv_icm20948_compass_dmp_cal(struct inv_icm20948 * s, const signed char *m, const signed char *compass_m); 106 | 107 | /** 108 | * @brief Applies mounting matrix and scaling to raw compass data. 109 | * @param[in] raw_data Raw compass data 110 | * @param[in] compensated_out Compensated compass data 111 | * @return 0 in case of success, -1 for any error 112 | */ 113 | int INV_EXPORT inv_icm20948_apply_raw_compass_matrix(struct inv_icm20948 * s, short *raw_data, long *compensated_out); 114 | 115 | #ifdef __cplusplus 116 | } 117 | #endif 118 | 119 | #endif // INV_ICM20948_SLAVE_COMPASS_H_SDFWQN__ 120 | 121 | /** @} */ 122 | -------------------------------------------------------------------------------- /ICM20948/Icm20948AuxTransport.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948AuxTransport.c -------------------------------------------------------------------------------- /ICM20948/Icm20948AuxTransport.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948AuxTransport.h -------------------------------------------------------------------------------- /ICM20948/Icm20948DataBaseControl.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948DataBaseControl.c -------------------------------------------------------------------------------- /ICM20948/Icm20948DataBaseControl.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948DataBaseControl.h -------------------------------------------------------------------------------- /ICM20948/Icm20948DataBaseDriver.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948DataBaseDriver.c -------------------------------------------------------------------------------- /ICM20948/Icm20948DataBaseDriver.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948DataBaseDriver.h -------------------------------------------------------------------------------- /ICM20948/Icm20948DataConverter.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948DataConverter.c -------------------------------------------------------------------------------- /ICM20948/Icm20948DataConverter.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948DataConverter.h -------------------------------------------------------------------------------- /ICM20948/Icm20948Defs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright © 2014-2015 InvenSense Inc. Portions Copyright © 2014-2015 Movea. All rights reserved. 4 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 5 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and 6 | * other intellectual property rights laws. 7 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 8 | * and any use, reproduction, disclosure or distribution of the Software without an express license 9 | * agreement from InvenSense is strictly prohibited. 10 | * ________________________________________________________________________________________________________ 11 | */ 12 | 13 | #ifndef _INV_ICM20948_DEFINES_H_ 14 | #define _INV_ICM20948_DEFINES_H_ 15 | 16 | #include 17 | #include 18 | 19 | #include "Icm20948Dmp3Driver.h" 20 | 21 | #ifdef __cplusplus 22 | extern "C" { 23 | #endif 24 | 25 | // compass chip list 26 | #define HW_AK8963 0x20 27 | #define HW_AK8975 0x21 28 | #define HW_AK8972 0x22 29 | #define HW_AK09911 0x23 30 | #define HW_AK09912 0x24 31 | #define HW_AK09916 0x25 32 | 33 | #define HW_ICM20648 0x01 34 | #define HW_ICM20948 0x02 35 | 36 | #define USE_ICM20948 1 37 | 38 | #if defined USE_ICM20648 39 | #define MEMS_CHIP HW_ICM20648 40 | #endif 41 | 42 | #if defined USE_ICM20948 43 | #define MEMS_CHIP HW_ICM20948 44 | #endif 45 | 46 | #if !defined(MEMS_CHIP) 47 | #error "MEMS_CHIP is not defined" 48 | #elif MEMS_CHIP != HW_ICM20648 \ 49 | && MEMS_CHIP != HW_ICM20948 50 | #error "Unknown value for MEMS_CHIP" 51 | #endif 52 | 53 | #define DMP_LOAD_START 0x90 54 | 55 | #define MPU_SUCCESS (0) 56 | #define MPU_COMPASS_NOT_FOUND (int)0x00ABCDEF 57 | 58 | #define MSEC_PER_SEC 1000 59 | #define NSEC_PER_MSEC 1000000 60 | #define NSEC_PER_SEC NSEC_PER_MSEC * MSEC_PER_SEC 61 | 62 | #define FIFO_DIVIDER 19 63 | 64 | #define REG_BANK_0 0x00 65 | #define REG_BANK_1 0x01 66 | 67 | #define DIAMOND_I2C_ADDRESS 0x68 68 | #define BANK_0 (0 << 7) 69 | #define BANK_1 (1 << 7) 70 | #define BANK_2 (2 << 7) 71 | #define BANK_3 (3 << 7) 72 | 73 | /*register and associated bit definition*/ 74 | /* bank 0 register map */ 75 | #define REG_WHO_AM_I (BANK_0 | 0x00) 76 | #define REG_LPF (BANK_0 | 0x01) 77 | 78 | #define REG_USER_CTRL (BANK_0 | 0x03) 79 | #define BIT_DMP_EN 0x80 80 | #define BIT_FIFO_EN 0x40 81 | #define BIT_I2C_MST_EN 0x20 82 | #define BIT_I2C_IF_DIS 0x10 83 | #define BIT_DMP_RST 0x08 84 | #define BIT_DIAMOND_DMP_RST 0x04 85 | 86 | #define REG_LP_CONFIG (BANK_0 | 0x05) 87 | #define BIT_I2C_MST_CYCLE 0x40 88 | #define BIT_ACCEL_CYCLE 0x20 89 | #define BIT_GYRO_CYCLE 0x10 90 | 91 | #define REG_PWR_MGMT_1 (BANK_0 | 0x06) 92 | #define BIT_H_RESET 0x80 93 | #define BIT_SLEEP 0x40 94 | #define BIT_LP_EN 0x20 95 | #define BIT_CLK_PLL 0x01 96 | 97 | #define REG_PWR_MGMT_2 (BANK_0 | 0x07) 98 | #define BIT_PWR_PRESSURE_STBY 0x40 99 | #define BIT_PWR_ACCEL_STBY 0x38 100 | #define BIT_PWR_GYRO_STBY 0x07 101 | #define BIT_PWR_ALL_OFF 0x7f 102 | 103 | #define REG_INT_PIN_CFG (BANK_0 | 0x0F) 104 | #define BIT_INT_LATCH_EN 0x20 105 | #define BIT_BYPASS_EN 0x02 106 | 107 | #define REG_INT_ENABLE (BANK_0 | 0x10) 108 | #define BIT_DMP_INT_EN 0x02 109 | 110 | #define REG_INT_ENABLE_1 (BANK_0 | 0x11) 111 | #define BIT_DATA_RDY_3_EN 0x08 112 | #define BIT_DATA_RDY_2_EN 0x04 113 | #define BIT_DATA_RDY_1_EN 0x02 114 | #define BIT_DATA_RDY_0_EN 0x01 115 | 116 | #define REG_INT_ENABLE_2 (BANK_0 | 0x12) 117 | #define BIT_FIFO_OVERFLOW_EN_0 0x1 118 | 119 | #define REG_INT_ENABLE_3 (BANK_0 | 0x13) 120 | 121 | #define REG_DMP_INT_STATUS (BANK_0 | 0x18) 122 | #define BIT_WAKE_ON_MOTION_INT 0x08 123 | #define BIT_MSG_DMP_INT 0x0002 124 | #define BIT_MSG_DMP_INT_0 0x0100 // CI Command 125 | 126 | #define BIT_MSG_DMP_INT_2 0x0200 // CIM Command - SMD 127 | #define BIT_MSG_DMP_INT_3 0x0400 // CIM Command - Pedometer 128 | 129 | #define BIT_MSG_DMP_INT_4 0x1000 // CIM Command - Pedometer binning 130 | #define BIT_MSG_DMP_INT_5 0x2000 // CIM Command - Bring To See Gesture 131 | #define BIT_MSG_DMP_INT_6 0x4000 // CIM Command - Look To See Gesture 132 | 133 | #define REG_INT_STATUS (BANK_0 | 0x19) 134 | #define BIT_DMP_INT 0x02 135 | 136 | #define REG_INT_STATUS_1 (BANK_0 | 0x1A) 137 | #define REG_INT_STATUS_2 (BANK_0 | 0x1B) 138 | 139 | #define REG_SINGLE_FIFO_PRIORITY_SEL (BANK_0 | 0x26) 140 | 141 | #define REG_GYRO_XOUT_H_SH (BANK_0 | 0x33) 142 | 143 | #define REG_TEMPERATURE (BANK_0 | 0x39) 144 | #define REG_TEMP_CONFIG (BANK_0 | 0x53) 145 | 146 | #define REG_EXT_SLV_SENS_DATA_00 (BANK_0 | 0x3B) 147 | #define REG_EXT_SLV_SENS_DATA_08 (BANK_0 | 0x43) 148 | #define REG_EXT_SLV_SENS_DATA_09 (BANK_0 | 0x44) 149 | #define REG_EXT_SLV_SENS_DATA_10 (BANK_0 | 0x45) 150 | 151 | #define REG_FIFO_EN (BANK_0 | 0x66) 152 | #define BIT_SLV_0_FIFO_EN 0x01 153 | 154 | #define REG_FIFO_EN_2 (BANK_0 | 0x67) 155 | #define BIT_PRS_FIFO_EN 0x20 156 | #define BIT_ACCEL_FIFO_EN 0x10 157 | #define BITS_GYRO_FIFO_EN 0x0E 158 | 159 | #define REG_FIFO_RST (BANK_0 | 0x68) 160 | 161 | #define REG_FIFO_COUNT_H (BANK_0 | 0x70) 162 | #define REG_FIFO_COUNT_L (BANK_0 | 0x71) 163 | #define REG_FIFO_R_W (BANK_0 | 0x72) 164 | 165 | #define REG_HW_FIX_DISABLE (BANK_0 | 0x75) 166 | 167 | #define REG_FIFO_CFG (BANK_0 | 0x76) 168 | #define BIT_MULTI_FIFO_CFG 0x01 169 | #define BIT_SINGLE_FIFO_CFG 0x00 170 | 171 | #define REG_ACCEL_XOUT_H_SH (BANK_0 | 0x2D) 172 | #define REG_ACCEL_XOUT_L_SH (BANK_0 | 0x2E) 173 | #define REG_ACCEL_YOUT_H_SH (BANK_0 | 0x2F) 174 | #define REG_ACCEL_YOUT_L_SH (BANK_0 | 0x30) 175 | #define REG_ACCEL_ZOUT_H_SH (BANK_0 | 0x31) 176 | #define REG_ACCEL_ZOUT_L_SH (BANK_0 | 0x32) 177 | 178 | #define REG_MEM_START_ADDR (BANK_0 | 0x7C) 179 | #define REG_MEM_R_W (BANK_0 | 0x7D) 180 | #define REG_MEM_BANK_SEL (BANK_0 | 0x7E) 181 | 182 | /* bank 1 register map */ 183 | #define REG_TIMEBASE_CORRECTION_PLL (BANK_1 | 0x28) 184 | #define REG_TIMEBASE_CORRECTION_RCOSC (BANK_1 | 0x29) 185 | #define REG_SELF_TEST1 (BANK_1 | 0x02) 186 | #define REG_SELF_TEST2 (BANK_1 | 0x03) 187 | #define REG_SELF_TEST3 (BANK_1 | 0x04) 188 | #define REG_SELF_TEST4 (BANK_1 | 0x0E) 189 | #define REG_SELF_TEST5 (BANK_1 | 0x0F) 190 | #define REG_SELF_TEST6 (BANK_1 | 0x10) 191 | 192 | #define REG_XA_OFFS_H (BANK_1 | 0x14) 193 | #define REG_XA_OFFS_L (BANK_1 | 0x15) 194 | #define REG_YA_OFFS_H (BANK_1 | 0x17) 195 | #define REG_YA_OFFS_L (BANK_1 | 0x18) 196 | #define REG_ZA_OFFS_H (BANK_1 | 0x1A) 197 | #define REG_ZA_OFFS_L (BANK_1 | 0x1B) 198 | 199 | /* bank 2 register map */ 200 | #define REG_GYRO_SMPLRT_DIV (BANK_2 | 0x00) 201 | 202 | #define REG_GYRO_CONFIG_1 (BANK_2 | 0x01) 203 | #define SHIFT_GYRO_FS_SEL 1 204 | #define SHIFT_GYRO_DLPCFG 3 205 | 206 | #define REG_GYRO_CONFIG_2 (BANK_2 | 0x02) 207 | #define BIT_GYRO_CTEN 0x38 208 | 209 | #define REG_XG_OFFS_USRH (BANK_2 | 0x03) 210 | #define REG_XG_OFFS_USRL (BANK_2 | 0x04) 211 | #define REG_YG_OFFS_USRH (BANK_2 | 0x05) 212 | #define REG_YG_OFFS_USRL (BANK_2 | 0x06) 213 | #define REG_ZG_OFFS_USRH (BANK_2 | 0x07) 214 | #define REG_ZG_OFFS_USRL (BANK_2 | 0x08) 215 | 216 | #define REG_ACCEL_SMPLRT_DIV_1 (BANK_2 | 0x10) 217 | #define REG_ACCEL_SMPLRT_DIV_2 (BANK_2 | 0x11) 218 | 219 | #define REG_ACCEL_CONFIG (BANK_2 | 0x14) 220 | #define SHIFT_ACCEL_FS 1 221 | 222 | #define REG_ACCEL_CONFIG_2 (BANK_2 | 0x15) 223 | #define BIT_ACCEL_CTEN 0x1C 224 | 225 | #define REG_PRS_ODR_CONFIG (BANK_2 | 0x20) 226 | #define REG_PRGM_START_ADDRH (BANK_2 | 0x50) 227 | 228 | #define REG_MOD_CTRL_USR (BANK_2 | 0x54) 229 | #define BIT_ODR_SYNC 0x7 230 | 231 | /* bank 3 register map */ 232 | #define REG_I2C_MST_ODR_CONFIG (BANK_3 | 0x0) 233 | 234 | #define REG_I2C_MST_CTRL (BANK_3 | 0x01) 235 | #define BIT_I2C_MST_P_NSR 0x10 236 | 237 | #define REG_I2C_MST_DELAY_CTRL (BANK_3 | 0x02) 238 | #define BIT_SLV0_DLY_EN 0x01 239 | #define BIT_SLV1_DLY_EN 0x02 240 | #define BIT_SLV2_DLY_EN 0x04 241 | #define BIT_SLV3_DLY_EN 0x08 242 | 243 | #define REG_I2C_SLV0_ADDR (BANK_3 | 0x03) 244 | #define REG_I2C_SLV0_REG (BANK_3 | 0x04) 245 | #define REG_I2C_SLV0_CTRL (BANK_3 | 0x05) 246 | #define REG_I2C_SLV0_DO (BANK_3 | 0x06) 247 | 248 | #define REG_I2C_SLV1_ADDR (BANK_3 | 0x07) 249 | #define REG_I2C_SLV1_REG (BANK_3 | 0x08) 250 | #define REG_I2C_SLV1_CTRL (BANK_3 | 0x09) 251 | #define REG_I2C_SLV1_DO (BANK_3 | 0x0A) 252 | 253 | #define REG_I2C_SLV2_ADDR (BANK_3 | 0x0B) 254 | #define REG_I2C_SLV2_REG (BANK_3 | 0x0C) 255 | #define REG_I2C_SLV2_CTRL (BANK_3 | 0x0D) 256 | #define REG_I2C_SLV2_DO (BANK_3 | 0x0E) 257 | 258 | #define REG_I2C_SLV3_ADDR (BANK_3 | 0x0F) 259 | #define REG_I2C_SLV3_REG (BANK_3 | 0x10) 260 | #define REG_I2C_SLV3_CTRL (BANK_3 | 0x11) 261 | #define REG_I2C_SLV3_DO (BANK_3 | 0x12) 262 | 263 | #define REG_I2C_SLV4_CTRL (BANK_3 | 0x15) 264 | 265 | #define INV_MPU_BIT_SLV_EN 0x80 266 | #define INV_MPU_BIT_BYTE_SW 0x40 267 | #define INV_MPU_BIT_REG_DIS 0x20 268 | #define INV_MPU_BIT_GRP 0x10 269 | #define INV_MPU_BIT_I2C_READ 0x80 270 | 271 | /* register for all banks */ 272 | #define REG_BANK_SEL 0x7F 273 | 274 | /* data definitions */ 275 | #define BYTES_PER_SENSOR 6 276 | #define FIFO_COUNT_BYTE 2 277 | #define HARDWARE_FIFO_SIZE 1024 278 | 279 | #define FIFO_SIZE (HARDWARE_FIFO_SIZE * 7 / 8) 280 | #define POWER_UP_TIME 100 281 | #define REG_UP_TIME_USEC 100 282 | #define DMP_RESET_TIME 20 283 | #define GYRO_ENGINE_UP_TIME 50 284 | #define MPU_MEM_BANK_SIZE 256 285 | #define IIO_BUFFER_BYTES 8 286 | #define HEADERED_NORMAL_BYTES 8 287 | #define HEADERED_Q_BYTES 16 288 | #define LEFT_OVER_BYTES 128 289 | #define BASE_SAMPLE_RATE 1125 290 | 291 | #ifdef FREQ_225 292 | #define MPU_DEFAULT_DMP_FREQ 225 293 | #define PEDOMETER_FREQ (MPU_DEFAULT_DMP_FREQ >> 2) 294 | #define DEFAULT_ACCEL_GAIN (33554432L * 5 / 11) 295 | #else 296 | #define MPU_DEFAULT_DMP_FREQ 102 297 | #define PEDOMETER_FREQ (MPU_DEFAULT_DMP_FREQ >> 1) 298 | #define DEFAULT_ACCEL_GAIN 33554432L 299 | #endif 300 | #define PED_ACCEL_GAIN 67108864L 301 | #define ALPHA_FILL_PED 858993459 302 | #define A_FILL_PED 214748365 303 | 304 | #define MIN_MST_ODR_CONFIG 4 305 | #define THREE_AXES 3 306 | #define NINE_ELEM (THREE_AXES * THREE_AXES) 307 | #define MPU_TEMP_SHIFT 16 308 | #define SOFT_IRON_MATRIX_SIZE (4 * 9) 309 | #define DMP_DIVIDER (BASE_SAMPLE_RATE / MPU_DEFAULT_DMP_FREQ) 310 | #define MAX_5_BIT_VALUE 0x1F 311 | #define BAD_COMPASS_DATA 0x7FFF 312 | #define DEFAULT_BATCH_RATE 400 313 | #define DEFAULT_BATCH_TIME (MSEC_PER_SEC / DEFAULT_BATCH_RATE) 314 | #define MAX_COMPASS_RATE 115 315 | #define MAX_PRESSURE_RATE 30 316 | #define MAX_ALS_RATE 5 317 | #define DATA_AKM_99_BYTES_DMP 10 318 | #define DATA_AKM_89_BYTES_DMP 9 319 | #define DATA_ALS_BYTES_DMP 8 320 | #define APDS9900_AILTL_REG 0x04 321 | #define BMP280_DIG_T1_LSB_REG 0x88 322 | #define COVARIANCE_SIZE 14 323 | #define ACCEL_COVARIANCE_SIZE (COVARIANCE_SIZE * sizeof(int)) 324 | #define COMPASS_COVARIANCE_SIZE (COVARIANCE_SIZE * sizeof(int)) 325 | #define TEMPERATURE_SCALE 3340827L 326 | #define TEMPERATURE_OFFSET 1376256L 327 | #define SECONDARY_INIT_WAIT 60 328 | #define MPU_SOFT_UPDT_ADDR 0x86 329 | #define MPU_SOFT_UPTD_MASK 0x0F 330 | #define AK99XX_SHIFT 23 331 | #define AK89XX_SHIFT 22 332 | #define OPERATE_GYRO_IN_DUTY_CYCLED_MODE (1<<4) 333 | #define OPERATE_ACCEL_IN_DUTY_CYCLED_MODE (1<<5) 334 | #define OPERATE_I2C_MASTER_IN_DUTY_CYCLED_MODE (1<<6) 335 | 336 | /* this is derived from 1000 divided by 55, which is the pedometer 337 | running frequency */ 338 | #define MS_PER_PED_TICKS 18 339 | 340 | /* data limit definitions */ 341 | #define MIN_FIFO_RATE 4 342 | #define MAX_FIFO_RATE MPU_DEFAULT_DMP_FREQ 343 | #define MAX_DMP_OUTPUT_RATE MPU_DEFAULT_DMP_FREQ 344 | #define MAX_READ_SIZE 128 345 | #define MAX_MPU_MEM 8192 346 | #define MAX_PRS_RATE 281 347 | 348 | /* data header defines */ 349 | #define PRESSURE_HDR 0x8000 350 | #define ACCEL_HDR 0x4000 351 | #define ACCEL_ACCURACY_HDR 0x4080 352 | #define GYRO_HDR 0x2000 353 | #define GYRO_ACCURACY_HDR 0x2080 354 | #define COMPASS_HDR 0x1000 355 | #define COMPASS_HDR_2 0x1800 356 | #define CPASS_ACCURACY_HDR 0x1080 357 | #define ALS_HDR 0x0800 358 | #define SIXQUAT_HDR 0x0400 359 | #define PEDQUAT_HDR 0x0200 360 | #define STEP_DETECTOR_HDR 0x0100 361 | 362 | #define COMPASS_CALIB_HDR 0x0080 363 | #define GYRO_CALIB_HDR 0x0040 364 | #define EMPTY_MARKER 0x0020 365 | #define END_MARKER 0x0010 366 | #define NINEQUAT_HDR 0x0008 367 | #define LPQ_HDR 0x0004 368 | 369 | #define STEP_INDICATOR_MASK 0x000f 370 | 371 | /* init parameters */ 372 | #define MPU_INIT_SMD_THLD 1500 373 | #define MPU_INIT_SENSOR_RATE 5 374 | #define MPU_INIT_GYRO_SCALE 3 375 | #define MPU_INIT_ACCEL_SCALE 0 376 | #define MPU_INIT_PED_INT_THRESH 2 377 | #define MPU_INIT_PED_STEP_THRESH 6 378 | #define COMPASS_SLAVEADDR_AKM_BASE 0x0C 379 | #define COMPASS_SLAVEADDR_AKM 0x0E 380 | 381 | #define BIT(x) ( 1 << x ) 382 | 383 | #define ENABLE 1 384 | #define DISABLE 0 385 | 386 | // interrupt configurations related to HW register 387 | #define FSYNC_INT BIT(7) 388 | #define MOTION_INT BIT(3) 389 | #define PLL_INT BIT(2) 390 | #define DMP_INT BIT(1) 391 | #define I2C_INT BIT(0) 392 | 393 | #define CHIP_AWAKE (0x01) 394 | #define CHIP_LP_ENABLE (0x02) 395 | 396 | //ACC_REQUESTED_FREQ 397 | #define DMP_ALGO_FREQ_56 56 398 | #define DMP_ALGO_FREQ_112 112 399 | #define DMP_ALGO_FREQ_225 225 400 | #define DMP_ALGO_FREQ_450 450 401 | #define DMP_ALGO_FREQ_900 900 402 | 403 | enum SMARTSENSOR_SERIAL_INTERFACE { 404 | SERIAL_INTERFACE_I2C = 1, 405 | SERIAL_INTERFACE_SPI, 406 | SERIAL_INTERFACE_INVALID 407 | }; 408 | 409 | enum mpu_accel_fs { 410 | MPU_FS_2G = 0, 411 | MPU_FS_4G, 412 | MPU_FS_8G, 413 | MPU_FS_16G, 414 | NUM_MPU_AFS 415 | }; 416 | 417 | enum mpu_gyro_fs { 418 | MPU_FS_250dps = 0, 419 | MPU_FS_500dps, 420 | MPU_FS_1000dps, 421 | MPU_FS_2000dps, 422 | NUM_MPU_GFS 423 | }; 424 | 425 | enum INV_ENGINE { 426 | ENGINE_GYRO = 0, 427 | ENGINE_ACCEL, 428 | ENGINE_I2C, 429 | ENGINE_NUM_MAX, 430 | }; 431 | 432 | /* enum for android sensor*/ 433 | enum ANDROID_SENSORS { 434 | ANDROID_SENSOR_META_DATA = 0, 435 | ANDROID_SENSOR_ACCELEROMETER, 436 | ANDROID_SENSOR_GEOMAGNETIC_FIELD, 437 | ANDROID_SENSOR_ORIENTATION, 438 | ANDROID_SENSOR_GYROSCOPE, 439 | ANDROID_SENSOR_LIGHT, 440 | ANDROID_SENSOR_PRESSURE, 441 | ANDROID_SENSOR_TEMPERATURE, 442 | ANDROID_SENSOR_WAKEUP_PROXIMITY, 443 | ANDROID_SENSOR_GRAVITY, 444 | ANDROID_SENSOR_LINEAR_ACCELERATION, 445 | ANDROID_SENSOR_ROTATION_VECTOR, 446 | ANDROID_SENSOR_HUMIDITY, 447 | ANDROID_SENSOR_AMBIENT_TEMPERATURE, 448 | ANDROID_SENSOR_MAGNETIC_FIELD_UNCALIBRATED, 449 | ANDROID_SENSOR_GAME_ROTATION_VECTOR, 450 | ANDROID_SENSOR_GYROSCOPE_UNCALIBRATED, 451 | ANDROID_SENSOR_WAKEUP_SIGNIFICANT_MOTION, 452 | ANDROID_SENSOR_STEP_DETECTOR, 453 | ANDROID_SENSOR_STEP_COUNTER, 454 | ANDROID_SENSOR_GEOMAGNETIC_ROTATION_VECTOR, 455 | ANDROID_SENSOR_HEART_RATE, 456 | ANDROID_SENSOR_PROXIMITY, 457 | 458 | ANDROID_SENSOR_WAKEUP_ACCELEROMETER, 459 | ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD, 460 | ANDROID_SENSOR_WAKEUP_ORIENTATION, 461 | ANDROID_SENSOR_WAKEUP_GYROSCOPE, 462 | ANDROID_SENSOR_WAKEUP_LIGHT, 463 | ANDROID_SENSOR_WAKEUP_PRESSURE, 464 | ANDROID_SENSOR_WAKEUP_GRAVITY, 465 | ANDROID_SENSOR_WAKEUP_LINEAR_ACCELERATION, 466 | ANDROID_SENSOR_WAKEUP_ROTATION_VECTOR, 467 | ANDROID_SENSOR_WAKEUP_RELATIVE_HUMIDITY, 468 | ANDROID_SENSOR_WAKEUP_AMBIENT_TEMPERATURE, 469 | ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD_UNCALIBRATED, 470 | ANDROID_SENSOR_WAKEUP_GAME_ROTATION_VECTOR, 471 | ANDROID_SENSOR_WAKEUP_GYROSCOPE_UNCALIBRATED, 472 | ANDROID_SENSOR_WAKEUP_STEP_DETECTOR, 473 | ANDROID_SENSOR_WAKEUP_STEP_COUNTER, 474 | ANDROID_SENSOR_WAKEUP_GEOMAGNETIC_ROTATION_VECTOR, 475 | ANDROID_SENSOR_WAKEUP_HEART_RATE, 476 | ANDROID_SENSOR_WAKEUP_TILT_DETECTOR, 477 | ANDROID_SENSOR_RAW_ACCELEROMETER, 478 | ANDROID_SENSOR_RAW_GYROSCOPE, 479 | ANDROID_SENSOR_NUM_MAX, 480 | 481 | ANDROID_SENSOR_B2S, 482 | ANDROID_SENSOR_FLIP_PICKUP, 483 | ANDROID_SENSOR_ACTIVITY_CLASSIFICATON, 484 | ANDROID_SENSOR_SCREEN_ROTATION, 485 | SELF_TEST, 486 | SETUP, 487 | GENERAL_SENSORS_MAX 488 | }; 489 | 490 | 491 | enum SENSOR_ACCURACY { 492 | SENSOR_ACCEL_ACCURACY = 0, 493 | SENSOR_GYRO_ACCURACY, 494 | SENSOR_COMPASS_ACCURACY, 495 | SENSOR_ACCURACY_NUM_MAX, 496 | }; 497 | 498 | #ifndef min 499 | #define min(x,y) (((x)<(y))?(x):(y)) 500 | #endif 501 | 502 | #ifndef max 503 | #define max(x,y) (((x)>(y))?(x):(y)) 504 | #endif 505 | 506 | #ifdef __cplusplus 507 | } 508 | #endif 509 | 510 | #endif /* #ifndef _INV_ICM20948_DEFINES_H_ */ 511 | 512 | -------------------------------------------------------------------------------- /ICM20948/Icm20948Dmp3Driver.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948Dmp3Driver.c -------------------------------------------------------------------------------- /ICM20948/Icm20948Dmp3Driver.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948Dmp3Driver.h -------------------------------------------------------------------------------- /ICM20948/Icm20948LoadFirmware.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948LoadFirmware.c -------------------------------------------------------------------------------- /ICM20948/Icm20948LoadFirmware.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948LoadFirmware.h -------------------------------------------------------------------------------- /ICM20948/Icm20948MPUFifoControl.c: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright © 2014-2015 InvenSense Inc. Portions Copyright © 2014-2015 Movea. All rights reserved. 4 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 5 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and 6 | * other intellectual property rights laws. 7 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 8 | * and any use, reproduction, disclosure or distribution of the Software without an express license 9 | * agreement from InvenSense is strictly prohibited. 10 | * ________________________________________________________________________________________________________ 11 | */ 12 | 13 | #include "Icm20948.h" 14 | #include "Icm20948MPUFifoControl.h" 15 | 16 | #include "Icm20948Defs.h" 17 | #include "Icm20948DataBaseControl.h" 18 | #include "Icm20948DataConverter.h" 19 | 20 | #include "Icm20948AuxCompassAkm.h" 21 | 22 | struct inv_fifo_decoded_t fd; 23 | 24 | static void inv_decode_3_16bit_elements(short *out_data, const unsigned char *in_data); 25 | static void inv_decode_3_32bit_elements(long *out_data, const unsigned char *in_data); 26 | 27 | int inv_icm20948_mpu_set_FIFO_RST_Diamond(struct inv_icm20948 * s, unsigned char value) 28 | { 29 | int result = 0; 30 | unsigned char reg; 31 | 32 | result |= inv_icm20948_read_mems_reg(s, REG_FIFO_RST, 1, ®); 33 | 34 | reg &= 0xe0; 35 | reg |= value; 36 | result |= inv_icm20948_write_mems_reg(s, REG_FIFO_RST, 1, ®); 37 | 38 | return result; 39 | } 40 | 41 | int inv_icm20948_identify_interrupt(struct inv_icm20948 * s, short *int_read) 42 | { 43 | unsigned char int_status; 44 | int result=0 ; 45 | 46 | if(int_read) 47 | *int_read = 0; 48 | 49 | result = inv_icm20948_read_mems_reg(s, REG_INT_STATUS, 1, &int_status); 50 | if(int_read) 51 | *int_read = int_status; 52 | 53 | result = inv_icm20948_read_mems_reg(s, REG_DMP_INT_STATUS, 1, &int_status); // DMP_INT_STATUS 54 | if(int_read) 55 | *int_read |= (int_status << 8); 56 | 57 | /*if(wake_on_motion_enabled) { 58 | result = inv_icm20948_read_mems_reg(s, REG_INT_STATUS, 1, &int_status);//INT_STATUS 59 | if(result) 60 | return result; 61 | *int_read |= reg_data[1]; 62 | }*/ 63 | /* 64 | * We do not need to handle FIFO overflow here. 65 | * When we read FIFO_SIZE we can determine if FIFO overflow has occured. 66 | */ 67 | //result = inv_icm20948_read_mems_reg(s, 0x1B, 1, &int_status); 68 | 69 | return result; 70 | } 71 | 72 | /** 73 | * @internal 74 | * @brief Get the length from the fifo 75 | * 76 | * @param[out] len amount of data currently stored in the fifo. 77 | * 78 | * @return MPU_SUCCESS or non-zero error code. 79 | **/ 80 | static int dmp_get_fifo_length(struct inv_icm20948 * s, uint_fast16_t * len ) 81 | { 82 | unsigned char fifoBuf[2]; 83 | int result = 0; 84 | 85 | if (NULL == len) 86 | return -1; 87 | 88 | /*---- read the 2 'count' registers and 89 | burst read the data from the FIFO ----*/ 90 | result = inv_icm20948_read_mems_reg(s, REG_FIFO_COUNT_H, 2, fifoBuf); 91 | if (result) 92 | { 93 | s->fifo_info.fifoError = -1; 94 | *len = 0; 95 | return result; 96 | } 97 | 98 | *len = (uint_fast16_t) (fifoBuf[0] << 8); 99 | *len += (uint_fast16_t) (fifoBuf[1]); 100 | 101 | return result; 102 | } 103 | 104 | /** 105 | * @internal 106 | * @brief Clears the FIFO status and its content. 107 | * @note Halt the DMP writing into the FIFO for the time 108 | * needed to reset the FIFO. 109 | * @return MPU_SUCCESS if successful, a non-zero error code otherwise. 110 | */ 111 | static int dmp_reset_fifo(struct inv_icm20948 * s) 112 | { 113 | uint_fast16_t len = HARDWARE_FIFO_SIZE; 114 | unsigned char tries = 0; 115 | int result = 0; 116 | 117 | while (len != 0 && tries < 6) 118 | { 119 | s->base_state.user_ctrl &= (~BIT_FIFO_EN); 120 | s->base_state.user_ctrl &= (~BIT_DMP_EN); 121 | result |= inv_icm20948_write_single_mems_reg(s, REG_USER_CTRL, s->base_state.user_ctrl); 122 | result |= inv_icm20948_mpu_set_FIFO_RST_Diamond(s, 0x1f); 123 | result |= inv_icm20948_mpu_set_FIFO_RST_Diamond(s, 0x1e); 124 | 125 | // Reset overflow flag 126 | s->fifo_info.fifo_overflow = 0; 127 | 128 | result |= dmp_get_fifo_length(s, &len); 129 | if (result) 130 | return result; 131 | 132 | tries++; 133 | } 134 | 135 | s->base_state.user_ctrl |= BIT_FIFO_EN; 136 | s->base_state.user_ctrl |= BIT_DMP_EN; 137 | result |= inv_icm20948_write_single_mems_reg(s, REG_USER_CTRL, s->base_state.user_ctrl); 138 | 139 | return result; 140 | } 141 | 142 | /** 143 | * @internal 144 | * @brief Read data from the fifo 145 | * 146 | * @param[out] data Location to store the date read from the fifo 147 | * @param[in] len Amount of data to read out of the fifo 148 | * 149 | * @return MPU_SUCCESS or non-zero error code 150 | **/ 151 | static int dmp_read_fifo(struct inv_icm20948 * s, unsigned char *data, uint_fast16_t len) 152 | { 153 | int result; 154 | uint_fast16_t bytesRead = 0; 155 | 156 | while (bytesReadfifo_info.fifoError = -1; 165 | return result; 166 | } 167 | 168 | bytesRead += thisLen; 169 | } 170 | 171 | return result; 172 | } 173 | 174 | /** 175 | * @internal 176 | * @brief used to get the FIFO data. 177 | * @param length 178 | * Max number of bytes to read from the FIFO that buffer is still able to sustain. 179 | * @param buffer Reads up to length into the buffer. 180 | * 181 | * @return number of bytes of read. 182 | **/ 183 | static uint_fast16_t dmp_get_fifo_all(struct inv_icm20948 * s, uint_fast16_t length, unsigned char *buffer, int *reset) 184 | { 185 | int result; 186 | uint_fast16_t in_fifo; 187 | 188 | if(reset) 189 | *reset = 0; 190 | 191 | result = dmp_get_fifo_length(s, &in_fifo); 192 | if (result) { 193 | s->fifo_info.fifoError = result; 194 | return 0; 195 | } 196 | 197 | // Nothing to read 198 | if (in_fifo == 0){ 199 | if(reset) 200 | *reset = 1; 201 | return 0; 202 | } 203 | 204 | /* Check if buffer is able to be filled in with in_fifo bytes */ 205 | if (in_fifo > length) { 206 | dmp_reset_fifo(s); 207 | s->fifo_info.fifoError = -1; 208 | if(reset) 209 | *reset = 1; 210 | return 0; 211 | } 212 | 213 | result = dmp_read_fifo(s, buffer, in_fifo); 214 | if (result) { 215 | s->fifo_info.fifoError = result; 216 | return 0; 217 | } 218 | return in_fifo; 219 | } 220 | 221 | /** Determines the packet size by decoding the header. Both header and header2 are set. header2 is set to zero 222 | * if it doesn't exist. sample_cnt_array is filled in if not null with number of samples expected for each sensor 223 | */ 224 | static uint_fast16_t get_packet_size_and_samplecnt(unsigned char *data, unsigned short *header, unsigned short *header2, unsigned short * sample_cnt_array) 225 | { 226 | int sz = HEADER_SZ; // 2 for header 227 | 228 | *header = (((unsigned short)data[0])<<8) | data[1]; 229 | 230 | if (*header & ACCEL_SET) { 231 | sz += ACCEL_DATA_SZ; 232 | if (sample_cnt_array) 233 | sample_cnt_array[ANDROID_SENSOR_ACCELEROMETER]++; 234 | if (sample_cnt_array) 235 | sample_cnt_array[ANDROID_SENSOR_RAW_ACCELEROMETER]++; 236 | } 237 | 238 | if (*header & GYRO_SET) { 239 | sz += GYRO_DATA_SZ; 240 | if (sample_cnt_array) 241 | sample_cnt_array[ANDROID_SENSOR_GYROSCOPE_UNCALIBRATED]++; 242 | sz += GYRO_BIAS_DATA_SZ; 243 | if (sample_cnt_array) 244 | sample_cnt_array[ANDROID_SENSOR_GYROSCOPE]++; 245 | if (sample_cnt_array) 246 | sample_cnt_array[ANDROID_SENSOR_RAW_GYROSCOPE]++; 247 | } 248 | 249 | if (*header & CPASS_SET) { 250 | sz += CPASS_DATA_SZ; 251 | if (sample_cnt_array) 252 | sample_cnt_array[ANDROID_SENSOR_MAGNETIC_FIELD_UNCALIBRATED]++; 253 | } 254 | 255 | if (*header & ALS_SET) { 256 | sz += ALS_DATA_SZ; 257 | if (sample_cnt_array) 258 | sample_cnt_array[ANDROID_SENSOR_LIGHT]++; 259 | } 260 | 261 | if (*header & QUAT6_SET) { 262 | sz += QUAT6_DATA_SZ; 263 | if (sample_cnt_array) 264 | sample_cnt_array[ANDROID_SENSOR_GAME_ROTATION_VECTOR]++; 265 | } 266 | 267 | if (*header & QUAT9_SET) { 268 | sz += QUAT9_DATA_SZ; 269 | if (sample_cnt_array) 270 | sample_cnt_array[ANDROID_SENSOR_ROTATION_VECTOR]++; 271 | } 272 | 273 | if (*header & PQUAT6_SET) 274 | sz += PQUAT6_DATA_SZ; 275 | 276 | if (*header & GEOMAG_SET) { 277 | sz += GEOMAG_DATA_SZ; 278 | if (sample_cnt_array) 279 | sample_cnt_array[ANDROID_SENSOR_GEOMAGNETIC_ROTATION_VECTOR]++; 280 | } 281 | 282 | if (*header & CPASS_CALIBR_SET) { 283 | sz += CPASS_CALIBR_DATA_SZ; 284 | if (sample_cnt_array) 285 | sample_cnt_array[ANDROID_SENSOR_GEOMAGNETIC_FIELD]++; 286 | } 287 | 288 | if (*header & PED_STEPDET_SET) { 289 | sz += PED_STEPDET_TIMESTAMP_SZ; 290 | if (sample_cnt_array) 291 | sample_cnt_array[ANDROID_SENSOR_STEP_DETECTOR]++; 292 | } 293 | 294 | if (*header & HEADER2_SET) { 295 | *header2 = (((unsigned short)data[2])<<8) | data[3]; 296 | sz += HEADER2_SZ; 297 | } else { 298 | *header2 = 0; 299 | } 300 | 301 | if (*header2 & ACCEL_ACCURACY_SET) { 302 | sz += ACCEL_ACCURACY_SZ; 303 | } 304 | if (*header2 & GYRO_ACCURACY_SET) { 305 | sz += GYRO_ACCURACY_SZ; 306 | } 307 | if (*header2 & CPASS_ACCURACY_SET) { 308 | sz += CPASS_ACCURACY_SZ; 309 | } 310 | if (*header2 & FLIP_PICKUP_SET) { 311 | sz += FLIP_PICKUP_SZ; 312 | if (sample_cnt_array) 313 | sample_cnt_array[ANDROID_SENSOR_FLIP_PICKUP]++; 314 | } 315 | if (*header2 & ACT_RECOG_SET) { 316 | sz += ACT_RECOG_SZ; 317 | if (sample_cnt_array) 318 | sample_cnt_array[ANDROID_SENSOR_ACTIVITY_CLASSIFICATON]++; 319 | } 320 | sz += ODR_CNT_GYRO_SZ; 321 | 322 | return sz; 323 | } 324 | 325 | static int check_fifo_decoded_headers(unsigned short header, unsigned short header2) 326 | { 327 | unsigned short header_bit_mask = 0; 328 | unsigned short header2_bit_mask = 0; 329 | 330 | // at least 1 bit must be set 331 | if (header == 0) 332 | return -1; 333 | 334 | header_bit_mask |= ACCEL_SET; 335 | header_bit_mask |= GYRO_SET; 336 | header_bit_mask |= CPASS_SET; 337 | header_bit_mask |= ALS_SET; 338 | header_bit_mask |= QUAT6_SET; 339 | header_bit_mask |= QUAT9_SET; 340 | header_bit_mask |= PQUAT6_SET; 341 | header_bit_mask |= GEOMAG_SET; 342 | header_bit_mask |= GYRO_CALIBR_SET; 343 | header_bit_mask |= CPASS_CALIBR_SET; 344 | header_bit_mask |= PED_STEPDET_SET; 345 | header_bit_mask |= HEADER2_SET; 346 | 347 | if (header & ~header_bit_mask) 348 | return -1; 349 | 350 | // at least 1 bit must be set if header 2 is set 351 | if (header & HEADER2_SET) { 352 | header2_bit_mask |= ACCEL_ACCURACY_SET; 353 | header2_bit_mask |= GYRO_ACCURACY_SET; 354 | header2_bit_mask |= CPASS_ACCURACY_SET; 355 | header2_bit_mask |= FLIP_PICKUP_SET; 356 | header2_bit_mask |= ACT_RECOG_SET; 357 | if (header2 == 0) 358 | return -1; 359 | if (header2 & ~header2_bit_mask) 360 | return -1; 361 | } 362 | 363 | return 0; 364 | } 365 | 366 | /** Software FIFO, mirror of DMP HW FIFO, hence of max HARDWARE_FIFO_SIZE */ 367 | static unsigned char fifo_data[HARDWARE_FIFO_SIZE]; 368 | 369 | /** Determine number of samples present in SW FIFO fifo_data containing fifo_size bytes to be analyzed. Total number 370 | * of samples filled in total_sample_cnt, number of samples per sensor filled in sample_cnt_array array 371 | */ 372 | static int extract_sample_cnt(struct inv_icm20948 * s, int fifo_size, unsigned short * total_sample_cnt, unsigned short * sample_cnt_array) 373 | { 374 | // Next SW FIFO index to be parsed 375 | int fifo_idx = 0; 376 | 377 | while (fifo_idx < fifo_size) { 378 | unsigned short header; 379 | unsigned short header2; 380 | int need_sz = get_packet_size_and_samplecnt(&fifo_data[fifo_idx], &header, &header2, sample_cnt_array); 381 | 382 | // Guarantee there is a full packet before continuing to decode the FIFO packet 383 | if (fifo_size-fifo_idx < need_sz) 384 | goto endSuccess; 385 | 386 | // Decode any error 387 | if (check_fifo_decoded_headers(header, header2)) { 388 | // in that case, stop processing, we might have overflowed so following bytes are non sense 389 | dmp_reset_fifo(s); 390 | return -1; 391 | } 392 | 393 | fifo_idx += need_sz; 394 | 395 | // One sample found, increment total sample counter 396 | (*total_sample_cnt)++; 397 | } 398 | 399 | endSuccess: 400 | // Augmented sensors are not part of DMP FIFO, they are computed by DMP driver based on GRV or RV presence in DMP FIFO 401 | // So their sample counts must rely on GRV and RV sample counts 402 | if (sample_cnt_array) { 403 | sample_cnt_array[ANDROID_SENSOR_GRAVITY] += sample_cnt_array[ANDROID_SENSOR_GAME_ROTATION_VECTOR]; 404 | sample_cnt_array[ANDROID_SENSOR_LINEAR_ACCELERATION] += sample_cnt_array[ANDROID_SENSOR_GAME_ROTATION_VECTOR]; 405 | sample_cnt_array[ANDROID_SENSOR_ORIENTATION] += sample_cnt_array[ANDROID_SENSOR_ROTATION_VECTOR]; 406 | } 407 | 408 | return 0; 409 | } 410 | 411 | int inv_icm20948_fifo_swmirror(struct inv_icm20948 * s, int *fifo_sw_size, unsigned short * total_sample_cnt, unsigned short * sample_cnt_array) 412 | { 413 | int reset=0; 414 | 415 | *total_sample_cnt = 0; 416 | 417 | // Mirror HW FIFO into local SW FIFO, taking into account remaining *fifo_sw_size bytes still present in SW FIFO 418 | if (*fifo_sw_size < HARDWARE_FIFO_SIZE ) { 419 | *fifo_sw_size += dmp_get_fifo_all(s, (HARDWARE_FIFO_SIZE - *fifo_sw_size),&fifo_data[*fifo_sw_size],&reset); 420 | 421 | if (reset) 422 | goto error; 423 | } 424 | 425 | // SW FIFO is mirror, we can now parse it to extract total number of samples and number of samples per sensor 426 | if (extract_sample_cnt(s, *fifo_sw_size, total_sample_cnt, sample_cnt_array)) 427 | goto error; 428 | 429 | return MPU_SUCCESS; 430 | 431 | error: 432 | *fifo_sw_size = 0; 433 | return -1; 434 | 435 | } 436 | 437 | int inv_icm20948_fifo_pop(struct inv_icm20948 * s, unsigned short *user_header, unsigned short *user_header2, int *fifo_sw_size) 438 | { 439 | int need_sz=0; // size in bytes of packet to be analyzed from FIFO 440 | unsigned char *fifo_ptr = fifo_data; // pointer to next byte in SW FIFO to be parsed 441 | 442 | if (*fifo_sw_size > 3) { 443 | // extract headers and number of bytes requested by next sample present in FIFO 444 | need_sz = get_packet_size_and_samplecnt(fifo_data, &fd.header, &fd.header2, 0); 445 | 446 | // Guarantee there is a full packet before continuing to decode the FIFO packet 447 | if (*fifo_sw_size < need_sz) { 448 | return s->fifo_info.fifoError; 449 | } 450 | 451 | fifo_ptr += HEADER_SZ; 452 | if (fd.header & HEADER2_SET) 453 | fifo_ptr += HEADER2_SZ; 454 | 455 | // extract payload data from SW FIFO 456 | fifo_ptr += inv_icm20948_inv_decode_one_ivory_fifo_packet(s, &fd, fifo_ptr); 457 | 458 | // remove first need_sz bytes from SW FIFO 459 | *fifo_sw_size -= need_sz; 460 | if(*fifo_sw_size) 461 | memmove(fifo_data, &fifo_data[need_sz], *fifo_sw_size);// Data left in FIFO 462 | 463 | *user_header = fd.header; 464 | *user_header2 = fd.header2; 465 | } 466 | 467 | return MPU_SUCCESS; 468 | } 469 | 470 | int inv_icm20948_dmp_process_fifo(struct inv_icm20948 * s, int *left_in_fifo, unsigned short *user_header, unsigned short *user_header2, long *time_stamp) 471 | { 472 | int result = MPU_SUCCESS; 473 | int reset=0; 474 | int need_sz=0; 475 | unsigned char *fifo_ptr = fifo_data; 476 | 477 | long long ts=0; 478 | 479 | if(!left_in_fifo) 480 | return -1; 481 | 482 | if (*left_in_fifo < HARDWARE_FIFO_SIZE ) 483 | { 484 | *left_in_fifo += dmp_get_fifo_all(s, (HARDWARE_FIFO_SIZE - *left_in_fifo),&fifo_data[*left_in_fifo],&reset); 485 | //sprintf(test_str, "Left in FIFO: %d\r\n",*left_in_fifo); 486 | //print_command_console(test_str); 487 | if (reset) 488 | { 489 | *left_in_fifo = 0; 490 | return -1; 491 | } 492 | } 493 | 494 | if (*left_in_fifo > 3) { 495 | // no need to extract number of sample per sensor for current function, so provide 0 as last parameter 496 | need_sz = get_packet_size_and_samplecnt(fifo_data, &fd.header, &fd.header2, 0); 497 | 498 | // Guarantee there is a full packet before continuing to decode the FIFO packet 499 | if (*left_in_fifo < need_sz) { 500 | result = s->fifo_info.fifoError; 501 | s->fifo_info.fifoError = 0; 502 | return result; 503 | } 504 | 505 | if(user_header) 506 | *user_header = fd.header; 507 | 508 | if(user_header2) 509 | *user_header2 = fd.header2; 510 | 511 | if (check_fifo_decoded_headers(fd.header, fd.header2)) { 512 | // Decode error 513 | dmp_reset_fifo(s); 514 | *left_in_fifo = 0; 515 | return -1; 516 | } 517 | 518 | fifo_ptr += HEADER_SZ; 519 | 520 | if (fd.header & HEADER2_SET) 521 | fifo_ptr += HEADER2_SZ; 522 | 523 | //time stamp 524 | ts = inv_icm20948_get_tick_count(); 525 | 526 | fifo_ptr += inv_icm20948_inv_decode_one_ivory_fifo_packet(s, &fd, fifo_ptr); 527 | 528 | if(time_stamp) 529 | *time_stamp = ts; 530 | 531 | /* Parse the data in the fifo, in the order of the data control register, starting with the MSB(accel) 532 | */ 533 | 534 | 535 | *left_in_fifo -= need_sz; 536 | if (*left_in_fifo) 537 | memmove(fifo_data, &fifo_data[need_sz], *left_in_fifo);// Data left in FIFO 538 | } 539 | 540 | return result; 541 | } 542 | 543 | static void inv_decode_3_32bit_elements(long *out_data, const unsigned char *in_data) 544 | { 545 | out_data[0] = ((long)(0xff & in_data[0]) << 24) | ((long)(0xff & in_data[1]) << 16) | ((long)(0xff & in_data[2]) << 8) | (0xff & in_data[3]); 546 | out_data[1] = ((long)(0xff & in_data[4]) << 24) | ((long)(0xff & in_data[5]) << 16) | ((long)(0xff & in_data[6]) << 8) | (0xff & in_data[7]); 547 | out_data[2] = ((long)(0xff & in_data[8]) << 24) | ((long)(0xff & in_data[9]) << 16) | ((long)(0xff & in_data[10]) << 8) | (0xff & in_data[11]); 548 | } 549 | static void inv_decode_3_16bit_elements(short *out_data, const unsigned char *in_data) 550 | { 551 | out_data[0] = ((short)(0xff & in_data[0]) << 8) | (0xff & in_data[1]); 552 | out_data[1] = ((short)(0xff & in_data[2]) << 8) | (0xff & in_data[3]); 553 | out_data[2] = ((short)(0xff & in_data[4]) << 8) | (0xff & in_data[5]); 554 | } 555 | 556 | /** Decodes one packet of data from Ivory FIFO 557 | * @param[in] fd Structure to be filled out with data. Assumes header and header2 are already set inside. 558 | * @param[in] fifo_ptr FIFO data, points to just after any header information 559 | * @return Returns the number of bytes consumed in FIFO data. 560 | */ 561 | int inv_icm20948_inv_decode_one_ivory_fifo_packet(struct inv_icm20948 * s, struct inv_fifo_decoded_t *fd, const unsigned char *fifo_ptr) 562 | { 563 | const unsigned char *fifo_ptr_start = fifo_ptr; 564 | short odr_cntr; 565 | if (fd->header & ACCEL_SET) { 566 | // do not cast data here, do that when you use it 567 | inv_decode_3_16bit_elements(fd->accel_s, fifo_ptr); 568 | fd->accel[0] = fd->accel_s[0] << 15; 569 | fd->accel[1] = fd->accel_s[1] << 15; 570 | fd->accel[2] = fd->accel_s[2] << 15; 571 | fifo_ptr += ACCEL_DATA_SZ; 572 | } 573 | 574 | if (fd->header & GYRO_SET) { 575 | inv_decode_3_16bit_elements(fd->gyro, fifo_ptr); 576 | fifo_ptr += GYRO_DATA_SZ; 577 | inv_decode_3_16bit_elements(fd->gyro_bias, fifo_ptr); 578 | fifo_ptr += GYRO_BIAS_DATA_SZ; 579 | } 580 | 581 | if (fd->header & CPASS_SET) { 582 | inv_decode_3_16bit_elements(fd->cpass_raw_data, fifo_ptr); 583 | inv_icm20948_apply_raw_compass_matrix(s, fd->cpass_raw_data, fd->compass); 584 | memcpy( fd->cpass_calibr_6chars, fifo_ptr, 6*sizeof(unsigned char)); 585 | fifo_ptr += CPASS_DATA_SZ; 586 | } 587 | 588 | if(fd->header & ALS_SET) { 589 | fifo_ptr += ALS_DATA_SZ; 590 | } 591 | 592 | if (fd->header & QUAT6_SET) { 593 | inv_decode_3_32bit_elements(fd->dmp_3e_6quat, fifo_ptr); 594 | fifo_ptr += QUAT6_DATA_SZ; 595 | } 596 | 597 | if (fd->header & QUAT9_SET) { 598 | inv_decode_3_32bit_elements(fd->dmp_3e_9quat, fifo_ptr); 599 | fd->dmp_rv_accuracyQ29 = ((0xff & fifo_ptr[12]) << 24) | ((0xff & fifo_ptr[13]) << 16); 600 | fifo_ptr += QUAT9_DATA_SZ; 601 | } 602 | 603 | if (fd->header & PED_STEPDET_SET) { 604 | fd->ped_step_det_ts = ((0xff & fifo_ptr[0]) << 24) | ((0xff & fifo_ptr[1]) << 16) | ((0xff & fifo_ptr[2]) << 8) | (0xff & fifo_ptr[3]); 605 | fifo_ptr += PED_STEPDET_TIMESTAMP_SZ; 606 | } 607 | 608 | if (fd->header & GEOMAG_SET) { 609 | inv_decode_3_32bit_elements(fd->dmp_3e_geomagquat, fifo_ptr); 610 | fd->dmp_geomag_accuracyQ29 = ((0xff & fifo_ptr[12]) << 24) | ((0xff & fifo_ptr[13]) << 16); 611 | fifo_ptr += GEOMAG_DATA_SZ; 612 | } 613 | 614 | if(fd->header & PRESSURE_SET) { 615 | fifo_ptr += PRESSURE_DATA_SZ; 616 | } 617 | if (fd->header & CPASS_CALIBR_SET) { 618 | inv_decode_3_32bit_elements(fd->cpass_calibr, fifo_ptr); 619 | memcpy( fd->cpass_calibr_12chars, fifo_ptr, 12*sizeof(unsigned char)); 620 | fifo_ptr += CPASS_CALIBR_DATA_SZ; 621 | } 622 | 623 | if (fd->header2 & ACCEL_ACCURACY_SET) { 624 | fd->accel_accuracy = ((0xff & fifo_ptr[0]) << 8) | (0xff & fifo_ptr[1]); 625 | fifo_ptr += ACCEL_ACCURACY_SZ; 626 | } 627 | 628 | if (fd->header2 & GYRO_ACCURACY_SET) { 629 | fd->gyro_accuracy = ((0xff & fifo_ptr[0]) << 8) | (0xff & fifo_ptr[1]); 630 | fifo_ptr += GYRO_ACCURACY_SZ; 631 | } 632 | 633 | if (fd->header2 & CPASS_ACCURACY_SET) { 634 | fd->cpass_accuracy = ((0xff & fifo_ptr[0]) << 8) | (0xff & fifo_ptr[1]); 635 | fifo_ptr += CPASS_ACCURACY_SZ; 636 | } 637 | 638 | if (fd->header2 & FLIP_PICKUP_SET) { 639 | fd->flip_pickup = ((0xff & fifo_ptr[0]) << 8) | (0xff & fifo_ptr[1]); 640 | fifo_ptr += FLIP_PICKUP_SZ; 641 | } 642 | 643 | if (fd->header2 & ACT_RECOG_SET) { 644 | fd->bac_state = ((0xff & fifo_ptr[0]) << 8) | (0xff & fifo_ptr[1]); 645 | fd->bac_ts = ((0xff & fifo_ptr[2]) << 24) | ((0xff & fifo_ptr[3]) << 16) | ((0xff & fifo_ptr[4]) << 8) | (0xff & fifo_ptr[5]); 646 | fifo_ptr += ACT_RECOG_SZ; 647 | } 648 | 649 | odr_cntr = ((0xff & fifo_ptr[0]) << 8) | (0xff & fifo_ptr[1]); 650 | // odr_cntr_gyro is odr_cntr & 0xfff 651 | // 9KHz cnt is odr_cntr >> 12 652 | // not used for now, needed only for FSYNC purpose 653 | (void)odr_cntr; 654 | fifo_ptr += FOOTER_SZ; 655 | 656 | fd->new_data = 1; // Record a new data set 657 | 658 | return fifo_ptr-fifo_ptr_start; 659 | } 660 | 661 | int inv_icm20948_dmp_get_accel(long acl[3]) 662 | { 663 | if(!acl) return -1; 664 | memcpy( acl, fd.accel, 3*sizeof(long)); 665 | return MPU_SUCCESS; 666 | } 667 | 668 | int inv_icm20948_dmp_get_raw_gyro(short raw_gyro[3]) 669 | { 670 | if(!raw_gyro) return -1; 671 | raw_gyro[0] = fd.gyro[0]; 672 | raw_gyro[1] = fd.gyro[1]; 673 | raw_gyro[2] = fd.gyro[2]; 674 | return MPU_SUCCESS; 675 | } 676 | 677 | 678 | int inv_icm20948_dmp_get_gyro_bias(short gyro_bias[3]) 679 | { 680 | if(!gyro_bias) return -1; 681 | memcpy(gyro_bias, fd.gyro_bias, 3*sizeof(short)); 682 | return MPU_SUCCESS; 683 | } 684 | 685 | 686 | int inv_icm20948_dmp_get_calibrated_gyro(signed long calibratedData[3], signed long raw[3], signed long bias[3]) 687 | { 688 | if(!calibratedData) return -1; 689 | if(!raw) return -1; 690 | if(!bias) return -1; 691 | 692 | calibratedData[0] = raw[0] - bias[0]; 693 | calibratedData[1] = raw[1] - bias[1]; 694 | calibratedData[2] = raw[2] - bias[2]; 695 | 696 | return MPU_SUCCESS; 697 | } 698 | 699 | int inv_icm20948_dmp_get_6quaternion(long quat[3]) 700 | { 701 | if(!quat) return -1; 702 | memcpy( quat, fd.dmp_3e_6quat, sizeof(fd.dmp_3e_6quat)); 703 | return MPU_SUCCESS; 704 | } 705 | 706 | int inv_icm20948_dmp_get_9quaternion(long quat[3]) 707 | { 708 | if(!quat) return -1; 709 | memcpy( quat, fd.dmp_3e_9quat, sizeof(fd.dmp_3e_9quat)); 710 | return MPU_SUCCESS; 711 | } 712 | 713 | int inv_icm20948_dmp_get_gmrvquaternion(long quat[3]) 714 | { 715 | if(!quat) return -1; 716 | memcpy( quat, fd.dmp_3e_geomagquat, sizeof(fd.dmp_3e_geomagquat)); 717 | return MPU_SUCCESS; 718 | } 719 | 720 | int inv_icm20948_dmp_get_raw_compass(long raw_compass[3]) 721 | { 722 | if(!raw_compass) return -1; 723 | memcpy( raw_compass, fd.compass, 3*sizeof(long)); 724 | return MPU_SUCCESS; 725 | } 726 | 727 | int inv_icm20948_dmp_get_calibrated_compass(long cal_compass[3]) 728 | { 729 | if(!cal_compass) return -1; 730 | memcpy( cal_compass, fd.cpass_calibr, 3*sizeof(long)); 731 | return MPU_SUCCESS; 732 | } 733 | 734 | int inv_icm20948_dmp_get_bac_state(uint16_t *bac_state) 735 | { 736 | if(!bac_state) return -1; 737 | *bac_state = fd.bac_state; 738 | return 0; 739 | } 740 | 741 | int inv_icm20948_dmp_get_bac_ts(long *bac_ts) 742 | { 743 | if(!bac_ts) return -1; 744 | *bac_ts = fd.bac_ts; 745 | return 0; 746 | } 747 | 748 | int inv_icm20948_dmp_get_flip_pickup_state(uint16_t *flip_pickup) 749 | { 750 | if(!flip_pickup) return -1; 751 | *flip_pickup = fd.flip_pickup; 752 | return 0; 753 | } 754 | 755 | /** Returns accuracy of accel. 756 | * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate. 757 | */ 758 | int inv_icm20948_get_accel_accuracy(void) 759 | { 760 | return fd.accel_accuracy; 761 | } 762 | 763 | /** Returns accuracy of gyro. 764 | * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate. 765 | */ 766 | int inv_icm20948_get_gyro_accuracy(void) 767 | { 768 | return fd.gyro_accuracy; 769 | } 770 | 771 | /** Returns accuracy of compass. 772 | * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate. 773 | */ 774 | int inv_icm20948_get_mag_accuracy(void) 775 | { 776 | return fd.cpass_accuracy; 777 | } 778 | 779 | /** Returns accuracy of geomagnetic rotation vector. 780 | * @return Accuracy of GMRV in Q29. 781 | */ 782 | int inv_icm20948_get_gmrv_accuracy(void) 783 | { 784 | return fd.dmp_geomag_accuracyQ29; 785 | } 786 | 787 | /** Returns accuracy of rotation vector. 788 | * @return Accuracy of RV in Q29. 789 | */ 790 | int inv_icm20948_get_rv_accuracy(void) 791 | { 792 | return fd.dmp_rv_accuracyQ29; 793 | } 794 | -------------------------------------------------------------------------------- /ICM20948/Icm20948MPUFifoControl.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948MPUFifoControl.h -------------------------------------------------------------------------------- /ICM20948/Icm20948SelfTest.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948SelfTest.c -------------------------------------------------------------------------------- /ICM20948/Icm20948SelfTest.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/2a1cf5707dfb1a6c884f6fefd3af221b46d35a88/ICM20948/Icm20948SelfTest.h -------------------------------------------------------------------------------- /ICM20948/Icm20948Serif.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | /** @defgroup DriverIcm20948Serif Icm20948 driver serif 25 | * @brief Interface for low-level serial (I2C/SPI) access 26 | * @ingroup DriverIcm20948 27 | * @{ 28 | */ 29 | 30 | #ifndef _INV_ICM20948_SERIF_H_ 31 | #define _INV_ICM20948_SERIF_H_ 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | #include "InvBool.h" 38 | #include "InvError.h" 39 | 40 | #include 41 | #include 42 | 43 | /** @brief ICM20948 serial interface 44 | */ 45 | struct inv_icm20948_serif { 46 | void * context; 47 | int (*read_reg)(void * context, uint8_t reg, uint8_t * buf, uint32_t len); 48 | int (*write_reg)(void * context, uint8_t reg, const uint8_t * buf, uint32_t len); 49 | uint32_t max_read; 50 | uint32_t max_write; 51 | inv_bool_t is_spi; 52 | }; 53 | 54 | static inline inv_bool_t inv_icm20948_serif_is_spi(struct inv_icm20948_serif * s) 55 | { 56 | assert(s); 57 | 58 | return s->is_spi; 59 | } 60 | 61 | static inline uint32_t inv_icm20948_serif_max_read(struct inv_icm20948_serif * s) 62 | { 63 | assert(s); 64 | 65 | return s->max_read; 66 | } 67 | 68 | static inline uint32_t inv_icm20948_serif_max_write(struct inv_icm20948_serif * s) 69 | { 70 | assert(s); 71 | 72 | return s->max_write; 73 | } 74 | 75 | static inline int inv_icm20948_serif_read_reg(struct inv_icm20948_serif * s, 76 | uint8_t reg, uint8_t * buf, uint32_t len) 77 | { 78 | assert(s); 79 | 80 | if(len > s->max_read) 81 | return INV_ERROR_SIZE; 82 | 83 | if(s->read_reg(s->context, reg, buf, len) != 0) 84 | return INV_ERROR_TRANSPORT; 85 | 86 | return 0; 87 | } 88 | 89 | static inline int inv_icm20948_serif_write_reg(struct inv_icm20948_serif * s, 90 | uint8_t reg, const uint8_t * buf, uint32_t len) 91 | { 92 | assert(s); 93 | 94 | if(len > s->max_write) 95 | return INV_ERROR_SIZE; 96 | 97 | if(s->write_reg(s->context, reg, buf, len) != 0) 98 | return INV_ERROR_TRANSPORT; 99 | 100 | return 0; 101 | } 102 | 103 | #ifdef __cplusplus 104 | } 105 | #endif 106 | 107 | #endif /* _INV_ICM20948_SERIF_H_ */ 108 | 109 | /** @} */ 110 | -------------------------------------------------------------------------------- /ICM20948/Icm20948Setup.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | /** @defgroup DriverIcm20948Setup Icm20948 driver setup 25 | * @brief Low-level function to setup an Icm20948 device 26 | * @ingroup DriverIcm20948 27 | * @{ 28 | */ 29 | 30 | #ifndef _INV_ICM20948_SETUP_H_ 31 | #define _INV_ICM20948_SETUP_H_ 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | #include 38 | 39 | #include "InvExport.h" 40 | #include "InvBool.h" 41 | 42 | /* forward declaration */ 43 | struct inv_icm20948; 44 | 45 | /** @brief Sensor identifier for control function 46 | */ 47 | enum inv_icm20948_sensor { 48 | INV_ICM20948_SENSOR_ACCELEROMETER, 49 | INV_ICM20948_SENSOR_GYROSCOPE, 50 | INV_ICM20948_SENSOR_RAW_ACCELEROMETER, 51 | INV_ICM20948_SENSOR_RAW_GYROSCOPE, 52 | INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED, 53 | INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED, 54 | INV_ICM20948_SENSOR_ACTIVITY_CLASSIFICATON, 55 | INV_ICM20948_SENSOR_STEP_DETECTOR, 56 | INV_ICM20948_SENSOR_STEP_COUNTER, 57 | INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR, 58 | INV_ICM20948_SENSOR_ROTATION_VECTOR, 59 | INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR, 60 | INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD, 61 | INV_ICM20948_SENSOR_WAKEUP_SIGNIFICANT_MOTION, 62 | INV_ICM20948_SENSOR_FLIP_PICKUP, 63 | INV_ICM20948_SENSOR_WAKEUP_TILT_DETECTOR, 64 | INV_ICM20948_SENSOR_GRAVITY, 65 | INV_ICM20948_SENSOR_LINEAR_ACCELERATION, 66 | INV_ICM20948_SENSOR_ORIENTATION, 67 | INV_ICM20948_SENSOR_B2S, 68 | INV_ICM20948_SENSOR_MAX, 69 | }; 70 | 71 | int INV_EXPORT inv_icm20948_get_whoami(struct inv_icm20948 * s, uint8_t * whoami); 72 | void INV_EXPORT inv_icm20948_init_matrix(struct inv_icm20948 * s); 73 | int INV_EXPORT inv_icm20948_set_matrix(struct inv_icm20948 * s, const float matrix[9], enum inv_icm20948_sensor sensor); 74 | int INV_EXPORT inv_icm20948_initialize(struct inv_icm20948 * s, const uint8_t *dmp3_image, uint32_t dmp3_image_size); 75 | int INV_EXPORT inv_icm20948_init_scale(struct inv_icm20948 * s); 76 | // int INV_EXPORT inv_icm20948_set_wom_threshold(struct inv_icm20948 * s, uint8_t threshold); 77 | int INV_EXPORT inv_icm20948_set_fsr(struct inv_icm20948 * s, enum inv_icm20948_sensor sensor, const void * fsr); 78 | int INV_EXPORT inv_icm20948_get_fsr(struct inv_icm20948 * s, enum inv_icm20948_sensor sensor, const void * fsr); 79 | int INV_EXPORT inv_icm20948_set_bias(struct inv_icm20948 * s, enum inv_icm20948_sensor sensor, const void * bias); 80 | int INV_EXPORT inv_icm20948_get_bias(struct inv_icm20948 * s, enum inv_icm20948_sensor sensor, void * bias); 81 | int INV_EXPORT inv_icm20948_initialize_auxiliary(struct inv_icm20948 * s); 82 | int INV_EXPORT inv_icm20948_soft_reset(struct inv_icm20948 * s); 83 | int INV_EXPORT inv_icm20948_enable_sensor(struct inv_icm20948 * s, enum inv_icm20948_sensor sensor, inv_bool_t state); 84 | int INV_EXPORT inv_icm20948_set_sensor_period(struct inv_icm20948 * s, enum inv_icm20948_sensor sensor, uint32_t period); 85 | int INV_EXPORT inv_icm20948_enable_batch_timeout(struct inv_icm20948 * s, unsigned short batchTimeoutMs); 86 | int INV_EXPORT inv_icm20948_poll_sensor(struct inv_icm20948 * s, void * context, 87 | void (*handler)(void * context, enum inv_icm20948_sensor sensor, uint64_t timestamp, const void * data, const void *arg)); 88 | int INV_EXPORT inv_icm20948_load(struct inv_icm20948 * s, const uint8_t * image, unsigned short size); 89 | int INV_EXPORT inv_icm20948_init_structure(struct inv_icm20948 * s); 90 | enum inv_icm20948_sensor INV_EXPORT inv_icm20948_sensor_android_2_sensor_type(int sensor); 91 | /** @brief Have the chip to enter low-power or low-noise mode 92 | * @param[in] lowpower_or_highperformance 0=low-power, 1=low-noise 93 | */ 94 | int INV_EXPORT inv_icm20948_set_lowpower_or_highperformance(struct inv_icm20948 * s, uint8_t lowpower_or_highperformance); 95 | int INV_EXPORT inv_icm20948_get_lowpower_or_highperformance(struct inv_icm20948 * s, uint8_t * lowpower_or_highperformance); 96 | #ifdef __cplusplus 97 | } 98 | #endif 99 | 100 | #endif /* _INV_ICM20948_SETUP_H_ */ 101 | 102 | /** @} */ 103 | -------------------------------------------------------------------------------- /ICM20948/Icm20948Transport.c: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | #include "Icm20948Transport.h" 25 | #include "Icm20948Serif.h" 26 | #include "Icm20948.h" 27 | 28 | struct inv_icm20948 * icm20948_instance; 29 | 30 | int inv_icm20948_read_reg(struct inv_icm20948 * s, uint8_t reg, uint8_t * buf, uint32_t len) 31 | { 32 | return inv_icm20948_serif_read_reg(&s->serif, reg, buf, len); 33 | } 34 | 35 | int inv_icm20948_write_reg(struct inv_icm20948 * s, uint8_t reg, const uint8_t * buf, uint32_t len) 36 | { 37 | return inv_icm20948_serif_write_reg(&s->serif, reg, buf, len); 38 | } 39 | 40 | void inv_icm20948_sleep_100us(unsigned long nHowMany100MicroSecondsToSleep) // time in 100 us 41 | { 42 | inv_icm20948_sleep_us(nHowMany100MicroSecondsToSleep * 100); 43 | } 44 | 45 | long inv_icm20948_get_tick_count(void) 46 | { 47 | return (long)inv_icm20948_get_time_us(); 48 | } 49 | 50 | /* driver transport function */ 51 | 52 | #include "Icm20948Defs.h" 53 | #include "Icm20948DataBaseDriver.h" 54 | #include "Icm20948DataBaseControl.h" 55 | 56 | void inv_icm20948_transport_init(struct inv_icm20948 * s) 57 | { 58 | s->lastBank = 0x7E; 59 | s->lLastBankSelected = 0xFF; 60 | } 61 | 62 | static uint8_t check_reg_access_lp_disable(struct inv_icm20948 * s, unsigned short reg) 63 | { 64 | switch(reg){ 65 | case REG_LP_CONFIG: /** (BANK_0 | 0x05) */ 66 | case REG_PWR_MGMT_1: /** (BANK_0 | 0x06) */ 67 | case REG_PWR_MGMT_2: /** (BANK_0 | 0x07) */ 68 | case REG_INT_PIN_CFG: /** (BANK_0 | 0x0F) */ 69 | case REG_INT_ENABLE: /** (BANK_0 | 0x10) */ 70 | case REG_FIFO_COUNT_H: /** (BANK_0 | 0x70) */ 71 | case REG_FIFO_COUNT_L: /** (BANK_0 | 0x71) */ 72 | case REG_FIFO_R_W: /** (BANK_0 | 0x72) */ 73 | return inv_icm20948_ctrl_get_batch_mode_status(s); 74 | case REG_FIFO_CFG: /** (BANK_0 | 0x76) */ 75 | case REG_MEM_BANK_SEL: /** (BANK_0 | 0x7E) */ 76 | case REG_BANK_SEL: /** 0x7F */ 77 | case REG_INT_STATUS: /** (BANK_0 | 0x19) */ 78 | case REG_DMP_INT_STATUS: /** (BANK_0 | 0x18) */ 79 | return 0; 80 | break; 81 | default: 82 | break; 83 | } 84 | return 1; 85 | } 86 | 87 | /** 88 | * @brief Set up the register bank register for accessing registers in 20630. 89 | * @param[in] register bank number 90 | * @return 0 if successful. 91 | */ 92 | 93 | static int inv_set_bank(struct inv_icm20948 * s, unsigned char bank) 94 | { 95 | int result; 96 | //if bank reg was set before, just return 97 | if(bank==s->lastBank) 98 | return 0; 99 | else 100 | s->lastBank = bank; 101 | 102 | result = inv_icm20948_read_reg(s, REG_BANK_SEL, &s->reg, 1); 103 | 104 | if (result) 105 | return result; 106 | 107 | s->reg &= 0xce; 108 | s->reg |= (bank << 4); 109 | result = inv_icm20948_write_reg(s, REG_BANK_SEL, &s->reg, 1); 110 | 111 | return result; 112 | } 113 | 114 | /* the following functions are used for configuring the secondary devices */ 115 | 116 | /** 117 | * @brief Write data to a register on MEMs. 118 | * @param[in] Register address 119 | * @param[in] Length of data 120 | * @param[in] Data to be written 121 | * @return 0 if successful. 122 | */ 123 | int inv_icm20948_write_mems_reg(struct inv_icm20948 * s, uint16_t reg, unsigned int length, const unsigned char *data) 124 | { 125 | int result = 0; 126 | unsigned int bytesWrite = 0; 127 | unsigned char regOnly = (unsigned char)(reg & 0x7F); 128 | 129 | unsigned char power_state = inv_icm20948_get_chip_power_state(s); 130 | 131 | if((power_state & CHIP_AWAKE) == 0) // Wake up chip since it is asleep 132 | result = inv_icm20948_set_chip_power_state(s, CHIP_AWAKE, 1); 133 | 134 | if(check_reg_access_lp_disable(s, reg)) // Check if register needs LP_EN to be disabled 135 | result |= inv_icm20948_set_chip_power_state(s, CHIP_LP_ENABLE, 0); //Disable LP_EN 136 | 137 | result |= inv_set_bank(s, reg >> 7); 138 | 139 | while (bytesWrite> 7); 178 | result |= inv_icm20948_write_reg(s, regOnly, &data, 1); 179 | 180 | if(check_reg_access_lp_disable(s, reg)) //Enable LP_EN since we disabled it at begining of this function. 181 | result |= inv_icm20948_set_chip_power_state(s, CHIP_LP_ENABLE, 1); 182 | 183 | return result; 184 | } 185 | 186 | /** 187 | * @brief Read data from a register on MEMs. 188 | * @param[in] Register address 189 | * @param[in] Length of data 190 | * @param[in] Data to be written 191 | * @return 0 if successful. 192 | */ 193 | int inv_icm20948_read_mems_reg(struct inv_icm20948 * s, uint16_t reg, unsigned int length, unsigned char *data) 194 | { 195 | int result = 0; 196 | unsigned int bytesRead = 0; 197 | unsigned char regOnly = (unsigned char)(reg & 0x7F); 198 | unsigned char i, dat[INV_MAX_SERIAL_READ]; 199 | unsigned char power_state = inv_icm20948_get_chip_power_state(s); 200 | 201 | if((power_state & CHIP_AWAKE) == 0) // Wake up chip since it is asleep 202 | result = inv_icm20948_set_chip_power_state(s, CHIP_AWAKE, 1); 203 | 204 | if(check_reg_access_lp_disable(s, reg)) // Check if register needs LP_EN to be disabled 205 | result |= inv_icm20948_set_chip_power_state(s, CHIP_LP_ENABLE, 0); //Disable LP_EN 206 | 207 | result |= inv_set_bank(s, reg >> 7); 208 | 209 | while (bytesReadbase_state.serial_interface == SERIAL_INTERFACE_SPI) { 213 | result |= inv_icm20948_read_reg(s, regOnly+bytesRead, &dat[bytesRead], thisLen); 214 | } else { 215 | result |= inv_icm20948_read_reg(s, regOnly+bytesRead, &data[bytesRead],thisLen); 216 | } 217 | 218 | if (result) 219 | return result; 220 | 221 | bytesRead += thisLen; 222 | } 223 | 224 | if(s->base_state.serial_interface == SERIAL_INTERFACE_SPI) { 225 | for (i=0; i< length; i++) { 226 | *data= dat[i]; 227 | data++; 228 | } 229 | } 230 | 231 | if(check_reg_access_lp_disable(s, reg)) // Check if register needs LP_EN to be enabled 232 | result |= inv_icm20948_set_chip_power_state(s, CHIP_LP_ENABLE, 1); //Enable LP_EN 233 | 234 | return result; 235 | } 236 | 237 | /** 238 | * @brief Read data from a register in DMP memory 239 | * @param[in] DMP memory address 240 | * @param[in] number of byte to be read 241 | * @param[in] input data from the register 242 | * @return 0 if successful. 243 | */ 244 | int inv_icm20948_read_mems(struct inv_icm20948 * s, unsigned short reg, unsigned int length, unsigned char *data) 245 | { 246 | int result=0; 247 | unsigned int bytesWritten = 0; 248 | unsigned int thisLen; 249 | unsigned char i, dat[INV_MAX_SERIAL_READ] = {0}; 250 | unsigned char power_state = inv_icm20948_get_chip_power_state(s); 251 | unsigned char lBankSelected; 252 | unsigned char lStartAddrSelected; 253 | 254 | if(!data) 255 | return -1; 256 | 257 | if((power_state & CHIP_AWAKE) == 0) // Wake up chip since it is asleep 258 | result = inv_icm20948_set_chip_power_state(s, CHIP_AWAKE, 1); 259 | 260 | if(check_reg_access_lp_disable(s, reg)) 261 | result |= inv_icm20948_set_chip_power_state(s, CHIP_LP_ENABLE, 0); 262 | 263 | result |= inv_set_bank(s, 0); 264 | 265 | lBankSelected = (reg >> 8); 266 | if (lBankSelected != s->lLastBankSelected) 267 | { 268 | result |= inv_icm20948_write_reg(s, REG_MEM_BANK_SEL, &lBankSelected, 1); 269 | if (result) 270 | return result; 271 | s->lLastBankSelected = lBankSelected; 272 | } 273 | 274 | while (bytesWritten < length) 275 | { 276 | lStartAddrSelected = (reg & 0xff); 277 | /* Sets the starting read or write address for the selected memory, inside of the selected page (see MEM_SEL Register). 278 | Contents are changed after read or write of the selected memory. 279 | This register must be written prior to each access to initialize the register to the proper starting address. 280 | The address will auto increment during burst transactions. Two consecutive bursts without re-initializing the start address would skip one address. */ 281 | result |= inv_icm20948_write_reg(s, REG_MEM_START_ADDR, &lStartAddrSelected, 1); 282 | if (result) 283 | return result; 284 | 285 | thisLen = min(INV_MAX_SERIAL_READ, length-bytesWritten); 286 | /* Write data */ 287 | if(s->base_state.serial_interface == SERIAL_INTERFACE_SPI) { 288 | result |= inv_icm20948_read_reg(s, REG_MEM_R_W, &dat[bytesWritten], thisLen); 289 | } else { 290 | result |= inv_icm20948_read_reg(s, REG_MEM_R_W, &data[bytesWritten], thisLen); 291 | } 292 | if (result) 293 | return result; 294 | 295 | bytesWritten += thisLen; 296 | reg += thisLen; 297 | } 298 | 299 | if(s->base_state.serial_interface == SERIAL_INTERFACE_SPI) { 300 | for (i=0; i< length; i++) { 301 | *data= dat[i]; 302 | data++; 303 | } 304 | } 305 | 306 | //Enable LP_EN if we disabled it at begining of this function. 307 | if(check_reg_access_lp_disable(s, reg)) 308 | result |= inv_icm20948_set_chip_power_state(s, CHIP_LP_ENABLE, 1); 309 | 310 | return result; 311 | } 312 | 313 | /** 314 | * @brief Write data to a register in DMP memory 315 | * @param[in] DMP memory address 316 | * @param[in] number of byte to be written 317 | * @param[out] output data from the register 318 | * @return 0 if successful. 319 | */ 320 | int inv_icm20948_write_mems(struct inv_icm20948 * s, unsigned short reg, unsigned int length, const unsigned char *data) 321 | { 322 | int result=0; 323 | unsigned int bytesWritten = 0; 324 | unsigned int thisLen; 325 | unsigned char lBankSelected; 326 | unsigned char lStartAddrSelected; 327 | 328 | unsigned char power_state = inv_icm20948_get_chip_power_state(s); 329 | 330 | if(!data) 331 | return -1; 332 | 333 | if((power_state & CHIP_AWAKE) == 0) // Wake up chip since it is asleep 334 | result = inv_icm20948_set_chip_power_state(s, CHIP_AWAKE, 1); 335 | 336 | result |= inv_icm20948_set_chip_power_state(s, CHIP_LP_ENABLE, 0); 337 | 338 | result |= inv_set_bank(s, 0); 339 | 340 | lBankSelected = (reg >> 8); 341 | if (lBankSelected != s->lLastBankSelected) 342 | { 343 | result |= inv_icm20948_write_reg(s, REG_MEM_BANK_SEL, &lBankSelected, 1); 344 | if (result) 345 | return result; 346 | s->lLastBankSelected = lBankSelected; 347 | } 348 | 349 | while (bytesWritten < length) 350 | { 351 | lStartAddrSelected = (reg & 0xff); 352 | /* Sets the starting read or write address for the selected memory, inside of the selected page (see MEM_SEL Register). 353 | Contents are changed after read or write of the selected memory. 354 | This register must be written prior to each access to initialize the register to the proper starting address. 355 | The address will auto increment during burst transactions. Two consecutive bursts without re-initializing the start address would skip one address. */ 356 | result |= inv_icm20948_write_reg(s, REG_MEM_START_ADDR, &lStartAddrSelected, 1); 357 | if (result) 358 | return result; 359 | 360 | thisLen = min(INV_MAX_SERIAL_WRITE, length-bytesWritten); 361 | 362 | /* Write data */ 363 | result |= inv_icm20948_write_reg(s, REG_MEM_R_W, &data[bytesWritten], thisLen); 364 | if (result) 365 | return result; 366 | 367 | bytesWritten += thisLen; 368 | reg += thisLen; 369 | } 370 | 371 | //Enable LP_EN since we disabled it at begining of this function. 372 | result |= inv_icm20948_set_chip_power_state(s, CHIP_LP_ENABLE, 1); 373 | 374 | return result; 375 | } 376 | 377 | /** 378 | * @brief Write single byte of data to a register on MEMs with no power control 379 | * @param[in] Register address 380 | * @param[in] Data to be written 381 | * @return 0 if successful. 382 | */ 383 | int inv_icm20948_write_single_mems_reg_core(struct inv_icm20948 * s, uint16_t reg, const uint8_t data) 384 | { 385 | int result = 0; 386 | unsigned char regOnly = (unsigned char)(reg & 0x7F); 387 | 388 | result |= inv_set_bank(s, reg >> 7); 389 | result |= inv_icm20948_write_reg(s, regOnly, &data, 1); 390 | 391 | return result; 392 | } 393 | -------------------------------------------------------------------------------- /ICM20948/Icm20948Transport.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | /** @defgroup DriverIcm20948Transport Icm20948 driver transport 25 | * @brief Low-level ICM20948 register access 26 | * @ingroup DriverIcm20948 27 | * @{ 28 | */ 29 | 30 | #ifndef _INV_ICM20948_TRANSPORT_H_ 31 | #define _INV_ICM20948_TRANSPORT_H_ 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | #include "InvExport.h" 38 | #include 39 | 40 | /* forward declaration */ 41 | struct inv_icm20948; 42 | 43 | /** @brief Max size that can be read across I2C or SPI data lines */ 44 | #define INV_MAX_SERIAL_READ 16 45 | /** @brief Max size that can be written across I2C or SPI data lines */ 46 | #define INV_MAX_SERIAL_WRITE 16 47 | 48 | void INV_EXPORT inv_icm20948_transport_init(struct inv_icm20948 * s); 49 | 50 | int INV_EXPORT inv_icm20948_read_reg(struct inv_icm20948 * s, uint8_t reg, uint8_t * buf, uint32_t len); 51 | 52 | int INV_EXPORT inv_icm20948_write_reg(struct inv_icm20948 * s, uint8_t reg, const uint8_t * buf, uint32_t len); 53 | 54 | void INV_EXPORT inv_icm20948_sleep_100us(unsigned long nHowMany100MicroSecondsToSleep); 55 | 56 | long INV_EXPORT inv_icm20948_get_tick_count(void); 57 | 58 | static inline int inv_icm20948_write_reg_one(struct inv_icm20948 * s, uint8_t reg, uint8_t reg_value) 59 | { 60 | return inv_icm20948_write_reg(s, reg, ®_value, 1); 61 | } 62 | 63 | static inline int inv_icm20948_read_reg_one(struct inv_icm20948 * s, uint8_t reg, uint8_t * reg_value) 64 | { 65 | return inv_icm20948_read_reg(s, reg, reg_value, 1); 66 | } 67 | 68 | static inline int inv_icm20948_set_reg_bits(struct inv_icm20948 * s, uint8_t reg, uint8_t bits_mask) 69 | { 70 | int rc; 71 | uint8_t reg_value; 72 | 73 | if((rc = inv_icm20948_read_reg_one(s, reg, ®_value)) != 0) 74 | return rc; 75 | 76 | reg_value |= bits_mask; 77 | 78 | if((rc = inv_icm20948_write_reg_one(s, reg, reg_value)) != 0) 79 | return rc; 80 | 81 | return 0; 82 | } 83 | 84 | static inline int inv_icm20948_clear_reg_bits(struct inv_icm20948 * s, uint8_t reg, uint8_t bits_mask) 85 | { 86 | int rc; 87 | uint8_t reg_value; 88 | 89 | if((rc = inv_icm20948_read_reg_one(s, reg, ®_value)) != 0) 90 | return rc; 91 | 92 | reg_value &= ~bits_mask; 93 | 94 | if((rc = inv_icm20948_write_reg_one(s, reg, reg_value)) != 0) 95 | return rc; 96 | 97 | return 0; 98 | } 99 | 100 | static inline int inv_icm20948_get_reg_bits(struct inv_icm20948 * s, uint8_t reg, 101 | uint8_t bits_mask, uint8_t * bits_mask_state) 102 | { 103 | int rc; 104 | 105 | if((rc = inv_icm20948_read_reg_one(s, reg, bits_mask_state)) != 0) 106 | return rc; 107 | 108 | *bits_mask_state &= bits_mask; 109 | 110 | return 0; 111 | } 112 | 113 | /** 114 | * @brief Write data to a register on MEMs. 115 | * @param[in] Register address 116 | * @param[in] Length of data 117 | * @param[in] Data to be written 118 | * @return 0 if successful. 119 | */ 120 | int INV_EXPORT inv_icm20948_write_mems_reg(struct inv_icm20948 * s, uint16_t reg, unsigned int length, const unsigned char *data); 121 | /** 122 | * @brief Write single byte of data to a register on MEMs. 123 | * @param[in] Register address 124 | * @param[in] Data to be written 125 | * @return 0 if successful. 126 | */ 127 | int INV_EXPORT inv_icm20948_write_single_mems_reg(struct inv_icm20948 * s, uint16_t reg, const unsigned char data); 128 | /** 129 | * @brief Read data from a register on MEMs. 130 | * @param[in] Register address 131 | * @param[in] Length of data 132 | * @param[in] Data to be written 133 | * @return 0 if successful. 134 | */ 135 | int INV_EXPORT inv_icm20948_read_mems_reg(struct inv_icm20948 * s, uint16_t reg, unsigned int length, unsigned char *data); 136 | /** 137 | * @brief Read data from a register in DMP memory 138 | * @param[in] DMP memory address 139 | * @param[in] number of byte to be read 140 | * @param[in] input data from the register 141 | * @return 0 if successful. 142 | */ 143 | int INV_EXPORT inv_icm20948_read_mems(struct inv_icm20948 * s, unsigned short reg, unsigned int length, unsigned char *data); 144 | /** 145 | * @brief Write data to a register in DMP memory 146 | * @param[in] DMP memory address 147 | * @param[in] number of byte to be written 148 | * @param[out] output data from the register 149 | * @return 0 if successful. 150 | */ 151 | int INV_EXPORT inv_icm20948_write_mems(struct inv_icm20948 * s, unsigned short reg, unsigned int length, const unsigned char *data); 152 | 153 | /** @brief Writes a single byte of data from a register on mems with no power control 154 | * @param[in] reg DMP memory address 155 | * @param[out] data Data to be written 156 | * @return 0 in case of success, -1 for any error 157 | */ 158 | int INV_EXPORT inv_icm20948_write_single_mems_reg_core(struct inv_icm20948 * s, uint16_t reg, const uint8_t data); 159 | 160 | #ifdef __cplusplus 161 | } 162 | #endif 163 | 164 | #endif /* _INV_ICM20948_TRANSPORT_H_ */ 165 | 166 | /** @} */ 167 | -------------------------------------------------------------------------------- /ICM20948/InvBool.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2017 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | /** @brief Custom definition for boolean type to avoid compiler disrepenrencies 25 | * @{ 26 | */ 27 | 28 | #ifndef _INV_BOOL_H_ 29 | #define _INV_BOOL_H_ 30 | 31 | typedef int inv_bool_t; 32 | 33 | #ifndef __cplusplus 34 | 35 | #ifndef true 36 | #define true 1 37 | #endif 38 | 39 | #ifndef false 40 | #define false 0 41 | #endif 42 | 43 | #endif /* __cplusplus */ 44 | 45 | #endif /* _INV_BOOL_H_ */ 46 | 47 | /** @} */ 48 | -------------------------------------------------------------------------------- /ICM20948/InvError.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | /** @defgroup InvError Error code 25 | * @brief Common error code 26 | * 27 | * @ingroup EmbUtils 28 | * @{ 29 | */ 30 | 31 | #ifndef _INV_ERROR_H_ 32 | #define _INV_ERROR_H_ 33 | 34 | /** @brief Common error code definition 35 | */ 36 | enum inv_error 37 | { 38 | INV_ERROR_SUCCESS = 0, /**< no error */ 39 | INV_ERROR = -1, /**< unspecified error */ 40 | INV_ERROR_NIMPL = -2, /**< function not implemented for given 41 | arguments */ 42 | INV_ERROR_TRANSPORT = -3, /**< error occured at transport level */ 43 | INV_ERROR_TIMEOUT = -4, /**< action did not complete in the expected 44 | time window */ 45 | INV_ERROR_SIZE = -5, /**< size/length of given arguments is not 46 | suitable to complete requested action */ 47 | INV_ERROR_OS = -6, /**< error related to OS */ 48 | INV_ERROR_IO = -7, /**< error related to IO operation */ 49 | INV_ERROR_MEM = -9, /**< not enough memory to complete requested 50 | action */ 51 | INV_ERROR_HW = -10, /**< error at HW level */ 52 | INV_ERROR_BAD_ARG = -11, /**< provided arguments are not good to 53 | perform requestion action */ 54 | INV_ERROR_UNEXPECTED = -12, /**< something unexpected happened */ 55 | INV_ERROR_FILE = -13, /**< cannot access file or unexpected format */ 56 | INV_ERROR_PATH = -14, /**< invalid file path */ 57 | INV_ERROR_IMAGE_TYPE = -15, /**< error when image type is not managed */ 58 | INV_ERROR_WATCHDOG = -16, /**< error when device doesn't respond 59 | to ping */ 60 | INV_ERROR_FIFO_OVERFLOW = -17, /**< FIFO overflow detected */ 61 | }; 62 | 63 | #endif /* _INV_ERROR_H_ */ 64 | 65 | /** @} */ 66 | -------------------------------------------------------------------------------- /ICM20948/InvExport.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | #ifndef _INV_IDD_EXPORT_H_ 25 | #define _INV_IDD_EXPORT_H_ 26 | 27 | #if defined(_WIN32) 28 | #if !defined(INV_EXPORT) && defined(INV_DO_DLL_EXPORT) 29 | #define INV_EXPORT __declspec(dllexport) 30 | #elif !defined(INV_EXPORT) && defined(INV_DO_DLL_IMPORT) 31 | #define INV_EXPORT __declspec(dllimport) 32 | #endif 33 | #endif 34 | 35 | #if !defined(INV_EXPORT) 36 | #define INV_EXPORT 37 | #endif 38 | 39 | #endif /* _INV_IDD_EXPORT_H_ */ 40 | -------------------------------------------------------------------------------- /ICM20948/Message.c: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2017 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | #include "Message.h" 25 | 26 | #include 27 | #include 28 | 29 | static int msg_level; 30 | static inv_msg_printer_t msg_printer; 31 | 32 | void inv_msg_printer_default(int level, const char * str, va_list ap) 33 | { 34 | (void)level, (void)str, (void)ap; 35 | } 36 | 37 | void inv_msg_setup(int level, inv_msg_printer_t printer) 38 | { 39 | msg_level = level; 40 | if (level < INV_MSG_LEVEL_OFF) 41 | msg_level = INV_MSG_LEVEL_OFF; 42 | else if (level > INV_MSG_LEVEL_MAX) 43 | msg_level = INV_MSG_LEVEL_MAX; 44 | msg_printer = printer; 45 | } 46 | 47 | void inv_msg(int level, const char * str, ...) 48 | { 49 | if(level && level <= msg_level && msg_printer) { 50 | va_list ap; 51 | va_start(ap, str); 52 | msg_printer(level, str, ap); 53 | va_end(ap); 54 | } 55 | } 56 | 57 | int inv_msg_get_level(void) 58 | { 59 | return msg_level; 60 | } 61 | -------------------------------------------------------------------------------- /ICM20948/Message.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2017 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | /** @defgroup Message Message 25 | * @brief Utility functions to display and redirect diagnostic messages 26 | * 27 | * Use INV_MSG_DISABLE or INV_MSG_ENABLE define before including 28 | * this header to enable/disable messages for a compilation unit. 29 | * 30 | * Under Linux, Windows or Arduino, messages are enabled by default. 31 | * Use INV_MSG_DISABLE to disable them. 32 | * 33 | * Under orther environmment, message are disabled by default. 34 | * Use INV_MSG_ENABLE to disable them. 35 | * 36 | * @ingroup EmbUtils 37 | * @{ 38 | */ 39 | 40 | #ifndef _INV_MESSAGE_H_ 41 | #define _INV_MESSAGE_H_ 42 | 43 | #include "InvExport.h" 44 | 45 | #ifdef __cplusplus 46 | extern "C" { 47 | #endif 48 | 49 | #include 50 | 51 | /** @brief For eMD target, disable log by default 52 | * If compile switch is set for a compilation unit 53 | * messages will be totally disabled by default 54 | */ 55 | #if !defined(__linux) && !defined(_WIN32) && !defined(ARDUINO) 56 | #define INV_MSG_DISABLE 1 57 | #endif 58 | 59 | 60 | /** @brief Allow to force enabling messaging using INV_MSG_ENABLE define */ 61 | #ifdef INV_MSG_ENABLE 62 | #undef INV_MSG_DISABLE 63 | #endif 64 | 65 | 66 | /** @brief Helper macro for calling inv_msg() 67 | * If INV_MSG_DISABLE compile switch is set for a compilation unit 68 | * messages will be totally disabled 69 | */ 70 | #define INV_MSG(level, ...) _INV_MSG(level, __VA_ARGS__) 71 | 72 | /** @brief Helper macro for calling inv_msg_setup() 73 | * If INV_MSG_DISABLE compile switch is set for a compilation unit 74 | * messages will be totally disabled 75 | */ 76 | #define INV_MSG_SETUP(level, printer) _INV_MSG_SETUP(level, printer) 77 | 78 | /** @brief Helper macro for calling inv_msg_setup_level() 79 | * If INV_MSG_DISABLE compile switch is set for a compilation unit 80 | * messages will be totally disabled 81 | */ 82 | #define INV_MSG_SETUP_LEVEL(level) _INV_MSG_SETUP_LEVEL(level) 83 | 84 | /** @brief Helper macro for calling inv_msg_setup_default() 85 | * If INV_MSG_DISABLE compile switch is set for a compilation unit 86 | * messages will be totally disabled 87 | */ 88 | #define INV_MSG_SETUP_DEFAULT() _INV_MSG_SETUP_DEFAULT() 89 | 90 | /** @brief Return current level 91 | * @warning This macro may expand as a function call 92 | */ 93 | #define INV_MSG_LEVEL _INV_MSG_LEVEL 94 | 95 | #if defined(INV_MSG_DISABLE) 96 | #define _INV_MSG(level, ...) (void)0 97 | #define _INV_MSG_SETUP(level, printer) (void)0 98 | #define _INV_MSG_SETUP_LEVEL(level) (void)0 99 | #define _INV_MSG_LEVEL INV_MSG_LEVEL_OFF 100 | #else 101 | #define _INV_MSG(level, ...) inv_msg(level, __VA_ARGS__) 102 | #define _INV_MSG_SETUP(level, printer) inv_msg_setup(level, printer) 103 | #define _INV_MSG_SETUP_LEVEL(level) inv_msg_setup(level, inv_msg_printer_default) 104 | #define _INV_MSG_SETUP_DEFAULT() inv_msg_setup_default() 105 | #define _INV_MSG_LEVEL inv_msg_get_level() 106 | #endif 107 | 108 | /** @brief message level definition 109 | */ 110 | enum inv_msg_level { 111 | INV_MSG_LEVEL_OFF = 0, 112 | INV_MSG_LEVEL_ERROR, 113 | INV_MSG_LEVEL_WARNING, 114 | INV_MSG_LEVEL_INFO, 115 | INV_MSG_LEVEL_VERBOSE, 116 | INV_MSG_LEVEL_DEBUG, 117 | INV_MSG_LEVEL_MAX 118 | }; 119 | 120 | 121 | /** @brief Prototype for print routine function 122 | */ 123 | typedef void (*inv_msg_printer_t)(int level, const char * str, va_list ap); 124 | 125 | 126 | /** @brief Set message level and printer function 127 | * @param[in] level only message above level will be passed to printer function 128 | * @param[in] printer user provided function in charge printing message 129 | * @return none 130 | */ 131 | void INV_EXPORT inv_msg_setup(int level, inv_msg_printer_t printer); 132 | 133 | 134 | /** @brief Default printer function that display messages to stderr 135 | * Function uses stdio. Care must be taken on embeded platfrom. 136 | * Function does nothing with IAR compiler. 137 | * @return none 138 | */ 139 | void INV_EXPORT inv_msg_printer_default(int level, const char * str, va_list ap); 140 | 141 | /** @brief Set message level 142 | * Default printer function will be used. 143 | * @param[in] level only message above level will be passed to printer function 144 | * @return none 145 | */ 146 | static inline void inv_msg_setup_level(int level) 147 | { 148 | inv_msg_setup(level, inv_msg_printer_default); 149 | } 150 | 151 | 152 | /** @brief Set default message level and printer 153 | * @return none 154 | */ 155 | static inline void inv_msg_setup_default(void) 156 | { 157 | inv_msg_setup(INV_MSG_LEVEL_INFO, inv_msg_printer_default); 158 | } 159 | 160 | /** @brief Return current message level 161 | * @return current message level 162 | */ 163 | int INV_EXPORT inv_msg_get_level(void); 164 | 165 | /** @brief Display a message (through means of printer function) 166 | * @param[in] level for the message 167 | * @param[in] str message string 168 | * @param[in] ... optional arguments 169 | * @return none 170 | */ 171 | void INV_EXPORT inv_msg(int level, const char * str, ...); 172 | 173 | 174 | #ifdef __cplusplus 175 | } 176 | #endif 177 | 178 | #endif /* INV_MESSAGE_H_ */ 179 | 180 | /** @} */ 181 | -------------------------------------------------------------------------------- /ICM20948/SensorTypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | /** @defgroup SensorTypes Sensor types 25 | * @brief Sensor related types definitions 26 | * @ingroup Drivers 27 | * @{ 28 | */ 29 | 30 | #ifndef _INV_SENSOR_TYPES_H_ 31 | #define _INV_SENSOR_TYPES_H_ 32 | 33 | #include "InvExport.h" 34 | 35 | #ifdef __cplusplus 36 | extern "C" { 37 | #endif 38 | 39 | #include 40 | #include "InvBool.h" 41 | 42 | /** @brief Sensor type identifier definition 43 | */ 44 | enum inv_sensor_type { 45 | INV_SENSOR_TYPE_RESERVED = 0 , /**< Reserved ID: do not use */ 46 | INV_SENSOR_TYPE_ACCELEROMETER = 1 , /**< Accelerometer */ 47 | INV_SENSOR_TYPE_MAGNETOMETER = 2 , /**< Magnetic field */ 48 | INV_SENSOR_TYPE_ORIENTATION = 3 , /**< Deprecated orientation */ 49 | INV_SENSOR_TYPE_GYROSCOPE = 4 , /**< Gyroscope */ 50 | INV_SENSOR_TYPE_LIGHT = 5 , /**< Ambient light sensor */ 51 | INV_SENSOR_TYPE_PRESSURE = 6 , /**< Barometer */ 52 | INV_SENSOR_TYPE_TEMPERATURE = 7 , /**< Temperature */ 53 | INV_SENSOR_TYPE_PROXIMITY = 8 , /**< Proximity */ 54 | INV_SENSOR_TYPE_GRAVITY = 9 , /**< Gravity */ 55 | INV_SENSOR_TYPE_LINEAR_ACCELERATION = 10, /**< Linear acceleration */ 56 | INV_SENSOR_TYPE_ROTATION_VECTOR = 11, /**< Rotation vector */ 57 | INV_SENSOR_TYPE_HUMIDITY = 12, /**< Relative humidity */ 58 | INV_SENSOR_TYPE_AMBIENT_TEMPERATURE = 13, /**< Ambient temperature */ 59 | INV_SENSOR_TYPE_UNCAL_MAGNETOMETER = 14, /**< Uncalibrated magnetic field */ 60 | INV_SENSOR_TYPE_GAME_ROTATION_VECTOR = 15, /**< Game rotation vector */ 61 | INV_SENSOR_TYPE_UNCAL_GYROSCOPE = 16, /**< Uncalibrated gyroscope */ 62 | INV_SENSOR_TYPE_SMD = 17, /**< Significant motion detection */ 63 | INV_SENSOR_TYPE_STEP_DETECTOR = 18, /**< Step detector */ 64 | INV_SENSOR_TYPE_STEP_COUNTER = 19, /**< Step counter */ 65 | INV_SENSOR_TYPE_GEOMAG_ROTATION_VECTOR = 20, /**< Geomagnetic rotation vector */ 66 | INV_SENSOR_TYPE_HEART_RATE = 21, /**< Heart rate */ 67 | INV_SENSOR_TYPE_TILT_DETECTOR = 22, /**< Tilt detector */ 68 | INV_SENSOR_TYPE_WAKE_GESTURE = 23, /**< Wake-up gesture */ 69 | INV_SENSOR_TYPE_GLANCE_GESTURE = 24, /**< Glance gesture */ 70 | INV_SENSOR_TYPE_PICK_UP_GESTURE = 25, /**< Pick-up gesture */ 71 | INV_SENSOR_TYPE_BAC = 26, /**< Basic Activity Classifier */ 72 | INV_SENSOR_TYPE_PDR = 27, /**< Pedestrian Dead Reckoning */ 73 | INV_SENSOR_TYPE_B2S = 28, /**< Bring to see */ 74 | INV_SENSOR_TYPE_3AXIS = 29, /**< 3 Axis sensor */ 75 | INV_SENSOR_TYPE_EIS = 30, /**< Electronic Image Stabilization */ 76 | INV_SENSOR_TYPE_OIS_0 = 31, /**< Optical Image Stabilization */ 77 | INV_SENSOR_TYPE_RAW_ACCELEROMETER = 32, /**< Raw accelerometer */ 78 | INV_SENSOR_TYPE_RAW_GYROSCOPE = 33, /**< Raw gyroscope */ 79 | INV_SENSOR_TYPE_RAW_MAGNETOMETER = 34, /**< Raw magnetometer */ 80 | INV_SENSOR_TYPE_RAW_TEMPERATURE = 35, /**< Raw temperature */ 81 | INV_SENSOR_TYPE_CUSTOM_PRESSURE = 36, /**< Custom Pressure Sensor */ 82 | INV_SENSOR_TYPE_MIC = 37, /**< Stream audio from microphone */ 83 | INV_SENSOR_TYPE_TSIMU = 38, /**< TS-IMU */ 84 | INV_SENSOR_TYPE_RAW_PPG = 39, /**< Raw Photoplethysmogram */ 85 | INV_SENSOR_TYPE_HRV = 40, /**< Heart rate variability */ 86 | INV_SENSOR_TYPE_SLEEP_ANALYSIS = 41, /**< Sleep analysis */ 87 | INV_SENSOR_TYPE_BAC_EXTENDED = 42, /**< Basic Activity Classifier Extended */ 88 | INV_SENSOR_TYPE_BAC_STATISTICS = 43, /**< Basic Activity Classifier Statistics */ 89 | INV_SENSOR_TYPE_FLOOR_CLIMB_COUNTER = 44, /**< Floor Climbed Counter */ 90 | INV_SENSOR_TYPE_ENERGY_EXPENDITURE = 45, /**< Energy Expenditure */ 91 | INV_SENSOR_TYPE_DISTANCE = 46, /**< Distance */ 92 | INV_SENSOR_TYPE_SHAKE = 47, /**< Shake Gesture */ 93 | INV_SENSOR_TYPE_DOUBLE_TAP = 48, /**< Double Tap */ 94 | INV_SENSOR_TYPE_CUSTOM0, /**< Custom sensor ID 0 */ 95 | INV_SENSOR_TYPE_CUSTOM1, /**< Custom sensor ID 1 */ 96 | INV_SENSOR_TYPE_CUSTOM2, /**< Custom sensor ID 2 */ 97 | INV_SENSOR_TYPE_CUSTOM3, /**< Custom sensor ID 3 */ 98 | INV_SENSOR_TYPE_CUSTOM4, /**< Custom sensor ID 4 */ 99 | INV_SENSOR_TYPE_CUSTOM5, /**< Custom sensor ID 5 */ 100 | INV_SENSOR_TYPE_CUSTOM6, /**< Custom sensor ID 6 */ 101 | INV_SENSOR_TYPE_CUSTOM7, /**< Custom sensor ID 7 */ 102 | INV_SENSOR_TYPE_WOM, /**< Wake-up on motion */ 103 | INV_SENSOR_TYPE_SEDENTARY_REMIND, /**< Sedentary Remind */ 104 | INV_SENSOR_TYPE_DATA_ENCRYPTION, /**< Data Encryption */ 105 | INV_SENSOR_TYPE_FSYNC_EVENT, /**< FSYNC event */ 106 | INV_SENSOR_TYPE_HIGH_RATE_GYRO, /**< High Rate Gyro */ 107 | INV_SENSOR_TYPE_CUSTOM_BSCD, /**< Custom BAC StepCounter Calorie counter and Distance counter */ 108 | INV_SENSOR_TYPE_HRM_LOGGER, /**< HRM ouput for logger */ 109 | /* Starting from there, the SensorID is coded with more than 6bits so check that communication protocol is adequate */ 110 | INV_SENSOR_TYPE_PRED_QUAT_0, /**< Predictive Quaternion instance 0 */ 111 | INV_SENSOR_TYPE_PRED_QUAT_1, /**< Predictive Quaternion instance 1 */ 112 | INV_SENSOR_TYPE_OIS_1, /**< Optical Image Stabilization instance 1 */ 113 | INV_SENSOR_TYPE_FIRMWARE, /**< Messages from the firmware */ 114 | 115 | INV_SENSOR_TYPE_MAX /**< sentinel value for sensor type */ 116 | }; 117 | 118 | #define INV_SENSOR_TYPE_CUSTOM_BASE INV_SENSOR_TYPE_CUSTOM0 119 | #define INV_SENSOR_TYPE_CUSTOM_END (INV_SENSOR_TYPE_CUSTOM7+1) 120 | 121 | #define INV_SENSOR_TYPE_META_DATA INV_SENSOR_TYPE_RESERVED /**< @deprecated */ 122 | #define INV_SENSOR_TYPE_GYROMETER INV_SENSOR_TYPE_GYROSCOPE /**< @deprecated */ 123 | #define INV_SENSOR_TYPE_UNCAL_GYROMETER INV_SENSOR_TYPE_UNCAL_GYROSCOPE /**< @deprecated */ 124 | #define INV_SENSOR_TYPE_ENERGY_EXPANDITURE INV_SENSOR_TYPE_ENERGY_EXPENDITURE /**< @deprecated */ 125 | #define INV_SENSOR_TYPE_OIS INV_SENSOR_TYPE_OIS_0 /**< @deprecated */ 126 | 127 | /** @brief Helper flag to indicate if sensor is a Wale-Up sensor 128 | */ 129 | #define INV_SENSOR_TYPE_WU_FLAG (unsigned int)(0x80000000) 130 | 131 | /** @brief Sensor status definition 132 | */ 133 | enum inv_sensor_status 134 | { 135 | INV_SENSOR_STATUS_DATA_UPDATED = 0, /**< new sensor data */ 136 | INV_SENSOR_STATUS_STATE_CHANGED = 1, /**< dummy sensor data indicating 137 | to a change in sensor state */ 138 | INV_SENSOR_STATUS_FLUSH_COMPLETE = 2, /**< dummy sensor data indicating 139 | a end of batch after a manual flush */ 140 | INV_SENSOR_STATUS_POLLED_DATA = 3, /**< sensor data value after manual request */ 141 | }; 142 | 143 | /** @brief Event definition for BAC sensor 144 | */ 145 | enum inv_sensor_bac_event { 146 | INV_SENSOR_BAC_EVENT_ACT_UNKNOWN = 0, 147 | INV_SENSOR_BAC_EVENT_ACT_IN_VEHICLE_BEGIN = 1, 148 | INV_SENSOR_BAC_EVENT_ACT_IN_VEHICLE_END = -1, 149 | INV_SENSOR_BAC_EVENT_ACT_WALKING_BEGIN = 2, 150 | INV_SENSOR_BAC_EVENT_ACT_WALKING_END = -2, 151 | INV_SENSOR_BAC_EVENT_ACT_RUNNING_BEGIN = 3, 152 | INV_SENSOR_BAC_EVENT_ACT_RUNNING_END = -3, 153 | INV_SENSOR_BAC_EVENT_ACT_ON_BICYCLE_BEGIN = 4, 154 | INV_SENSOR_BAC_EVENT_ACT_ON_BICYCLE_END = -4, 155 | INV_SENSOR_BAC_EVENT_ACT_TILT_BEGIN = 5, 156 | INV_SENSOR_BAC_EVENT_ACT_TILT_END = -5, 157 | INV_SENSOR_BAC_EVENT_ACT_STILL_BEGIN = 6, 158 | INV_SENSOR_BAC_EVENT_ACT_STILL_END = -6, 159 | }; 160 | 161 | /** @brief Event definition for BAC Ext sensor 162 | */ 163 | enum inv_sensor_bacext_event { 164 | INV_SENSOR_BACEXT_EVENT_ACT_UNKNOWN = 0, 165 | INV_SENSOR_BACEXT_EVENT_ACT_WALKING_START = 1, 166 | INV_SENSOR_BACEXT_EVENT_ACT_WALKING_END = -1, 167 | INV_SENSOR_BACEXT_EVENT_ACT_RUNNING_START = 2, 168 | INV_SENSOR_BACEXT_EVENT_ACT_RUNNING_END = -2, 169 | INV_SENSOR_BACEXT_EVENT_ACT_ON_BICYCLE_START = 3, 170 | INV_SENSOR_BACEXT_EVENT_ACT_ON_BICYCLE_END = -3, 171 | INV_SENSOR_BACEXT_EVENT_ACT_IN_VEHICLE_SIT_START = 4, 172 | INV_SENSOR_BACEXT_EVENT_ACT_IN_VEHICLE_SIT_END = -4, 173 | INV_SENSOR_BACEXT_EVENT_ACT_IN_VEHICLE_STAND_START = 5, 174 | INV_SENSOR_BACEXT_EVENT_ACT_IN_VEHICLE_STAND_END = -5, 175 | INV_SENSOR_BACEXT_EVENT_ACT_STILL_SIT_START = 6, 176 | INV_SENSOR_BACEXT_EVENT_ACT_STILL_SIT_END = -6, 177 | INV_SENSOR_BACEXT_EVENT_ACT_STILL_STAND_START = 7, 178 | INV_SENSOR_BACEXT_EVENT_ACT_STILL_STAND_END = -7 179 | }; 180 | 181 | /** @brief Maximum size of an event data 182 | */ 183 | #define INV_SENSOR_EVENT_DATA_SIZE 64 184 | 185 | /** @brief For backward compatibility only - do not use 186 | */ 187 | #define IVN_SENSOR_EVENT_DATA_SIZE INV_SENSOR_EVENT_DATA_SIZE 188 | 189 | /** @brief Sensor event definition 190 | */ 191 | typedef struct inv_sensor_event 192 | { 193 | unsigned int sensor; /**< sensor type */ 194 | int status; /**< sensor data status as of 195 | enum inv_sensor_status */ 196 | uint64_t timestamp; /**< sensor data timestamp in us */ 197 | union { 198 | struct { 199 | float vect[3]; /**< x,y,z vector data */ 200 | float bias[3]; /**< x,y,z bias vector data */ 201 | uint8_t accuracy_flag; /**< accuracy flag */ 202 | } acc; /**< 3d accelerometer data in g */ 203 | struct { 204 | float vect[3]; /**< x,y,z vector data */ 205 | float bias[3]; /**< x,y,z bias vector data */ 206 | uint8_t accuracy_flag; /**< accuracy flag */ 207 | } linAcc; /**< 3d linear accelerometer data in g */ 208 | struct { 209 | float vect[3]; /**< x,y,z vector data */ 210 | float bias[3]; /**< x,y,z bias vector data */ 211 | uint8_t accuracy_flag; /**< accuracy flag */ 212 | } grav; /**< 3d gravity vector data in g */ 213 | struct { 214 | float vect[3]; /**< x,y,z vector data */ 215 | float bias[3]; /**< x,y,z bias vector data (for uncal sensor variant) */ 216 | uint8_t accuracy_flag; /**< accuracy flag */ 217 | } mag; /**< 3d magnetometer data in uT */ 218 | struct { 219 | float vect[3]; /**< x,y,z vector data */ 220 | float bias[3]; /**< x,y,z bias vector data (for uncal sensor variant) */ 221 | uint8_t accuracy_flag; /**< accuracy flag */ 222 | } gyr; /**< 3d gyroscope data in deg/s */ 223 | struct { 224 | float quat[4]; /**< w,x,y,z quaternion data */ 225 | float accuracy; /**< heading accuracy in deg */ 226 | uint8_t accuracy_flag; /**< accuracy flag specific for GRV*/ 227 | } quaternion6DOF; /**< quaternion 6DOF data */ 228 | struct { 229 | float quat[4]; /**< w,x,y,z quaternion data */ 230 | float accuracy; /**< heading accuracy in deg */ 231 | uint8_t accuracy_flag; /**< accuracy flag specific for GRV*/ 232 | } quaternion9DOF; /**< quaternion 9DOF data */ 233 | struct { 234 | float x,y,z; /**< x,y,z angles in deg as defined by Google Orientation sensor */ 235 | uint8_t accuracy_flag; /**< heading accuracy in deg */ 236 | } orientation; /**< orientation data */ 237 | struct { 238 | float bpm; /**< beat per minute */ 239 | uint8_t confidence; /**< confidence level */ 240 | uint8_t sqi; /**< signal quality as seen by the the HRM engine */ 241 | } hrm; /**< heart rate monitor data */ /**< heart rate monitor data */ 242 | struct { 243 | int32_t acc[3]; /**< accel data used by hrm algorithm */ 244 | int32_t gyr[3]; /**< gyro data used by hrm algorithm */ 245 | uint32_t ppg_value; /**< ppg value read from HRM sensor */ 246 | float ppm; /**< beat per minute */ 247 | uint8_t confidence; /**< confidence level */ 248 | uint8_t sqi; /**< signal quality as seen by the the HRM engine */ 249 | uint8_t touch_status; /**< touch status, detected or not by the PPG */ 250 | uint8_t gyrEnable; /**< 1 gyro is enable else 0 */ 251 | } hrmlogger; /**< heart rate monitor logger data */ 252 | struct { 253 | uint8_t rr_count; 254 | uint8_t paddingDummy; /**< dummy byte for padding */ 255 | int16_t rr_interval[4]; /**< beat-to-beat(RR) interval */ 256 | } hrv; /**< heart rate variability data */ 257 | struct { 258 | uint32_t ppg_value; /**< ppg value read from HRM sensor */ 259 | uint8_t touch_status; /**< touch status, detected or not */ 260 | } rawppg; /**< raw heart rate monitor data */ 261 | struct { 262 | uint8_t sleep_phase; /**< state of sleep phases: 0 not defined, 1 restless sleep, 2 light sleep, 3 deep sleep */ 263 | uint32_t timestamp; /**< time stamp of the sleep phase transition (seconds)*/ 264 | int32_t sleep_onset; /**< time until first period of 20 min sleep without more than 1 min wake */ 265 | int32_t sleep_latency; /**< time until first sleep phase */ 266 | uint32_t time_in_bed; /**< time in bed (seconds) */ 267 | uint32_t total_sleep_time; /**< total sleep time (seconds) */ 268 | uint8_t sleep_efficiency; /**< ratio between total sleep time and time in bed */ 269 | } sleepanalysis; /**< sleep analysis data */ 270 | struct { 271 | int event; /**< BAC extended data begin/end event as of 272 | enum inv_sensor_bac_ext_event */ 273 | } bacext; /**< activity classifier (BAC) extended data */ 274 | struct { 275 | uint32_t durationWalk; /**< ms */ 276 | uint32_t durationRun; /**< ms */ 277 | uint32_t durationTransportSit; /**< ms */ 278 | uint32_t durationTransportStand;/**< ms */ 279 | uint32_t durationBiking; /**< ms */ 280 | uint32_t durationStillSit; /**< ms */ 281 | uint32_t durationStillStand; /**< ms */ 282 | uint32_t durationTotalSit; /**< Still-Sit + Transport-Sit + Biking (ms) */ 283 | uint32_t durationTotalStand; /**< Still-Stand + Transport-Stand (ms) */ 284 | uint32_t stepWalk; /**< walk step count */ 285 | uint32_t stepRun; /**< run step count */ 286 | } bacstat; /**< activity classifier (BAC) statistics data */ 287 | struct { 288 | int32_t floorsUp; /**< number of floors climbed Up on foot by user. */ 289 | int32_t floorsDown; /**< number of floors climbed Down on foot by user. */ 290 | } floorclimb; /**< floor climbed data */ 291 | struct { 292 | int32_t instantEEkcal; /**< energy expenditure in kilocalorie/min since last output. Format is q15: 2^15 = 1 kcal/min */ 293 | int32_t instantEEmets; /**< energy expenditure in METs(Metabolic Equivalent of Task) since last output. Format is q15: 2^15 = 1 METs */ 294 | int32_t cumulativeEEkcal; /**< cumulative energy expenditure since the last reset in kilocalorie. Format is q0: 1 = 1 kcal */ 295 | int32_t cumulativeEEmets; /**< cumulative energy expenditure since the last reset in METs (Metabolic Equivalent of Task). Format is q0: 1 = 1 METs */ 296 | } energyexp; /**< energy expenditure data */ 297 | struct { 298 | int32_t distanceWalk; /**< distance in meters */ 299 | int32_t distanceRun; /**< distance in meters */ 300 | } distance; /**< distance data */ 301 | struct { 302 | int32_t table[7]; /**< data encrypted table */ 303 | } dataencryption; 304 | struct { 305 | float tmp; /**< temperature in deg celcius */ 306 | } temperature; /**< temperature data */ 307 | struct { 308 | float percent; /**< relative humidity in % */ 309 | } humidity; /**< humidity data */ 310 | struct { 311 | uint64_t count; /**< number of steps */ 312 | } step; /**< step-counter data */ 313 | struct { 314 | uint32_t level; /**< light level in lux */ 315 | } light; /**< light data */ 316 | struct { 317 | uint32_t distance; /**< distance in mm */ 318 | } proximity; /**< proximity data */ 319 | struct { 320 | uint32_t pressure; /**< pressure in Pa */ 321 | } pressure; /**< pressure data */ 322 | struct { 323 | int event; /**< BAC data begin/end event as of 324 | enum inv_sensor_bac_event */ 325 | } bac; /**< BAC data */ 326 | struct { 327 | uint8_t direction; /**< 1: forward. 2: reverse. */ 328 | } b2s; 329 | struct { 330 | uint32_t fxdata[12]; /**< PDR data in fixpoint*/ 331 | } pdr; /**< PDR data */ 332 | struct { 333 | float vect[3]; /**< x,y,z vector data */ 334 | float bias[3]; /**< x,y,z bias vector data (for uncal sensor variant) */ 335 | int16_t delta_ts; /**< timestamp delta between standard gyro and EIS gyro */ 336 | } eis; /**< EIS data 337 | @warning experimental: structure is likely to change in near future */ 338 | struct { 339 | int32_t vect[3]; /**< x,y,z vector data */ 340 | uint32_t fsr; /**< full scale range */ 341 | } raw3d; /**< 3d raw acc, mag or gyr*/ 342 | struct { 343 | int32_t raw; /**< raw temperature value */ 344 | uint32_t sensitivity; /**< raw temperature sensitivity */ 345 | } rawtemp; /**< Raw temperature data*/ 346 | struct { 347 | uint8_t status[6]; /**< raw temperature value */ 348 | } tsimu_status; /**< TSIMU status data*/ 349 | inv_bool_t event; /**< event state for gesture-like sensor 350 | (SMD, B2S, Step-detector, Tilt-detector, Wake, Glance, Pick-Up, Shake, Double-tap, ...) */ 351 | struct { 352 | int16_t delay_count; /**< delay counter in us between FSYNC tag and previous gyro data */ 353 | } fsync_event; /** < FSYNC tag (EIS sensor) */ 354 | struct { 355 | unsigned flags; /** WOM status flags: non-zero value - motion detected 356 | bit0 - motion detected around X axis 357 | bit1 - motion detected around Y axis 358 | bit2 - motion detected around Z axis 359 | */ 360 | } wom; /** Wake-up on motion data */ 361 | struct { 362 | uint8_t * buffer; /**< pointer to buffer */ 363 | uint32_t size; /**< current buffer size */ 364 | } audio_buffer; /**< buffer of audio data */ 365 | struct { 366 | struct { 367 | int event; /**< BAC data begin/end event as of enum inv_sensor_bac_event */ 368 | } bac; /**< BAC data */ 369 | struct { 370 | uint64_t count; /**< number of steps */ 371 | } step; /**< step-counter data */ 372 | int32_t cumulativeEEkcal; /**< cumulative energy expenditure since the last reset in kilocalorie. Format is q0: 1 = 1 kcal */ 373 | int32_t distance; /**< sum of walk and run distance in meters */ 374 | } bscd; /**< buffer of custom BSCD */ 375 | struct { 376 | int32_t raw_pressure; /**< raw pressure */ 377 | float pressure; /**< pressure in Pa */ 378 | int32_t raw_temperature; /**< raw temperature */ 379 | float temperature; /**< temperature in deg C */ 380 | } custom_pressure; /**< pressure data */ 381 | uint8_t reserved[INV_SENSOR_EVENT_DATA_SIZE]; /**< reserved sensor data for future sensor */ 382 | } data; /**< sensor data */ 383 | } inv_sensor_event_t; 384 | 385 | /** @brief Sensor listener event callback definition 386 | * @param[in] event reference to sensor event 387 | * @param[in] context listener context 388 | * @return none 389 | */ 390 | typedef void (*inv_sensor_listener_event_cb_t)(const inv_sensor_event_t * event, 391 | void * context); 392 | 393 | /** @brief Sensor event listener definition 394 | */ 395 | typedef struct inv_sensor_listener { 396 | inv_sensor_listener_event_cb_t event_cb; /**< sensor event callback */ 397 | void * context; /**< listener context */ 398 | } inv_sensor_listener_t; 399 | 400 | /** @brief Helper to initialize a listener object 401 | */ 402 | static inline void inv_sensor_listener_init(inv_sensor_listener_t * listener, 403 | inv_sensor_listener_event_cb_t event_cb, void * context) 404 | { 405 | listener->event_cb = event_cb; 406 | listener->context = context; 407 | } 408 | 409 | /** @brief Helper to notify a listener of a new sensor event 410 | */ 411 | static inline void inv_sensor_listener_notify(const inv_sensor_listener_t * listener, 412 | const inv_sensor_event_t * event) 413 | { 414 | if(listener) { 415 | listener->event_cb(event, listener->context); 416 | } 417 | } 418 | 419 | /** @brief Helper macro to retrieve sensor type (without wake-up flag) from a sensor id. 420 | */ 421 | #define INV_SENSOR_ID_TO_TYPE(sensor) \ 422 | ((unsigned int)(sensor) & ~INV_SENSOR_TYPE_WU_FLAG) 423 | 424 | /** @brief Helper macro that check if given sensor is of known type 425 | */ 426 | #define INV_SENSOR_IS_VALID(sensor) \ 427 | (INV_SENSOR_ID_TO_TYPE(sensor) < INV_SENSOR_TYPE_MAX) 428 | 429 | /** @brief Helper macro that check if given sensor is a wake-up sensor 430 | */ 431 | #define INV_SENSOR_IS_WU(sensor) \ 432 | (((int)(sensor) & INV_SENSOR_TYPE_WU_FLAG) != 0) 433 | 434 | /** @brief Utility function that returns a string from a sensor id 435 | * Empty string is returned if sensor is invalid 436 | */ 437 | const char INV_EXPORT * inv_sensor_str(int sensor); 438 | 439 | /** @brief Alias for inv_sensor_str 440 | */ 441 | #define inv_sensor_2str inv_sensor_str 442 | 443 | #ifdef __cplusplus 444 | } 445 | #endif 446 | 447 | #endif /* _INV_SENSOR_TYPES_H_ */ 448 | 449 | /** @} */ 450 | -------------------------------------------------------------------------------- /ICM20948/dataconverter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ________________________________________________________________________________________________________ 3 | * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. 4 | * 5 | * This software, related documentation and any modifications thereto (collectively “Software”) is subject 6 | * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright 7 | * and other intellectual property rights laws. 8 | * 9 | * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software 10 | * and any use, reproduction, disclosure or distribution of the Software without an express license agreement 11 | * from InvenSense is strictly prohibited. 12 | * 13 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS 14 | * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 15 | * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. 16 | * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL 17 | * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY 18 | * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, 19 | * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE 20 | * OF THE SOFTWARE. 21 | * ________________________________________________________________________________________________________ 22 | */ 23 | 24 | /** @defgroup DataConverter Data Converter 25 | * @brief Helper functions to convert integer 26 | * @ingroup EmbUtils 27 | * @{ 28 | */ 29 | 30 | #ifndef _INV_DATA_CONVERTER_H_ 31 | #define _INV_DATA_CONVERTER_H_ 32 | 33 | #include "InvExport.h" 34 | 35 | #ifdef __cplusplus 36 | extern "C" { 37 | #endif 38 | 39 | #include 40 | 41 | /** @brief Converts a 32-bit long to a little endian byte stream 42 | */ 43 | uint8_t INV_EXPORT * inv_dc_int32_to_little8(int32_t x, uint8_t * little8); 44 | 45 | /** @brief Converts a 16-bit integer to a little endian byte stream 46 | */ 47 | uint8_t INV_EXPORT * inv_dc_int16_to_little8(int16_t x, uint8_t * little8); 48 | 49 | /** @brief Converts a 32-bit long to a big endian byte stream 50 | */ 51 | uint8_t INV_EXPORT * inv_dc_int32_to_big8(int32_t x, uint8_t *big8); 52 | 53 | /** @brief Converts a 16-bit long to a big endian byte stream 54 | */ 55 | uint8_t INV_EXPORT * inv_dc_int16_to_big8(int16_t x, uint8_t * big8); 56 | 57 | /** @brief Converts a little endian byte stream into a 32-bit integer 58 | */ 59 | int32_t INV_EXPORT inv_dc_little8_to_int32(const uint8_t * little8); 60 | 61 | /** @brief Converts a little endian byte stream into a 16-bit integer 62 | */ 63 | int16_t INV_EXPORT inv_dc_le_to_int16(const uint8_t * little8); 64 | 65 | /** @brief Converts big endian on 16 bits into an unsigned short 66 | */ 67 | int16_t INV_EXPORT inv_dc_big16_to_int16(uint8_t * data); 68 | 69 | /** @brief Converts an array of 32-bit signed fixed-point integers to an array of floats 70 | * @param[in] in Pointer to the first element of the array of 32-bit signed fixed-point integers 71 | * @param[in] len Length of the array 72 | * @param[in] qx Number of bits used to represent the decimal part of the fixed-point integers 73 | * @param[out] out Pointer to the memory area where the output will be stored 74 | */ 75 | void INV_EXPORT inv_dc_sfix32_to_float(const int32_t * in, uint32_t len, uint8_t qx, float * out); 76 | 77 | /** @brief Converts an array of floats to an array of 32-bit signed fixed-point integers 78 | * @param[in] in Pointer to the first element of the array of floats 79 | * @param[in] len Length of the array 80 | * @param[in] qx Number of bits used to represent the decimal part of the fixed-point integers 81 | * @param[out] out Pointer to the memory area where the output will be stored 82 | */ 83 | void INV_EXPORT inv_dc_float_to_sfix32(const float * in, uint32_t len, uint8_t qx, int32_t * out); 84 | 85 | #ifdef __cplusplus 86 | } 87 | #endif 88 | 89 | #endif /* _INV_DATA_CONVERTER_H_ */ 90 | 91 | /** @} */ 92 | -------------------------------------------------------------------------------- /ICM20948/examples/ICM20948_Arduino_DMP.ino: -------------------------------------------------------------------------------- 1 | #include "Arduino-ICM20948.h" 2 | #include 3 | 4 | 5 | ArduinoICM20948 icm20948; 6 | ArduinoICM20948Settings icmSettings = 7 | { 8 | .i2c_speed = 400000, // i2c clock speed 9 | .is_SPI = false, // Enable SPI, if disable use i2c 10 | .cs_pin = 10, // SPI chip select pin 11 | .spi_speed = 7000000, // SPI clock speed in Hz, max speed is 7MHz 12 | .mode = 1, // 0 = low power mode, 1 = high performance mode 13 | .enable_gyroscope = true, // Enables gyroscope output 14 | .enable_accelerometer = true, // Enables accelerometer output 15 | .enable_magnetometer = true, // Enables magnetometer output // Enables quaternion output 16 | .enable_gravity = true, // Enables gravity vector output 17 | .enable_linearAcceleration = true, // Enables linear acceleration output 18 | .enable_quaternion6 = true, // Enables quaternion 6DOF output 19 | .enable_quaternion9 = true, // Enables quaternion 9DOF output 20 | .enable_har = true, // Enables activity recognition 21 | .enable_steps = true, // Enables step counter 22 | .gyroscope_frequency = 1, // Max frequency = 225, min frequency = 1 23 | .accelerometer_frequency = 1, // Max frequency = 225, min frequency = 1 24 | .magnetometer_frequency = 1, // Max frequency = 70, min frequency = 1 25 | .gravity_frequency = 1, // Max frequency = 225, min frequency = 1 26 | .linearAcceleration_frequency = 1, // Max frequency = 225, min frequency = 1 27 | .quaternion6_frequency = 50, // Max frequency = 225, min frequency = 50 28 | .quaternion9_frequency = 50, // Max frequency = 225, min frequency = 50 29 | .har_frequency = 50, // Max frequency = 225, min frequency = 50 30 | .steps_frequency = 50 // Max frequency = 225, min frequency = 50 31 | 32 | }; 33 | 34 | const uint8_t number_i2c_addr = 2; 35 | uint8_t poss_addresses[number_i2c_addr] = {0X69, 0X68}; 36 | uint8_t ICM_address; 37 | bool ICM_found = false; 38 | 39 | 40 | void i2c_scan(){ 41 | uint8_t error; 42 | for(uint8_t add_int = 0; add_int < number_i2c_addr; add_int++ ){ 43 | Serial.printf("Scanning 0x%02X for slave...", poss_addresses[add_int]); 44 | Wire.beginTransmission(poss_addresses[add_int]); 45 | error = Wire.endTransmission(); 46 | if (error == 0){ 47 | Serial.println("found."); 48 | if(poss_addresses[add_int] == 0x69 || poss_addresses[add_int] == 0x68){ 49 | Serial.println("\t- address is ICM."); 50 | ICM_address = poss_addresses[add_int]; 51 | ICM_found = true; 52 | } 53 | } 54 | } 55 | } 56 | void run_icm20948_quat6_controller(bool inEuler = false) 57 | { 58 | float quat_w, quat_x, quat_y, quat_z; 59 | float roll, pitch, yaw; 60 | char sensor_string_buff[128]; 61 | if (inEuler) 62 | { 63 | if (icm20948.euler6DataIsReady()) 64 | { 65 | icm20948.readEuler6Data(&roll, &pitch, &yaw); 66 | sprintf(sensor_string_buff, "Euler6 roll, pitch, yaw(deg): [%f,%f,%f]", roll, pitch, yaw); 67 | Serial.println(sensor_string_buff); 68 | } 69 | } 70 | else 71 | { 72 | if (icm20948.quat6DataIsReady()) 73 | { 74 | icm20948.readQuat6Data(&quat_w, &quat_x, &quat_y, &quat_z); 75 | sprintf(sensor_string_buff, "Quat6 w, x, y, z (deg): [%f,%f,%f,%f]", quat_w, quat_x, quat_y, quat_z); 76 | Serial.println(sensor_string_buff); 77 | } 78 | } 79 | 80 | } 81 | void run_icm20948_quat9_controller(bool inEuler = false) 82 | { 83 | float quat_w, quat_x, quat_y, quat_z; 84 | float roll, pitch, yaw; 85 | char sensor_string_buff[128]; 86 | if (inEuler) 87 | { 88 | if (icm20948.euler9DataIsReady()) 89 | { 90 | icm20948.readEuler9Data(&roll, &pitch, &yaw); 91 | sprintf(sensor_string_buff, "Euler9 roll, pitch, yaw(deg): [%f,%f,%f]", roll, pitch, yaw); 92 | Serial.println(sensor_string_buff); 93 | } 94 | } 95 | else 96 | { 97 | if (icm20948.quat9DataIsReady()) 98 | { 99 | icm20948.readQuat9Data(&quat_w, &quat_x, &quat_y, &quat_z); 100 | sprintf(sensor_string_buff, "Quat9 w, x, y, z (deg): [%f,%f,%f,%f]", quat_w, quat_x, quat_y, quat_z); 101 | Serial.println(sensor_string_buff); 102 | } 103 | } 104 | 105 | } 106 | void run_icm20948_accel_controller() 107 | { 108 | float x, y, z; 109 | char sensor_string_buff[128]; 110 | if (icm20948.accelDataIsReady()) 111 | { 112 | icm20948.readAccelData(&x, &y, &z); 113 | sprintf(sensor_string_buff, "Acceleration x, y, z (g): [%f,%f,%f]", x, y, z); 114 | Serial.println(sensor_string_buff); 115 | } 116 | 117 | } 118 | void run_icm20948_gyro_controller() 119 | { 120 | float x, y, z; 121 | char sensor_string_buff[128]; 122 | if (icm20948.gyroDataIsReady()) 123 | { 124 | icm20948.readGyroData(&x, &y, &z); 125 | sprintf(sensor_string_buff, "Gyroscope x, y, z (rad/s): [%f,%f,%f]", x, y, z); 126 | Serial.println(sensor_string_buff); 127 | } 128 | 129 | } 130 | void run_icm20948_mag_controller() 131 | { 132 | float x, y, z; 133 | char sensor_string_buff[128]; 134 | if (icm20948.magDataIsReady()) 135 | { 136 | icm20948.readMagData(&x, &y, &z); 137 | sprintf(sensor_string_buff, "Magnetometer x, y, z (mT): [%f,%f,%f]", x, y, z); 138 | Serial.println(sensor_string_buff); 139 | } 140 | 141 | } 142 | void run_icm20948_linearAccel_controller() 143 | { 144 | float x, y, z; 145 | char sensor_string_buff[128]; 146 | if (icm20948.linearAccelDataIsReady()) 147 | { 148 | icm20948.readLinearAccelData(&x, &y, &z); 149 | sprintf(sensor_string_buff, "Linear Acceleration x, y, z (g): [%f,%f,%f]", x, y, z); 150 | Serial.println(sensor_string_buff); 151 | } 152 | 153 | } 154 | void run_icm20948_grav_controller() 155 | { 156 | float x, y, z; 157 | char sensor_string_buff[128]; 158 | if (icm20948.gravDataIsReady()) 159 | { 160 | icm20948.readGravData(&x, &y, &z); 161 | sprintf(sensor_string_buff, "Gravity Vector x, y, z (g): [%f,%f,%f]", x, y, z); 162 | Serial.println(sensor_string_buff); 163 | } 164 | 165 | } 166 | void run_icm20948_har_controller() 167 | { 168 | char activity; 169 | char sensor_string_buff[128]; 170 | if (icm20948.harDataIsReady()) 171 | { 172 | icm20948.readHarData(&activity); 173 | sprintf(sensor_string_buff, "Current Activity : %c", activity); 174 | Serial.println(sensor_string_buff); 175 | } 176 | 177 | } 178 | void run_icm20948_steps_controller() 179 | { 180 | unsigned long steps; 181 | char sensor_string_buff[128]; 182 | if (icm20948.stepsDataIsReady()) 183 | { 184 | icm20948.readStepsData(&steps); 185 | sprintf(sensor_string_buff, "Steps Completed : %lu", steps); 186 | Serial.println(sensor_string_buff); 187 | } 188 | 189 | } 190 | 191 | void setup() 192 | { 193 | Serial.begin(115200); 194 | 195 | Wire.begin(); 196 | Wire.setClock(400000); 197 | Serial.println("Starting ICM"); 198 | delay(10); 199 | i2c_scan(); 200 | if (ICM_found) 201 | { 202 | icm20948.init(icmSettings); 203 | } 204 | 205 | 206 | } 207 | void loop() 208 | { 209 | if (ICM_found) 210 | { 211 | icm20948.task(); 212 | //run_icm20948_accel_controller(); 213 | //run_icm20948_gyro_controller(); 214 | //run_icm20948_mag_controller(); 215 | //run_icm20948_linearAccel_controller(); 216 | //run_icm20948_grav_controller(); 217 | //run_icm20948_quat6_controller(true); 218 | //run_icm20948_quat9_controller(true); 219 | run_icm20948_har_controller(); 220 | run_icm20948_steps_controller(); 221 | } 222 | delay(10); 223 | 224 | } 225 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Arduino ICM20948 DMP library 2 | ====================== 3 | This repository contains a modified version of the ICM20948 library with extra functionality that InvenSense did not complete. It also includes an example project for running the ICM-20948 with Arduino or other Arduino compatible microcontrollers. 4 | 5 | Thanks to the contributor of the original library https://github.com/ZaneL/Teensy-ICM-20948 6 | 7 | ## Functionality 8 | 9 | * Communication over SPI and i2c 10 | * Original DMP firmware with 9DOF sensor fusion 11 | * Easy to set DMP settings 12 | * Provides Accel, Gyro, Mag as Calib, Uncalib and Raw 13 | * Provides Linear Accel, Gravity Vector, 6&9DoF Quaternion and Euler, Activity Recognition and Step Counter 14 | * The sensor is implemented as an Object by using the ArduinoICM20948 class (Check example) 15 | 16 | ## Setup 17 | 18 | Initialize the icm20948 Object 19 | ``` 20 | ArduinoICM20948 icm20948; 21 | ``` 22 | 23 | Initialize the icm20948 Settings 24 | ``` 25 | ArduinoICM20948Settings icmSettings = 26 | { 27 | .i2c_speed = 400000, // i2c clock speed 28 | .is_SPI = false, // Enable SPI, if disable use i2c 29 | .cs_pin = 10, // SPI chip select pin 30 | .spi_speed = 7000000, // SPI clock speed in Hz, max speed is 7MHz 31 | .mode = 1, // 0 = low power mode, 1 = high performance mode 32 | .enable_gyroscope = true, // Enables gyroscope output 33 | .enable_accelerometer = true, // Enables accelerometer output 34 | .enable_magnetometer = true, // Enables magnetometer output // Enables quaternion output 35 | .enable_gravity = true, // Enables gravity vector output 36 | .enable_linearAcceleration = true, // Enables linear acceleration output 37 | .enable_quaternion6 = true, // Enables quaternion 6DOF output 38 | .enable_quaternion9 = true, // Enables quaternion 9DOF output 39 | .enable_har = true, // Enables activity recognition 40 | .enable_steps = true, // Enables step counter 41 | .gyroscope_frequency = 1, // Max frequency = 225, min frequency = 1 42 | .accelerometer_frequency = 1, // Max frequency = 225, min frequency = 1 43 | .magnetometer_frequency = 1, // Max frequency = 70, min frequency = 1 44 | .gravity_frequency = 1, // Max frequency = 225, min frequency = 1 45 | .linearAcceleration_frequency = 1, // Max frequency = 225, min frequency = 1 46 | .quaternion6_frequency = 50, // Max frequency = 225, min frequency = 50 47 | .quaternion9_frequency = 50, // Max frequency = 225, min frequency = 50 48 | .har_frequency = 50, // Max frequency = 225, min frequency = 50 49 | .steps_frequency = 50 // Max frequency = 225, min frequency = 50 50 | }; 51 | ``` 52 | 53 | Load the Settings to the icm20948 at setup 54 | ``` 55 | icm20948.init(icmSettings); 56 | ``` 57 | 58 | Call task to poll the sensor data for all enabled outputs - do this each time you require new data 59 | ``` 60 | icm20948.task(); 61 | ``` 62 | 63 | Read desired output 64 | ``` 65 | icm20948.readGyroData(float *x, float *y, float *z) 66 | 67 | icm20948.readAccelData(float *x, float *y, float *z) 68 | 69 | icm20948.readMagData(float *x, float *y, float *z) 70 | 71 | icm20948.readLinearAccelData(float* x, float* y, float* z) 72 | 73 | icm20948.readGravData(float* x, float* y, float* z) 74 | 75 | icm20948.readQuat6Data(float *w, float *x, float *y, float *z) 76 | 77 | icm20948.readEuler6Data(float *roll, float *pitch, float *yaw) 78 | 79 | icm20948.readQuat9Data(float* w, float* x, float* y, float* z) 80 | 81 | icm20948.readEuler9Data(float* roll, float* pitch, float* yaw) 82 | 83 | //pass char and returns with n = Nothing, d = Drive, w = Walk, r = Run, b = Bike, t = Tilt, s = Still 84 | icm20948.readHarData(char* activity) 85 | 86 | icm20948.readStepsData(unsigned long* step_count) 87 | ``` 88 | 89 | ## Contact 90 | 91 | Contact at jsouriadakis@outlook.com 92 | 93 | Feel free to give feedback and suggest changes. 94 | --------------------------------------------------------------------------------