3

I was trying to get the data from MPU6050 using the Arduino library mentioned here.

The following code is an edited version of the sample code given in the library folder which seems to be working fine.

#define printfloatx(Name,Variable,Spaces,Precision,EndTxt) { Serial.print(F(Name)); {char S[(Spaces + Precision + 3)];Serial.print(F(" ")); Serial.print(dtostrf((float)Variable,Spaces,Precision ,S));}Serial.print(F(EndTxt)); }//Name,Variable,Spaces,Precision,EndTxt

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps_V6_12.h"

#include "Wire.h"

MPU6050 accelgyro;
MPU6050 mpu;


I2Cdev a;

/*
    IMU Zero variables
*/

int16_t data_acc[3];
int16_t data_Gyro[3];

/*
   IMU initialization variables
*/
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
bool dmpReady = false;  // set true if DMP init was successful
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)

/*
   IMU angle finding variables
*/
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector



void Initialize()
{
  // initialize device
  Serial.println("Initializing I2C devices...");
  accelgyro.initialize();

  // verify connection
  Serial.println("Testing device connections...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  Serial.println("tuning Each Dot = 100 readings");
  accelgyro.CalibrateAccel(10);
  accelgyro.CalibrateGyro(10);

  a.readWords(0x68, 0x06, 3, (uint16_t *)data_acc);
  a.readWords(0x68, 0x13, 3, (uint16_t *)data_Gyro);
} // Initialize




void setup() {
  Wire.begin();
  Serial.begin(115200);
  Initialize();
  Wire.setClock(400000);
  while (!Serial);
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  Serial.println(F("\nSend any character to begin DMP programming and demo: "));
  while (Serial.available() && Serial.read()); // empty buffer
  while (!Serial.available());                 // wait for data
  while (Serial.available() && Serial.read()); // empty buffer again

  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(data_Gyro[0]);
  mpu.setYGyroOffset(data_Gyro[1]);
  mpu.setZGyroOffset(data_Gyro[2]);
  mpu.setXAccelOffset(data_acc[0]);
  mpu.setYAccelOffset(data_acc[1]);
  mpu.setZAccelOffset(data_acc[2]);

  if (devStatus == 0) {
    // Calibration Time: generate offsets and calibrate our MPU6050
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    Serial.println();
    mpu.PrintActiveOffsets();
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
    mpuIntStatus = mpu.getIntStatus();
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

  Serial.println("-------------- done --------------");
} // setup

void loop()
{
  if (!dmpReady) return;
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    Serial.print("ypr\t");
    Serial.print(ypr[0] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(ypr[1] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(ypr[2] * 180 / M_PI);
    Serial.println();
  }

}

Then I have activated the input capture pin to use with the interrupt and the code is as follows

#define printfloatx(Name,Variable,Spaces,Precision,EndTxt) { Serial.print(F(Name)); {char S[(Spaces + Precision + 3)];Serial.print(F(" ")); Serial.print(dtostrf((float)Variable,Spaces,Precision ,S));}Serial.print(F(EndTxt)); }//Name,Variable,Spaces,Precision,EndTxt

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps_V6_12.h"

#include "Wire.h"

MPU6050 accelgyro;
volatile MPU6050 mpu;


I2Cdev a;

/*
    IMU Zero variables
*/

int16_t data_acc[3];
int16_t data_Gyro[3];

/*
   IMU initialization variables
*/
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
bool dmpReady = false;  // set true if DMP init was successful
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)

/*
   IMU angle finding variables
*/
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
volatile float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector



void Initialize()
{
  // initialize device
  Serial.println("Initializing I2C devices...");
  accelgyro.initialize();

  // verify connection
  Serial.println("Testing device connections...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  Serial.println("tuning Each Dot = 100 readings");
  accelgyro.CalibrateAccel(10);
  accelgyro.CalibrateGyro(10);

  a.readWords(0x68, 0x06, 3, (uint16_t *)data_acc);
  a.readWords(0x68, 0x13, 3, (uint16_t *)data_Gyro);
} // Initialize

void timer4_initialization() {
  pinMode(49, INPUT);
  TCCR4A = 0; TCCR4B = 0; TCCR4C = 0;
  TCCR4B = B11000000;
  TIFR4 = _BV(ICF4);
  TIMSK4 = B00100000;
}

ISR(TIMER4_CAPT_vect) {
  if (!dmpReady) return;
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  }
}



void setup() {
  Wire.begin();
  Serial.begin(115200);
  Initialize();
  Wire.setClock(400000);
  while (!Serial);
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  Serial.println(F("\nSend any character to begin DMP programming and demo: "));
  while (Serial.available() && Serial.read()); // empty buffer
  while (!Serial.available());                 // wait for data
  while (Serial.available() && Serial.read()); // empty buffer again

  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(data_Gyro[0]);
  mpu.setYGyroOffset(data_Gyro[1]);
  mpu.setZGyroOffset(data_Gyro[2]);
  mpu.setXAccelOffset(data_acc[0]);
  mpu.setYAccelOffset(data_acc[1]);
  mpu.setZAccelOffset(data_acc[2]);

  if (devStatus == 0) {
    // Calibration Time: generate offsets and calibrate our MPU6050
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    Serial.println();
    mpu.PrintActiveOffsets();
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
    mpuIntStatus = mpu.getIntStatus();
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

  Serial.println("-------------- done --------------");

  delay(2000);

  timer4_initialization();

} // setup

void loop()
{
  Serial.print("ypr\t");
  Serial.print(ypr[0] * 180 / M_PI);
  Serial.print("\t");
  Serial.print(ypr[1] * 180 / M_PI);
  Serial.print("\t");
  Serial.print(ypr[2] * 180 / M_PI);
  Serial.println();
}


Here when I am using with the interrupt I am not getting anything printed, seems like the program is frozen somewhere in the interrupt -> function call.

What is the mistake I am making here.

How to take the measurement from MPU6050 using ISR routines.

1
  • are you saying that it does not even print Initializing I2C devices...?
    – jsotola
    Sep 6 '21 at 15:01
4

The functions mpu.dmpGetCurrentFIFOPacket(fifoBuffer), etc, are using I2C for getting the data. The I2C interface on the Uno and Mega (which you tagged your question with) is implemented in hardware and needs interrupts for handling the data bytes (taking the received byte out of the register or writing data to the register for sending).

By default interrupts are disabled inside of an ISR. Thus the mentioned functions cannot execute correctly. They are waiting for an interrupt, which is never going to happen, since the functions are executed while interrupts are disabled.

It is possible to reactivate interrupts in an ISR, but that is something, that you should only do in a few situations and only, when you know exactly, what you are doing.

Instead you should just set a volatile flag variable inside the ISR. In the main code (in loop()) you can then check for that flag, execute the I2C code when it is set and reset the flag afterwards. And you can then also just print, when there is new data instead of every loop iteration. So somewhat like this:

volatile byte timer_flag = 0;

ISR(TIMER4_CAPT_vect) {
  timer_flag = 1;
}

void loop(){
    if(timer_flag){
        if (!dmpReady) return;
        if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            // then print the values here, since we got new data from the MPU
        }
        timer_flag = 0; // Resetting the flag for the next timer interrupt
    }
}
0

I've tweaked chrisl's answer to show how to copy the ISR data and reset the ISR flag in an atomic manner.

volatile bool isr_timer_flag = false;
volatile uint32_t isr_counter = 0;  // Added to demonstrate how to copy a multibyte data type atomically.

ISR(TIMER4_CAPT_vect)
{
    isr_timer_flag = true;
    isr_counter++;
}

void loop()
{
    //
    // Get a copy of the data and reset the flag atomically by temporarily disabling interrupts.
    //

    // Disable interrupts by clearing the global interrupt flag.
    cli();

    // Copy the data.
    bool timer_flag = isr_timer_flag;
    uint32_t counter = isr_counter;

    // Reset the ISR flag for the next timer interrupt.
    // Resetting it here avoids clearing any re-flag during the following if statements.
    isr_timer_flag = false;

    // Enable interrupts by setting the global interrupt flag.
    sei();

    // Now use the local copy of the ISR flag.
    // The ISR flag could be re-flagged at any time during the remainder of the loop,
    // but a fresh copy will be obtained on the next iteration of the loop.

    if (timer_flag)
    {
        if (!dmpReady) return;
        if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
        {
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            // then print the values here, since we got new data from the MPU
        }
    }
}

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.