EFM32 Gecko Software Documentation  efm32g-doc-5.1.2
icm20648.c
Go to the documentation of this file.
1 /***************************************************************************/
16 #include <stdint.h>
17 #include <stdio.h>
18 #include "em_usart.h"
19 #include "em_gpio.h"
20 #include "em_cmu.h"
21 
22 #include "thunderboard/util.h"
23 #include "thunderboard/board.h"
24 #include "thunderboard/icm20648.h"
25 
26  /**************************************************************************/
32  /**************************************************************************/
40 static void ICM20648_chipSelectSet( bool select );
41 
44 /***************************************************************************/
53 uint32_t ICM20648_init( void )
54 {
55 
56  uint8_t data;
57 
58  /* Disable the chip to avoid misconfiguration due transients during the SPI configuration */
59  BOARD_imuEnable( false );
60 
61  /* Enable and setup the SPI bus */
63 
64  /* Enable the chip */
65  BOARD_imuEnable( true );
66 
67  /* Issue reset */
69 
70  /* Disable I2C interface, use SPI */
72 
73  /* Read Who am I register, should get 0x71 */
75 
76  /*printf( "Who am I: 0x%x\r\n", data );*/
77  /* If not - return */
78  if( data != ICM20648_DEVICE_ID ) {
80  }
81 
82  /* Auto selects the best available clock source – PLL if ready, else use the Internal oscillator */
84 
85  /* PLL startup time - maybe it is too long but better be on the safe side, no spec in the datasheet */
86  UTIL_delay( 30 );
87 
88  /* INT pin: active low, open drain, IT status read clears. It seems that latched mode does not work, the INT pin cannot be cleared if set */
90 
91  return ICM20648_OK;
92 
93 }
94 
95 /***************************************************************************/
103 uint32_t ICM20648_deInit( void )
104 {
105 
106  /* Disable the chip */
107  BOARD_imuEnable( false );
108 
109  return ICM20648_OK;
110 
111 }
112 
113 /***************************************************************************/
120 uint32_t ICM20648_reset( void )
121 {
122 
123  /* Set H_RESET bit to initiate soft reset */
125 
126  /* Wait 100ms to complete the reset sequence */
127  UTIL_delay( 100 );
128 
129  return ICM20648_OK;
130 
131 }
132 
133 /***************************************************************************/
145 uint32_t ICM20648_accelDataRead( float *accel )
146 {
147 
148  uint8_t rawData[6];
149  float accelRes;
150  int16_t temp;
151 
152  /* Retrieve the current resolution */
153  ICM20648_accelResolutionGet( &accelRes );
154 
155  /* Read the six raw data registers into data array */
157 
158  /* Convert the MSB and LSB into a signed 16-bit value and multiply by the resolution to get the G value */
159  temp = ( (int16_t) rawData[0] << 8 ) | rawData[1];
160  accel[0] = (float) temp * accelRes;
161  temp = ( (int16_t) rawData[2] << 8 ) | rawData[3];
162  accel[1] = (float) temp * accelRes;
163  temp = ( (int16_t) rawData[4] << 8 ) | rawData[5];
164  accel[2] = (float) temp * accelRes;
165 
166  return ICM20648_OK;
167 
168 }
169 
170 /***************************************************************************/
182 uint32_t ICM20648_gyroDataRead( float *gyro )
183 {
184 
185  uint8_t rawData[6];
186  float gyroRes;
187  int16_t temp;
188 
189  /* Retrieve the current resolution */
190  ICM20648_gyroResolutionGet( &gyroRes );
191 
192  /* Read the six raw data registers into data array */
194 
195  /* Convert the MSB and LSB into a signed 16-bit value and multiply by the resolution to get the dps value */
196  temp = ( (int16_t) rawData[0] << 8 ) | rawData[1];
197  gyro[0] = (float) temp * gyroRes;
198  temp = ( (int16_t) rawData[2] << 8 ) | rawData[3];
199  gyro[1] = (float) temp * gyroRes;
200  temp = ( (int16_t) rawData[4] << 8 ) | rawData[5];
201  gyro[2] = (float) temp * gyroRes;
202 
203  return ICM20648_OK;
204 
205 }
206 
207 /***************************************************************************/
217 uint32_t ICM20648_accelResolutionGet( float *accelRes )
218 {
219 
220  uint8_t reg;
221 
222  /* Read the actual acceleration full scale setting */
225 
226  /* Calculate the resolution */
227  switch( reg ) {
228 
230  *accelRes = 2.0 / 32768.0;
231  break;
232 
234  *accelRes = 4.0 / 32768.0;
235  break;
236 
238  *accelRes = 8.0 / 32768.0;
239  break;
240 
242  *accelRes = 16.0 / 32768.0;
243  break;
244  }
245 
246  return ICM20648_OK;
247 
248 }
249 
250 /***************************************************************************/
260 uint32_t ICM20648_gyroResolutionGet( float *gyroRes )
261 {
262 
263  uint8_t reg;
264 
265  /* Read the actual gyroscope full scale setting */
268 
269  /* Calculate the resolution */
270  switch( reg ) {
271 
273  *gyroRes = 250.0 / 32768.0;
274  break;
275 
277  *gyroRes = 500.0 / 32768.0;
278  break;
279 
281  *gyroRes = 1000.0 / 32768.0;
282  break;
283 
285  *gyroRes = 2000.0 / 32768.0;
286  break;
287  }
288 
289  return ICM20648_OK;
290 
291 }
292 
293 /***************************************************************************/
305 uint32_t ICM20648_accelFullscaleSet( uint8_t accelFs )
306 {
307 
308  uint8_t reg;
309 
312  reg &= ~( ICM20648_MASK_ACCEL_FULLSCALE );
313  reg |= accelFs;
315 
316  return ICM20648_OK;
317 
318 }
319 
320 /***************************************************************************/
332 uint32_t ICM20648_gyroFullscaleSet( uint8_t gyroFs )
333 {
334 
335  uint8_t reg;
336 
339  reg &= ~( ICM20648_MASK_GYRO_FULLSCALE );
340  reg |= gyroFs;
342 
343  return ICM20648_OK;
344 
345 }
346 
347 /***************************************************************************/
359 uint32_t ICM20648_sampleRateSet( float sampleRate )
360 {
361 
362  ICM20648_gyroSampleRateSet( sampleRate );
363  ICM20648_accelSampleRateSet( sampleRate );
364 
365  return ICM20648_OK;
366 
367 }
368 
369 /***************************************************************************/
380 float ICM20648_gyroSampleRateSet( float sampleRate )
381 {
382 
383  uint8_t gyroDiv;
384  float gyroSampleRate;
385 
386  /* Calculate the sample rate divider */
387  gyroSampleRate = ( 1125.0 / sampleRate ) - 1.0;
388 
389  /* Check if it fits in the divider register */
390  if( gyroSampleRate > 255.0 ) {
391  gyroSampleRate = 255.0;
392  }
393 
394  if( gyroSampleRate < 0 ) {
395  gyroSampleRate = 0.0;
396  }
397 
398  /* Write the value to the register */
399  gyroDiv = (uint8_t) gyroSampleRate;
401 
402  /* Calculate the actual sample rate from the divider value */
403  gyroSampleRate = 1125.0 / ( gyroDiv + 1 );
404 
405  return gyroSampleRate;
406 
407 }
408 
409 /***************************************************************************/
420 float ICM20648_accelSampleRateSet( float sampleRate )
421 {
422 
423  uint16_t accelDiv;
424  float accelSampleRate;
425 
426  /* Calculate the sample rate divider */
427  accelSampleRate = ( 1125.0 / sampleRate ) - 1.0;
428 
429  /* Check if it fits in the divider registers */
430  if( accelSampleRate > 4095.0 ) {
431  accelSampleRate = 4095.0;
432  }
433 
434  if( accelSampleRate < 0 ) {
435  accelSampleRate = 0.0;
436  }
437 
438  /* Write the value to the registers */
439  accelDiv = (uint16_t) accelSampleRate;
440  ICM20648_registerWrite( ICM20648_REG_ACCEL_SMPLRT_DIV_1, (uint8_t) ( accelDiv >> 8 ) );
441  ICM20648_registerWrite( ICM20648_REG_ACCEL_SMPLRT_DIV_2, (uint8_t) ( accelDiv & 0xFF ) );
442 
443  /* Calculate the actual sample rate from the divider value */
444  accelSampleRate = 1125.0 / ( accelDiv + 1 );
445 
446  return accelSampleRate;
447 
448 }
449 
450 /***************************************************************************/
462 uint32_t ICM20648_gyroBandwidthSet( uint8_t gyroBw )
463 {
464 
465  uint8_t reg;
466 
467  /* Read the GYRO_CONFIG_1 register */
469  reg &= ~( ICM20648_MASK_GYRO_BW );
470 
471  /* Write the new bandwidth value to the gyro config register */
472  reg |= ( gyroBw & ICM20648_MASK_GYRO_BW );
474 
475  return ICM20648_OK;
476 
477 }
478 
479 /***************************************************************************/
491 uint32_t ICM20648_accelBandwidthSet( uint8_t accelBw )
492 {
493 
494  uint8_t reg;
495 
496  /* Read the GYRO_CONFIG_1 register */
498  reg &= ~( ICM20648_MASK_ACCEL_BW );
499 
500  /* Write the new bandwidth value to the gyro config register */
501  reg |= ( accelBw & ICM20648_MASK_ACCEL_BW );
503 
504  return ICM20648_OK;
505 
506 }
507 
508 /***************************************************************************/
518 uint32_t ICM20648_sleepModeEnable( bool enable )
519 {
520 
521  uint8_t reg;
522 
524 
525  if( enable ) {
526  /* Sleep: set the SLEEP bit */
527  reg |= ICM20648_BIT_SLEEP;
528  }
529  else {
530  /* Wake up: clear the SLEEP bit */
531  reg &= ~( ICM20648_BIT_SLEEP );
532  }
533 
535 
536  return ICM20648_OK;
537 
538 }
539 
540 /***************************************************************************/
551 uint32_t ICM20648_cycleModeEnable( bool enable )
552 {
553 
554  uint8_t reg;
555 
556  reg = 0x00;
557 
558  if( enable ) {
560  }
561 
563 
564  return ICM20648_OK;
565 
566 }
567 
568 /***************************************************************************/
584 uint32_t ICM20648_sensorEnable( bool accel, bool gyro, bool temp )
585 {
586 
587  uint8_t pwrManagement1;
588  uint8_t pwrManagement2;
589 
590  ICM20648_registerRead( ICM20648_REG_PWR_MGMT_1, 1, &pwrManagement1 );
591  pwrManagement2 = 0;
592 
593  /* To enable the accelerometer clear the DISABLE_ACCEL bits in PWR_MGMT_2 */
594  if( accel ) {
595  pwrManagement2 &= ~( ICM20648_BIT_PWR_ACCEL_STBY );
596  }
597  else {
598  pwrManagement2 |= ICM20648_BIT_PWR_ACCEL_STBY;
599  }
600 
601  /* To enable gyro clear the DISABLE_GYRO bits in PWR_MGMT_2 */
602  if( gyro ) {
603  pwrManagement2 &= ~( ICM20648_BIT_PWR_GYRO_STBY );
604  }
605  else {
606  pwrManagement2 |= ICM20648_BIT_PWR_GYRO_STBY;
607  }
608 
609  /* To enable the temperature sensor clear the TEMP_DIS bit in PWR_MGMT_1 */
610  if( temp ) {
611  pwrManagement1 &= ~( ICM20648_BIT_TEMP_DIS );
612  }
613  else {
614  pwrManagement1 |= ICM20648_BIT_TEMP_DIS;
615  }
616 
617  /* Write back the modified values */
620 
621  return ICM20648_OK;
622 
623 }
624 
625 /***************************************************************************/
641 uint32_t ICM20648_lowPowerModeEnter( bool enAccel, bool enGyro, bool enTemp )
642 {
643 
644  uint8_t data;
645 
647 
648  if( enAccel || enGyro || enTemp ) {
649 
650  /* Make sure that the chip is not in sleep */
651  ICM20648_sleepModeEnable( false );
652 
653  /* And in continuous mode */
654  ICM20648_cycleModeEnable( false );
655 
656  /* Enable the accelerometer and the gyroscope*/
657  ICM20648_sensorEnable( enAccel, enGyro, enTemp );
658  UTIL_delay( 50 );
659 
660  /* Enable cycle mode */
661  ICM20648_cycleModeEnable( true );
662 
663  /* Set the LP_EN bit to enable low power mode */
664  data |= ICM20648_BIT_LP_EN;
665 
666  }
667  else {
668 
669  /* Enable continuous mode */
670  ICM20648_cycleModeEnable( false );
671 
672  /* Clear the LP_EN bit to disable low power mode */
673  data &= ~ICM20648_BIT_LP_EN;
674 
675  }
676 
677  /* Write the updated value to the PWR_MGNT_1 register */
679 
680  return ICM20648_OK;
681 
682 }
683 
684 /***************************************************************************/
697 uint32_t ICM20648_interruptEnable( bool dataReadyEnable, bool womEnable )
698 {
699 
700  uint8_t intEnable;
701 
702  /* All interrupts disabled by default */
703  intEnable = 0;
704 
705  /* Enable one or both of the interrupt sources if required */
706  if( womEnable ) {
707  intEnable = ICM20648_BIT_WOM_INT_EN;
708  }
709  /* Write value to register */
711 
712  /* All interrupts disabled by default */
713  intEnable = 0;
714 
715  if( dataReadyEnable ) {
716  intEnable = ICM20648_BIT_RAW_DATA_0_RDY_EN;
717  }
718 
719  /* Write value to register */
721 
722  return ICM20648_OK;
723 
724 }
725 
726 /***************************************************************************/
737 uint32_t ICM20648_interruptStatusRead( uint32_t *intStatus )
738 {
739 
740  uint8_t reg[4];
741 
743  *intStatus = (uint32_t) reg[0];
744  *intStatus |= ( ( (uint32_t) reg[1] ) << 8 );
745  *intStatus |= ( ( (uint32_t) reg[2] ) << 16 );
746  *intStatus |= ( ( (uint32_t) reg[3] ) << 24 );
747 
748  return ICM20648_OK;
749 
750 }
751 
752 /***************************************************************************/
760 {
761 
762  uint8_t status;
763  bool ret;
764 
765  ret = false;
767 
768  if( status & ICM20648_BIT_RAW_DATA_0_RDY_INT ) {
769  ret = true;
770  }
771 
772  return ret;
773 
774 }
775 
776 /***************************************************************************/
793 uint32_t ICM20648_wakeOnMotionITEnable( bool enable, uint8_t womThreshold, float sampleRate )
794 {
795 
796  if( enable ) {
797 
798  /* Make sure that the chip is not in sleep */
799  ICM20648_sleepModeEnable( false );
800 
801  /* And in continuous mode */
802  ICM20648_cycleModeEnable( false );
803 
804  /* Enable only the accelerometer */
805  ICM20648_sensorEnable( true, false, false );
806 
807  /* Set sample rate */
808  ICM20648_sampleRateSet( sampleRate );
809 
810  /* Set the bandwidth to 1210Hz */
812 
813  /* Accel: 2G full scale */
815 
816  /* Enable the Wake On Motion interrupt */
817  ICM20648_interruptEnable( false, true );
818  UTIL_delay( 50 );
819 
820  /* Enable Wake On Motion feature */
822 
823  /* Set the wake on motion threshold value */
825 
826  /* Enable low power mode */
827  ICM20648_lowPowerModeEnter( true, false, false );
828 
829  }
830  else {
831 
832  /* Disable Wake On Motion feature */
834 
835  /* Disable the Wake On Motion interrupt */
836  ICM20648_interruptEnable( false, false );
837 
838  /* Disable cycle mode */
839  ICM20648_cycleModeEnable( false );
840 
841  }
842 
843  return ICM20648_OK;
844 
845 }
846 
847 /***************************************************************************/
863 uint32_t ICM20648_accelGyroCalibrate( float *accelBiasScaled, float *gyroBiasScaled )
864 {
865 
866  uint8_t data[12];
867  uint16_t i, packetCount, fifoCount;
868  int32_t gyroBias[3] = { 0, 0, 0 };
869  int32_t accelBias[3] = { 0, 0, 0 };
870  int32_t accelTemp[3];
871  int32_t gyroTemp[3];
872  int32_t accelBiasFactory[3];
873  int32_t gyroBiasStored[3];
874  float gyroRes, accelRes;
875 
876  /* Enable the accelerometer and the gyro */
877  ICM20648_sensorEnable( true, true, false );
878 
879  /* Set 1kHz sample rate */
880  ICM20648_sampleRateSet( 1100.0 );
881 
882  /* 246Hz BW for the accelerometer and 200Hz for the gyroscope */
885 
886  /* Set the most sensitive range: 2G full scale and 250dps full scale */
889 
890  /* Retrieve the resolution per bit */
891  ICM20648_accelResolutionGet( &accelRes );
892  ICM20648_gyroResolutionGet( &gyroRes );
893 
894  /* The accel sensor needs max 30ms, the gyro max 35ms to fully start */
895  /* Experiments show that the gyro needs more time to get reliable results */
896  UTIL_delay( 50 );
897 
898  /* Disable the FIFO */
901 
902  /* Enable accelerometer and gyro to store the data in FIFO */
904 
905  /* Reset the FIFO */
908 
909  /* Enable the FIFO */
911 
912  /* The max FIFO size is 4096 bytes, one set of measurements takes 12 bytes */
913  /* (3 axes, 2 sensors, 2 bytes each value ) 340 samples use 4080 bytes of FIFO */
914  /* Loop until at least 4080 samples gathered */
915  fifoCount = 0;
916  while( fifoCount < 4080 ) {
917  UTIL_delay( 5 );
918  /* Read FIFO sample count */
920  /* Convert to a 16 bit value */
921  fifoCount = ( (uint16_t) ( data[0] << 8 ) | data[1] );
922  }
923 
924  /* Disable accelerometer and gyro to store the data in FIFO */
926 
927  /* Read FIFO sample count */
929 
930  /* Convert to a 16 bit value */
931  fifoCount = ( (uint16_t) ( data[0] << 8 ) | data[1] );
932 
933  /* Calculate the number of data sets (3 axis of accel an gyro, two bytes each = 12 bytes) */
934  packetCount = fifoCount / 12;
935 
936  /* Retrieve the data from the FIFO */
937  for( i = 0; i < packetCount; i++ ) {
938 
940  /* Convert to 16 bit signed accel and gyro x,y and z values */
941  accelTemp[0] = ( (int16_t) ( data[0] << 8 ) | data[1] );
942  accelTemp[1] = ( (int16_t) ( data[2] << 8 ) | data[3] );
943  accelTemp[2] = ( (int16_t) ( data[4] << 8 ) | data[5] );
944  gyroTemp[0] = ( (int16_t) ( data[6] << 8 ) | data[7] );
945  gyroTemp[1] = ( (int16_t) ( data[8] << 8 ) | data[9] );
946  gyroTemp[2] = ( (int16_t) ( data[10] << 8 ) | data[11] );
947 
948  /* Sum the values */
949  accelBias[0] += accelTemp[0];
950  accelBias[1] += accelTemp[1];
951  accelBias[2] += accelTemp[2];
952  gyroBias[0] += gyroTemp[0];
953  gyroBias[1] += gyroTemp[1];
954  gyroBias[2] += gyroTemp[2];
955 
956  }
957 
958  /* Divide by packet count to get the average */
959  accelBias[0] /= packetCount;
960  accelBias[1] /= packetCount;
961  accelBias[2] /= packetCount;
962  gyroBias[0] /= packetCount;
963  gyroBias[1] /= packetCount;
964  gyroBias[2] /= packetCount;
965 
966  /* Acceleormeter: add or remove (depending on the orientation of the chip) 1G (gravity) from the Z axis value */
967  if( accelBias[2] > 0L ) {
968  accelBias[2] -= (int32_t) ( 1.0 / accelRes );
969  }
970  else {
971  accelBias[2] += (int32_t) ( 1.0 / accelRes );
972  }
973 
974  /* Convert the values to degrees per sec for displaying */
975  gyroBiasScaled[0] = (float) gyroBias[0] * gyroRes;
976  gyroBiasScaled[1] = (float) gyroBias[1] * gyroRes;
977  gyroBiasScaled[2] = (float) gyroBias[2] * gyroRes;
978 
979  /* Read stored gyro trim values. After reset these values are all 0 */
981  gyroBiasStored[0] = ( (int16_t) ( data[0] << 8 ) | data[1] );
983  gyroBiasStored[1] = ( (int16_t) ( data[0] << 8 ) | data[1] );
985  gyroBiasStored[2] = ( (int16_t) ( data[0] << 8 ) | data[1] );
986 
987  /* The gyro bias should be stored in 1000dps full scaled format. We measured in 250dps to get */
988  /* the best sensitivity, so need to divide by 4 */
989  /* Substract from the stored calibration value */
990  gyroBiasStored[0] -= gyroBias[0] / 4;
991  gyroBiasStored[1] -= gyroBias[1] / 4;
992  gyroBiasStored[2] -= gyroBias[2] / 4;
993 
994  /* Split the values into two bytes */
995  data[0] = ( gyroBiasStored[0] >> 8 ) & 0xFF;
996  data[1] = ( gyroBiasStored[0] ) & 0xFF;
997  data[2] = ( gyroBiasStored[1] >> 8 ) & 0xFF;
998  data[3] = ( gyroBiasStored[1] ) & 0xFF;
999  data[4] = ( gyroBiasStored[2] >> 8 ) & 0xFF;
1000  data[5] = ( gyroBiasStored[2] ) & 0xFF;
1001 
1002  /* Write the gyro bias values to the chip */
1009 
1010  /* Calculate the accelerometer bias values to store in the hardware accelerometer bias registers. These registers contain */
1011  /* factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold */
1012  /* non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature */
1013  /* compensation calculations(? the datasheet is not clear). Accelerometer bias registers expect bias input */
1014  /* as 2048 LSB per g, so that the accelerometer biases calculated above must be divided by 8. */
1015 
1016  /* Read factory accelerometer trim values */
1018  accelBiasFactory[0] = ( (int16_t) ( data[0] << 8 ) | data[1] );
1020  accelBiasFactory[1] = ( (int16_t) ( data[0] << 8 ) | data[1] );
1022  accelBiasFactory[2] = ( (int16_t) ( data[0] << 8 ) | data[1] );
1023 
1024  /* Construct total accelerometer bias, including calculated average accelerometer bias from above */
1025  /* Scale the 2g full scale (most sensitive range) results to 16g full scale - divide by 8 */
1026  /* Clear the last bit (temperature compensation? - the datasheet is not clear) */
1027  /* Substract from the factory calibration value */
1028 
1029  accelBiasFactory[0] -= ( ( accelBias[0] / 8 ) & ~1 );
1030  accelBiasFactory[1] -= ( ( accelBias[1] / 8 ) & ~1 );
1031  accelBiasFactory[2] -= ( ( accelBias[2] / 8 ) & ~1 );
1032 
1033  /* Split the values into two bytes */
1034  data[0] = ( accelBiasFactory[0] >> 8 ) & 0xFF;
1035  data[1] = ( accelBiasFactory[0] ) & 0xFF;
1036  data[2] = ( accelBiasFactory[1] >> 8 ) & 0xFF;
1037  data[3] = ( accelBiasFactory[1] ) & 0xFF;
1038  data[4] = ( accelBiasFactory[2] >> 8 ) & 0xFF;
1039  data[5] = ( accelBiasFactory[2] ) & 0xFF;
1040 
1041  /* Store them in the accelerometer offset registers */
1048 
1049  /* Convert the values to G for displaying */
1050  accelBiasScaled[0] = (float) accelBias[0] * accelRes;
1051  accelBiasScaled[1] = (float) accelBias[1] * accelRes;
1052  accelBiasScaled[2] = (float) accelBias[2] * accelRes;
1053 
1054  /* Turn off FIFO */
1056 
1057  /* Disable all sensors */
1058  ICM20648_sensorEnable( false, false, false );
1059 
1060  return ICM20648_OK;
1061 
1062 }
1063 
1064 /***************************************************************************/
1077 uint32_t ICM20648_gyroCalibrate( float *gyroBiasScaled )
1078 {
1079 
1080  uint8_t data[12];
1081  uint16_t i, packetCount, fifoCount;
1082  int32_t gyroBias[3] = { 0, 0, 0 };
1083  int32_t gyroTemp[3];
1084  int32_t gyroBiasStored[3];
1085  float gyroRes;
1086 
1087  /* Enable the accelerometer and the gyro */
1088  ICM20648_sensorEnable( true, true, false );
1089 
1090  /* Set 1kHz sample rate */
1091  ICM20648_sampleRateSet( 1100.0 );
1092 
1093  /* Configure bandwidth for gyroscope to 12Hz */
1095 
1096  /* Configure sensitivity to 250dps full scale */
1098 
1099  /* Retrieve the resolution per bit */
1100  ICM20648_gyroResolutionGet( &gyroRes );
1101 
1102  /* The accel sensor needs max 30ms, the gyro max 35ms to fully start */
1103  /* Experiments show that the gyro needs more time to get reliable results */
1104  UTIL_delay( 50 );
1105 
1106  /* Disable the FIFO */
1109 
1110  /* Enable accelerometer and gyro to store the data in FIFO */
1112 
1113  /* Reset the FIFO */
1116 
1117  /* Enable the FIFO */
1119 
1120  /* The max FIFO size is 4096 bytes, one set of measurements takes 12 bytes */
1121  /* (3 axes, 2 sensors, 2 bytes each value ) 340 samples use 4080 bytes of FIFO */
1122  /* Loop until at least 4080 samples gathered */
1123  fifoCount = 0;
1124  while( fifoCount < 4080 ) {
1125 
1126  UTIL_delay( 5 );
1127 
1128  /* Read FIFO sample count */
1130 
1131  /* Convert to a 16 bit value */
1132  fifoCount = ( (uint16_t) ( data[0] << 8 ) | data[1] );
1133 
1134  }
1135 
1136  /* Disable accelerometer and gyro to store the data in FIFO */
1138 
1139  /* Read FIFO sample count */
1141 
1142  /* Convert to a 16 bit value */
1143  fifoCount = ( (uint16_t) ( data[0] << 8 ) | data[1] );
1144 
1145  /* Calculate the number of data sets (3 axis of accel an gyro, two bytes each = 12 bytes) */
1146  packetCount = fifoCount / 12;
1147 
1148  /* Retrieve the data from the FIFO */
1149  for( i = 0; i < packetCount; i++ ) {
1150 
1152  /* Convert to 16 bit signed accel and gyro x,y and z values */
1153  gyroTemp[0] = ( (int16_t) ( data[6] << 8 ) | data[7] );
1154  gyroTemp[1] = ( (int16_t) ( data[8] << 8 ) | data[9] );
1155  gyroTemp[2] = ( (int16_t) ( data[10] << 8 ) | data[11] );
1156 
1157  /* Sum the values */
1158  gyroBias[0] += gyroTemp[0];
1159  gyroBias[1] += gyroTemp[1];
1160  gyroBias[2] += gyroTemp[2];
1161 
1162  }
1163 
1164  /* Divide by packet count to get the average */
1165  gyroBias[0] /= packetCount;
1166  gyroBias[1] /= packetCount;
1167  gyroBias[2] /= packetCount;
1168 
1169  /* Convert the values to degrees per sec for displaying */
1170  gyroBiasScaled[0] = (float) gyroBias[0] * gyroRes;
1171  gyroBiasScaled[1] = (float) gyroBias[1] * gyroRes;
1172  gyroBiasScaled[2] = (float) gyroBias[2] * gyroRes;
1173 
1174  /* Read stored gyro trim values. After reset these values are all 0 */
1176  gyroBiasStored[0] = ( (int16_t) ( data[0] << 8 ) | data[1] );
1177 
1179  gyroBiasStored[1] = ( (int16_t) ( data[0] << 8 ) | data[1] );
1180 
1182  gyroBiasStored[2] = ( (int16_t) ( data[0] << 8 ) | data[1] );
1183 
1184  /* The gyro bias should be stored in 1000dps full scaled format. We measured in 250dps to get */
1185  /* the best sensitivity, so need to divide by 4 */
1186  /* Substract from the stored calibration value */
1187  gyroBiasStored[0] -= gyroBias[0] / 4;
1188  gyroBiasStored[1] -= gyroBias[1] / 4;
1189  gyroBiasStored[2] -= gyroBias[2] / 4;
1190 
1191  /* Split the values into two bytes */
1192  data[0] = ( gyroBiasStored[0] >> 8 ) & 0xFF;
1193  data[1] = ( gyroBiasStored[0] ) & 0xFF;
1194  data[2] = ( gyroBiasStored[1] >> 8 ) & 0xFF;
1195  data[3] = ( gyroBiasStored[1] ) & 0xFF;
1196  data[4] = ( gyroBiasStored[2] >> 8 ) & 0xFF;
1197  data[5] = ( gyroBiasStored[2] ) & 0xFF;
1198 
1199  /* Write the gyro bias values to the chip */
1206 
1207  /* Turn off FIFO */
1209 
1210  /* Disable all sensors */
1211  ICM20648_sensorEnable( false, false, false );
1212 
1213  return ICM20648_OK;
1214 
1215 }
1216 
1217 /***************************************************************************/
1227 uint32_t ICM20648_temperatureRead( float *temperature )
1228 {
1229 
1230  uint8_t data[2];
1231  int16_t raw_temp;
1232 
1233  /* Read temperature registers */
1235 
1236  /* Convert to int16 */
1237  raw_temp = (int16_t) ( ( data[0] << 8 ) + data[1] );
1238 
1239  /* Calculate the Celsius value from the raw reading */
1240  *temperature = ( (float) raw_temp / 333.87 ) + 21.0;
1241 
1242  return ICM20648_OK;
1243 
1244 }
1245 
1246 /***************************************************************************/
1256 uint32_t ICM20648_getDeviceID( uint8_t *devID )
1257 {
1258 
1260 
1261  return ICM20648_OK;
1262 
1263 }
1264 
1265 /***************************************************************************/
1272 uint32_t ICM20648_spiInit( void )
1273 {
1274 
1275  USART_TypeDef *usart = ICM20648_SPI_USART;
1276  USART_InitSync_TypeDef init = ICM20648_USART_INITSYNC;
1277 
1278  /* Enabling clock to USART */
1280  CMU_ClockEnable( ICM20648_SPI_CLK, true );
1281  CMU_ClockEnable( cmuClock_GPIO, true );
1282 
1283  /* IO configuration */
1284  GPIO_PinModeSet( ICM20648_PORT_SPI_MOSI, ICM20648_PIN_SPI_MOSI, gpioModePushPull, 0 ); /* TX - MOSI */
1285  GPIO_PinModeSet( ICM20648_PORT_SPI_MISO, ICM20648_PIN_SPI_MISO, gpioModeInput, 0 ); /* RX - MISO */
1286  GPIO_PinModeSet( ICM20648_PORT_SPI_SCLK, ICM20648_PIN_SPI_SCLK, gpioModePushPull, 0 ); /* Clock */
1287  GPIO_PinModeSet( ICM20648_PORT_SPI_CS, ICM20648_PIN_SPI_CS, gpioModePushPull, 1 ); /* CS */
1288 
1289  USART_Reset( ICM20648_SPI_USART );
1290 
1291  /* Initialize USART, in SPI master mode. */
1292  USART_InitSync( usart, &init );
1293 
1294  /* Enable pins at correct UART/USART location. */
1295  usart->ROUTEPEN = USART_ROUTEPEN_RXPEN | USART_ROUTEPEN_TXPEN | USART_ROUTEPEN_CLKPEN;
1296  usart->ROUTELOC0 = ( usart->ROUTELOC0 & ~( _USART_ROUTELOC0_TXLOC_MASK | _USART_ROUTELOC0_RXLOC_MASK | _USART_ROUTELOC0_CLKLOC_MASK ) );
1297  usart->ROUTELOC0 |= ( ICM20648_LOCATION_SPI_MOSI << _USART_ROUTELOC0_TXLOC_SHIFT );
1298  usart->ROUTELOC0 |= ( ICM20648_LOCATION_SPI_MISO << _USART_ROUTELOC0_RXLOC_SHIFT );
1299  usart->ROUTELOC0 |= ( ICM20648_LOCATION_SPI_SCLK << _USART_ROUTELOC0_CLKLOC_SHIFT );
1300 
1301  return ICM20648_OK;
1302 
1303 }
1304 
1305 /***************************************************************************/
1323 void ICM20648_registerRead( uint16_t addr, int numBytes, uint8_t *data )
1324 {
1325 
1326  uint8_t regAddr;
1327  uint8_t bank;
1328 
1329  regAddr = (uint8_t) ( addr & 0x7F );
1330  bank = (uint8_t) ( addr >> 7 );
1331 
1332  ICM20648_bankSelect( bank );
1333 
1334  /* Enable chip select */
1335  ICM20648_chipSelectSet( true );
1336 
1337  /* Set R/W bit to 1 - read */
1338  USART_Tx( ICM20648_SPI_USART, ( regAddr | 0x80 ) );
1339  USART_Rx( ICM20648_SPI_USART );
1340  /* Transmit 0's to provide clock and read the data */
1341  while( numBytes-- ) {
1342  USART_Tx( ICM20648_SPI_USART, 0x00 );
1343  *data++ = USART_Rx( ICM20648_SPI_USART );
1344  }
1345 
1346  /* Disable chip select */
1347  ICM20648_chipSelectSet( false );
1348 
1349  return;
1350 
1351 }
1352 
1353 /***************************************************************************/
1368 void ICM20648_registerWrite( uint16_t addr, uint8_t data )
1369 {
1370 
1371  uint8_t regAddr;
1372  uint8_t bank;
1373 
1374  regAddr = (uint8_t) ( addr & 0x7F );
1375  bank = (uint8_t) ( addr >> 7 );
1376 
1377  ICM20648_bankSelect( bank );
1378 
1379  /* Enable chip select */
1380  ICM20648_chipSelectSet( true );
1381 
1382  /* clear R/W bit - write, send the address */
1383  USART_Tx( ICM20648_SPI_USART, ( regAddr & 0x7F ) );
1384  USART_Rx( ICM20648_SPI_USART );
1385 
1386  /* Send the data */
1387  USART_Tx( ICM20648_SPI_USART, data );
1388  USART_Rx( ICM20648_SPI_USART );
1389 
1390  /* Disable chip select */
1391  ICM20648_chipSelectSet( false );
1392 
1393  return;
1394 
1395 }
1396 
1397 /***************************************************************************/
1407 void ICM20648_bankSelect( uint8_t bank )
1408 {
1409 
1410  /* Enable chip select */
1411  ICM20648_chipSelectSet( true );
1412 
1413  /* Select the Bank Select register */
1414  USART_Tx( ICM20648_SPI_USART, ICM20648_REG_BANK_SEL );
1415  USART_Rx( ICM20648_SPI_USART );
1416 
1417  /* Write the desired bank address 0..3 */
1418  USART_Tx( ICM20648_SPI_USART, ( bank << 4 ) );
1419  USART_Rx( ICM20648_SPI_USART );
1420 
1421  /* Disable chip select */
1422  ICM20648_chipSelectSet( false );
1423 
1424  return;
1425 
1426 }
1427 
1430 /***************************************************************************/
1440 static void ICM20648_chipSelectSet( bool select )
1441 {
1442 
1443  if( select ) {
1444  GPIO_PinOutClear( ICM20648_PORT_SPI_CS, ICM20648_PIN_SPI_CS );
1445  }
1446  else {
1447  GPIO_PinOutSet( ICM20648_PORT_SPI_CS, ICM20648_PIN_SPI_CS );
1448  }
1449 
1450  return;
1451 
1452 }
1453 
Clock management unit (CMU) API.
uint32_t ICM20648_wakeOnMotionITEnable(bool enable, uint8_t womThreshold, float sampleRate)
Sets up and enables the Wake-up On Motion feature.
Definition: icm20648.c:793
#define ICM20648_REG_USER_CTRL
Definition: icm20648.h:56
void USART_Tx(USART_TypeDef *usart, uint8_t data)
Transmit one 4-9 bit frame.
Definition: em_usart.c:1084
#define ICM20648_MASK_GYRO_FULLSCALE
Definition: icm20648.h:165
#define ICM20648_BIT_LP_EN
Definition: icm20648.h:72
void USART_InitSync(USART_TypeDef *usart, const USART_InitSync_TypeDef *init)
Init USART for synchronous mode.
Definition: em_usart.c:640
#define ICM20648_REG_PWR_MGMT_2
Definition: icm20648.h:76
#define ICM20648_ACCEL_FULLSCALE_8G
Definition: icm20648.h:210
uint32_t ICM20648_sampleRateSet(float sampleRate)
Sets the sample rate both of the accelerometer and the gyroscope.
Definition: icm20648.c:359
uint32_t ICM20648_deInit(void)
De-initializes the ICM20648 sensor by disconnecting the supply and SPI lines.
Definition: icm20648.c:103
#define ICM20648_GYRO_FULLSCALE_250DPS
Definition: icm20648.h:167
uint32_t ICM20648_interruptEnable(bool dataReadyEnable, bool womEnable)
Enables or disables the interrupts in the ICM20648 chip.
Definition: icm20648.c:697
#define ICM20648_BIT_I2C_IF_DIS
Definition: icm20648.h:60
#define ICM20648_REG_ACCEL_XOUT_H_SH
Definition: icm20648.h:106
#define ICM20648_REG_TEMPERATURE_H
Definition: icm20648.h:120
#define ICM20648_MASK_ACCEL_BW
Definition: icm20648.h:207
#define ICM20648_BIT_ACCEL_INTEL_MODE
Definition: icm20648.h:198
uint32_t ICM20648_temperatureRead(float *temperature)
Reads the temperature sensor raw value and converts to Celsius.
Definition: icm20648.c:1227
#define ICM20648_REG_FIFO_EN_2
Definition: icm20648.h:126
uint8_t USART_Rx(USART_TypeDef *usart)
Receive one 4-8 bit frame, (or part of 10-16 bit frame).
Definition: em_usart.c:923
#define ICM20648_REG_FIFO_R_W
Definition: icm20648.h:135
void UTIL_delay(uint32_t ms)
Delays number of msTick Systicks (1 ms)
Definition: util.c:97
#define ICM20648_REG_ACCEL_WOM_THR
Definition: icm20648.h:200
#define ICM20648_REG_INT_STATUS
Definition: icm20648.h:97
#define ICM20648_BIT_INT_OPEN
Definition: icm20648.h:83
#define ICM20648_REG_ACCEL_SMPLRT_DIV_1
Definition: icm20648.h:193
Driver for the Invensense ICM20648 6-axis motion sensor.
#define ICM20648_BIT_ACCEL_INTEL_EN
Definition: icm20648.h:197
void ICM20648_registerWrite(uint16_t addr, uint8_t data)
Writes a register in the ICM20648 device.
Definition: icm20648.c:1368
uint32_t ICM20648_gyroResolutionGet(float *gyroRes)
Gets the actual resolution of the gyroscope.
Definition: icm20648.c:260
#define ICM20648_BIT_H_RESET
Definition: icm20648.h:70
uint32_t ICM20648_gyroDataRead(float *gyro)
Reads the raw gyroscope value and converts to deg/sec value based on the actual resolution.
Definition: icm20648.c:182
Universal synchronous/asynchronous receiver/transmitter (USART/UART) peripheral API.
#define ICM20648_DEVICE_ID
Definition: icm20648.h:273
#define ICM20648_BIT_SLEEP
Definition: icm20648.h:71
#define ICM20648_REG_GYRO_CONFIG_1
Definition: icm20648.h:161
#define ICM20648_BIT_PWR_ACCEL_STBY
Definition: icm20648.h:77
#define ICM20648_REG_GYRO_XOUT_H_SH
Definition: icm20648.h:113
uint32_t ICM20648_gyroBandwidthSet(uint8_t gyroBw)
Sets the bandwidth of the gyroscope.
Definition: icm20648.c:462
#define ICM20648_BIT_ACCEL_FIFO_EN
Definition: icm20648.h:127
uint32_t ICM20648_lowPowerModeEnter(bool enAccel, bool enGyro, bool enTemp)
Enables or disables the sensors in low power mode in the ICM20648 chip.
Definition: icm20648.c:641
void ICM20648_registerRead(uint16_t addr, int numBytes, uint8_t *data)
Reads register from the ICM20648 device.
Definition: icm20648.c:1323
#define ICM20648_REG_YA_OFFSET_L
Definition: icm20648.h:150
#define ICM20648_BIT_ACCEL_CYCLE
Definition: icm20648.h:66
#define ICM20648_BIT_TEMP_DIS
Definition: icm20648.h:73
float ICM20648_gyroSampleRateSet(float sampleRate)
Sets the sample rate of the accelerometer.
Definition: icm20648.c:380
#define ICM20648_REG_ACCEL_CONFIG
Definition: icm20648.h:202
void ICM20648_bankSelect(uint8_t bank)
Select the desired register bank.
Definition: icm20648.c:1407
#define ICM20648_BIT_WOM_INT_EN
Definition: icm20648.h:87
#define ICM20648_GYRO_FULLSCALE_500DPS
Definition: icm20648.h:168
#define ICM20648_REG_ZG_OFFS_USRL
Definition: icm20648.h:189
#define ICM20648_REG_BANK_SEL
Definition: icm20648.h:271
#define ICM20648_REG_ZA_OFFSET_H
Definition: icm20648.h:151
#define ICM20648_REG_YG_OFFS_USRL
Definition: icm20648.h:187
void GPIO_PinModeSet(GPIO_Port_TypeDef port, unsigned int pin, GPIO_Mode_TypeDef mode, unsigned int out)
Set the mode for a GPIO pin.
Definition: em_gpio.c:269
#define ICM20648_REG_INT_ENABLE_1
Definition: icm20648.h:89
#define ICM20648_REG_XG_OFFS_USRH
Definition: icm20648.h:184
General Purpose IO (GPIO) peripheral API.
#define ICM20648_REG_INT_ENABLE
Definition: icm20648.h:86
#define ICM20648_OK
Definition: icm20648.h:33
uint32_t ICM20648_spiInit(void)
Initializes the SPI bus in order to communicate with the ICM20648.
Definition: icm20648.c:1272
Utility Functions for the Thunderboard Sense.
__STATIC_INLINE void GPIO_PinOutSet(GPIO_Port_TypeDef port, unsigned int pin)
Set a single pin in GPIO data out register to 1.
Definition: em_gpio.h:856
#define ICM20648_REG_ACCEL_SMPLRT_DIV_2
Definition: icm20648.h:194
void CMU_ClockEnable(CMU_Clock_TypeDef clock, bool enable)
Enable/disable a clock.
Definition: em_cmu.c:1453
#define ICM20648_BITS_GYRO_FIFO_EN
Definition: icm20648.h:128
#define ICM20648_REG_FIFO_RST
Definition: icm20648.h:130
uint32_t ICM20648_accelGyroCalibrate(float *accelBiasScaled, float *gyroBiasScaled)
Accelerometer and gyroscope calibration function. Reads the gyroscope and accelerometer values while ...
Definition: icm20648.c:863
uint32_t ICM20648_init(void)
Initializes the ICM20648 sensor. Enables the power supply and SPI lines, sets up the host SPI control...
Definition: icm20648.c:53
#define ICM20648_GYRO_FULLSCALE_2000DPS
Definition: icm20648.h:170
#define ICM20648_ACCEL_FULLSCALE_4G
Definition: icm20648.h:209
#define ICM20648_BIT_RAW_DATA_0_RDY_EN
Definition: icm20648.h:90
uint32_t ICM20648_accelDataRead(float *accel)
Reads the raw acceleration value and converts to g value based on the actual resolution.
Definition: icm20648.c:145
#define ICM20648_REG_XA_OFFSET_H
Definition: icm20648.h:147
#define ICM20648_BIT_INT_ACTL
Definition: icm20648.h:82
#define ICM20648_REG_XA_OFFSET_L
Definition: icm20648.h:148
#define ICM20648_ACCEL_BW_246HZ
Definition: icm20648.h:214
void USART_Reset(USART_TypeDef *usart)
Reset USART/UART to same state as after a HW reset.
Definition: em_usart.c:856
#define ICM20648_REG_FIFO_MODE
Definition: icm20648.h:131
#define ICM20648_REG_ACCEL_INTEL_CTRL
Definition: icm20648.h:196
uint32_t ICM20648_interruptStatusRead(uint32_t *intStatus)
Reads the interrupt status registers of the ICM20648 chip.
Definition: icm20648.c:737
#define ICM20648_ERROR_INVALID_DEVICE_ID
Definition: icm20648.h:34
#define ICM20648_REG_INT_STATUS_1
Definition: icm20648.h:101
#define ICM20648_REG_GYRO_SMPLRT_DIV
Definition: icm20648.h:159
#define ICM20648_REG_FIFO_COUNT_H
Definition: icm20648.h:133
#define ICM20648_GYRO_BW_12HZ
Definition: icm20648.h:178
uint32_t BOARD_imuEnable(bool enable)
Enables or disables the accelerometer and gyroscope sensor.
Definition: board.c:448
#define ICM20648_ACCEL_FULLSCALE_2G
Definition: icm20648.h:208
#define ICM20648_REG_YG_OFFS_USRH
Definition: icm20648.h:186
#define ICM20648_REG_PWR_MGMT_1
Definition: icm20648.h:69
uint32_t ICM20648_gyroFullscaleSet(uint8_t gyroFs)
Sets the full scale value of the gyroscope.
Definition: icm20648.c:332
float ICM20648_accelSampleRateSet(float sampleRate)
Sets the sample rate of the gyroscope.
Definition: icm20648.c:420
bool ICM20648_isDataReady(void)
Checks if new data is available for read.
Definition: icm20648.c:759
#define ICM20648_BIT_PWR_GYRO_STBY
Definition: icm20648.h:78
#define ICM20648_ACCEL_FULLSCALE_16G
Definition: icm20648.h:211
uint32_t ICM20648_sensorEnable(bool accel, bool gyro, bool temp)
Enables or disables the sensors in the ICM20648 chip.
Definition: icm20648.c:584
#define ICM20648_REG_ZA_OFFSET_L
Definition: icm20648.h:152
#define ICM20648_GYRO_FULLSCALE_1000DPS
Definition: icm20648.h:169
uint32_t ICM20648_reset(void)
Performs soft reset on the ICM20648 chip.
Definition: icm20648.c:120
uint32_t ICM20648_getDeviceID(uint8_t *devID)
Reads the device ID of the ICM20648.
Definition: icm20648.c:1256
uint32_t ICM20648_accelFullscaleSet(uint8_t accelFs)
Sets the full scale value of the accelerometer.
Definition: icm20648.c:305
#define ICM20648_REG_WHO_AM_I
Definition: icm20648.h:54
uint32_t ICM20648_cycleModeEnable(bool enable)
Enables or disables the cycle mode operation of the accel and gyro.
Definition: icm20648.c:551
#define ICM20648_MASK_GYRO_BW
Definition: icm20648.h:166
#define ICM20648_MASK_ACCEL_FULLSCALE
Definition: icm20648.h:206
__STATIC_INLINE void GPIO_PinOutClear(GPIO_Port_TypeDef port, unsigned int pin)
Set a single pin in GPIO data out port register to 0.
Definition: em_gpio.h:811
#define ICM20648_ACCEL_BW_1210HZ
Definition: icm20648.h:212
#define ICM20648_REG_ZG_OFFS_USRH
Definition: icm20648.h:188
#define ICM20648_BIT_RAW_DATA_0_RDY_INT
Definition: icm20648.h:102
#define ICM20648_REG_INT_PIN_CFG
Definition: icm20648.h:81
uint32_t ICM20648_sleepModeEnable(bool enable)
Enables or disables the sleep mode of the device.
Definition: icm20648.c:518
#define ICM20648_REG_LP_CONFIG
Definition: icm20648.h:64
#define ICM20648_BIT_CLK_PLL
Definition: icm20648.h:74
#define ICM20648_BIT_FIFO_EN
Definition: icm20648.h:58
BOARD module header file.
uint32_t ICM20648_gyroCalibrate(float *gyroBiasScaled)
Gyroscope calibration function. Reads the gyroscope values while the device is at rest and in level...
Definition: icm20648.c:1077
#define ICM20648_REG_XG_OFFS_USRL
Definition: icm20648.h:185
#define ICM20648_BIT_GYRO_CYCLE
Definition: icm20648.h:67
uint32_t ICM20648_accelResolutionGet(float *accelRes)
Gets the actual resolution of the accelerometer.
Definition: icm20648.c:217
#define ICM20648_REG_YA_OFFSET_H
Definition: icm20648.h:149
uint32_t ICM20648_accelBandwidthSet(uint8_t accelBw)
Sets the bandwidth of the accelerometer.
Definition: icm20648.c:491