This is my code below and actually with the settings I have implemented, I should achieve a sample rate of 560 Hz according to the data sheet, but I am only recording an update rate of only 320 Hz… I have spent weeks on fixing it but it is still not working. Could anyone tell me what I am doing wrong? Thank you in advance 
.
const int FIRMWARE_VERSION_MAJOR = 1;
const int FIRMWARE_VERSION_MINOR = 7;
// OLA Specifics:
const byte PIN_IMU_POWER = 27; // The Red SparkFun version of the OLA (V10) uses pin 27
//const byte PIN_IMU_POWER = 22; // The Black SparkX version of the OLA (X04) uses pin 22
const byte PIN_IMU_INT = 37;
const byte PIN_IMU_CHIP_SELECT = 44;
const byte PIN_PWR_LED = 29;
const byte PIN_STAT_LED = 19;
const byte PIN_TRIGGER = 11;
#define OLA_IDENTIFIER 0x117
#define HARDWARE_VERSION_MAJOR 1
#define HARDWARE_VERSION_MINOR 0
uint8_t count = 0;
uint8_t csum;
uint8_t imu_id = 1;
unsigned long start = micros();
uint32_t baudrate = 115200;
#define MISO 6
#include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU
#define USE_SPI // Uncomment this to use SPI
#define SERIAL_PORT Serial
#define SPI_PORT SPI // Your desired SPI port. Used only when "USE_SPI" is defined
#define CS_PIN PIN_IMU_CHIP_SELECT // Which pin you connect CS to. Used only when "USE_SPI" is defined. OLA uses pin 44.
#define WIRE_PORT Wire // Your desired Wire port. Used when "USE_SPI" is not defined
#define AD0_VAL 1 // The value of the last bit of the I2C address.
// On the SparkFun 9DoF IMU breakout the default is 1, and when
// the ADR jumper is closed the value becomes 0
#ifdef USE_SPI
ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
#else
ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object
#endif
// Some vars to control or respond to interrupts
volatile bool isrFired = false;
volatile bool sensorSleep = false;
void setup() {
powerLEDOn(); // Turn the power LED on - if the hardware supports it
pinMode(PIN_STAT_LED, OUTPUT);
digitalWrite(PIN_STAT_LED, HIGH);
Serial.begin(baudrate); //Default for initial debug messages if necessary
Serial.println();
#ifdef USE_SPI
SPI_PORT.begin();
#else
WIRE_PORT.begin();
WIRE_PORT.setClock(400000);
#endif
enableCIPOpullUp(); // Enable CIPO pull-up on the OLA
pinMode(PIN_IMU_INT, INPUT_PULLUP); // Using a pullup b/c ICM-20948 Breakout board has an onboard pullup as well and we don't want them to compete
attachInterrupt(digitalPinToInterrupt(PIN_IMU_INT), icmISR, FALLING); // Set up a falling interrupt
//beginIMU();
pinMode(PIN_IMU_POWER, OUTPUT);
pinMode(PIN_IMU_CHIP_SELECT, OUTPUT);
digitalWrite(PIN_IMU_CHIP_SELECT, HIGH); //Be sure IMU is deselected
Serial.println("HELLO");
//Reset ICM by power cycling it
imuPowerOff();
delay(10);
imuPowerOn(); // Enable power for the OLA IMU
delay(100); // Wait for the IMU to power up
SERIAL_PORT.begin(baudrate);
while(!SERIAL_PORT){};
bool initialized = false;
while( !initialized ){
#ifdef USE_SPI
myICM.begin( CS_PIN, SPI_PORT );
#else
myICM.begin( WIRE_PORT, AD0_VAL );
#endif
if( myICM.status != ICM_20948_Stat_Ok ){
SERIAL_PORT.println( "Trying again..." );
delay(500);
}else{
initialized = true;
}
}
digitalWrite(PIN_STAT_LED, LOW); // Turn the STAT LED off now that everything is configured
// Here we are doing a SW reset to make sure the device starts in a known state
myICM.swReset( );
if( myICM.status != ICM_20948_Stat_Ok){
SERIAL_PORT.print(F("Software Reset returned: "));
SERIAL_PORT.println(myICM.statusString());
}
delay(250);
// Now wake the sensor up
myICM.sleep( sensorSleep );
myICM.lowPower( false );
// The next few configuration functions accept a bit-mask of sensors for which the settings should be applied.
// Set Gyro and Accelerometer to a particular sample mode
// options: ICM_20948_Sample_Mode_Continuous
// ICM_20948_Sample_Mode_Cycled
myICM.setSampleMode( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled );
ICM_20948_smplrt_t mySmplrt;
mySmplrt.a = 1; // specify the accel sample rate to maximum: see Table 19 in datasheet DS-000189-ICM-20948-v1.3.pdf - as
mySmplrt.g = 1; // specify the gyro sample rate to maximum: see Table 17 in datasheet DS-000189-ICM-20948-v1.3.pdf - as
myICM.setSampleRate( ICM_20948_Internal_Acc, mySmplrt );
myICM.setSampleRate( ICM_20948_Internal_Gyr, mySmplrt );
ICM_20948_dlpcfg_t myDLPcfg;
myDLPcfg.a = acc_d473bw_n499bw;
myDLPcfg.g = gyr_d361bw4_n376bw5;
myICM.setDLPFcfg( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myDLPcfg );
/*
if( myICM.status != ICM_20948_Stat_Ok){
SERIAL_PORT.print(F("setDLPcfg returned: "));
SERIAL_PORT.println(myICM.statusString());
}
*/
// Choose whether or not to use DLPF
// Here we're also showing another way to access the status values, and that it is OK to supply individual sensor masks to these functions
ICM_20948_Status_e accDLPEnableStat = myICM.enableDLPF( ICM_20948_Internal_Acc, true );
ICM_20948_Status_e gyrDLPEnableStat = myICM.enableDLPF( ICM_20948_Internal_Gyr, true );
myICM.cfgIntActiveLow(true); // Active low to be compatible with the breakout board's pullup resistor
myICM.cfgIntOpenDrain(false); // Push-pull, though open-drain would also work thanks to the pull-up resistors on the breakout
myICM.cfgIntLatch(true); // Latch the interrupt until cleared
myICM.intEnableRawDataReady(true); // enable interrupts on raw data ready
}
void loop() {
// Check for a trigger event
if( isrFired ){ // If our isr flag is set then clear the interrupts on the ICM
isrFired = false;
getData(count);
myICM.clearInterrupts();
}
// //Compute the time it took
// unsigned long endtime = nanos();
// Serial.println(endtime);
// Serial.println(start);
// double delta = 1000000/(double(endtime - start));
// start = micros();
// Serial.print("Sample rate = :");
//
// Serial.println(delta);
}
//Query each enabled sensor for its most recent data
void getData(uint8_t count)
{
//count++;
csum=0;
uint16_t header = (0xFFFF);
//Serial.write(0x0A);
//Serial.write(imu_id);
//Serial.write(count);
uint16_t imu_id_count = imu_id | ((uint16_t)count)<<8; //This combines the imu_id and the counter to one 2 Byte value
myICM.getAGMT(); //Update values
//csum = (imu_id + count);
uint16_t data[9]= {header, imu_id_count,(uint16_t)myICM.agmt.gyr.axes.x,(uint16_t)myICM.agmt.gyr.axes.y,(uint16_t)myICM.agmt.gyr.axes.z,(uint16_t)myICM.agmt.acc.axes.x,(uint16_t)myICM.agmt.acc.axes.y,(uint16_t)myICM.agmt.acc.axes.z, csum};
byte *buf;
buf = (byte *) data;
for (int i = 0; i < 18 ; i++)
{
csum += *(buf+i);
//Serial.println(buf[i]);
}
//Here the header is subtracted from csum... Because the header takes 2 Byte, we substract each byte value from csum
csum = csum - 255;
csum = csum - 255;
buf[16] = csum;
Serial.write(buf,18);
//Serial.println(buf[16]);
//Serial.println(csum);
//Serial.write(csum);
}
void imuPowerOn()
{
pinMode(PIN_IMU_POWER, OUTPUT);
digitalWrite(PIN_IMU_POWER, HIGH);
}
void imuPowerOff()
{
pinMode(PIN_IMU_POWER, OUTPUT);
digitalWrite(PIN_IMU_POWER, LOW);
}
void powerLEDOn()
{
#if(HARDWARE_VERSION_MAJOR >= 1)
pinMode(PIN_PWR_LED, OUTPUT);
digitalWrite(PIN_PWR_LED, HIGH); // Turn the Power LED on
#endif
}
bool enableCIPOpullUp()
{
//Add CIPO pull-up
ap3_err_t retval = AP3_OK;
am_hal_gpio_pincfg_t cipoPinCfg = AP3_GPIO_DEFAULT_PINCFG;
cipoPinCfg.uFuncSel = AM_HAL_PIN_6_M0MISO;
cipoPinCfg.eDriveStrength = AM_HAL_GPIO_PIN_DRIVESTRENGTH_12MA;
cipoPinCfg.eGPOutcfg = AM_HAL_GPIO_PIN_OUTCFG_PUSHPULL;
cipoPinCfg.uIOMnum = AP3_SPI_IOM;
cipoPinCfg.ePullup = AM_HAL_GPIO_PIN_PULLUP_1_5K;
padMode(MISO, cipoPinCfg, &retval);
return (retval == AP3_OK);
}
//Trigger Pin ISR
void icmISR( void ){
isrFired = true;
count++; // Can't use I2C within ISR on 328p, so just set a flag to know that data is available
}
void powerLEDOff()
{
#if(HARDWARE_VERSION_MAJOR >= 1)
pinMode(PIN_PWR_LED, OUTPUT);
digitalWrite(PIN_PWR_LED, LOW); // Turn the Power LED off
#endif
}
