I have the code below but I want to use DMP because the drift is too much and I tried implementing complementary filter but the outcome did not seem convincing to me. Can someone help me use DMP without changing the settings like DLPF and interrupt. I want to keep majority on the code here and only implement the DMP to get more accurate values. It is also possible to implement a complementary filter. I implemented it but it did not work for me.
-
I will also like to keep the sample rate at 35.2 Hz.
-
I will like to also use interrupt when new data is available. I dont know if there is an interrupt for DMP, but would the interrupt for raw_data_available work as well?
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 = 1;
uint8_t csum;
uint8_t imu_id = 17;
uint32_t baudrate = 230400;
float x_accel;
float y_accel;
float z_accel;
float x_gyro;
float y_gyro;
float z_gyro;
#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();
// Serial.flush();
// while (Serial.available()){
// Serial.read(); // remove 1 character
// }
#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_Continuous);
ICM_20948_smplrt_t mySmplrt;
mySmplrt.a = 31; // specify the accel sample rate to maximum: see Table 19 in datasheet DS-000189-ICM-20948-v1.3.pdf - as
mySmplrt.g = 31; // 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 );
// // replies only when you receive data:
// while (!Serial.find("Start Sensor")) {}
/*
if( myICM.status != ICM_20948_Stat_Ok){
SERIAL_PORT.print(F("setDLPcfg returned: "));
SERIAL_PORT.println(myICM.statusString());
}
*/
//delay(5000);
// 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(false); // Latch the interrupt until cleared
myICM.intEnableRawDataReady(true); // enable interrupts on raw data ready
}
void loop() {
// replies only when you receive data:
// if (Serial.available() > 0) {
// // read the incoming byte:
// if (Serial.find("SEND ME SENSOR_ID")) {
// // say what you got:
// Serial.print("\n imu_id:="+String(imu_id) + " \n");
// Serial.end();
// }
// }
// Check for a trigger event
if( isrFired ){ // If our isr flag is set then clear the interrupts on the ICM
isrFired = false;
x_accel = (myICM.agmt.acc.axes.x/16384);
y_accel = (myICM.agmt.acc.axes.y/16384);
z_accel = (myICM.agmt.acc.axes.z/16384);
x_gyro = myICM.agmt.gyr.axes.x/131.0;
y_gyro = myICM.agmt.gyr.axes.y/131.0;
z_gyro = myICM.agmt.gyr.axes.z/131.0;
//getData(count);
}
}
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
}