Master Thesis  V1.0
Research and Design of Sensor Node for NMSD Treatment
ICM20948.c
Go to the documentation of this file.
1 /*
2  * ICM20948.cpp
3  *
4  * Created on: Nov 12, 2019
5  * Author: jonac
6  */
7 
8 
9 /***************************************************************************/
17 #include "ICM20948.h"
18 #include "em_device.h"
19 #include "em_chip.h"
20 #include "em_cmu.h"
21 #include "em_gpio.h"
22 #include "em_usart.h"
23 #include "debug_dbprint.h"
24 #include "delay.h"
25 #include <stdint.h>
26 #include "pinout.h"
27 
28 #include "timer.h" /* Home brew millis() & micros() Arduino like functionality */
29 
30 #include "I2C.h" /* DRAMCO inspired I2C read - write - readwrite library */
31 #include "em_i2c.h"
32 #include "i2cspm.h" /* I2C higher level library to easily setup and use I2C */
33 #include "em_core.h"
34 #include "em_wdog.h"
35 #include "rtcdriver.h"
36 #include "em_rtc.h"
37 
38 #include "bsp.h"
39 
40 bool ICM_20948_Initialized = false;
41 #define M_PI 3.14159265358979323846
42 int32_t mag_minimumRange = 40;
44 
46 float mag_res = 4912.0f / 32752.0f;
47 int16_t _hxcounts,_hycounts,_hzcounts;
51 // transformation matrix
52 /* transform the magnetometer values to match the coordinate system of the IMU */
53 int16_t tX[3] = {1, 0, 0};
54 int16_t tY[3] = {0, -1, 0};
55 int16_t tZ[3] = {0, 0, -1};
57 float _hx, _hy, _hz;
59 float accRawScaling = 32767.5f;
60 float gyroRawScaling = 32767.5f;
61 float magRawScaling = 32767.5f;
63 float _magScale = (4912.0f) / (32767.5f);
65 // static to keep callibration values in memory
66 static float _hxb = 0, _hyb = 0, _hzb = 0;
67 static float _hxs = 1.0f, _hys = 1.0f,_hzs = 1.0f;
70 extern bool IMU_MEASURING;
71 
73 /***************************************************************************/
85 void ICM_20948_Init ()
86 {
87  /* Enable GPIO clock in order to set pin states */
88  CMU_ClockEnable(cmuClock_HFPER, true);
89  CMU_ClockEnable(cmuClock_GPIO, true);
90  delay(10);
91 
92  /* Setup SPI / IIC interface for ICM_20948 */
93  /* Comment / Uncomment to select */
94  //ICM_20948_Init_SPI();
95  IIC_Init();
96  delay(10);
97 
98  /* Power the ICM_20948 */
99  ICM_20948_power(true);
100  delay(200);
101 
102  /* Reset ICM_20948, just to be sure */
103  ICM_20948_reset();
104  delay(100); // 100ms delay needed for reset sequence
105 
106 
107  ICM_20948_Init2();
108 
109 
110 #if DIY == 0
111  BSP_LedSet(0);
112 #endif
113 
114  /* CALIBRATION */
116  float accelCal[3];
117  float gyroCal[3];
118  float magOffset[3];
119  float magScale[3];
120  ICM_20948_accelGyroCalibrate( accelCal, gyroCal );
122 
123 #if DIY == 0
124  BSP_LedClear(0);
125 #endif
126 
127 #if DIY == 0
128  BSP_LedSet(1);
129 #endif
130 
131  ICM_20948_Init2();
132 
134 #if DIY == 1
135  GPIO_PinModeSet(LED_PORT, LED_PIN, gpioModePushPull, 0);
136  GPIO_PinOutSet(LED_PORT, LED_PIN);
137 #endif
138  uint8_t temp[1];
139  ICM_20948_set_mag_mode(AK09916_MODE_100HZ);
140  delay(100);
141  ICM_20948_read_mag_register(0x31, 1, temp);
142  ICM_20948_read_mag_register(0x11, 8, temp);
143 
144  ICM_20948_calibrate_mag( magOffset, magScale );
145 
146 #if DIY == 1
147  GPIO_PinOutClear(LED_PORT, LED_PIN);
148  GPIO_PinModeSet(LED_PORT, LED_PIN, gpioModeDisabled, 0);
149 #endif
150 
152 #if DIY == 0
153  BSP_LedClear(1);
154 #endif
155 
156 
157 
158 
159  ICM_20948_Initialized = true;
160 }
161 
162 /***************************************************************************/
169 void ICM_20948_Init2()
170 {
171 
172 
173  /* Auto select best available clock source PLL if ready, else use internal oscillator */
174  ICM_20948_registerWrite(ICM_20948_REG_PWR_MGMT_1, ICM_20948_BIT_CLK_PLL);
175  /* PLL startup time - no spec in data sheet */
176  delay(30);
177 
178  /* Reset I2C Slave module and use SPI */
179  /* Enable I2C Master I/F module */ /* ICM_20948_BIT_I2C_IF_DIS om te vermijden dat toch I2C zal gebruikt worden */
180  //ICM_20948_registerWrite(ICM_20948_REG_USER_CTRL, ICM_20948_BIT_I2C_MST_EN);//ICM_20948_BIT_I2C_IF_DIS | ICM_20948_BIT_I2C_MST_EN);
181 
182  /* Set I2C Master clock frequency */
183  //ICM_20948_registerWrite(ICM_20948_REG_I2C_MST_CTRL, ICM_20948_I2C_MST_CTRL_CLK_400KHZ);
184 
185  uint8_t temp[8];
186 
187  /* Read ICM20948 "Who am I" register */
188  ICM_20948_registerRead(ICM_20948_REG_WHO_AM_I, 1, temp);
189 
190  /* Check if "Who am I" register was successfully read */
191  if (temp[0] == ICM20948_DEVICE_ID) {
192  #if DEBUG_DBPRINT == 1 /* DEBUG_DBPRINT */
193  dbprint("ICM20948 WHOAMI OKE");
194  #endif /* DEBUG_DBPRINT */
195 
196  }
197 
198 
199 
200  /* Make sure ICM_20948 is not in sleep mode */
201  ICM_20948_sleepModeEnable( false );
202 
203  /* Enable all ICM_20948 sensors */
204  ICM_20948_sensorEnable(true, true, true);
205  delay(10);
206 
207  /* Set ICM_20948 gyro and accel sample rate to 75Hz */
208  ICM_20948_sampleRateSet(50);
209 
210  /* Set full scale range: gyro & accel */
211  ICM_20948_gyroFullscaleSet(ICM_20948_GYRO_FULLSCALE_2000DPS);
212  ICM_20948_accelFullscaleSet(ICM_20948_ACCEL_FULLSCALE_4G);
213 
214  /* Setup 50us interrupt */
215  ICM_20948_latchEnable(true);
216 
217  // TODO: Bandwidth gyro + accel
218  // Standard low pass filters are enabled
219 // ICM_20948_accelBandwidthSet( ICM_20948_ACCEL_BW_1210HZ );
220 // ICM_20948_gyroBandwidthSet( ICM_20948_GYRO_BW_120HZ );
221 
222  /* Auto select best available clock source PLL if ready, else use internal oscillator */
223  ICM_20948_registerWrite(ICM_20948_REG_PWR_MGMT_1, ICM_20948_BIT_CLK_PLL);
224  delay(30);
225 
226 
227  /* Magnetometer */
228  /* IIC passtrough: magnetometer can be accessed on IIC bus */
229  ICM_20948_registerWrite(ICM_20948_REG_INT_PIN_CFG, 0x02);
230  delay(10);
231 
232  /* Reset magnetometer */
233  ICM_20948_write_mag_register(0x32, 0x01);
234  delay(100); // reset takes 100ms
235 
236  /* Read AK09916 "Who am I" register */
237  ICM_20948_read_mag_register(AK09916_REG_WHO_AM_I, 1, temp);
238 
239  /* Check if AK09916 "Who am I" register was successfully read */
240  if (temp[0] == AK09916_DEVICE_ID) {
241  #if DEBUG_DBPRINT == 1 /* DEBUG_DBPRINT */
242  dbprint("AK09916 WHOAMI OKE");
243  #endif /* DEBUG_DBPRINT */
244 
245  }
246 
247  /* Configure magnetometer */
248  ICM_20948_set_mag_mode(AK09916_MODE_50HZ);
249  delay(10);
250 
251  ICM_20948_read_mag_register(0x31, 1, temp);
252  /* Update data by reading through till ST2 register */
253  ICM_20948_read_mag_register(0x11, 8, temp);
254 
255 
256 
257 
258 
259  // /* Set interrupt to trigger every 100ms when the data is ready */
260  // IMU_MEASURING = true;
261  // ICM_20948_interruptEnable(true, false);
262 
263 }
264 
265 /**************************************************************************/
273 void ICM_20948_Init_SPI ()
274 {
275 
276  /*****************************/
277 
278  /* Enable necessary clocks */
279  CMU_ClockEnable(cmuClock_HFPER, true); /* GPIO and USART0/1 are High Frequency Peripherals */
280  CMU_ClockEnable(cmuClock_GPIO, true);
281  CMU_ClockEnable(cmuClock_USART0, true);
282 
283  /* Configure GPIO */
284  /* In the case of gpioModePushPull", the last argument directly sets the pin state */
285  GPIO_PinModeSet(ICM_20948_CLK_PORT, ICM_20948_CLK_PIN, gpioModePushPull, 0); // US0_CLK is push pull
286  /* Normally this is gpioPortE: pin 13 */
287  /* Apparently, can't use the US0_CS port (PE13) to manually set/clear CS line */
288  GPIO_PinModeSet(ICM_20948_CS_PORT, ICM_20948_CS_PIN, gpioModePushPull, 1); // US0_CS is push pull
289  GPIO_PinModeSet(ICM_20948_MISO_PORT, ICM_20948_MISO_PIN, gpioModeInput, 1); // US0_RX (MISO) is input
290  GPIO_PinModeSet(ICM_20948_MOSI_PORT, ICM_20948_MOSI_PIN, gpioModePushPull, 1); // US0_TX (MOSI) is push pull
291 
292  // Start with default config, then modify as necessary
293  USART_InitSync_TypeDef config = USART_INITSYNC_DEFAULT;
294  config.master = true; // master mode
295  //config.baudrate = 1000000; // CLK freq is 1 MHz
296  config.autoCsEnable = true; // CS pin controlled by hardware, not firmware
297  config.clockMode = usartClockMode0; // clock idle low, sample on rising/first edge
298  config.msbf = true; // send MSB first
299  config.enable = usartDisable; // making sure to keep USART disabled until we've set everything up
300 
301  /* Modify some settings */
302  config.clockMode = usartClockMode0; /* clock idle low, sample on rising/first edge (Clock polarity/phase mode = CPOL/CPHA) */
303  config.refFreq = 0; /* USART/UART reference clock assumed when configuring baud rate setup. Set to 0 to use the currently configured reference clock. */
304  config.baudrate = 4000000; // CLK freq is 4 MHz
305  config.databits = usartDatabits8; /* master mode */
306  config.autoTx = false; /* If enabled: Enable AUTOTX mode. Transmits as long as RX is not full. Generates underflows if TX is empty. */
307  config.autoCsEnable = false; /* CS pin controlled by hardware, not firmware */
308 
309  /* Initialize USART0/1 with the configured parameters */
310  USART_InitSync(USART0, &config);
311 
312  // Set and enable USART pin locations
313  USART0->ROUTE = USART_ROUTE_CLKPEN | USART_ROUTE_CSPEN | USART_ROUTE_TXPEN | USART_ROUTE_RXPEN | USART_ROUTE_LOCATION_LOC0;
314 
315  /* Enable USART0/1 */
316  USART_Enable(USART0, usartEnable);
317 
318  /* Set CS high (active low!) */
319  /* Normally this isn't needed, in the case of gpioModePushPull", the last argument directly sets the pin state */
320  GPIO_PinOutSet(gpioPortE, 13);
321 
322 
323  /*****************************/
324 }
325 
326 /**************************************************************************/
344 void ICM_20948_enable_SPI(bool enable)
345 {
346  if (enable)
347  {
348  /* Enable USART clock and peripheral */
349  CMU_ClockEnable(cmuClock_USART0, true);
350 
351  USART_Enable(USART0, usartEnable);
352 
353  /* Configure GPIO */
354  /* In the case of gpioModePushPull", the last argument directly sets the pin state */
355  GPIO_PinModeSet(ICM_20948_CLK_PORT, ICM_20948_CLK_PIN, gpioModePushPull, 0); // US0_CLK is push pull
356  /* Normally this is gpioPortE: pin 13 */
357  /* Apparently, can't use the US0_CS port (PE13) to manually set/clear CS line */
358  GPIO_PinModeSet(ICM_20948_CS_PORT, ICM_20948_CS_PIN, gpioModePushPull, 1); // US0_CS is push pull
359  GPIO_PinModeSet(ICM_20948_MISO_PORT, ICM_20948_MISO_PIN, gpioModeInput, 1); // US0_RX (MISO) is input
360  GPIO_PinModeSet(ICM_20948_MOSI_PORT, ICM_20948_MOSI_PIN, gpioModePushPull, 1); // US0_TX (MOSI) is push pull
361  }
362  else
363  {
364  /* Disable USART clock and peripheral */
365  CMU_ClockEnable(cmuClock_USART0, false);
366 
367  USART_Enable(USART0, usartDisable);
368 
369  /* Configure GPIO */
370  /* In the case of gpioModePushPull", the last argument directly sets the pin state */
371  GPIO_PinModeSet(ICM_20948_CLK_PORT, ICM_20948_CLK_PIN, gpioModeDisabled, 0); // US0_CLK is push pull
372  /* Normally this is gpioPortE: pin 13 */
373  /* Apparently, can't use the US0_CS port (PE13) to manually set/clear CS line */
374  GPIO_PinModeSet(ICM_20948_CS_PORT, ICM_20948_CS_PIN, gpioModeDisabled, 1); // US0_CS is push pull
375  GPIO_PinModeSet(ICM_20948_MISO_PORT, ICM_20948_MISO_PIN, gpioModeDisabled, 1); // US0_RX (MISO) is input
376  GPIO_PinModeSet(ICM_20948_MOSI_PORT, ICM_20948_MOSI_PIN, gpioModeDisabled, 1); // US0_TX (MOSI) is push pull
377 
378  }
379 }
380 
381 /**************************************************************************/
392 void ICM_20948_power (bool enable)
393 {
394  /* Initialize VDD pin if not already the case */
395 // if (!ICM_20948_Initialized)
396 // {
397 // /* In the case of gpioModePushPull", the last argument directly sets the pin state */
398 // GPIO_PinModeSet(ICM_20948_POWER_PORT, ICM_20948_POWER_PIN, gpioModePushPull, enable);
399 
400 // ICM_20948_Initialized = true;
401 // }
402 // else
403 // {
404  if (enable) GPIO_PinModeSet(ICM_20948_POWER_PORT, ICM_20948_POWER_PIN, gpioModePushPull, enable); /* Enable VDD pin */
405  else GPIO_PinModeSet(ICM_20948_POWER_PORT, ICM_20948_POWER_PIN, gpioModeDisabled, 0); /* Disable VDD pin */
406 // }
407 }
408 
409 /**************************************************************************/
414 void ICM_20948_printAllData()
415 {
416  /* Gyro [DPS]
417  * Accel [G *1000] (to print it in Integers
418  */
419 
420  /* Variables to store gyro & accel data
421  * GYRO: X --> ICM_20948_gyro[0];
422  * Y --> ICM_20948_gyro[1];
423  * Z --> ICM_20948_gyro[2];
424  * ACCEL: X --> ICM_20948_accel[0]:
425  * Y --> ICM_20948_accel[1]:
426  * Z --> ICM_20948_accel[2]:
427  */
428 #if DEBUG_DBPRINTs == 1 /* DEBUG_DBPRINT */
429  dbprint(" Gyro x: ");
430  dbprintInt((int) ICM_20948_gyro[0] );
431  dbprint(" Gyro y: ");
432  dbprintInt((int) ICM_20948_gyro[1] );
433  dbprint(" Gyro z: ");
434  dbprintInt((int) ICM_20948_gyro[2] );
435  dbprint(" Accel x: ");
436  dbprintInt((int) (ICM_20948_accel[0]*1000) );
437  dbprint(" Accel y: ");
438  dbprintInt((int) (ICM_20948_accel[1]*1000) );
439  dbprint(" Accel z: ");
440  dbprintInt((int) (ICM_20948_accel[2]*1000) );
441  dbprint(" Magn x: ");
442  dbprintInt((int) ICM_20948_magn[0] );
443  dbprint(" Magn y: ");
444  dbprintInt((int) ICM_20948_magn[1] );
445  dbprint(" Magn z: ");
446  dbprintlnInt((int) ICM_20948_magn[2] );
447 #endif /* DEBUG_DBPRINT */
448 
449 }
450 
451 /**************************************************************************/
459 void ICM_20948_check_WhoAmI()
460 {
461  uint8_t WhoAmI;
462  while( WhoAmI != 0xEA )
463  {
464  WhoAmI = ICM_20948_read( ICM_20948_BANK_0 | 0x00 );
465  }
466 }
467 
468 /**************************************************************************/
480 void ICM_20948_chipSelectSet(bool enable)
481 {
482  if ( enable )
483  {
484  /* Set CS low (active low) to start transmission */
485  GPIO_PinOutClear(ICM_20948_CS_PORT, ICM_20948_CS_PIN);
486  }
487  else
488  {
489  /* Set CS high (active low) */
490  GPIO_PinOutSet(ICM_20948_CS_PORT, ICM_20948_CS_PIN);
491  }
492 }
493 
494 //void ICM_20948_bankSelect(uint8_t bank)
495 //{
496 // /* Enable chip select */
497 // ICM_20948_chipSelectSet(true);
498 //
499 // /* Select the Bank Select register */
500 // USART_Tx(SPI, ICM_20948_REG_BANK_SEL);
501 // USART_Rx(SPI);
502 //
503 // /* Write the desired bank address 0..3 */
504 // USART_Tx(SPI, (bank << 4) );
505 // USART_Rx(SPI);
506 //
507 // /* Disable chip select */
508 // ICM_20948_chipSelectSet(false);
509 //
510 // return;
511 //}
512 
513 /**************************************************************************/
528 void ICM_20948_bankSelect(uint8_t bank)
529 {
530  uint8_t wBuffer[2];
531  wBuffer[0] = ICM_20948_REG_BANK_SEL;
532  wBuffer[1] = (bank << 4);
533 
534  IIC_WriteBuffer(ICM_20948_I2C_ADDRESS, wBuffer, 2);
535 }
536 
537 /**************************************************************************/
545 uint8_t ICM_20948_read ( uint16_t addr )
546 {
547  uint8_t data;
548  uint8_t reg_address;
549  uint8_t bank;
550 
551  /* Select last 7 bytes (A6 - A0) to use as register address */
552  /* Address format: | R/W | A6 | A5 | A4 | A3 | A2 | A1 | A0 | */
553  /* Use AND to change only last 7 bytes */
554  reg_address = (uint8_t) (addr & 0b01111111);
555 
556  bank = (uint8_t) (addr >> 7);
557 
558  /* Select right bank */
559  ICM_20948_bankSelect(bank);
560 
561  /* Enable CS */
562  ICM_20948_chipSelectSet(true);
563 
564  /* Set the first byte to 1 to read (according to data sheet) */
565  /* Address format: | R/W | A6 | A5 | A4 | A3 | A2 | A1 | A0 | */
566  /* Read setup */
567  USART_SpiTransfer(SPI, (reg_address | 0b10000000));
568 
569  /* Read data */
570  data = USART_SpiTransfer(SPI, 0x00);
571 
572  /* Disable CS */
573  ICM_20948_chipSelectSet(false);
574 
575  return (data);
576 }
577 
578 //void ICM_20948_registerRead(uint16_t addr, int numBytes, uint8_t *data)
579 //{
580 // uint8_t regAddr;
581 // uint8_t bank;
582 //
583 // regAddr = (uint8_t) (addr & 0x7F);
584 // bank = (uint8_t) (addr >> 7);
585 //
586 // ICM_20948_bankSelect(bank);
587 //
588 // /* Enable chip select */
589 // ICM_20948_chipSelectSet(true);
590 //
591 // /* Set R/W bit to 1 - read */
592 // USART_Tx(SPI, (regAddr | 0x80) );
593 // USART_Rx(SPI);
594 // /* Transmit 0's to provide clock and read the data */
595 // while ( numBytes-- ) {
596 // USART_Tx(SPI, 0x00);
597 // *data++ = USART_Rx(SPI);
598 // }
599 //
600 // /* Disable chip select */
601 // ICM_20948_chipSelectSet(false);
602 //
603 // return;
604 //}
605 
606 
607 //void ICM_20948_registerWrite(uint16_t addr, uint8_t data)
608 //{
609 // uint8_t regAddr;
610 // uint8_t bank;
611 //
612 // regAddr = (uint8_t) (addr & 0x7F);
613 // bank = (uint8_t) (addr >> 7);
614 //
615 // ICM_20948_bankSelect(bank);
616 //
617 // /* Enable chip select */
618 // ICM_20948_chipSelectSet(true);
619 //
620 // /* clear R/W bit - write, send the address */
621 // USART_Tx(SPI, (regAddr & 0x7F) );
622 // USART_Rx(SPI);
623 //
624 // /* Send the data */
625 // //USART_SpiTransfer( SPI, data );
626 //
627 // USART_Tx(SPI, data);
628 // USART_Rx(SPI);
629 //
630 // /* Disable chip select */
631 // ICM_20948_chipSelectSet(false);
632 //
633 // return;
634 //}
635 
636 
637 /**************************************************************************/
654 void ICM_20948_registerRead(uint16_t addr, int numBytes, uint8_t *data)
655 {
656  uint8_t regAddr;
657  uint8_t bank;
658 
659 
660 
661  uint8_t rLength = (uint8_t) numBytes;
662 
663 
664  regAddr = (uint8_t) (addr & 0x7F);
665  bank = (uint8_t) (addr >> 7);
666 
667  ICM_20948_bankSelect(bank);
668 
669  uint8_t wBuffer[2];
670  wBuffer[0] = regAddr;
671  wBuffer[1] = 0x00; /* 0x00 to read */
672 
673  // TODO
674 
675  IIC_WriteReadBuffer(ICM_20948_I2C_ADDRESS, wBuffer, 1, data, rLength);
676 
677 
678  return;
679 }
680 
681 
682 /**************************************************************************/
697 void ICM_20948_registerWrite(uint16_t addr, uint8_t data)
698 {
699  uint8_t regAddr;
700  uint8_t bank;
701 
702  regAddr = (uint8_t) (addr & 0x7F);
703  bank = (uint8_t) (addr >> 7);
704 
705  ICM_20948_bankSelect(bank);
706 
707  uint8_t wBuffer[2];
708  wBuffer[0] = regAddr;
709  wBuffer[1] = data;
710 
711 
712  IIC_WriteBuffer(ICM_20948_I2C_ADDRESS, wBuffer, 2);
713 
714  return;
715 }
716 
717 /**************************************************************************/
728 uint32_t ICM_20948_sleepModeEnable(bool enable)
729 {
730  uint8_t reg;
731 
732  /* Read the Sleep Enable register */
733  ICM_20948_registerRead(ICM_20948_REG_PWR_MGMT_1, 1, &reg);
734 
735  if ( enable ) {
736  /* Sleep: set the SLEEP bit */
737  reg |= ICM_20948_BIT_SLEEP;
738  } else {
739  /* Wake up: clear the SLEEP bit */
740  //reg &= ~(ICM_20948_BIT_SLEEP); /* this was the provided code */
741 
742  /* My own solution */
743  /* AND to define the bits that can be changed: here bit nr 6 */
744  reg &= 0b10111111;
745  /*OR met 0 to set the SLEEP bit to 0, not really necessary in this case */
746  reg |= 0b00000000;
747  }
748 
749  ICM_20948_registerWrite(ICM_20948_REG_PWR_MGMT_1, reg);
750 
751 
752  return ICM_20948_OK;
753 }
754 
755 
756 /**************************************************************************/
762 uint32_t ICM_20948_reset(void)
763 {
764  /* Set H_RESET bit to initiate soft reset */
765  ICM_20948_registerWrite(ICM_20948_REG_PWR_MGMT_1, ICM_20948_BIT_H_RESET);
766 
767  /* Wait 100ms to complete the reset sequence */
768  delay(100);
769 
770  return ICM_20948_OK;
771 }
772 
773 
774 /**************************************************************************/
783 uint32_t ICM_20948_gyroDataRead(float *gyro)
784 {
785  uint8_t rawData[6];
786  float gyroRes;
787  int16_t temp;
788 
789  /* Retrieve the current resolution */
790  ICM_20948_gyroResolutionGet(&gyroRes);
791 
792  /* Read the six raw data registers into data array */
793  ICM_20948_registerRead(GYRO_XOUT_H, 6, &rawData[0]);
794 
795  /* Convert the MSB and LSB into a signed 16-bit value and multiply by the resolution to get the dps value */
796  temp = ( (int16_t) rawData[0] << 8) | rawData[1];
797  gyro[0] = (float) temp * gyroRes;
798  temp = ( (int16_t) rawData[2] << 8) | rawData[3];
799  gyro[1] = (float) temp * gyroRes;
800  temp = ( (int16_t) rawData[4] << 8) | rawData[5];
801  gyro[2] = (float) temp * gyroRes;
802 
803  return ICM_20948_OK;
804 }
805 
806 
807 /**************************************************************************/
817 uint32_t ICM_20948_gyroResolutionGet(float *gyroRes)
818 {
819  uint8_t reg;
820 
821  /* Read the actual gyroscope full scale setting */
822  ICM_20948_registerRead(ICM_20948_REG_GYRO_CONFIG_1, 1, &reg);
823  reg &= ICM_20948_MASK_GYRO_FULLSCALE;
824 
825  /* Calculate the resolution */
826  switch ( reg ) {
827  case ICM_20948_GYRO_FULLSCALE_250DPS:
828  *gyroRes = 250.0 / 32768.0;
829  break;
830 
831  case ICM_20948_GYRO_FULLSCALE_500DPS:
832  *gyroRes = 500.0 / 32768.0;
833  break;
834 
835  case ICM_20948_GYRO_FULLSCALE_1000DPS:
836  *gyroRes = 1000.0 / 32768.0;
837  break;
838 
839  case ICM_20948_GYRO_FULLSCALE_2000DPS:
840  *gyroRes = 2000.0 / 32768.0;
841  break;
842  }
843 
844  return ICM_20948_OK;
845 }
846 
847 
848 /**************************************************************************/
857 uint32_t ICM_20948_interruptStatusRead(uint32_t *intStatus)
858 {
859  uint8_t reg[4];
860 
861  ICM_20948_registerRead(ICM_20948_REG_INT_STATUS, 4, reg);
862  *intStatus = (uint32_t) reg[0];
863  *intStatus |= ( ( (uint32_t) reg[1]) << 8);
864  *intStatus |= ( ( (uint32_t) reg[2]) << 16);
865  *intStatus |= ( ( (uint32_t) reg[3]) << 24);
866 
867  return ICM_20948_OK;
868 }
869 
870 
871 /**************************************************************************/
885 uint32_t ICM_20948_interruptEnable(bool dataReadyEnable, bool womEnable)
886 {
887  uint8_t intEnable;
888 
889  /* All interrupts disabled by default */
890  intEnable = 0;
891 
892  /* Enable one or both of the interrupt sources if required */
893  if ( womEnable ) {
894  intEnable = ICM_20948_BIT_WOM_INT_EN;
895  }
896  /* Write value to register */
897  ICM_20948_registerWrite(ICM_20948_REG_INT_ENABLE, intEnable);
898 
899  /* All interrupts disabled by default */
900  intEnable = 0;
901 
902  if ( dataReadyEnable ) {
903  intEnable = ICM_20948_BIT_RAW_DATA_0_RDY_EN;
904  }
905 
906  /* Write value to register */
907  ICM_20948_registerWrite(ICM_20948_REG_INT_ENABLE_1, intEnable);
908 
909  return ICM_20948_OK;
910 }
911 
912 
913 /**************************************************************************/
924 uint32_t ICM_20948_sampleRateSet(float sampleRate)
925 {
926  ICM_20948_gyroSampleRateSet(sampleRate);
927  ICM_20948_accelSampleRateSet(sampleRate);
928 
929  return ICM_20948_OK;
930 }
931 
932 
933 /**************************************************************************/
947 float ICM_20948_gyroSampleRateSet(float sampleRate)
948 {
949  uint8_t gyroDiv;
950  float gyroSampleRate;
951 
952  /* Calculate the sample rate divider */
953  gyroSampleRate = (1125.0 / sampleRate) - 1.0;
954 
955  /* Check if it fits in the divider register */
956  if ( gyroSampleRate > 255.0 ) {
957  gyroSampleRate = 255.0;
958  }
959 
960  if ( gyroSampleRate < 0 ) {
961  gyroSampleRate = 0.0;
962  }
963 
964  /* Write the value to the register */
965  gyroDiv = (uint8_t) gyroSampleRate;
966  ICM_20948_registerWrite(ICM_20948_REG_GYRO_SMPLRT_DIV, gyroDiv);
967 
968  /* Calculate the actual sample rate from the divider value */
969  gyroSampleRate = 1125.0 / (gyroDiv + 1);
970 
971  return gyroSampleRate;
972 }
973 
974 /**************************************************************************/
988 float ICM_20948_accelSampleRateSet(float sampleRate)
989 {
990  uint16_t accelDiv;
991  float accelSampleRate;
992 
993  /* Calculate the sample rate divider */
994  accelSampleRate = (1125.0 / sampleRate) - 1.0;
995 
996  /* Check if it fits in the divider registers */
997  if ( accelSampleRate > 4095.0 ) {
998  accelSampleRate = 4095.0;
999  }
1000 
1001  if ( accelSampleRate < 0 ) {
1002  accelSampleRate = 0.0;
1003  }
1004 
1005  /* Write the value to the registers */
1006  accelDiv = (uint16_t) accelSampleRate;
1007  ICM_20948_registerWrite(ICM_20948_REG_ACCEL_SMPLRT_DIV_1, (uint8_t) (accelDiv >> 8) );
1008  ICM_20948_registerWrite(ICM_20948_REG_ACCEL_SMPLRT_DIV_2, (uint8_t) (accelDiv & 0xFF) );
1009 
1010  /* Calculate the actual sample rate from the divider value */
1011  accelSampleRate = 1125.0 / (accelDiv + 1);
1012 
1013  return accelSampleRate;
1014 }
1015 
1016 /**************************************************************************/
1037 uint32_t ICM_20948_lowPowerModeEnter(bool enAccel, bool enGyro, bool enTemp)
1038 {
1039  uint8_t data;
1040 
1041  ICM_20948_registerRead(ICM_20948_REG_PWR_MGMT_1, 1, &data);
1042 
1043  if ( enAccel || enGyro || enTemp ) {
1044  /* Make sure that the chip is not in sleep */
1045  ICM_20948_sleepModeEnable(false);
1046 
1047  /* And in continuous mode */
1048  ICM_20948_cycleModeEnable(false);
1049 
1050  /* Enable the accelerometer and the gyroscope*/
1051  ICM_20948_sensorEnable(enAccel, enGyro, enTemp);
1052  delay(50);
1053 
1054  /* Enable cycle mode */
1055  ICM_20948_cycleModeEnable(true);
1056 
1057  /* Set the LP_EN bit to enable low power mode */
1058  data |= ICM_20948_BIT_LP_EN;
1059  } else {
1060  /* Enable continuous mode */
1061  ICM_20948_cycleModeEnable(false);
1062 
1063  /* Clear the LP_EN bit to disable low power mode */
1064  data &= ~ICM_20948_BIT_LP_EN;
1065  }
1066 
1067  /* Write the updated value to the PWR_MGNT_1 register */
1068  ICM_20948_registerWrite(ICM_20948_REG_PWR_MGMT_1, data);
1069 
1070  return ICM_20948_OK;
1071 }
1072 
1073 
1074 /**************************************************************************/
1095 uint32_t ICM_20948_sensorEnable(bool accel, bool gyro, bool temp)
1096 {
1097  uint8_t pwrManagement1;
1098  uint8_t pwrManagement2;
1099 
1100  ICM_20948_registerRead(ICM_20948_REG_PWR_MGMT_1, 1, &pwrManagement1);
1101  pwrManagement2 = 0;
1102 
1103  /* To enable the accelerometer clear the DISABLE_ACCEL bits in PWR_MGMT_2 */
1104  if ( accel ) {
1105  pwrManagement2 &= ~(ICM_20948_BIT_PWR_ACCEL_STBY);
1106  } else {
1107  pwrManagement2 |= ICM_20948_BIT_PWR_ACCEL_STBY;
1108  }
1109 
1110  /* To enable gyro clear the DISABLE_GYRO bits in PWR_MGMT_2 */
1111  if ( gyro ) {
1112  pwrManagement2 &= ~(ICM_20948_BIT_PWR_GYRO_STBY);
1113  } else {
1114  pwrManagement2 |= ICM_20948_BIT_PWR_GYRO_STBY;
1115  }
1116 
1117  /* To enable the temperature sensor clear the TEMP_DIS bit in PWR_MGMT_1 */
1118  if ( temp ) {
1119  pwrManagement1 &= ~(ICM_20948_BIT_TEMP_DIS);
1120  } else {
1121  pwrManagement1 |= ICM_20948_BIT_TEMP_DIS;
1122  }
1123 
1124  /* Write back the modified values */
1125  ICM_20948_registerWrite(ICM_20948_REG_PWR_MGMT_1, pwrManagement1);
1126  ICM_20948_registerWrite(ICM_20948_REG_PWR_MGMT_2, pwrManagement2);
1127 
1128  return ICM_20948_OK;
1129 }
1130 
1131 
1132 /**************************************************************************/
1142 uint32_t ICM_20948_cycleModeEnable(bool enable)
1143 {
1144  uint8_t reg;
1145 
1146  reg = 0x00;
1147 
1148  if ( enable ) {
1149  reg = ICM_20948_BIT_ACCEL_CYCLE | ICM_20948_BIT_GYRO_CYCLE;
1150  }
1151 
1152  ICM_20948_registerWrite(ICM_20948_REG_LP_CONFIG, reg);
1153 
1154  return ICM_20948_OK;
1155 }
1156 
1157 
1158 /**************************************************************************/
1170 uint32_t ICM_20948_accelDataRead(float *accel)
1171 {
1172  uint8_t rawData[6];
1173  float accelRes;
1174  int16_t temp;
1175 
1176  /* Retrieve the current resolution */
1177  ICM_20948_accelResolutionGet(&accelRes);
1178 
1179  /* Read the six raw data registers into data array */
1180  ICM_20948_registerRead(ICM_20948_REG_ACCEL_XOUT_H_SH, 6, &rawData[0]);
1181 
1182  /* Convert the MSB and LSB into a signed 16-bit value and multiply by the resolution to get the G value */
1183  temp = ( (int16_t) rawData[0] << 8) | rawData[1];
1184  accel[0] = (float) temp * accelRes;
1185  temp = ( (int16_t) rawData[2] << 8) | rawData[3];
1186  accel[1] = (float) temp * accelRes;
1187  temp = ( (int16_t) rawData[4] << 8) | rawData[5];
1188  accel[2] = (float) temp * accelRes;
1189 
1190  return ICM_20948_OK;
1191 }
1192 
1193 /**************************************************************************/
1202 uint32_t ICM_20948_accelResolutionGet(float *accelRes)
1203 {
1204  uint8_t reg;
1205 
1206  /* Read the actual acceleration full scale setting */
1207  ICM_20948_registerRead(ICM_20948_REG_ACCEL_CONFIG, 1, &reg);
1208  reg &= ICM_20948_MASK_ACCEL_FULLSCALE;
1209 
1210  /* Calculate the resolution */
1211  switch ( reg ) {
1212  case ICM_20948_ACCEL_FULLSCALE_2G:
1213  *accelRes = 2.0 / 32768.0;
1214  break;
1215 
1216  case ICM_20948_ACCEL_FULLSCALE_4G:
1217  *accelRes = 4.0 / 32768.0;
1218  break;
1219 
1220  case ICM_20948_ACCEL_FULLSCALE_8G:
1221  *accelRes = 8.0 / 32768.0;
1222  break;
1223 
1224  case ICM_20948_ACCEL_FULLSCALE_16G:
1225  *accelRes = 16.0 / 32768.0;
1226  break;
1227  }
1228 
1229  return ICM_20948_OK;
1230 }
1231 
1232 
1233 /**************************************************************************/
1242 uint32_t ICM_20948_accelFullscaleSet(uint8_t accelFs)
1243 {
1244  uint8_t reg;
1245 
1246  accelFs &= ICM_20948_MASK_ACCEL_FULLSCALE;
1247  ICM_20948_registerRead(ICM_20948_REG_ACCEL_CONFIG, 1, &reg);
1248  reg &= ~(ICM_20948_MASK_ACCEL_FULLSCALE);
1249  reg |= accelFs;
1250  ICM_20948_registerWrite(ICM_20948_REG_ACCEL_CONFIG, reg);
1251 
1252  return ICM_20948_OK;
1253 }
1254 
1255 
1256 /**************************************************************************/
1265 uint32_t ICM_20948_gyroFullscaleSet(uint8_t gyroFs)
1266 {
1267  uint8_t reg;
1268 
1269  gyroFs &= ICM_20948_MASK_GYRO_FULLSCALE;
1270  ICM_20948_registerRead(ICM_20948_REG_GYRO_CONFIG_1, 1, &reg);
1271  reg &= ~(ICM_20948_MASK_GYRO_FULLSCALE);
1272  reg |= gyroFs;
1273  ICM_20948_registerWrite(ICM_20948_REG_GYRO_CONFIG_1, reg);
1274 
1275  return ICM_20948_OK;
1276 }
1277 
1278 
1279 /**************************************************************************/
1295 uint32_t ICM_20948_wakeOnMotionITEnable(bool enable, uint8_t womThreshold, float sampleRate)
1296 {
1297  if ( enable ) {
1298  /* Make sure that the chip is not in sleep */
1299  ICM_20948_sleepModeEnable(false);
1300 
1301  /* And in continuous mode */
1302  ICM_20948_cycleModeEnable(false);
1303 
1304  /* Enable only the accelerometer */
1305  ICM_20948_sensorEnable(true, false, false);
1306 
1307  /* Set sample rate */
1308  ICM_20948_sampleRateSet(sampleRate);
1309 
1310  /* Set the bandwidth to 1210Hz */
1311  ICM_20948_accelBandwidthSet(ICM_20948_ACCEL_BW_1210HZ);
1312 
1313  /* Accel: 2G full scale */
1314  ICM_20948_accelFullscaleSet(ICM_20948_ACCEL_FULLSCALE_2G);
1315 
1316  /* Enable the Wake On Motion interrupt */
1317  ICM_20948_interruptEnable(false, true);
1318  delay(50);
1319 
1320  /* Enable Wake On Motion feature */
1321  ICM_20948_registerWrite(ICM_20948_REG_ACCEL_INTEL_CTRL, ICM_20948_BIT_ACCEL_INTEL_EN | ICM_20948_BIT_ACCEL_INTEL_MODE);
1322 
1323  /* Set the wake on motion threshold value */
1324  ICM_20948_registerWrite(ICM_20948_REG_ACCEL_WOM_THR, womThreshold);
1325 
1326  /* Enable low power mode */
1327  ICM_20948_lowPowerModeEnter(true, false, false);
1328  } else {
1329  /* Disable Wake On Motion feature */
1330  ICM_20948_registerWrite(ICM_20948_REG_ACCEL_INTEL_CTRL, 0x00);
1331 
1332  /* Disable the Wake On Motion interrupt */
1333  ICM_20948_interruptEnable(false, false);
1334 
1335  /* Disable cycle mode */
1336  ICM_20948_cycleModeEnable(false);
1337  }
1338 
1339  return ICM_20948_OK;
1340 }
1341 
1342 
1343 /**************************************************************************/
1352 uint32_t ICM_20948_gyroBandwidthSet(uint8_t gyroBw)
1353 {
1354  uint8_t reg;
1355 
1356  /* Read the GYRO_CONFIG_1 register */
1357  ICM_20948_registerRead(ICM_20948_REG_GYRO_CONFIG_1, 1, &reg);
1358  reg &= ~(ICM_20948_MASK_GYRO_BW);
1359 
1360  /* Write the new bandwidth value to the gyro config register */
1361  reg |= (gyroBw & ICM_20948_MASK_GYRO_BW);
1362  ICM_20948_registerWrite(ICM_20948_REG_GYRO_CONFIG_1, reg);
1363 
1364  return ICM_20948_OK;
1365 }
1366 
1367 /**************************************************************************/
1376 uint32_t ICM_20948_accelBandwidthSet(uint8_t accelBw)
1377 {
1378  uint8_t reg;
1379 
1380  /* Read the GYRO_CONFIG_1 register */
1381  ICM_20948_registerRead(ICM_20948_REG_ACCEL_CONFIG, 1, &reg);
1382  reg &= ~(ICM_20948_MASK_ACCEL_BW);
1383 
1384  /* Write the new bandwidth value to the gyro config register */
1385  reg |= (accelBw & ICM_20948_MASK_ACCEL_BW);
1386  ICM_20948_registerWrite(ICM_20948_REG_ACCEL_CONFIG, reg);
1387 
1388  return ICM_20948_OK;
1389 }
1390 
1391 
1392 /**************************************************************************/
1403 uint32_t ICM_20948_latchEnable(bool enable)
1404 {
1405  uint8_t temp;
1406  ICM_20948_registerRead(ICM_20948_REG_INT_PIN_CFG, 1, &temp);
1407 
1408  if(enable)
1409  {
1410  /* Set interrupt pin to pulse, no need to read a interrupt register for proceeding */
1411  /* 1 � INT1 pin level held until interrupt status is cleared.
1412  * 0 � INT1 pin indicates interrupt pulse is width 50 us <----
1413  */
1414  ICM_20948_registerWrite(ICM_20948_REG_INT_PIN_CFG, temp | 0b00010000);
1415  //ICM_20948_registerWrite(ICM_20948_REG_INT_PIN_CFG, 0b00010000);
1416  }else{
1417  ICM_20948_registerWrite(ICM_20948_REG_INT_PIN_CFG, 0b00000000);
1418  }
1419 
1420  return ICM_20948_OK;
1421 }
1422 
1423 
1424 
1425 /**********************************************************************/
1426 /************** Magnetometer *********************/
1427 /**********************************************************************/
1428 
1429 /**************************************************************************/
1443 void ICM_20948_set_mag_transfer(bool read) {
1444  uint8_t MAG_BIT_READ = AK09916_BIT_I2C_SLV_ADDR | ICM_20948_BIT_I2C_SLV_READ;
1445  uint8_t MAG_BIT_WRITE = AK09916_BIT_I2C_SLV_ADDR;
1446 
1447  bool read_old = !read;
1448 
1449  /* Set transfer mode if it has changed */
1450  if (read != read_old) {
1451  if (read) {
1452  ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV0_ADDR, MAG_BIT_READ);
1453  }
1454  else {
1455  ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV0_ADDR, MAG_BIT_WRITE);
1456  }
1457  read_old = read;
1458  }
1459  return;
1460 }
1461 
1462 /**************************************************************************/
1472 void waitForSlave4()
1473 {
1474  // Wait until the data is ready:
1475 
1476  delay(100);
1477  unsigned char status = ICM_20948_read(ICM_20948_REG_I2C_MST_STATUS);
1478  if (status & ICM_20948_BIT_SLV4_NACK)
1479  {
1480 #if DEBUG_DBPRINT == 1 /* DEBUG_DBPRINT */
1481  dbprint("Failed to communicate with compass: NACK");
1482 #endif /* DEBUG_DBPRINT */
1483  }
1484  if (status & ICM_20948_BIT_SLV4_DONE)
1485  {
1486 #if DEBUG_DBPRINT == 1 /* DEBUG_DBPRINT */
1487  dbprint("Transaction Done");
1488 #endif /* DEBUG_DBPRINT */
1489  }
1490 }
1491 
1492 
1493 /**************************************************************************/
1509 unsigned char readMagRegister(unsigned char magreg)
1510 {
1511  // We use slave4, which is oneshot:
1512  ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV4_ADDR, ICM_20948_BIT_I2C_READ | AK09916_BIT_I2C_SLV_ADDR);
1513  ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV4_REG, magreg);
1514  ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV4_CTRL, ICM_20948_BIT_I2C_SLV_EN);
1515 
1516  waitForSlave4();
1517 
1518  return ICM_20948_read(ICM_20948_REG_I2C_SLV4_DI);
1519 }
1520 
1521 
1522 
1523 /**************************************************************************/
1538 void writeMagRegister(unsigned char magreg, unsigned char val)
1539 {
1540  // We use slave4, which is oneshot:
1541  ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV4_ADDR, AK09916_BIT_I2C_SLV_ADDR);
1542  ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV4_REG, magreg);
1543  ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV4_DO, val);
1544  ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV4_CTRL, ICM_20948_BIT_I2C_SLV_EN);
1545 
1546  waitForSlave4();
1547 }
1548 
1549 
1550 
1551 
1552 
1553 
1554 
1555 
1556 
1557 
1558 /***************************************************************************/
1575 //void ICM_20948_read_mag_register(uint8_t addr, uint8_t numBytes, uint8_t *data) { // functie correct!
1576 //
1577 // /* Set transfer mode to read */
1578 // ICM_20948_set_mag_transfer(true);
1579 //
1580 // /* Set SLV0_REG to magnetometer register address */
1581 // ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV0_REG, addr);
1582 //
1583 // /* Request bytes */
1584 // ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV0_CTRL, ICM_20948_BIT_I2C_SLV_EN | numBytes);
1585 //
1586 // /* Wait some time for registers to fill */
1587 // delay(1);
1588 //
1589 // /* Read bytes from the ICM20948 EXT_SLV_SENS_DATA registers */
1590 // ICM_20948_registerRead(ICM_20948_REG_EXT_SLV_SENS_DATA_00, numBytes, data);
1591 //
1592 // return;
1593 //}
1594 void ICM_20948_read_mag_register(uint8_t addr, uint8_t numBytes, uint8_t *data) {
1595  uint8_t wBuffer[2];
1596  wBuffer[0] = addr;
1597  wBuffer[1] = 0x00;
1598 
1599  IIC_WriteReadBuffer( ( AK09916_BIT_I2C_SLV_ADDR << 1 ), wBuffer, 1, data, numBytes);
1600 }
1601 
1602 /**************************************************************************/
1615 //void ICM_20948_write_mag_register(uint8_t addr, uint8_t data) { // functie correct!
1616 //
1617 // /* Set transfer mode to write */
1618 // ICM_20948_set_mag_transfer(false);
1619 // //ICM_20948_registerRead(ICM_20948_REG_I2C_SLV0_ADDR, 1, a);
1620 //
1621 // /* Set SLV0_REG to magnetometer register address */
1622 // ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV0_REG, addr);
1623 //
1624 //
1625 // /* Store data to write inside SLV0_DO */
1626 // //ICM_20948_registerRead(ICM_20948_REG_I2C_SLV0_DO, 1, a);
1627 // ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV0_DO, data);
1628 // //ICM_20948_registerRead(ICM_20948_REG_I2C_SLV0_DO, 1, a);
1629 //
1630 // /* Send one byte */
1631 // ICM_20948_registerWrite(ICM_20948_REG_I2C_SLV0_CTRL, ICM_20948_BIT_I2C_SLV_EN | 0x01);
1632 //
1633 // //ICM_20948_registerRead(ICM_20948_REG_I2C_SLV0_DO, 1, a);
1634 // //ICM_20948_registerRead(ICM_20948_REG_I2C_SLV0_ADDR, 1, a);
1635 //
1636 // /* Wait some time for registers to fill */
1637 // //delay(10);
1638 //
1639 // uint8_t temp[1];
1640 // ICM_20948_read_mag_register(addr, 1, temp);
1641 // if(temp[0] == data)
1642 // {
1643 // dbprintln("Mag data write correct!");
1644 // }
1645 //
1646 // return;
1647 //}
1648 void ICM_20948_write_mag_register(uint8_t addr, uint8_t data) {
1649  uint8_t wBuffer[2];
1650  wBuffer[0] = addr;
1651  wBuffer[1] = data;
1652 
1653  IIC_WriteBuffer( ( AK09916_BIT_I2C_SLV_ADDR << 1 ), wBuffer, 2);
1654 }
1655 
1656 
1657 /**************************************************************************/
1670 uint32_t ICM_20948_set_mag_mode(uint8_t magMode){
1671  switch ( magMode ) {
1672  case AK09916_BIT_MODE_POWER_DOWN:
1673  break;
1674  case AK09916_MODE_SINGLE:
1675  break;
1676  case AK09916_MODE_10HZ:
1677  break;
1678  case AK09916_MODE_20HZ:
1679  break;
1680  case AK09916_MODE_50HZ:
1681  break;
1682  case AK09916_MODE_100HZ:
1683  break;
1684  case AK09916_MODE_ST:
1685  break;
1686  default:
1687  return ERROR;
1688  }
1689 
1690  ICM_20948_write_mag_register(AK09916_REG_CONTROL_2, magMode);
1691 
1692  return OK;
1693 }
1694 /**************************************************************************/
1707 void ICM_20948_magRawDataRead(float *raw_magn) {
1708 
1709  uint8_t data[8];
1710  ICM_20948_read_mag_register(0x11, 8, data);
1711 
1712  /* Convert the LSB and MSB into a signed 16-bit value */
1713  _hxcounts = (((int16_t) data[1] << 8) | data[0] );
1714  _hycounts = (((int16_t) data[3] << 8) | data[2] );
1715  _hzcounts = (((int16_t) data[5] << 8) | data[4] );
1716 
1717  /* Transform to same coordinate system as gyro & accel */
1718  raw_magn[0] = (float)(tX[0]*_hxcounts + tX[1]*_hycounts + tX[2]*_hzcounts);
1719  raw_magn[1] = (float)(tY[0]*_hxcounts + tY[1]*_hycounts + tY[2]*_hzcounts);
1720  raw_magn[2] = (float)(tZ[0]*_hxcounts + tZ[1]*_hycounts + tZ[2]*_hzcounts);
1721 
1722 #if DEBUG_DBPRINTs == 1 /* DEBUG_DBPRINT */
1723  dbprint("rawMag x: ");
1724  dbprintInt((int) raw_magn[0]);
1725  dbprint(" rawMag y: ");
1726  dbprintInt((int) raw_magn[1]);
1727  dbprint(" minMag z: ");
1728  dbprintInt((int) raw_magn[2]);
1729  dbprintln("");
1730 #endif /* DEBUG_DBPRINT */
1731 
1732 }
1733 
1734 
1735 /**************************************************************************/
1745 void ICM_20948_magDataRead(float *magn) {
1746 
1747  uint8_t data[8];
1748  ICM_20948_read_mag_register(0x11, 8, data);
1749 
1750  /* Convert the LSB and MSB into a signed 16-bit value */
1751  _hxcounts = (((int16_t) data[1] << 8) | data[0] );
1752  _hycounts = (((int16_t) data[3] << 8) | data[2] );
1753  _hzcounts = (((int16_t) data[5] << 8) | data[4] );
1754 
1755  magn[0] = (((float)(tX[0]*_hxcounts + tX[1]*_hycounts + tX[2]*_hzcounts) * _magScale) + _hxb)*_hxs;
1756  magn[1] = (((float)(tY[0]*_hxcounts + tY[1]*_hycounts + tY[2]*_hzcounts) * _magScale) + _hyb)*_hys;
1757  magn[2] = (((float)(tZ[0]*_hxcounts + tZ[1]*_hycounts + tZ[2]*_hzcounts) * _magScale) + _hzb)*_hzs;
1758 
1759 }
1760 
1761 
1762 /**************************************************************************/
1768 uint32_t ICM_20948_reset_mag(void) {
1769  /* Set SRST bit to initiate soft reset */
1770  ICM_20948_write_mag_register(AK09916_REG_CONTROL_3, AK09916_BIT_SRST);
1771 
1772  /* Wait 100ms to complete reset sequence */
1773  delay(100);
1774 
1775  return OK;
1776 }
1777 
1778 
1779 /* Embedded 2 */
1780 
1781 /**************************************************************************/
1793 void ICM_20948_magn_to_angle(float *magn, float *angle){
1794 
1795  angle[0] = atan2(magn[1], magn[0]) * M_PI/180.0f;
1796 
1797 }
1798 
1799 
1803 
1804 /***************************************************************************/
1820 uint32_t ICM_20948_accelGyroCalibrate(float *accelBiasScaled, float *gyroBiasScaled)
1821 {
1822  uint8_t data[12];
1823  uint16_t i, packetCount, fifoCount;
1824  int32_t gyroBias[3] = { 0, 0, 0 };
1825  int32_t accelBias[3] = { 0, 0, 0 };
1826  int32_t accelTemp[3];
1827  int32_t gyroTemp[3];
1828  int32_t accelBiasFactory[3];
1829  int32_t gyroBiasStored[3];
1830  float gyroRes, accelRes;
1831 
1832  /* Enable the accelerometer and the gyro */
1833  ICM_20948_sensorEnable(true, true, false);
1834 
1835  /* Set 1kHz sample rate */
1836  ICM_20948_sampleRateSet(1100.0);
1837 
1838  /* 246Hz BW for the accelerometer and 200Hz for the gyroscope */
1839  ICM_20948_accelBandwidthSet(ICM_20948_ACCEL_BW_246HZ);
1840  ICM_20948_gyroBandwidthSet(ICM_20948_GYRO_BW_12HZ);
1841 
1842  /* Set the most sensitive range: 2G full scale and 250dps full scale */
1843  ICM_20948_accelFullscaleSet(ICM_20948_ACCEL_FULLSCALE_2G);
1844  ICM_20948_gyroFullscaleSet(ICM_20948_GYRO_FULLSCALE_250DPS);
1845 
1846  /* Retrieve the resolution per bit */
1847  ICM_20948_accelResolutionGet(&accelRes);
1848  ICM_20948_gyroResolutionGet(&gyroRes);
1849 
1850  /* The accel sensor needs max 30ms, the gyro max 35ms to fully start */
1851  /* Experiments show that the gyro needs more time to get reliable results */
1852  delay(50);
1853 
1854  /* Disable the FIFO */
1855  ICM_20948_registerWrite(ICM_20948_REG_USER_CTRL, ICM_20948_BIT_FIFO_EN);
1856  ICM_20948_registerWrite(ICM_20948_REG_FIFO_MODE, 0x0F);
1857 
1858  /* Enable accelerometer and gyro to store the data in FIFO */
1859  ICM_20948_registerWrite(ICM_20948_REG_FIFO_EN_2, ICM_20948_BIT_ACCEL_FIFO_EN | ICM_20948_BITS_GYRO_FIFO_EN);
1860 
1861  /* Reset the FIFO */
1862  ICM_20948_registerWrite(ICM_20948_REG_FIFO_RST, 0x0F);
1863  ICM_20948_registerWrite(ICM_20948_REG_FIFO_RST, 0x00);
1864 
1865  /* Enable the FIFO */
1866  ICM_20948_registerWrite(ICM_20948_REG_USER_CTRL, ICM_20948_BIT_FIFO_EN);
1867 
1868  /* The max FIFO size is 4096 bytes, one set of measurements takes 12 bytes */
1869  /* (3 axes, 2 sensors, 2 bytes each value ) 340 samples use 4080 bytes of FIFO */
1870  /* Loop until at least 4080 samples gathered */
1871  fifoCount = 0;
1872  while ( fifoCount < 4080 ) {
1873  delay(5);
1874  /* Read FIFO sample count */
1875  ICM_20948_registerRead(ICM_20948_REG_FIFO_COUNT_H, 2, &data[0]);
1876  /* Convert to a 16 bit value */
1877  fifoCount = ( (uint16_t) (data[0] << 8) | data[1]);
1878  }
1879 
1880  /* Disable accelerometer and gyro to store the data in FIFO */
1881  ICM_20948_registerWrite(ICM_20948_REG_FIFO_EN_2, 0x00);
1882 
1883  /* Read FIFO sample count */
1884  ICM_20948_registerRead(ICM_20948_REG_FIFO_COUNT_H, 2, &data[0]);
1885 
1886  /* Convert to a 16 bit value */
1887  fifoCount = ( (uint16_t) (data[0] << 8) | data[1]);
1888 
1889  /* Calculate the number of data sets (3 axis of accel an gyro, two bytes each = 12 bytes) */
1890  packetCount = fifoCount / 12;
1891 
1892  /* Retrieve the data from the FIFO */
1893  for ( i = 0; i < packetCount; i++ ) {
1894  ICM_20948_registerRead(ICM_20948_REG_FIFO_R_W, 12, &data[0]);
1895  /* Convert to 16 bit signed accel and gyro x,y and z values */
1896  accelTemp[0] = ( (int16_t) (data[0] << 8) | data[1]);
1897  accelTemp[1] = ( (int16_t) (data[2] << 8) | data[3]);
1898  accelTemp[2] = ( (int16_t) (data[4] << 8) | data[5]);
1899  gyroTemp[0] = ( (int16_t) (data[6] << 8) | data[7]);
1900  gyroTemp[1] = ( (int16_t) (data[8] << 8) | data[9]);
1901  gyroTemp[2] = ( (int16_t) (data[10] << 8) | data[11]);
1902 
1903  /* Sum the values */
1904  accelBias[0] += accelTemp[0];
1905  accelBias[1] += accelTemp[1];
1906  accelBias[2] += accelTemp[2];
1907  gyroBias[0] += gyroTemp[0];
1908  gyroBias[1] += gyroTemp[1];
1909  gyroBias[2] += gyroTemp[2];
1910  }
1911 
1912  /* Divide by packet count to get the average */
1913  accelBias[0] /= packetCount;
1914  accelBias[1] /= packetCount;
1915  accelBias[2] /= packetCount;
1916  gyroBias[0] /= packetCount;
1917  gyroBias[1] /= packetCount;
1918  gyroBias[2] /= packetCount;
1919 
1920  /* Acceleormeter: add or remove (depending on the orientation of the chip) 1G (gravity) from the Z axis value */
1921  if ( accelBias[2] > 0L ) {
1922  accelBias[2] -= (int32_t) (1.0 / accelRes);
1923  } else {
1924  accelBias[2] += (int32_t) (1.0 / accelRes);
1925  }
1926 
1927  /* Convert the values to degrees per sec for displaying */
1928  gyroBiasScaled[0] = (float) gyroBias[0] * gyroRes;
1929  gyroBiasScaled[1] = (float) gyroBias[1] * gyroRes;
1930  gyroBiasScaled[2] = (float) gyroBias[2] * gyroRes;
1931 
1932  /* Read stored gyro trim values. After reset these values are all 0 */
1933  ICM_20948_registerRead(ICM_20948_REG_XG_OFFS_USRH, 2, &data[0]);
1934  gyroBiasStored[0] = ( (int16_t) (data[0] << 8) | data[1]);
1935  ICM_20948_registerRead(ICM_20948_REG_YG_OFFS_USRH, 2, &data[0]);
1936  gyroBiasStored[1] = ( (int16_t) (data[0] << 8) | data[1]);
1937  ICM_20948_registerRead(ICM_20948_REG_ZG_OFFS_USRH, 2, &data[0]);
1938  gyroBiasStored[2] = ( (int16_t) (data[0] << 8) | data[1]);
1939 
1940  /* The gyro bias should be stored in 1000dps full scaled format. We measured in 250dps to get */
1941  /* the best sensitivity, so need to divide by 4 */
1942  /* Substract from the stored calibration value */
1943  gyroBiasStored[0] -= gyroBias[0] / 4;
1944  gyroBiasStored[1] -= gyroBias[1] / 4;
1945  gyroBiasStored[2] -= gyroBias[2] / 4;
1946 
1947  /* Split the values into two bytes */
1948  data[0] = (gyroBiasStored[0] >> 8) & 0xFF;
1949  data[1] = (gyroBiasStored[0]) & 0xFF;
1950  data[2] = (gyroBiasStored[1] >> 8) & 0xFF;
1951  data[3] = (gyroBiasStored[1]) & 0xFF;
1952  data[4] = (gyroBiasStored[2] >> 8) & 0xFF;
1953  data[5] = (gyroBiasStored[2]) & 0xFF;
1954 
1955  /* Write the gyro bias values to the chip */
1956  ICM_20948_registerWrite(ICM_20948_REG_XG_OFFS_USRH, data[0]);
1957  ICM_20948_registerWrite(ICM_20948_REG_XG_OFFS_USRL, data[1]);
1958  ICM_20948_registerWrite(ICM_20948_REG_YG_OFFS_USRH, data[2]);
1959  ICM_20948_registerWrite(ICM_20948_REG_YG_OFFS_USRL, data[3]);
1960  ICM_20948_registerWrite(ICM_20948_REG_ZG_OFFS_USRH, data[4]);
1961  ICM_20948_registerWrite(ICM_20948_REG_ZG_OFFS_USRL, data[5]);
1962 
1963  /* Calculate the accelerometer bias values to store in the hardware accelerometer bias registers. These registers contain */
1964  /* factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold */
1965  /* non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature */
1966  /* compensation calculations(? the datasheet is not clear). Accelerometer bias registers expect bias input */
1967  /* as 2048 LSB per g, so that the accelerometer biases calculated above must be divided by 8. */
1968 
1969  /* Read factory accelerometer trim values */
1970  ICM_20948_registerRead(ICM_20948_REG_XA_OFFSET_H, 2, &data[0]);
1971  accelBiasFactory[0] = ( (int16_t) (data[0] << 8) | data[1]);
1972  ICM_20948_registerRead(ICM_20948_REG_YA_OFFSET_H, 2, &data[0]);
1973  accelBiasFactory[1] = ( (int16_t) (data[0] << 8) | data[1]);
1974  ICM_20948_registerRead(ICM_20948_REG_ZA_OFFSET_H, 2, &data[0]);
1975  accelBiasFactory[2] = ( (int16_t) (data[0] << 8) | data[1]);
1976 
1977  /* Construct total accelerometer bias, including calculated average accelerometer bias from above */
1978  /* Scale the 2g full scale (most sensitive range) results to 16g full scale - divide by 8 */
1979  /* Clear the last bit (temperature compensation? - the datasheet is not clear) */
1980  /* Substract from the factory calibration value */
1981 
1982  accelBiasFactory[0] -= ( (accelBias[0] / 8) & ~1);
1983  accelBiasFactory[1] -= ( (accelBias[1] / 8) & ~1);
1984  accelBiasFactory[2] -= ( (accelBias[2] / 8) & ~1);
1985 
1986  /* Split the values into two bytes */
1987  data[0] = (accelBiasFactory[0] >> 8) & 0xFF;
1988  data[1] = (accelBiasFactory[0]) & 0xFF;
1989  data[2] = (accelBiasFactory[1] >> 8) & 0xFF;
1990  data[3] = (accelBiasFactory[1]) & 0xFF;
1991  data[4] = (accelBiasFactory[2] >> 8) & 0xFF;
1992  data[5] = (accelBiasFactory[2]) & 0xFF;
1993 
1994  /* Store them in the accelerometer offset registers */
1995  ICM_20948_registerWrite(ICM_20948_REG_XA_OFFSET_H, data[0]);
1996  ICM_20948_registerWrite(ICM_20948_REG_XA_OFFSET_L, data[1]);
1997  ICM_20948_registerWrite(ICM_20948_REG_YA_OFFSET_H, data[2]);
1998  ICM_20948_registerWrite(ICM_20948_REG_YA_OFFSET_L, data[3]);
1999  ICM_20948_registerWrite(ICM_20948_REG_ZA_OFFSET_H, data[4]);
2000  ICM_20948_registerWrite(ICM_20948_REG_ZA_OFFSET_L, data[5]);
2001 
2002  /* Convert the values to G for displaying */
2003  accelBiasScaled[0] = (float) accelBias[0] * accelRes;
2004  accelBiasScaled[1] = (float) accelBias[1] * accelRes;
2005  accelBiasScaled[2] = (float) accelBias[2] * accelRes;
2006 
2007  /* Turn off FIFO */
2008  ICM_20948_registerWrite(ICM_20948_REG_USER_CTRL, 0x00);
2009 
2010  /* Disable all sensors */
2011  ICM_20948_sensorEnable(false, false, false);
2012 
2013  return ICM_20948_OK;
2014 }
2015 
2016 
2017 
2018 /***************************************************************************/
2031 uint32_t ICM_20948_gyroCalibrate(float *gyroBiasScaled)
2032 {
2033  uint8_t data[12];
2034  uint16_t i, packetCount, fifoCount;
2035  int32_t gyroBias[3] = { 0, 0, 0 };
2036  int32_t gyroTemp[3];
2037  int32_t gyroBiasStored[3];
2038  float gyroRes;
2039 
2040  /* Enable the accelerometer and the gyro */
2041  ICM_20948_sensorEnable(true, true, false);
2042 
2043  /* Set 1kHz sample rate */
2044  ICM_20948_sampleRateSet(1100.0);
2045 
2046  /* Configure bandwidth for gyroscope to 12Hz */
2047  ICM_20948_gyroBandwidthSet(ICM_20948_GYRO_BW_12HZ);
2048 
2049  /* Configure sensitivity to 250dps full scale */
2050  ICM_20948_gyroFullscaleSet(ICM_20948_GYRO_FULLSCALE_250DPS);
2051 
2052  /* Retrieve the resolution per bit */
2053  ICM_20948_gyroResolutionGet(&gyroRes);
2054 
2055  /* The accel sensor needs max 30ms, the gyro max 35ms to fully start */
2056  /* Experiments show that the gyro needs more time to get reliable results */
2057  delay(50);
2058 
2059  /* Disable the FIFO */
2060  ICM_20948_registerWrite(ICM_20948_REG_USER_CTRL, ICM_20948_BIT_FIFO_EN);
2061  ICM_20948_registerWrite(ICM_20948_REG_FIFO_MODE, 0x0F);
2062 
2063  /* Enable accelerometer and gyro to store the data in FIFO */
2064  ICM_20948_registerWrite(ICM_20948_REG_FIFO_EN_2, ICM_20948_BITS_GYRO_FIFO_EN);
2065 
2066  /* Reset the FIFO */
2067  ICM_20948_registerWrite(ICM_20948_REG_FIFO_RST, 0x0F);
2068  ICM_20948_registerWrite(ICM_20948_REG_FIFO_RST, 0x00);
2069 
2070  /* Enable the FIFO */
2071  ICM_20948_registerWrite(ICM_20948_REG_USER_CTRL, ICM_20948_BIT_FIFO_EN);
2072 
2073  /* The max FIFO size is 4096 bytes, one set of measurements takes 12 bytes */
2074  /* (3 axes, 2 sensors, 2 bytes each value ) 340 samples use 4080 bytes of FIFO */
2075  /* Loop until at least 4080 samples gathered */
2076  fifoCount = 0;
2077  while ( fifoCount < 4080 ) {
2078  delay(5);
2079 
2080  /* Read FIFO sample count */
2081  ICM_20948_registerRead(ICM_20948_REG_FIFO_COUNT_H, 2, &data[0]);
2082 
2083  /* Convert to a 16 bit value */
2084  fifoCount = ( (uint16_t) (data[0] << 8) | data[1]);
2085  }
2086 
2087  /* Disable accelerometer and gyro to store the data in FIFO */
2088  ICM_20948_registerWrite(ICM_20948_REG_FIFO_EN_2, 0x00);
2089 
2090  /* Read FIFO sample count */
2091  ICM_20948_registerRead(ICM_20948_REG_FIFO_COUNT_H, 2, &data[0]);
2092 
2093  /* Convert to a 16 bit value */
2094  fifoCount = ( (uint16_t) (data[0] << 8) | data[1]);
2095 
2096  /* Calculate the number of data sets (3 axis of accel an gyro, two bytes each = 12 bytes) */
2097  packetCount = fifoCount / 12;
2098 
2099  /* Retrieve the data from the FIFO */
2100  for ( i = 0; i < packetCount; i++ ) {
2101  ICM_20948_registerRead(ICM_20948_REG_FIFO_R_W, 12, &data[0]);
2102  /* Convert to 16 bit signed accel and gyro x,y and z values */
2103  gyroTemp[0] = ( (int16_t) (data[6] << 8) | data[7]);
2104  gyroTemp[1] = ( (int16_t) (data[8] << 8) | data[9]);
2105  gyroTemp[2] = ( (int16_t) (data[10] << 8) | data[11]);
2106 
2107  /* Sum the values */
2108  gyroBias[0] += gyroTemp[0];
2109  gyroBias[1] += gyroTemp[1];
2110  gyroBias[2] += gyroTemp[2];
2111  }
2112 
2113  /* Divide by packet count to get the average */
2114  gyroBias[0] /= packetCount;
2115  gyroBias[1] /= packetCount;
2116  gyroBias[2] /= packetCount;
2117 
2118  /* Convert the values to degrees per sec for displaying */
2119  gyroBiasScaled[0] = (float) gyroBias[0] * gyroRes;
2120  gyroBiasScaled[1] = (float) gyroBias[1] * gyroRes;
2121  gyroBiasScaled[2] = (float) gyroBias[2] * gyroRes;
2122 
2123  /* Read stored gyro trim values. After reset these values are all 0 */
2124  ICM_20948_registerRead(ICM_20948_REG_XG_OFFS_USRH, 2, &data[0]);
2125  gyroBiasStored[0] = ( (int16_t) (data[0] << 8) | data[1]);
2126 
2127  ICM_20948_registerRead(ICM_20948_REG_YG_OFFS_USRH, 2, &data[0]);
2128  gyroBiasStored[1] = ( (int16_t) (data[0] << 8) | data[1]);
2129 
2130  ICM_20948_registerRead(ICM_20948_REG_ZG_OFFS_USRH, 2, &data[0]);
2131  gyroBiasStored[2] = ( (int16_t) (data[0] << 8) | data[1]);
2132 
2133  /* The gyro bias should be stored in 1000dps full scaled format. We measured in 250dps to get */
2134  /* the best sensitivity, so need to divide by 4 */
2135  /* Substract from the stored calibration value */
2136  gyroBiasStored[0] -= gyroBias[0] / 4;
2137  gyroBiasStored[1] -= gyroBias[1] / 4;
2138  gyroBiasStored[2] -= gyroBias[2] / 4;
2139 
2140  /* Split the values into two bytes */
2141  data[0] = (gyroBiasStored[0] >> 8) & 0xFF;
2142  data[1] = (gyroBiasStored[0]) & 0xFF;
2143  data[2] = (gyroBiasStored[1] >> 8) & 0xFF;
2144  data[3] = (gyroBiasStored[1]) & 0xFF;
2145  data[4] = (gyroBiasStored[2] >> 8) & 0xFF;
2146  data[5] = (gyroBiasStored[2]) & 0xFF;
2147 
2148  /* Write the gyro bias values to the chip */
2149  ICM_20948_registerWrite(ICM_20948_REG_XG_OFFS_USRH, data[0]);
2150  ICM_20948_registerWrite(ICM_20948_REG_XG_OFFS_USRL, data[1]);
2151  ICM_20948_registerWrite(ICM_20948_REG_YG_OFFS_USRH, data[2]);
2152  ICM_20948_registerWrite(ICM_20948_REG_YG_OFFS_USRL, data[3]);
2153  ICM_20948_registerWrite(ICM_20948_REG_ZG_OFFS_USRH, data[4]);
2154  ICM_20948_registerWrite(ICM_20948_REG_ZG_OFFS_USRL, data[5]);
2155 
2156  /* Turn off FIFO */
2157  ICM_20948_registerWrite(ICM_20948_REG_USER_CTRL, 0x00);
2158 
2159  /* Disable all sensors */
2160  ICM_20948_sensorEnable(false, false, false);
2161 
2162  return ICM_20948_OK;
2163 }
2164 
2165 
2166 /*Code based on:
2167  * https://appelsiini.net/2018/calibrate-magnetometer/
2168  * */
2169 
2170 
2171 /***************************************************************************/
2186 uint32_t ICM_20948_min_max_mag(int16_t *minMag, int16_t *maxMag) {
2187  float m[3];
2188 
2189  minMag[0] = 32767; // alles kleiner dan dit wordt het minimum, daar staat er hier geen '-'!
2190  minMag[1] = 32767;
2191  minMag[2] = 32767;
2192 
2193  maxMag[0] = -32768; // hier hetzelfde, starten met de kleinste waarde, dan aanpassen dat het groter en groter wordt tot het maximum gevonden wordt!
2194  maxMag[1] = -32768;
2195  maxMag[2] = -32768;
2196 
2197  uint32_t max_time = 15000;
2198  uint32_t t_start = millis();
2199 
2200  /* Flags for if axis mag_minimumRange is fulfilled */
2201  bool miniumRange_mx = false;
2202  bool miniumRange_my = false;
2203  bool miniumRange_mz = false;
2204 
2205 
2206  while (!miniumRange_mx || !miniumRange_my || !miniumRange_mz) {
2207  while ((millis() - t_start) < max_time) {
2208 
2209  /* read magnetometer measurement */
2210  ICM_20948_magDataRead( m );
2211 
2212  if (m[0] < minMag[0]) {
2213  minMag[0] = m[0];
2214  }
2215  if (m[0] > maxMag[0]) {
2216  maxMag[0] = m[0];
2217  }
2218 
2219  if (m[1] < minMag[1]) {
2220  minMag[1] = m[1];
2221  }
2222  if (m[1] > maxMag[1]) {
2223  maxMag[1] = m[1];
2224  }
2225 
2226  if (m[2] < minMag[2]) {
2227  minMag[2] = m[2];
2228  }
2229  if (m[2] > maxMag[2]) {
2230  maxMag[2] = m[2];
2231  }
2232 
2233  if ((maxMag[0] - minMag[0]) > mag_minimumRange) {
2234  miniumRange_mx = true;
2235  }
2236  if ((maxMag[1] - minMag[1]) > mag_minimumRange) {
2237  miniumRange_my = true;
2238  }
2239  if ((maxMag[2] - minMag[2]) > mag_minimumRange) {
2240  miniumRange_mz = true;
2241  }
2242 
2243 // DEBUG_PRINT(min_mx); DEBUG_PRINT("\t"); DEBUG_PRINT(min_my); DEBUG_PRINT("\t"); DEBUG_PRINTLN(min_mz);
2244 // DEBUG_PRINT(max_mx); DEBUG_PRINT("\t"); DEBUG_PRINT(max_my); DEBUG_PRINT("\t"); DEBUG_PRINTLN(max_mz);
2245 // DEBUG_PRINTLN();
2246 #if DEBUG_DBPRINT == 1 /* DEBUG_DBPRINT */
2247  dbprint("minMag: ");
2248  dbprintInt((int) minMag[0]);
2249  dbprint(" minMag: ");
2250  dbprintInt((int) minMag[1]);
2251  dbprint(" minMag: ");
2252  dbprintInt((int) minMag[2]);
2253  dbprint(" maxMag: ");
2254  dbprintInt((int) maxMag[0]);
2255  dbprint(" maxMag: ");
2256  dbprintInt((int) maxMag[1]);
2257  dbprint(" maxMag: ");
2258  dbprintInt((int) maxMag[2]);
2259  dbprintln("");
2260 #endif /* DEBUG_DBPRINT */
2261  }
2262  }
2263 
2264  return OK;
2265 }
2266 
2267 
2268 
2274 /**************************************************************************/
2289 bool ICM_20948_calibrate_mag(float *offset, float *scale) {
2290  int16_t minMag[3];
2291  int16_t maxMag[3];
2292  int32_t sum_mx, sum_my, sum_mz;
2293  int32_t dif_mx, dif_my, dif_mz, dif_m;
2294 
2295 // /* Reset hard and soft iron correction before calibration. */
2296  _hxb = 0.0f;
2297  _hyb = 0.0f;
2298  _hzb = 0.0f;
2299  _hxs = 1.0f;
2300  _hys = 1.0f;
2301  _hzs = 1.0f;
2302 
2303 // DEBUG_PRINTLN(F("Calibrating magnetometer. Move the device in a figure eight ..."));
2304 
2305  ICM_20948_min_max_mag( minMag, maxMag );
2306 
2307  sum_mx = (int32_t) maxMag[0] + minMag[0];
2308  sum_my = (int32_t) maxMag[1] + minMag[1];
2309  sum_mz = (int32_t) maxMag[2] + minMag[2];
2310 
2311  dif_mx = (int32_t) maxMag[0] - minMag[0];
2312  dif_my = (int32_t) maxMag[1] - minMag[1];
2313  dif_mz = (int32_t) maxMag[2] - minMag[2];
2314 
2315  offset[0] = -0.5f * sum_mx;
2316  offset[1] = -0.5f * sum_my;
2317  offset[2] = -0.5f * sum_mz;
2318 
2319  dif_m = (dif_mx + dif_my + dif_mz) / 3;
2320 
2321  scale[0] = (float) dif_m / dif_mx;
2322  scale[1] = (float) dif_m / dif_my;
2323  scale[2] = (float) dif_m / dif_mz;
2324 
2325 // Store offset values in local variables, to be accessable by magread functions
2326  _hxb = offset[0];
2327  _hyb = offset[1];
2328  _hzb = offset[2];
2329 
2330  _hxs = scale[0];
2331  _hys = scale[1];
2332  _hzs = scale[2];
2333 
2334 
2335 // DEBUG_PRINTLN(F("Hard iron correction values (center values):"));
2336 // DEBUG_PRINT2(offset_mx, 1);
2337 // DEBUG_PRINT("\t");
2338 // DEBUG_PRINT2(offset_my, 1);
2339 // DEBUG_PRINT("\t");
2340 // DEBUG_PRINT2(offset_mz, 1);
2341 // DEBUG_PRINT("\t");
2342 // DEBUG_PRINTLN();
2343 //
2344 // DEBUG_PRINTLN(F("Soft iron correction values (scale values):"));
2345 // DEBUG_PRINT2(scale_mx, 4);
2346 // DEBUG_PRINT("\t");
2347 // DEBUG_PRINT2(scale_my, 4);
2348 // DEBUG_PRINT("\t");
2349 // DEBUG_PRINT2(scale_mz, 4);
2350 // DEBUG_PRINTLN();
2351 
2352  return true;
2353 }
2354 
2355 
2356 
tZ
int16_t tZ[3]
Definition: ICM20948.c:55
ICM_20948_REG_FIFO_MODE
#define ICM_20948_REG_FIFO_MODE
Definition: pinout.h:206
ICM_20948_CLK_PORT
#define ICM_20948_CLK_PORT
Definition: pinout.h:52
SPI
#define SPI
Definition: pinout.h:64
ICM_20948_REG_PWR_MGMT_2
#define ICM_20948_REG_PWR_MGMT_2
Definition: pinout.h:141
ICM_20948_REG_INT_ENABLE_1
#define ICM_20948_REG_INT_ENABLE_1
Definition: pinout.h:154
ICM_20948_BIT_SLEEP
#define ICM_20948_BIT_SLEEP
Definition: pinout.h:136
ICM_20948_ACCEL_FULLSCALE_8G
#define ICM_20948_ACCEL_FULLSCALE_8G
Definition: pinout.h:285
ICM_20948_gyroBandwidthSet
uint32_t ICM_20948_gyroBandwidthSet(uint8_t gyroBw)
Set gyroscope bandwidth.
Definition: ICM20948.c:1352
ICM_20948_magRawDataRead
void ICM_20948_magRawDataRead(float *raw_magn)
Read magnetometer raw values.
Definition: ICM20948.c:1707
ICM_20948_MOSI_PORT
#define ICM_20948_MOSI_PORT
Definition: pinout.h:56
ICM_20948_BIT_LP_EN
#define ICM_20948_BIT_LP_EN
Definition: pinout.h:137
ICM_20948_GYRO_FULLSCALE_1000DPS
#define ICM_20948_GYRO_FULLSCALE_1000DPS
Definition: pinout.h:244
ICM_20948_chipSelectSet
void ICM_20948_chipSelectSet(bool enable)
Set chipselect pin for SPI functionality.
Definition: ICM20948.c:480
ICM_20948_GYRO_FULLSCALE_2000DPS
#define ICM_20948_GYRO_FULLSCALE_2000DPS
Definition: pinout.h:245
ICM_20948_MASK_ACCEL_FULLSCALE
#define ICM_20948_MASK_ACCEL_FULLSCALE
Definition: pinout.h:281
magRawScaling
float magRawScaling
Definition: ICM20948.c:61
data
MeasurementData_t data
Definition: main.c:122
ICM_20948_accelResolutionGet
uint32_t ICM_20948_accelResolutionGet(float *accelRes)
Get accelerometer resultion.
Definition: ICM20948.c:1202
ICM_20948_REG_ACCEL_WOM_THR
#define ICM_20948_REG_ACCEL_WOM_THR
Definition: pinout.h:275
ICM20948_DEVICE_ID
#define ICM20948_DEVICE_ID
Definition: pinout.h:351
ICM_20948_REG_FIFO_COUNT_H
#define ICM_20948_REG_FIFO_COUNT_H
Definition: pinout.h:208
ICM_20948_latchEnable
uint32_t ICM_20948_latchEnable(bool enable)
Enables / disables the 50 micro seconds interrupt pin functionality.
Definition: ICM20948.c:1403
accRawScaling
float accRawScaling
Definition: ICM20948.c:59
ICM_20948_ACCEL_BW_246HZ
#define ICM_20948_ACCEL_BW_246HZ
Definition: pinout.h:289
ICM_20948_enable_SPI
void ICM_20948_enable_SPI(bool enable)
Enables the necessary SPI interface to communicate with ICM20948.
Definition: ICM20948.c:344
_hzcounts
int16_t _hzcounts
Definition: ICM20948.c:47
AK09916_REG_WHO_AM_I
#define AK09916_REG_WHO_AM_I
Definition: pinout.h:395
ICM_20948_accelDataRead
uint32_t ICM_20948_accelDataRead(float *accel)
Read RAW accelerometer data.
Definition: ICM20948.c:1170
AK09916_BIT_I2C_SLV_ADDR
#define AK09916_BIT_I2C_SLV_ADDR
Definition: pinout.h:396
IMU_MEASURING
bool IMU_MEASURING
Definition: main.c:126
ICM_20948_write_mag_register
void ICM_20948_write_mag_register(uint8_t addr, uint8_t data)
Writes a uint8_t to the I2C address of the magnetometer of the ICM_20948.
Definition: ICM20948.c:1648
ICM_20948_lowPowerModeEnter
uint32_t ICM_20948_lowPowerModeEnter(bool enAccel, bool enGyro, bool enTemp)
Enter low power mode for selected sensors.
Definition: ICM20948.c:1037
ICM_20948_POWER_PORT
#define ICM_20948_POWER_PORT
Definition: pinout.h:31
ICM_20948_POWER_PIN
#define ICM_20948_POWER_PIN
Definition: pinout.h:30
ICM_20948_accelFullscaleSet
uint32_t ICM_20948_accelFullscaleSet(uint8_t accelFs)
Set accelerometer full scale range.
Definition: ICM20948.c:1242
ICM_20948_set_mag_transfer
void ICM_20948_set_mag_transfer(bool read)
Set magnetometer data transfer on integrated I2C controller inside ICM20948.
Definition: ICM20948.c:1443
ICM_20948_MISO_PIN
#define ICM_20948_MISO_PIN
Definition: pinout.h:53
ICM_20948_magn_to_angle
void ICM_20948_magn_to_angle(float *magn, float *angle)
Test function for non-tilt compensated compass.
Definition: ICM20948.c:1793
_hycounts
int16_t _hycounts
Definition: ICM20948.c:47
ICM_20948_Initialized
bool ICM_20948_Initialized
Definition: ICM20948.c:40
_hz
float _hz
Definition: ICM20948.c:57
AK09916_MODE_ST
#define AK09916_MODE_ST
Definition: pinout.h:390
ICM_20948_BIT_RAW_DATA_0_RDY_EN
#define ICM_20948_BIT_RAW_DATA_0_RDY_EN
Definition: pinout.h:155
mag_minimumRange
int32_t mag_minimumRange
Definition: ICM20948.c:42
ICM_20948_REG_ZG_OFFS_USRH
#define ICM_20948_REG_ZG_OFFS_USRH
Definition: pinout.h:263
waitForSlave4
void waitForSlave4()
Wait for I2C slave (internal I2C controller on ICM20948)
Definition: ICM20948.c:1472
AK09916_MODE_50HZ
#define AK09916_MODE_50HZ
Definition: pinout.h:388
ICM_20948_gyroCalibrate
uint32_t ICM_20948_gyroCalibrate(float *gyroBiasScaled)
Gyroscope calibration function. Reads the gyroscope values while the device is at rest and in level....
Definition: ICM20948.c:2031
AK09916_MODE_20HZ
#define AK09916_MODE_20HZ
Definition: pinout.h:387
ICM_20948_wakeOnMotionITEnable
uint32_t ICM_20948_wakeOnMotionITEnable(bool enable, uint8_t womThreshold, float sampleRate)
Set wake on motion interrupt.
Definition: ICM20948.c:1295
ICM_20948_REG_FIFO_RST
#define ICM_20948_REG_FIFO_RST
Definition: pinout.h:205
dbprint
void dbprint(char *message)
ICM_20948_REG_XG_OFFS_USRL
#define ICM_20948_REG_XG_OFFS_USRL
Definition: pinout.h:260
delay
void delay(uint32_t msDelay)
Wait for a certain amount of milliseconds in EM2/3.
Definition: delay.c:116
ICM_20948_REG_I2C_SLV4_DI
#define ICM_20948_REG_I2C_SLV4_DI
Definition: pinout.h:337
ERROR
#define ERROR
Definition: pinout.h:401
ICM_20948_BIT_WOM_INT_EN
#define ICM_20948_BIT_WOM_INT_EN
Definition: pinout.h:152
dbprintInt
void dbprintInt(int32_t value)
ICM_20948_BIT_GYRO_CYCLE
#define ICM_20948_BIT_GYRO_CYCLE
Definition: pinout.h:132
ICM20948.h
Advanced funcions to control and read data from the ICM-20948.
ICM_20948_BIT_TEMP_DIS
#define ICM_20948_BIT_TEMP_DIS
Definition: pinout.h:138
ICM_20948_I2C_ADDRESS
#define ICM_20948_I2C_ADDRESS
Definition: pinout.h:26
ICM_20948_BIT_PWR_GYRO_STBY
#define ICM_20948_BIT_PWR_GYRO_STBY
Definition: pinout.h:143
ICM_20948_REG_XG_OFFS_USRH
#define ICM_20948_REG_XG_OFFS_USRH
Definition: pinout.h:259
ICM_20948_sensorEnable
uint32_t ICM_20948_sensorEnable(bool accel, bool gyro, bool temp)
Enable selected sensors.
Definition: ICM20948.c:1095
ICM_20948_accelSampleRateSet
float ICM_20948_accelSampleRateSet(float sampleRate)
Sets the accelerometer sample rate.
Definition: ICM20948.c:988
millis
uint32_t millis(void)
Millis Arduino like functionality for EFM32HG.
Definition: timer.c:33
tX
int16_t tX[3]
Definition: ICM20948.c:53
ICM_20948_REG_YG_OFFS_USRL
#define ICM_20948_REG_YG_OFFS_USRL
Definition: pinout.h:262
ICM_20948_REG_USER_CTRL
#define ICM_20948_REG_USER_CTRL
Definition: pinout.h:121
ICM_20948_REG_YA_OFFSET_H
#define ICM_20948_REG_YA_OFFSET_H
Definition: pinout.h:224
ICM_20948_BIT_ACCEL_CYCLE
#define ICM_20948_BIT_ACCEL_CYCLE
Definition: pinout.h:131
ICM_20948_GYRO_BW_12HZ
#define ICM_20948_GYRO_BW_12HZ
Definition: pinout.h:253
ICM_20948_ACCEL_BW_1210HZ
#define ICM_20948_ACCEL_BW_1210HZ
Definition: pinout.h:287
ICM_20948_ACCEL_FULLSCALE_4G
#define ICM_20948_ACCEL_FULLSCALE_4G
Definition: pinout.h:284
ICM_20948_REG_INT_ENABLE
#define ICM_20948_REG_INT_ENABLE
Definition: pinout.h:151
ICM_20948_REG_BANK_SEL
#define ICM_20948_REG_BANK_SEL
Definition: pinout.h:346
ICM_20948_min_max_mag
uint32_t ICM_20948_min_max_mag(int16_t *minMag, int16_t *maxMag)
Calculate minimum and maximum magnetometer values, which are used for calibration.
Definition: ICM20948.c:2186
AK09916_REG_CONTROL_3
#define AK09916_REG_CONTROL_3
Definition: pinout.h:392
tY
int16_t tY[3]
Definition: ICM20948.c:54
ICM_20948_MOSI_PIN
#define ICM_20948_MOSI_PIN
Definition: pinout.h:55
AK09916_MODE_100HZ
#define AK09916_MODE_100HZ
Definition: pinout.h:389
ICM_20948_sampleRateSet
uint32_t ICM_20948_sampleRateSet(float sampleRate)
Combination function to set sample rate of gyro and accel at once.
Definition: ICM20948.c:924
ICM_20948_GYRO_FULLSCALE_500DPS
#define ICM_20948_GYRO_FULLSCALE_500DPS
Definition: pinout.h:243
ICM_20948_BIT_ACCEL_FIFO_EN
#define ICM_20948_BIT_ACCEL_FIFO_EN
Definition: pinout.h:202
ICM_20948_REG_FIFO_R_W
#define ICM_20948_REG_FIFO_R_W
Definition: pinout.h:210
ICM_20948_REG_XA_OFFSET_L
#define ICM_20948_REG_XA_OFFSET_L
Definition: pinout.h:223
LED_PORT
#define LED_PORT
Definition: pinout.h:20
ICM_20948_Init
void ICM_20948_Init()
Init function for ICM20948.
Definition: ICM20948.c:85
_hx
float _hx
Definition: ICM20948.c:57
ICM_20948_BIT_H_RESET
#define ICM_20948_BIT_H_RESET
Definition: pinout.h:135
ICM_20948_REG_I2C_SLV4_DO
#define ICM_20948_REG_I2C_SLV4_DO
Definition: pinout.h:336
dbprintlnInt
void dbprintlnInt(int32_t value)
ICM_20948_gyroDataRead
uint32_t ICM_20948_gyroDataRead(float *gyro)
Read gyroscope data from IMU.
Definition: ICM20948.c:783
ICM_20948_REG_GYRO_SMPLRT_DIV
#define ICM_20948_REG_GYRO_SMPLRT_DIV
Definition: pinout.h:234
ICM_20948_REG_ACCEL_SMPLRT_DIV_2
#define ICM_20948_REG_ACCEL_SMPLRT_DIV_2
Definition: pinout.h:269
IIC_Init
void IIC_Init(void)
Setup I2C functionality.
Definition: I2C.c:52
ICM_20948_REG_I2C_SLV0_ADDR
#define ICM_20948_REG_I2C_SLV0_ADDR
Definition: pinout.h:313
timer.h
millis and micros functionality
ICM_20948_REG_ACCEL_CONFIG
#define ICM_20948_REG_ACCEL_CONFIG
Definition: pinout.h:277
IIC_WriteBuffer
bool IIC_WriteBuffer(uint8_t iicAddress, uint8_t *wBuffer, uint8_t wLength)
I2C write functionality.
Definition: I2C.c:112
AK09916_MODE_SINGLE
#define AK09916_MODE_SINGLE
Definition: pinout.h:385
ICM_20948_accelBandwidthSet
uint32_t ICM_20948_accelBandwidthSet(uint8_t accelBw)
Set acceleromter bandwidth.
Definition: ICM20948.c:1376
ICM_20948_REG_YG_OFFS_USRH
#define ICM_20948_REG_YG_OFFS_USRH
Definition: pinout.h:261
_hy
float _hy
Definition: ICM20948.c:57
ICM_20948_BIT_SLV4_NACK
#define ICM_20948_BIT_SLV4_NACK
Definition: pinout.h:166
ICM_20948_Init2
void ICM_20948_Init2()
Second init function for ICM20948, use when waking from sleep.
Definition: ICM20948.c:169
ICM_20948_MISO_PORT
#define ICM_20948_MISO_PORT
Definition: pinout.h:54
ICM_20948_BIT_I2C_SLV_EN
#define ICM_20948_BIT_I2C_SLV_EN
Definition: pinout.h:339
ICM_20948_interruptEnable
uint32_t ICM_20948_interruptEnable(bool dataReadyEnable, bool womEnable)
Enable interrupt for data ready or wake on motion.
Definition: ICM20948.c:885
ICM_20948_reset_mag
uint32_t ICM_20948_reset_mag(void)
Reset magnetometer.
Definition: ICM20948.c:1768
ICM_20948_cycleModeEnable
uint32_t ICM_20948_cycleModeEnable(bool enable)
Enable cycle mode on IMU.
Definition: ICM20948.c:1142
readMagRegister
unsigned char readMagRegister(unsigned char magreg)
Read magnetometer register from I2C controller in ICM20948.
Definition: ICM20948.c:1509
ICM_20948_read
uint8_t ICM_20948_read(uint16_t addr)
Read single register from IMU using SPI.
Definition: ICM20948.c:545
_magScale
float _magScale
Definition: ICM20948.c:63
ICM_20948_read_mag_register
void ICM_20948_read_mag_register(uint8_t addr, uint8_t numBytes, uint8_t *data)
Read register in the AK09916 magnetometer device.
Definition: ICM20948.c:1594
ICM_20948_MASK_GYRO_FULLSCALE
#define ICM_20948_MASK_GYRO_FULLSCALE
Definition: pinout.h:240
ICM_20948_BIT_FIFO_EN
#define ICM_20948_BIT_FIFO_EN
Definition: pinout.h:123
ICM_20948_gyroSampleRateSet
float ICM_20948_gyroSampleRateSet(float sampleRate)
Sets the gyro sample rate.
Definition: ICM20948.c:947
delay.h
Delay functionality.
ICM_20948_set_mag_mode
uint32_t ICM_20948_set_mag_mode(uint8_t magMode)
Set magnetometer mode (sample rate / on-off)
Definition: ICM20948.c:1670
AK09916_BIT_SRST
#define AK09916_BIT_SRST
Definition: pinout.h:393
ICM_20948_MASK_GYRO_BW
#define ICM_20948_MASK_GYRO_BW
Definition: pinout.h:241
ICM_20948_calibrate_mag
bool ICM_20948_calibrate_mag(float *offset, float *scale)
Magnetometer calibration function. Get magnetometer minimum and maximum values, while moving the devi...
Definition: ICM20948.c:2289
ICM_20948_ACCEL_FULLSCALE_2G
#define ICM_20948_ACCEL_FULLSCALE_2G
Definition: pinout.h:283
ICM_20948_BIT_I2C_READ
#define ICM_20948_BIT_I2C_READ
Definition: pinout.h:343
LED_PIN
#define LED_PIN
Definition: pinout.h:21
ICM_20948_REG_LP_CONFIG
#define ICM_20948_REG_LP_CONFIG
Definition: pinout.h:129
ICM_20948_CS_PORT
#define ICM_20948_CS_PORT
Definition: pinout.h:49
ICM_20948_REG_ZA_OFFSET_L
#define ICM_20948_REG_ZA_OFFSET_L
Definition: pinout.h:227
OK
#define OK
Definition: pinout.h:402
ICM_20948_REG_ZA_OFFSET_H
#define ICM_20948_REG_ZA_OFFSET_H
Definition: pinout.h:226
pinout.h
File to keep track of most the pins used, pins for BLE are in own file BLE.h.
ICM_20948_registerRead
void ICM_20948_registerRead(uint16_t addr, int numBytes, uint8_t *data)
Read registers from IMU.
Definition: ICM20948.c:654
AK09916_DEVICE_ID
#define AK09916_DEVICE_ID
Definition: pinout.h:368
ICM_20948_BANK_0
#define ICM_20948_BANK_0
Definition: pinout.h:70
AK09916_BIT_MODE_POWER_DOWN
#define AK09916_BIT_MODE_POWER_DOWN
Definition: pinout.h:384
ICM_20948_Init_SPI
void ICM_20948_Init_SPI()
Initiate SPI functionality for ICM20948.
Definition: ICM20948.c:273
ICM_20948_REG_YA_OFFSET_L
#define ICM_20948_REG_YA_OFFSET_L
Definition: pinout.h:225
ICM_20948_REG_I2C_SLV4_CTRL
#define ICM_20948_REG_I2C_SLV4_CTRL
Definition: pinout.h:335
ICM_20948_magDataRead
void ICM_20948_magDataRead(float *magn)
Convert raw magnetometer values to calibrated and scaled ones.
Definition: ICM20948.c:1745
ICM_20948_BIT_CLK_PLL
#define ICM_20948_BIT_CLK_PLL
Definition: pinout.h:139
ICM_20948_power
void ICM_20948_power(bool enable)
Turns the GPIO pin high to provide power to the ICM20948.
Definition: ICM20948.c:392
ICM_20948_OK
#define ICM_20948_OK
Definition: pinout.h:43
ICM_20948_accelGyroCalibrate
uint32_t ICM_20948_accelGyroCalibrate(float *accelBiasScaled, float *gyroBiasScaled)
Accelerometer and gyroscope calibration function. Reads the gyroscope and accelerometer values while ...
Definition: ICM20948.c:1820
ICM_20948_REG_ZG_OFFS_USRL
#define ICM_20948_REG_ZG_OFFS_USRL
Definition: pinout.h:264
ICM_20948_REG_INT_STATUS
#define ICM_20948_REG_INT_STATUS
Definition: pinout.h:172
ICM_20948_interruptStatusRead
uint32_t ICM_20948_interruptStatusRead(uint32_t *intStatus)
Read staus of interrupt IMU.
Definition: ICM20948.c:857
ICM_20948_BIT_ACCEL_INTEL_EN
#define ICM_20948_BIT_ACCEL_INTEL_EN
Definition: pinout.h:272
ICM_20948_REG_ACCEL_XOUT_H_SH
#define ICM_20948_REG_ACCEL_XOUT_H_SH
Definition: pinout.h:181
ICM_20948_sleepModeEnable
uint32_t ICM_20948_sleepModeEnable(bool enable)
Sleep mode enable function.
Definition: ICM20948.c:728
GYRO_XOUT_H
#define GYRO_XOUT_H
Definition: pinout.h:82
ICM_20948_reset
uint32_t ICM_20948_reset(void)
Reset IMU.
Definition: ICM20948.c:762
ICM_20948_bankSelect
void ICM_20948_bankSelect(uint8_t bank)
Select appropriate bank in ICM20948 based on first bits of address.
Definition: ICM20948.c:528
ICM_20948_BIT_ACCEL_INTEL_MODE
#define ICM_20948_BIT_ACCEL_INTEL_MODE
Definition: pinout.h:273
AK09916_MODE_10HZ
#define AK09916_MODE_10HZ
Definition: pinout.h:386
ICM_20948_CS_PIN
#define ICM_20948_CS_PIN
Definition: pinout.h:48
writeMagRegister
void writeMagRegister(unsigned char magreg, unsigned char val)
Write magnetometer register using internal I2C interface on ICM20948.
Definition: ICM20948.c:1538
ICM_20948_BIT_PWR_ACCEL_STBY
#define ICM_20948_BIT_PWR_ACCEL_STBY
Definition: pinout.h:142
ICM_20948_REG_INT_PIN_CFG
#define ICM_20948_REG_INT_PIN_CFG
Definition: pinout.h:146
ICM_20948_REG_ACCEL_SMPLRT_DIV_1
#define ICM_20948_REG_ACCEL_SMPLRT_DIV_1
Definition: pinout.h:268
ICM_20948_REG_WHO_AM_I
#define ICM_20948_REG_WHO_AM_I
Definition: pinout.h:119
M_PI
#define M_PI
Definition: ICM20948.c:41
mag_res
float mag_res
Definition: ICM20948.c:46
ICM_20948_BIT_I2C_SLV_READ
#define ICM_20948_BIT_I2C_SLV_READ
Definition: pinout.h:360
ICM_20948_ACCEL_FULLSCALE_16G
#define ICM_20948_ACCEL_FULLSCALE_16G
Definition: pinout.h:286
ICM_20948_REG_XA_OFFSET_H
#define ICM_20948_REG_XA_OFFSET_H
Definition: pinout.h:222
AK09916_REG_CONTROL_2
#define AK09916_REG_CONTROL_2
Definition: pinout.h:383
ICM_20948_gyroResolutionGet
uint32_t ICM_20948_gyroResolutionGet(float *gyroRes)
Set gyro resolution.
Definition: ICM20948.c:817
IIC_WriteReadBuffer
bool IIC_WriteReadBuffer(uint8_t iicAddress, uint8_t *wBuffer, uint8_t wLength, uint8_t *rBuffer, uint8_t rLength)
I2C read/write functionality, to read and write at the same time.
Definition: I2C.c:190
ICM_20948_REG_GYRO_CONFIG_1
#define ICM_20948_REG_GYRO_CONFIG_1
Definition: pinout.h:236
ICM_20948_gyroFullscaleSet
uint32_t ICM_20948_gyroFullscaleSet(uint8_t gyroFs)
Set gyroscope full scale range.
Definition: ICM20948.c:1265
ICM_20948_REG_PWR_MGMT_1
#define ICM_20948_REG_PWR_MGMT_1
Definition: pinout.h:134
ICM_20948_MASK_ACCEL_BW
#define ICM_20948_MASK_ACCEL_BW
Definition: pinout.h:282
ICM_20948_check_WhoAmI
void ICM_20948_check_WhoAmI()
Check WhoAmI register of ICM20948.
Definition: ICM20948.c:459
ICM_20948_BITS_GYRO_FIFO_EN
#define ICM_20948_BITS_GYRO_FIFO_EN
Definition: pinout.h:203
ICM_20948_printAllData
void ICM_20948_printAllData()
Print all gyro + accel + magn data using dbprint.
Definition: ICM20948.c:414
dbprintln
void dbprintln(char *message)
gyroRawScaling
float gyroRawScaling
Definition: ICM20948.c:60
ICM_20948_REG_I2C_SLV4_ADDR
#define ICM_20948_REG_I2C_SLV4_ADDR
Definition: pinout.h:333
ICM_20948_registerWrite
void ICM_20948_registerWrite(uint16_t addr, uint8_t data)
Writes registers from IMU.
Definition: ICM20948.c:697
_hxcounts
int16_t _hxcounts
Definition: ICM20948.c:47
ICM_20948_REG_FIFO_EN_2
#define ICM_20948_REG_FIFO_EN_2
Definition: pinout.h:201
debug_dbprint.h
Enable or disable printing to UART with dbprint.
ICM_20948_REG_I2C_SLV4_REG
#define ICM_20948_REG_I2C_SLV4_REG
Definition: pinout.h:334
ICM_20948_CLK_PIN
#define ICM_20948_CLK_PIN
Definition: pinout.h:51
ICM_20948_REG_ACCEL_INTEL_CTRL
#define ICM_20948_REG_ACCEL_INTEL_CTRL
Definition: pinout.h:271
ICM_20948_BIT_SLV4_DONE
#define ICM_20948_BIT_SLV4_DONE
Definition: pinout.h:164
ICM_20948_GYRO_FULLSCALE_250DPS
#define ICM_20948_GYRO_FULLSCALE_250DPS
Definition: pinout.h:242
I2C.h
I2C function to read-write the I2C bus, based on DRAMCO code.
ICM_20948_REG_I2C_MST_STATUS
#define ICM_20948_REG_I2C_MST_STATUS
Definition: pinout.h:162