imu.c

Go to the documentation of this file.

/***************************************************************************/
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>

#include "em_device.h"
#include "gpiointerrupt.h"

#ifdef RADIO_BLE
#include "native_gecko.h"
#endif

#include "thunderboard/board.h"
#include "thunderboard/icm20648.h"
#include "thunderboard/imu/imu.h"
#include "thunderboard/util.h"

/****************************************************************************/
/* Local function prototypes                                                */
/****************************************************************************/
static void gpioInterrupt( uint8_t pin );

uint8_t IMU_state = IMU_STATE_DISABLED;    
/***************************************************************************/
/***************************************************************************/
static float      gyroSampleRate;                
static float      accelSampleRate;               
static bool       dataReady;                     
static uint32_t   IMU_interruptCount = 0;        
static uint32_t   IMU_isDataReadyQueryCount = 0; 
static uint32_t   IMU_isDataReadyTrueCount = 0;  
IMU_SensorFusion fuseObj;                        
/***************************************************************************/
/***************************************************************************/
uint32_t IMU_init( void )
{

   uint32_t status;
   uint8_t devid;
   float gyroBiasScaled[3];

   /* GPIOINT driver */
   GPIOINT_Init();

   IMU_state = IMU_STATE_INITIALIZING;
   IMU_fuseNew( &fuseObj );

   /* Initialize acc/gyro driver */
   printf( "IMU init..." );
   status = ICM20648_init();
   if( status != ICM20648_OK ) {
      printf( "Failed! status = %08Xh\r\n", status );
      goto cleanup;
   }
   printf( "OK\r\n" );

   status = ICM20648_getDeviceID( &devid );
   if( status != ICM20648_OK ) {
      goto cleanup;
   }
   printf( "IMU device ID = %02Xh\r\n", devid );

   /* Gyro calibration */
   IMU_state = IMU_STATE_CALIBRATING;
   printf( "IMU gyro calibration..." );
   status = ICM20648_gyroCalibrate( gyroBiasScaled );
   if( status != ICM20648_OK ) {
      printf( "Failed! status = %08Xh\r\n", status );
      goto cleanup;
   }
   printf( "OK\r\n" );

   printf( "Gyroscope bias (deg/s)  : " );
   printf( "gx: % 6.4f, gy: % 6.4f, gz: % 6.4f\r\n", gyroBiasScaled[0], gyroBiasScaled[1], gyroBiasScaled[2] );

   IMU_state = IMU_STATE_INITIALIZING;

cleanup:

   if( status != ICM20648_OK ){
      ICM20648_deInit();
      IMU_state = IMU_STATE_DISABLED;
   }

   return status;

}

/***************************************************************************/
uint32_t IMU_deInit( void )
{

   uint32_t status;

   IMU_state = IMU_STATE_DISABLED;
   status    = ICM20648_deInit();

   return status;

}

/***************************************************************************/
uint8_t IMU_getState( void )
{
   return IMU_state;
}

/***************************************************************************/
void IMU_config( float sampleRate )
{

   uint32_t itStatus;

   /* Set IMU state */
   IMU_state = IMU_STATE_INITIALIZING;

   /* Register PIC interrupt callback */
   BOARD_imuEnableIRQ( true );
   GPIOINT_CallbackRegister( BOARD_PIC_INT_WAKE_PIN, gpioInterrupt );

   /* Clear the interrupts */
   BOARD_picWriteReg( BOARD_PIC_REG_INT_CLEAR, 0 );
   ICM20648_interruptStatusRead( &itStatus );

   /* Enable accel sensor */
   ICM20648_sensorEnable( true, true, false );

   /* Set sample rate */
   gyroSampleRate  = ICM20648_gyroSampleRateSet( sampleRate );
   accelSampleRate = ICM20648_accelSampleRateSet( sampleRate );

   printf( "IMU sample rate set to %f Hz (accel), %f Hz (gyro)\r\n", accelSampleRate, gyroSampleRate );

   /* Filter bandwidth: 12kHz, otherwise the results may be off */
   ICM20648_accelBandwidthSet( ICM20648_ACCEL_BW_1210HZ );
   ICM20648_gyroBandwidthSet( ICM20648_GYRO_BW_51HZ );

   /* Accel: 2G full scale */
   ICM20648_accelFullscaleSet( ICM20648_ACCEL_FULLSCALE_2G );

   /* Gyro: 250 degrees per sec full scale */
   ICM20648_gyroFullscaleSet( ICM20648_GYRO_FULLSCALE_250DPS );

   UTIL_delay( 50 );

   /* Enable the raw data ready interrupt */
   ICM20648_interruptEnable( true, false );

   /* Enter low power mode */
   ICM20648_lowPowerModeEnter( true, true, false );

   /* Clear the interrupts */
   BOARD_picWriteReg( BOARD_PIC_REG_INT_CLEAR, 0 );
   ICM20648_interruptStatusRead( &itStatus );

   /* IMU fuse config & setup */
   IMU_fuseAccelerometerSetSampleRate( &fuseObj, accelSampleRate );
   IMU_fuseGyroSetSampleRate( &fuseObj, gyroSampleRate );
   IMU_fuseReset( &fuseObj );

   IMU_state = IMU_STATE_READY;

   return;

}

