MPU6050 + QMC5883L + MadgwickAHRS filter frequency problem

I’ve managed to fuse the data from MPU’s Acc/Gyro and QMC Magnetometer using Madgwick filter. Basicly what the code does in main loop is gathering values from Acc, Gyro and Mag. Then making data fusion with MadgwickAHRS, calculating yaw, pitch, roll angles and sending them through serial port with 2000000 baud rate. I’m using “Visualiser” code for Processing from MadgwickAHRS library directory to see the results.

As You may now, to get the rotation angle from gyro we need to integrate gyro readings. And I think that is my problem. For now I measured the void loop() cycle whith micros() function and it takes ~8621 microseconds. So the loop frequency is about 128Hz. So I set the filter frequency at 128Hz ( " filter.begin(128); ") and I get very accurate results. But I don’t think that is the correct solution. Because if I would change the baud rate or add a few lines of code, the loop frequency would change.
I tried to execute the filter.update() function, for example, every 10000 microseconds and set the filter frequency to 100Hz, but that’s not working the way i expected.

Any tips how to make this code more reliable?

btw. the board I’m using is Arduino UNO r.3

#include <MadgwickAHRS.h>
#include<Wire.h>

const int MPU_addr = 0x68, QMC_addr = 0x0D; // I2C address of the MPU-6050 and QMC
float AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ, MaX, MaY, MaZ ;
long GyX_cal = 0, GyY_cal = 0, GyZ_cal = 0, AcX_cal = 0, AcY_cal = 0;
unsigned long loop_timer=0, interval = 0;
Madgwick filter;
float calibrated_values[3];
float uncalibrated_values[3];
float roll = 0, pitch = 0, heading = 0;
void setup()
{
  
  Wire.begin();
  Serial.begin(2000000);
  MPU_init();
  QMC_init();
  Gyro_cal();
  filter.begin(128);
 loop_timer = micros(); 
}
void loop()
{

//  MPU reading values
  MPU_read();
  
//  Gyroscope offset / calibration
  GyX -= GyX_cal;
  GyY -= GyY_cal;
  GyZ -= GyZ_cal;

// Accelerometer offset / calibration

  AcX -= AcX_cal;
  AcY -= AcY_cal;
  AcZ -= 0;
  

  GyX *=0.015267;  
  GyY *=0.015267;
  GyZ *=0.015267;


  QMC_read();
  
  uncalibrated_values[0]= MaX;
  uncalibrated_values[1]= MaY;
  uncalibrated_values[2]= MaZ;
  transformation(uncalibrated_values);
  MaX = calibrated_values[0] ;
   MaY = calibrated_values[1] ;
  MaZ = calibrated_values[2] ;


    filter.update(GyX, GyY, GyZ, AcX, AcY, AcZ, MaX, MaY, MaZ);

  
    
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();
    

    Serial.print("Orientation: ");
    Serial.print(heading);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);
//    Serial.println(micros()-loop_timer);

  loop_timer = micros(); 
}





void MPU_init()
{
//Activate the MPU-6050
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
//Configure the accelerometer (+/-8g)
  Wire.beginTransmission(MPU_addr); 
  Wire.write(0x1C);
  Wire.write(0x08);
  Wire.endTransmission(true);
//Configure the gyro (500dps full scale)
  Wire.beginTransmission(MPU_addr);  
  Wire.write(0x1B);  
  Wire.write(0x08);   
  Wire.endTransmission(true);
}


void QMC_init()
{
  Wire.beginTransmission(QMC_addr); //start talking
  Wire.write(0x0B); // Tell the HMC5883 to Continuously Measure
  Wire.write(0x01); // value
  Wire.endTransmission();
  Wire.beginTransmission(QMC_addr); //start talking
  Wire.write(0x09); // Tell the HMC5883 to Continuously Measure
  Wire.write(0x1D); // value
  Wire.endTransmission();
  
}



void MPU_read()
{ //Subroutine for reading the raw gyro and accelerometer data
   Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  Wire.endTransmission(); 
}

void QMC_read()
{
  Wire.beginTransmission(QMC_addr); 
  Wire.write(0x00); 
  Wire.endTransmission();
  Wire.requestFrom(QMC_addr, 14); 
  while (Wire.available() < 3); 
  MaX = (float)(Wire.read() | Wire.read() <<8);
  MaY = (float)(Wire.read() | Wire.read() << 8);
  MaZ = (float)(Wire.read() | Wire.read() << 8);
  Wire.endTransmission();
}



void Gyro_cal()
{
  Serial.println("Wait. Calibrating IMU.");
  for (int cal_int = 0; cal_int < 2000 ; cal_int ++)    //Run this code 2000 times
  {
    MPU_read();
    GyX_cal += GyX;
    GyY_cal += GyY; 
    GyZ_cal += GyZ;

    AcX_cal += AcX;
    AcY_cal += AcY;
    delay(3);
  }
  GyX_cal /= 2000;
  GyY_cal /= 2000;
  GyZ_cal /= 2000;
  AcX_cal /= 2000;
  AcY_cal /= 2000; 
  Serial.println("Calibration done!");
}


void transformation(float uncalibrated_values[3])    
{
  double calibration_matrix[3][3] = 
  {
 
    {1.169, -0.007, 0.147},
    {-0.041, 1.167, 0.085},
    {-0.028, -0.001, 1.398}  
  };

  double bias[3] = 
  {
    -563.145,
    -80.378,
    -31.331
  };  
  //calculation
  for (int i=0; i<3; ++i) uncalibrated_values[i] = uncalibrated_values[i] - bias[i];
  float result[3] = {0, 0, 0};
  for (int i=0; i<3; ++i)
    for (int j=0; j<3; ++j)
      result[i] += calibration_matrix[i][j] * uncalibrated_values[j];
  for (int i=0; i<3; ++i) calibrated_values[i] = result[i];
}

Any tips how to make this code more reliable?

Use modern, more reliable and much better code. The best open source AHRS code is state of the art RTIMULib. For one implementation, see this post: https://forum.pololu.com/t/state-of-the-art-ahrs-for-25/8091

The original author is not longer supporting RTIMUlib actively (and as far as I know, it seems to be bug-free) but others have posted branches. It may not support your compass, but that is not a major problem.