Using I2C inside timer interrupt to read IMU sensor with kalman filter problem.

Hello. My system uses an Arduino micro to read and measure the data from an MPU6050 sensor and FSR sensors and send it through Bluetooth to the mainboard. Without using the timer interrupt the code was working fine, but I need the interrupt to send the data with the same frequency as the mainboard. This is my code:

#include <Wire.h>
#include <Kalman.h>
#include<movingAvg.h>

#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

movingAvg mySensor0(10);
movingAvg mySensor1(10);
movingAvg mySensor2(10);
movingAvg mySensor3(10);

movingAvg myF0(10);
movingAvg myF1(10);
movingAvg myF2(10);
movingAvg myF3(10);



int SensorPin0 = A0; //analog pin 0
int SensorPin1 = A1;
int SensorPin2 = A2;
int SensorPin3 = A3;
int MeanDataforce = 0;
int SensorReading0 = 0;
int SensorReading1 = 0;
int SensorReading2 = 0;
int SensorReading3 = 0;
float F0 = 0;
float F1 = 0;
float F2 = 0;
float F3 = 0;
char F_sign =0;
uint8_t F_h, F_l;

char pAngle_sign = 0;
uint8_t pAngle_h, pAngle_l;

/* IMU Data */
int16_t accX, accY, accZ, tempRaw, gyroX, gyroY, gyroZ;
double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle, gyroZangle; // Angle calculate using the gyro only
double compAngleX, compAngleY, compAngleZ; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
double CurX, CurY, CurZ;
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

void PedalAngleValue(void);
void arduino2arduino(float CurX);
void BluetoothConnection(void);

int state = 0;

void setupTimer0() {
  noInterrupts();
  // Clear registers
  TCCR0A = 0;
  TCCR0B = 0;
  TCNT0 = 0;

  // 100.16025641025641 Hz (16000000/((155+1)*1024))
  OCR0A = 155;
  // CTC
  TCCR0A |= (1 << WGM01);
  // Prescaler 1024
  TCCR0B |= (1 << CS02) | (1 << CS00);
  // Output Compare Match A Interrupt Enable
  TIMSK0 |= (1 << OCIE0A);
  interrupts();
}

void setup() {
  // initialize digital pin 8 as an output.

  Serial.begin(115200);
  Serial1.begin(115200);

  

setupTimer0();
  
  mySensor0.begin();
  mySensor1.begin();
  mySensor2.begin();
  mySensor3.begin();

  myF0.begin();
  myF1.begin();
  myF2.begin();
  myF3.begin();
  
/*MPU setup*/
  Wire.begin();
#if ARDUINO >= 157
  Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
  TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);

  // atan2 outputs the value of -¥ð to ¥ð (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2¥ð and then from radians to degrees
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;

  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);

  timer = micros();

}

ISR(TIMER0_COMPA_vect) {

  
  CurX = kalAngleX - 270;

/* Update all the IMU values */

  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]);
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);


  // atan2 outputs the value of -¥ð to ¥ð (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2¥ð and then from radians to degrees
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;

  double gyroXrate = (double)gyroX / 131.0;


  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000); // Calculate the angle using a Kalman filter

  timer = micros();



/*FORCE SENSOR*/
    int SensorReading0 = analogRead(SensorPin0);
    int SensorReading1 = analogRead(SensorPin1);
    int SensorReading2 = analogRead(SensorPin2);
    int SensorReading3 = analogRead(SensorPin3);

    int avg0 = mySensor0.reading(SensorReading0);
    int avg1 = mySensor1.reading(SensorReading1);
    int avg2 = mySensor2.reading(SensorReading2);
    int avg3 = mySensor3.reading(SensorReading3);
    
  int F0 = 0.003462*exp(0.0195*avg0); //GOOD
  int F1 = 0.00065*exp(0.01889*avg1); //good
  int F2 = 0.003954*exp(0.01884*avg2); //GOOD
  int F3 = 0.000777*exp(0.02288*avg3); //good

    int Favg0 = myF0.reading(F0);
    int Favg1 = myF1.reading(F1);
    int Favg2 = myF2.reading(F2);
    int Favg3 = myF3.reading(F3);
    int MeanDataforce = Favg0 + Favg1 + Favg2 + Favg3;



  F_h = (int) (MeanDataforce);


pAngle_sign = '+';

if (CurX < 0)
  {
    CurX = -CurX;
    pAngle_sign = '-';
  }
  pAngle_h = (int) (CurX);


//  Serial1.write(200);
//  Serial1.write(pAngle_h);
//
//  Serial1.write(F_h);
//  Serial1.write(201);
  
  Serial.print(pAngle_h);
  Serial.print(" ");
  Serial.print(pAngle_sign);
  Serial.print(" ");
  Serial.println(F_h);