/***************************************************************************/
void IMU_accelerationGet( int16_t avec[3] )
{

   if( fuseObj.aAccumulatorCount > 0 ) {
      avec[0] = (int16_t) ( 1000.0f * fuseObj.aAccumulator[0] / fuseObj.aAccumulatorCount );
      avec[1] = (int16_t) ( 1000.0f * fuseObj.aAccumulator[1] / fuseObj.aAccumulatorCount );
      avec[2] = (int16_t) ( 1000.0f * fuseObj.aAccumulator[2] / fuseObj.aAccumulatorCount );
      fuseObj.aAccumulator[0] = 0;
      fuseObj.aAccumulator[1] = 0;
      fuseObj.aAccumulator[2] = 0;
      fuseObj.aAccumulatorCount = 0;
   }
   else {
      avec[0] = 0;
      avec[1] = 0;
      avec[2] = 0;
   }
   return;

}

/***************************************************************************/
void IMU_orientationGet( int16_t ovec[3] )
{

   ovec[0] = (int16_t) ( 100.0f * IMU_RAD_TO_DEG_FACTOR * fuseObj.orientation[0] );
   ovec[1] = (int16_t) ( 100.0f * IMU_RAD_TO_DEG_FACTOR * fuseObj.orientation[1] );
   ovec[2] = (int16_t) ( 100.0f * IMU_RAD_TO_DEG_FACTOR * fuseObj.orientation[2] );

   return;

}

/***************************************************************************/
void IMU_gyroGet( int16_t gvec[3] )
{

   gvec[0] = (int16_t) ( 100.0f * fuseObj.gVector[0] );
   gvec[1] = (int16_t) ( 100.0f * fuseObj.gVector[1] );
   gvec[2] = (int16_t) ( 100.0f * fuseObj.gVector[2] );

   return;

}

/***************************************************************************/
uint32_t IMU_gyroCalibrate( void )
{

   uint32_t status;

   status = IMU_OK;

   /* Disable interrupt */
   ICM20648_interruptEnable( false, false );

   IMU_deInit();
   status = IMU_init();

   /* Restart regular sampling */
   IMU_config( gyroSampleRate );

   return status;

}

/***************************************************************************/
void IMU_update( void )
{

   IMU_fuseUpdate( &fuseObj );

   return;

}

/***************************************************************************/
void IMU_reset( void )
{

   IMU_fuseReset( &fuseObj );

   return;

}

/***************************************************************************/
void IMU_getAccelerometerData( float avec[3] )
{

   if( IMU_state != IMU_STATE_READY ){
      avec[0] = 0;
      avec[1] = 0;
      avec[2] = 0;
      return;
   }

   ICM20648_accelDataRead( avec );

   return;

}

/***************************************************************************/
void IMU_getGyroCorrectionAngles( float acorr[3] )
{

   acorr[0] = fuseObj.angleCorrection[0];
   acorr[1] = fuseObj.angleCorrection[1];
   acorr[2] = fuseObj.angleCorrection[2];

   return;

}

/***************************************************************************/
void IMU_getGyroData( float gvec[3] )
{

   if( IMU_state != IMU_STATE_READY ){
      gvec[0] = 0;
      gvec[1] = 0;
      gvec[2] = 0;
      return;
   }

   ICM20648_gyroDataRead( gvec );

   return;

}

/***************************************************************************/
bool IMU_isDataReady( void )
{

   bool ready;

   if( IMU_state != IMU_STATE_READY ){
      return false;
   }

   ready = ICM20648_isDataReady();
   IMU_isDataReadyQueryCount++;

   if( ready ) {
      IMU_isDataReadyTrueCount++;
   }

   return ready;

}

/***************************************************************************/
bool IMU_isDataReadyFlag( void )
{

   bool ready;
   unsigned int pin;

   if( IMU_state != IMU_STATE_READY ){
      return false;
   }

   ready = dataReady;

   if( BOARD_picIsLegacyIntCtrl() ) {

      pin = GPIO_PinInGet( BOARD_PIC_INT_WAKE_PORT, BOARD_PIC_INT_WAKE_PIN );
      if( pin == 0 ) {
         ready = true;
      }
   }

   return ready;

}

/***************************************************************************/
void IMU_clearDataReadyFlag( void )
{

   dataReady = false;

   if( BOARD_picIsLegacyIntCtrl() ) {
      BOARD_picWriteReg( BOARD_PIC_REG_INT_CLEAR, 0 );
   }

   return;

}

/***************************************************************************/
static void gpioInterrupt( uint8_t pin )
{

   dataReady = true;
   IMU_interruptCount++;

#ifdef RADIO_BLE
   gecko_external_signal( 1 );
#endif

   return;

}