EFM32 Wonder Gecko Software Documentation  efm32wg-doc-5.1.2
imu.c
Go to the documentation of this file.
1 /***************************************************************************/
16 #include <stdint.h>
17 #include <stdbool.h>
18 #include <stdio.h>
19 
20 #include "em_device.h"
21 #include "gpiointerrupt.h"
22 
23 #ifdef RADIO_BLE
24 #include "native_gecko.h"
25 #endif
26 
27 #include "thunderboard/board.h"
28 #include "thunderboard/icm20648.h"
29 #include "thunderboard/imu/imu.h"
30 #include "thunderboard/util.h"
31 
34 /****************************************************************************/
35 /* Local function prototypes */
36 /****************************************************************************/
37 static void gpioInterrupt( uint8_t pin );
38 
43 /***************************************************************************/
51 /***************************************************************************/
57 static float gyroSampleRate;
58 static float accelSampleRate;
59 static bool dataReady;
60 static uint32_t IMU_interruptCount = 0;
61 static uint32_t IMU_isDataReadyQueryCount = 0;
62 static uint32_t IMU_isDataReadyTrueCount = 0;
63 IMU_SensorFusion fuseObj;
69 /***************************************************************************/
75 /***************************************************************************/
82 uint32_t IMU_init( void )
83 {
84 
85  uint32_t status;
86  uint8_t devid;
87  float gyroBiasScaled[3];
88 
89  /* GPIOINT driver */
90  GPIOINT_Init();
91 
93  IMU_fuseNew( &fuseObj );
94 
95  /* Initialize acc/gyro driver */
96  printf( "IMU init..." );
97  status = ICM20648_init();
98  if( status != ICM20648_OK ) {
99  printf( "Failed! status = %08Xh\r\n", status );
100  goto cleanup;
101  }
102  printf( "OK\r\n" );
103 
104  status = ICM20648_getDeviceID( &devid );
105  if( status != ICM20648_OK ) {
106  goto cleanup;
107  }
108  printf( "IMU device ID = %02Xh\r\n", devid );
109 
110  /* Gyro calibration */
112  printf( "IMU gyro calibration..." );
113  status = ICM20648_gyroCalibrate( gyroBiasScaled );
114  if( status != ICM20648_OK ) {
115  printf( "Failed! status = %08Xh\r\n", status );
116  goto cleanup;
117  }
118  printf( "OK\r\n" );
119 
120  printf( "Gyroscope bias (deg/s) : " );
121  printf( "gx: % 6.4f, gy: % 6.4f, gz: % 6.4f\r\n", gyroBiasScaled[0], gyroBiasScaled[1], gyroBiasScaled[2] );
122 
124 
125 cleanup:
126 
127  if( status != ICM20648_OK ){
128  ICM20648_deInit();
130  }
131 
132  return status;
133 
134 }
135 
136 /***************************************************************************/
143 uint32_t IMU_deInit( void )
144 {
145 
146  uint32_t status;
147 
149  status = ICM20648_deInit();
150 
151  return status;
152 
153 }
154 
155 /***************************************************************************/
162 uint8_t IMU_getState( void )
163 {
164  return IMU_state;
165 }
166 
167 /***************************************************************************/
177 void IMU_config( float sampleRate )
178 {
179 
180  uint32_t itStatus;
181 
182  /* Set IMU state */
184 
185  /* Register PIC interrupt callback */
186  BOARD_imuEnableIRQ( true );
188 
189  /* Clear the interrupts */
191  ICM20648_interruptStatusRead( &itStatus );
192 
193  /* Enable accel sensor */
194  ICM20648_sensorEnable( true, true, false );
195 
196  /* Set sample rate */
197  gyroSampleRate = ICM20648_gyroSampleRateSet( sampleRate );
198  accelSampleRate = ICM20648_accelSampleRateSet( sampleRate );
199 
200  printf( "IMU sample rate set to %f Hz (accel), %f Hz (gyro)\r\n", accelSampleRate, gyroSampleRate );
201 
202  /* Filter bandwidth: 12kHz, otherwise the results may be off */
205 
206  /* Accel: 2G full scale */
208 
209  /* Gyro: 250 degrees per sec full scale */
211 
212  UTIL_delay( 50 );
213 
214  /* Enable the raw data ready interrupt */
215  ICM20648_interruptEnable( true, false );
216 
217  /* Enter low power mode */
218  ICM20648_lowPowerModeEnter( true, true, false );
219 
220  /* Clear the interrupts */
222  ICM20648_interruptStatusRead( &itStatus );
223 
224  /* IMU fuse config & setup */
225  IMU_fuseAccelerometerSetSampleRate( &fuseObj, accelSampleRate );
226  IMU_fuseGyroSetSampleRate( &fuseObj, gyroSampleRate );
227  IMU_fuseReset( &fuseObj );
228 
230 
231  return;
232 
233 }
234 
235 /***************************************************************************/
245 void IMU_accelerationGet( int16_t avec[3] )
246 {
247 
248  if( fuseObj.aAccumulatorCount > 0 ) {
249  avec[0] = (int16_t) ( 1000.0f * fuseObj.aAccumulator[0] / fuseObj.aAccumulatorCount );
250  avec[1] = (int16_t) ( 1000.0f * fuseObj.aAccumulator[1] / fuseObj.aAccumulatorCount );
251  avec[2] = (int16_t) ( 1000.0f * fuseObj.aAccumulator[2] / fuseObj.aAccumulatorCount );
252  fuseObj.aAccumulator[0] = 0;
253  fuseObj.aAccumulator[1] = 0;
254  fuseObj.aAccumulator[2] = 0;
255  fuseObj.aAccumulatorCount = 0;
256  }
257  else {
258  avec[0] = 0;
259  avec[1] = 0;
260  avec[2] = 0;
261  }
262  return;
263 
264 }
265 
266 /***************************************************************************/
276 void IMU_orientationGet( int16_t ovec[3] )
277 {
278 
279  ovec[0] = (int16_t) ( 100.0f * IMU_RAD_TO_DEG_FACTOR * fuseObj.orientation[0] );
280  ovec[1] = (int16_t) ( 100.0f * IMU_RAD_TO_DEG_FACTOR * fuseObj.orientation[1] );
281  ovec[2] = (int16_t) ( 100.0f * IMU_RAD_TO_DEG_FACTOR * fuseObj.orientation[2] );
282 
283  return;
284 
285 }
286 
287 /***************************************************************************/
297 void IMU_gyroGet( int16_t gvec[3] )
298 {
299 
300  gvec[0] = (int16_t) ( 100.0f * fuseObj.gVector[0] );
301  gvec[1] = (int16_t) ( 100.0f * fuseObj.gVector[1] );
302  gvec[2] = (int16_t) ( 100.0f * fuseObj.gVector[2] );
303 
304  return;
305 
306 }
307 
308 /***************************************************************************/
315 uint32_t IMU_gyroCalibrate( void )
316 {
317 
318  uint32_t status;
319 
320  status = IMU_OK;
321 
322  /* Disable interrupt */
323  ICM20648_interruptEnable( false, false );
324 
325  IMU_deInit();
326  status = IMU_init();
327 
328  /* Restart regular sampling */
329  IMU_config( gyroSampleRate );
330 
331  return status;
332 
333 }
334 
335 /***************************************************************************/
343 void IMU_update( void )
344 {
345 
346  IMU_fuseUpdate( &fuseObj );
347 
348  return;
349 
350 }
351 
352 /***************************************************************************/
359 void IMU_reset( void )
360 {
361 
362  IMU_fuseReset( &fuseObj );
363 
364  return;
365 
366 }
367 
368 /***************************************************************************/
378 void IMU_getAccelerometerData( float avec[3] )
379 {
380 
381  if( IMU_state != IMU_STATE_READY ){
382  avec[0] = 0;
383  avec[1] = 0;
384  avec[2] = 0;
385  return;
386  }
387 
388  ICM20648_accelDataRead( avec );
389 
390  return;
391 
392 }
393 
394 /***************************************************************************/
404 void IMU_getGyroCorrectionAngles( float acorr[3] )
405 {
406 
407  acorr[0] = fuseObj.angleCorrection[0];
408  acorr[1] = fuseObj.angleCorrection[1];
409  acorr[2] = fuseObj.angleCorrection[2];
410 
411  return;
412 
413 }
414 
415 /***************************************************************************/
425 void IMU_getGyroData( float gvec[3] )
426 {
427 
428  if( IMU_state != IMU_STATE_READY ){
429  gvec[0] = 0;
430  gvec[1] = 0;
431  gvec[2] = 0;
432  return;
433  }
434 
435  ICM20648_gyroDataRead( gvec );
436 
437  return;
438 
439 }
440 
441 /***************************************************************************/
448 bool IMU_isDataReady( void )
449 {
450 
451  bool ready;
452 
453  if( IMU_state != IMU_STATE_READY ){
454  return false;
455  }
456 
457  ready = ICM20648_isDataReady();
458  IMU_isDataReadyQueryCount++;
459 
460  if( ready ) {
461  IMU_isDataReadyTrueCount++;
462  }
463 
464  return ready;
465 
466 }
467 
468 /***************************************************************************/
479 {
480 
481  bool ready;
482  unsigned int pin;
483 
484  if( IMU_state != IMU_STATE_READY ){
485  return false;
486  }
487 
488  ready = dataReady;
489 
490  if( BOARD_picIsLegacyIntCtrl() ) {
491 
493  if( pin == 0 ) {
494  ready = true;
495  }
496  }
497 
498  return ready;
499 
500 }
501 
502 /***************************************************************************/
511 {
512 
513  dataReady = false;
514 
515  if( BOARD_picIsLegacyIntCtrl() ) {
517  }
518 
519  return;
520 
521 }
522 
525 /***************************************************************************/
536 static void gpioInterrupt( uint8_t pin )
537 {
538 
539  dataReady = true;
540  IMU_interruptCount++;
541 
542 #ifdef RADIO_BLE
543  gecko_external_signal( 1 );
544 #endif
545 
546  return;
547 
548 }
549 
Structure to store the sensor fusion data.
Definition: imu.h:74
bool IMU_isDataReadyFlag(void)
Checks if there is new accel/gyro data available for read. In case of ver 0.3.0 and older of the PIC ...
Definition: imu.c:478
void IMU_config(float sampleRate)
Configures the IMU.
Definition: imu.c:177
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
bool BOARD_picIsLegacyIntCtrl(void)
Checks if the legacy interrupt control method should be used.
Definition: board.c:857
void UTIL_delay(uint32_t ms)
Delays number of msTick Systicks (1 ms)
Definition: util.c:97
#define IMU_STATE_READY
Definition: imu.h:58
Driver for the Invensense ICM20648 6-axis motion sensor.
CMSIS Cortex-M Peripheral Access Layer for Silicon Laboratories microcontroller devices.
#define ICM20648_GYRO_BW_51HZ
Definition: icm20648.h:176
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
void GPIOINT_CallbackRegister(uint8_t pin, GPIOINT_IrqCallbackPtr_t callbackPtr)
Registers user callback for given pin number.
Definition: gpiointerrupt.c:91
uint32_t BOARD_picWriteReg(uint8_t reg, uint8_t value)
Writes a register in the IO expander.
Definition: board.c:676
uint32_t ICM20648_gyroBandwidthSet(uint8_t gyroBw)
Sets the bandwidth of the gyroscope.
Definition: icm20648.c:462
#define BOARD_PIC_INT_WAKE_PORT
Definition: board.h:55
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
uint8_t IMU_getState(void)
Returns IMU state.
Definition: imu.c:162
float ICM20648_gyroSampleRateSet(float sampleRate)
Sets the sample rate of the accelerometer.
Definition: icm20648.c:380
float angleCorrection[3]
Definition: imu.h:93
void IMU_getGyroData(float gvec[3])
Retrieves the raw gyroscope data from the IMU.
Definition: imu.c:425
uint32_t BOARD_imuEnableIRQ(bool enable)
Enables or disables the accelerometer and gyroscope GPIO interrupt.
Definition: board.c:469
void IMU_fuseAccelerometerSetSampleRate(IMU_SensorFusion *f, float rate)
Sets the accelerometer sample rate in the IMU_SensorFusion structure.
Definition: imu_fuse.c:124
#define IMU_OK
Definition: imu.h:39
void IMU_update(void)
Gets a new set of data from the accel and gyro sensor and updates the fusion calculation.
Definition: imu.c:343
void IMU_getAccelerometerData(float avec[3])
Retrieves the raw acceleration data from the IMU.
Definition: imu.c:378
#define ICM20648_OK
Definition: icm20648.h:33
uint32_t IMU_gyroCalibrate(void)
Performs gyroscope calibration to cancel gyro bias.
Definition: imu.c:315
Utility Functions for the Thunderboard Sense.
void IMU_gyroGet(int16_t gvec[3])
Retrieves the processed gyroscope data.
Definition: imu.c:297
void IMU_fuseReset(IMU_SensorFusion *f)
Clears the values of the sensor fusion object.
Definition: imu_fuse.c:327
uint32_t IMU_init(void)
Initializes and calibrates the IMU.
Definition: imu.c:82
Inertial Measurement Unit DCM matrix related routines.
#define BOARD_PIC_REG_INT_CLEAR
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 IMU_STATE_CALIBRATING
Definition: imu.h:60
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
void IMU_clearDataReadyFlag(void)
Clears the IMU data ready flag. In case of ver 0.3.0 and older of the PIC firmware the interrupt regi...
Definition: imu.c:510
#define IMU_STATE_INITIALIZING
Definition: imu.h:59
void IMU_getGyroCorrectionAngles(float acorr[3])
Retrieves the processed gyroscope correction angles.
Definition: imu.c:404
uint32_t ICM20648_interruptStatusRead(uint32_t *intStatus)
Reads the interrupt status registers of the ICM20648 chip.
Definition: icm20648.c:737
void IMU_accelerationGet(int16_t avec[3])
Retrieves the processed acceleration data.
Definition: imu.c:245
bool IMU_isDataReady(void)
Checks if there is new accel/gyro data available for read.
Definition: imu.c:448
void IMU_fuseNew(IMU_SensorFusion *f)
Initializes a new IMU_SensorFusion structure.
Definition: imu_fuse.c:51
void GPIOINT_Init(void)
Initialization of GPIOINT module.
Definition: gpiointerrupt.c:67
#define ICM20648_ACCEL_FULLSCALE_2G
Definition: icm20648.h:208
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
GPIOINT API definition.
float aAccumulator[3]
Definition: imu.h:82
uint8_t IMU_state
Definition: imu.c:41
uint32_t ICM20648_sensorEnable(bool accel, bool gyro, bool temp)
Enables or disables the sensors in the ICM20648 chip.
Definition: icm20648.c:584
uint32_t ICM20648_getDeviceID(uint8_t *devID)
Reads the device ID of the ICM20648.
Definition: icm20648.c:1256
#define BOARD_PIC_INT_WAKE_PIN
Definition: board.h:56
uint32_t ICM20648_accelFullscaleSet(uint8_t accelFs)
Sets the full scale value of the accelerometer.
Definition: icm20648.c:305
void IMU_fuseUpdate(IMU_SensorFusion *f)
Updates the fusion calculation.
Definition: imu_fuse.c:353
#define ICM20648_ACCEL_BW_1210HZ
Definition: icm20648.h:212
uint32_t aAccumulatorCount
Definition: imu.h:83
#define IMU_RAD_TO_DEG_FACTOR
Definition: imu.h:51
__STATIC_INLINE unsigned int GPIO_PinInGet(GPIO_Port_TypeDef port, unsigned int pin)
Read the pad value for a single pin in a GPIO port.
Definition: em_gpio.h:781
void IMU_fuseGyroSetSampleRate(IMU_SensorFusion *f, float rate)
Sets the gyro sample rate and related values in the IMU_SensorFusion structure.
Definition: imu_fuse.c:100
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
float orientation[3]
Definition: imu.h:94
void IMU_reset(void)
Resets the fusion calculation.
Definition: imu.c:359
float gVector[3]
Definition: imu.h:87
#define IMU_STATE_DISABLED
Definition: imu.h:57
void IMU_orientationGet(int16_t ovec[3])
Retrieves the processed orientation data.
Definition: imu.c:276
uint32_t IMU_deInit(void)
De-initializes the IMU chip.
Definition: imu.c:143
uint32_t ICM20648_accelBandwidthSet(uint8_t accelBw)
Sets the bandwidth of the accelerometer.
Definition: icm20648.c:491