//
//
//  Serial.print(" ");
//  Serial.print(F0);
//  Serial.print(" ");
//  Serial.print(F1);
//  Serial.print(" ");
//  Serial.print(F2);
//  Serial.print(" ");
//  Serial.println(F3);


  
}

void loop() {

}

When I try to compile it “uploads” but right after that the USB becomes unknown and I can’t read any data nor upload again another code until I reset the board. I was able to find that the problem is between the timer interrupt and the I2C because if I remove the I2C parts from inside the interrupt the code compiles just fine and I can read the force output data. I’m not so familiar with I2C and with timer interrupts, so I can’t figure out how to fix this problem. If anyone has any idea of what I need to change please let me know. Thank you

Question: I2C using Wire Library during interrupt service routine #60

rtek1000:
Question: I2C using Wire Library during interrupt service routine #60

Thank you, but that didn't work.

Are you using this code ? KalmanFilter/MPU6050.ino at master · TKJElectronics/KalmanFilter · GitHub.
The i2cRead() function has a timeout after the Wire.requestFrom(). I told him over and over again that it is nonsense-code, but he does not fix it. I even tried to be annoying, but that didn't help either.

Inside the i2cRead() is also a micros() call. So you are using Serial functions, float calculations, while-loops and micros() calls in a interrupt. You should do all that code in the loop().
A millis() timer of 100Hz should work.
Start with the Blink Without Delay: https://www.arduino.cc/en/tutorial/BlinkWithoutDelay.

If you want to be as close to 100Hz as possible and stay in sync with the 100Hz, then make a interval of 10 ms and do this:

// previousMillis = currentMillis; // delay by sketch will delay the 10ms
previousMillis += interval; // keep in sync with the time

The "Kalman filter" code from TKJElectrinics doesn't work either, but he is proud of it!

Complete waste of everyone's time.

Koepel:
Are you using this code ? https://github.com/TKJElectronics/KalmanFilter/blob/master/examples/MPU6050/MPU6050.ino.
The i2cRead() function has a timeout after the Wire.requestFrom(). I told him over and over again that it is nonsense-code, but he does not fix it. I even tried to be annoying, but that didn’t help either.

Inside the i2cRead() is also a micros() call. So you are using Serial functions, float calculations, while-loops and micros() calls in a interrupt. You should do all that code in the loop().
A millis() timer of 100Hz should work.
Start with the Blink Without Delay: https://www.arduino.cc/en/tutorial/BlinkWithoutDelay.

If you want to be as close to 100Hz as possible and stay in sync with the 100Hz, then make a interval of 10 ms and do this:

// previousMillis = currentMillis; // delay by sketch will delay the 10ms

previousMillis += interval; // keep in sync with the time

Yes, I’m using that code. Didn’t know it was so nonsense since it was working just fine for my system, but thank you for that info.
So, if I understood you right, I removed the timer interrupt, added everything to the loop again and used the time interval to make the loop work at 100Hz. It works now, but how can I be sure that it is really close to 100Hz? Can you just check the code to see if I did everything ok?
And thank you so much for the help!!

#include <Wire.h>
#include <Kalman.h>
#include<movingAvg.h>

#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

movingAvg mySensor0(10);
movingAvg mySensor1(10);
movingAvg mySensor2(10);
movingAvg mySensor3(10);

movingAvg myF0(10);
movingAvg myF1(10);
movingAvg myF2(10);
movingAvg myF3(10);



int SensorPin0 = A0; //analog pin 0
int SensorPin1 = A1;
int SensorPin2 = A2;
int SensorPin3 = A3;
int MeanDataforce = 0;
int SensorReading0 = 0;
int SensorReading1 = 0;
int SensorReading2 = 0;
int SensorReading3 = 0;
float F0 = 0;
float F1 = 0;
float F2 = 0;
float F3 = 0;
char F_sign =0;
uint8_t F_h, F_l;

char pAngle_sign = 0;
uint8_t pAngle_h, pAngle_l;

/* IMU Data */
int16_t accX, accY, accZ, tempRaw, gyroX, gyroY, gyroZ;
double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle, gyroZangle; // Angle calculate using the gyro only
double compAngleX, compAngleY, compAngleZ; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
double CurX, CurY, CurZ;
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

void PedalAngleValue(void);
void arduino2arduino(float CurX);
void BluetoothConnection(void);

int state = 0;

unsigned long previousMillis = 0; 
const long interval = 10;     


