MPU6050 DMP rpi4 crashed

[AbdelmalekMatoug]

Dear Community,

I am encountering an issue with the MPU6050 when attempting to use its Digital Motion Processor (DMP) on a Raspberry Pi 4. Every time I try to enable or use the DMP, the Raspberry Pi crashes. I have tried several different approaches and troubleshooting steps, but nothing has resolved the issue.

Has anyone else encountered this problem? Any suggestions or insights would be greatly appreciated.

Thank you in advance for your help!

#include <bcm2835.h>
#include <stdio.h>
#include <pthread.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#define INTERRUPT_PIN RPI_V2_GPIO_P1_16 // GPIO 23 (Pin 16 on RPi header)

// MPU6050 object
MPU6050 mpu(0x68);

// MPU6050 Interrupt Variables
bool dmpReady = false;
uint16_t packetSize;
volatile bool newDataAvailable = false;

// Yaw, Pitch, Roll storage
float ypr[3];

// Function to setup the interrupt
void setupInterrupt() {
if (!bcm2835_init()) {
printf("BCM2835 initialization failed!\n");
exit(1);
}
printf("BCM2835 initialized successfully.\n");

bcm2835_gpio_fsel(INTERRUPT_PIN, BCM2835_GPIO_FSEL_INPT);
bcm2835_gpio_set_pud(INTERRUPT_PIN, BCM2835_GPIO_PUD_UP);
bcm2835_gpio_ren(INTERRUPT_PIN);
bcm2835_gpio_eds(INTERRUPT_PIN);
printf("Interrupt setup complete on GPIO pin %d.\n", INTERRUPT_PIN);

}

void interruptHandler() {
if (bcm2835_gpio_eds(INTERRUPT_PIN)) {
bcm2835_gpio_set_eds(INTERRUPT_PIN); // Clear interrupt flag
newDataAvailable = true; // Set flag
}
}

// Function to reset the I2C bus
void resetI2CBus() {
printf("Resetting I2C bus...\n");
bcm2835_i2c_end(); // End I2C communication
bcm2835_delay(100); // Short delay
I2Cdev::initialize("/dev/i2c-1"); // Reinitialize I2C
bcm2835_i2c_set_baudrate(100000); // Set I2C clock speed to 100 kHz
printf("I2C bus reset complete.\n");
}

// MPU6050 Setup
void setupMPU() {
I2Cdev::initialize("/dev/i2c-1");
bcm2835_i2c_set_baudrate(100000); // Set I2C clock speed to 100 kHz
printf("I2C initialized.\n");

mpu.initialize();
printf("MPU6050 initialized.\n");

if (!mpu.testConnection()) {
    printf("MPU6050 connection failed! Resetting I2C bus and reinitializing MPU6050...\n");
    resetI2CBus();
    mpu.initialize();
    if (!mpu.testConnection()) {
        printf("MPU6050 reinitialization failed! Exiting...\n");
        exit(1);
    }
} else {
    printf("MPU6050 connection successful.\n");
}

// Initialize DMP
int devStatus = mpu.dmpInitialize();
if (devStatus == 0) {
    mpu.setDMPEnabled(true);
    packetSize = mpu.dmpGetFIFOPacketSize();
    dmpReady = true;
    printf("DMP Initialized and enabled. Packet size: %d\n", packetSize);
} else {
    printf("DMP Initialization failed (code %d)\n", devStatus);
    exit(1);
}

// Calibrate MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
printf("MPU6050 calibration complete.\n");

// Enable Interrupts
setupInterrupt();

}

// Thread for FIFO reads
void* fifoReadThread(void* arg) {
while (1) {
if (newDataAvailable) {
newDataAvailable = false; // Reset flag
uint16_t fifoCount = mpu.getFIFOCount();
if (fifoCount >= packetSize) {
uint8_t fifoBuffer[packetSize];
mpu.getFIFOBytes(fifoBuffer, packetSize);

            // Process FIFO data
            Quaternion q;
            VectorFloat gravity;
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

            // Print Yaw, Pitch, Roll values
            printf("YPR: %7.2f %7.2f %7.2f\n",
                   ypr[0] * 180 / M_PI,
                   ypr[1] * 180 / M_PI,
                   ypr[2] * 180 / M_PI);
        }
    }
}
return NULL;

}

int main() {
// Initialize BCM2835 and MPU6050
if (!bcm2835_init()) {
printf("BCM2835 initialization failed!\n");
return 1;
}
setupMPU();

// Create a thread for FIFO reads
pthread_t fifoThread;
pthread_create(&fifoThread, NULL, fifoReadThread, NULL);

// Main loop
while (1) {
    // Do other tasks or sleep
    bcm2835_delay(100);
}

// Cleanup
pthread_join(fifoThread, NULL);
bcm2835_close();
return 0;

}