void setup() {
  // initialize digital pin 8 as an output.

  Serial.begin(115200);
  Serial1.begin(115200);

  
  mySensor0.begin();
  mySensor1.begin();
  mySensor2.begin();
  mySensor3.begin();

  myF0.begin();
  myF1.begin();
  myF2.begin();
  myF3.begin();
  
/*MPU setup*/
  Wire.begin();
#if ARDUINO >= 157
  Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
  TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);

  // atan2 outputs the value of -¥ð to ¥ð (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2¥ð and then from radians to degrees
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;

  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);

  timer = micros();

}


void loop() {



  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {
    
previousMillis += interval; // keep in sync with the time

  CurX = kalAngleX - 270;

/* Update all the IMU values */

  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]);
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);


  // atan2 outputs the value of -¥ð to ¥ð (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2¥ð and then from radians to degrees
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;

  double gyroXrate = (double)gyroX / 131.0;


  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000); // Calculate the angle using a Kalman filter

  timer = micros();

  /*FORCE SENSOR*/
    int SensorReading0 = analogRead(SensorPin0);
    int SensorReading1 = analogRead(SensorPin1);
    int SensorReading2 = analogRead(SensorPin2);
    int SensorReading3 = analogRead(SensorPin3);

    int avg0 = mySensor0.reading(SensorReading0);
    int avg1 = mySensor1.reading(SensorReading1);
    int avg2 = mySensor2.reading(SensorReading2);
    int avg3 = mySensor3.reading(SensorReading3);
    
  int F0 = 0.003462*exp(0.0195*avg0); //GOOD
  int F1 = 0.00065*exp(0.01889*avg1); //good
  int F2 = 0.003954*exp(0.01884*avg2); //GOOD
  int F3 = 0.000777*exp(0.02288*avg3); //good

    int Favg0 = myF0.reading(F0);
    int Favg1 = myF1.reading(F1);
    int Favg2 = myF2.reading(F2);
    int Favg3 = myF3.reading(F3);
    int MeanDataforce = Favg0 + Favg1 + Favg2 + Favg3;



  F_h = (int) (MeanDataforce);


pAngle_sign = '+';

if (CurX < 0)
  {
    CurX = -CurX;
    pAngle_sign = '-';
  }
  pAngle_h = (int) (CurX);


//  Serial1.write(200);
//  Serial1.write(pAngle_h);
//
//  Serial1.write(F_h);
//  Serial1.write(201);
  
  Serial.print(pAngle_h);
  Serial.print(" ");
  Serial.print(pAngle_sign);
  Serial.print(" ");
  Serial.println(F_h);
//
//
//  Serial.print(" ");
//  Serial.print(F0);
//  Serial.print(" ");
//  Serial.print(F1);
//  Serial.print(" ");
//  Serial.print(F2);
//  Serial.print(" ");
//  Serial.println(F3);

  }

}

jremington:
The “Kalman filter” code from TKJElectrinics doesn’t work either, but he is proud of it!

Complete waste of everyone’s time.

Since I only use it to see the X-axis angulation I thought it was showing a really good output. Do you maybe have another code that you consider good?

Yes it works. That nonsense code is a timeout without reason, and the while-loops in your sketch make no sense. If everything works, then it is no problem. When there is trouble on the I2C bus, that code will cause even more trouble for no good reason.
The filter that you use also “works”, unless someone uses the full 3 axis values and assumes that a Kalman filter is used, then the math turns out that it is something else.

There is a AHRS filter: MPU-9250 Hookup Guide - learn.sparkfun.com.

Did you connect the MPU-6050 to a 5V Arduino board ? Then you have a Arduino board with a 5V I2C bus and a sensor with a 3.3V I2C bus. The MPU-6050 might work when you have pullup resistors of 4k7 to 3.3V, but newer sensors don’t like a 5V I2C bus on their SDA and SCL pins.

The millis() is interrupt driven, but that interrupt is not exactly 1000 Hz, so the millis() value is corrected sometimes. With that correction the millis() is as accurate as the 16 MHz crystal or resonator of the Arduino board in the long run.
Because of that correction, your sketch has sometimes 11 ms instead of 10 ms.

It is possible to use micros() instead of millis() for the 10 ms.

The MPU-6050 has an internal processing unit, called ‘dmp’. When you upload dmp firmware, then the sensor itself can sample with 100 Hz and put the data in a FIFO buffer and signal the Arduino with an interrupt. The Arduino sets a flag in the interrupt and in the loop the Arduino checks that flag and reads the FIFO buffer from the sensor.

Since I only use it to see the X-axis angulation I thought it was showing a really good output. Do you maybe have another code that you consider good?

The code does something, but it is not a Kalman filter, and does not work nearly as well as the correct code (which is quite a bit more complicated).

For a single variable, a weighted moving average (also called an exponential filter) is equivalent to a Kalman filter.