Mahony calibration problem

andreaarcarisi · May 8, 2024, 4:30pm

Hi, recently after having performed and validated the calibration of my sensors I tried to use the Mahony filter to correctly acquire yaw pitch roll, only that I keep getting the tuning wrong because the data doesn't add up to me.

the magnetometer, accelerometer and gyroscope have already been calibrated and verified with specific codes (magnetic north heading and values around the axes for gyro and acc). now on this code I tried to change the Kp and Ki gains but nothing, the data is never consistent with the movements.

So I'm asking if I'm doing something wrong in the code and how to correctly tune the filter (what procedure to do, I had tried the Ziegler Nichols method but nothing works)

 #include <Wire.h>
 #include <math.h>

/*MPU6050*/
 #define MPU6050_ADDRESS 0x68
 #define MPU6050_REG_ACCEL_XOUT_H 0x3B
 #define MPU6050_REG_GYRO_XOUT_H 0x43
 float accBiasX = 0, accBiasY = 0, accBiasZ = 0;
 float gyroBiasX = 0, gyroBiasY = 0, gyroBiasZ = 0;

/*QMC5883L*/
 #define QMC5883L_ADDRESS 0x0D
 #define QMC5883L_REG_X_LSB 0x00
 #define QMC5883L_REG_Y_LSB 0x02
 #define QMC5883L_REG_Z_LSB 0x04
// Definizione dei valori di CAL_PARAM
 float CAL_PARAM[9] = {-0.0150011772491854,-0.0106985402435778,0.0355930495987404,0.610897253682805,0.582287592807394,0.592696731175137,0.0762073179374682,0.0361950160491302,0.0452707353405034};
 float h_calib[3]; // Vettore per i risultati della calibrazione


/*MAHONY*/
 #define Kp 30.0
 #define Ki 0.00
static float declination = 3.7;

 unsigned long now = 0, last = 0; //micros() timers
 float deltat = 0;  //loop time in seconds
 unsigned long now_ms, last_ms = 0; //millis() timers
 unsigned long print_ms = 100; //print every "print_ms" milliseconds
 static float q[4] = {1.0, 0.0, 0.0, 0.0};
 static float yaw, pitch, roll; //Euler angle output
 

void setup() {

 /*SET GLOBAL*/
  Wire.begin();
  Serial.begin(115200);
  pinMode(2, OUTPUT);

/*SET MPU6050*/
  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(0x6B); // Power Management register
  Wire.write(0); // Accendi il dispositivo (wakes up the MPU-6050)
  Wire.endTransmission(true);

/*SET QMC5883L*/
  Wire.beginTransmission(QMC5883L_ADDRESS);
  Wire.write(0x09); // Registri di controllo
  Wire.write(0x0D); // Modalità continuativa con 50Hz
  Wire.endTransmission();
  
/*CALIB MPU6050*/
  calibrateMPU6050();
}

void loop() {

/* DATA MPU6050*/
  int16_t accX, accY, accZ;
  int16_t gyroX, gyroY, gyroZ;

  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(MPU6050_REG_ACCEL_XOUT_H);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDRESS, 6);
  if (Wire.available() >= 6) {
    accX = Wire.read() << 8 | Wire.read();
    accY = Wire.read() << 8 | Wire.read();
    accZ = Wire.read() << 8 | Wire.read();
  }

  // Leggi i dati del giroscopio dal MPU6050
  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(MPU6050_REG_GYRO_XOUT_H);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDRESS, 6);
  if (Wire.available() >= 6) {
    gyroX = Wire.read() << 8 | Wire.read();
    gyroY = Wire.read() << 8 | Wire.read();
    gyroZ = Wire.read() << 8 | Wire.read();
  }

/*CALIB MPU6050*/
  float accX_calibrated = ((float)accX - accBiasX) / 16384.0 * 9.81;
  float accY_calibrated = ((float)accY - accBiasY) / 16384.0 * 9.81;
  float accZ_calibrated = ((float)accZ - accBiasZ) / 16384.0 * 9.81;
  float gyroX_calibrated = ((float)gyroX - gyroBiasX) / 131.0;
  float gyroY_calibrated = ((float)gyroY - gyroBiasY) / 131.0;
  float gyroZ_calibrated = ((float)gyroZ - gyroBiasZ) / 131.0;

/*DATA QMC5883L*/
  int16_t magX, magY, magZ;

  Wire.beginTransmission(QMC5883L_ADDRESS);
  Wire.write(QMC5883L_REG_X_LSB);
  Wire.endTransmission(false);
  Wire.requestFrom(QMC5883L_ADDRESS, 6);

  if (Wire.available() >= 6) {
    magX = Wire.read() | (Wire.read() << 8); // legge dato, poi legge segno e sposta sx
    magY = Wire.read() | (Wire.read() << 8);
    magZ = Wire.read() | (Wire.read() << 8);
  }


/*CALIB QMC5883L*/
 /*  
  float magX_scaled = ((float)magX * 2.0) / 32767.0;
  float magY_scaled = ((float)magY * 2.0) / 32767.0;
  float magZ_scaled = ((float)magZ * 2.0) / 32767.0;
*/

// Definizione dei valori mX_s, mY_s, mZ_s
float mX_s = ((float)magX * 2.0) / 32767.0;
float mY_s = ((float)magY * 2.0) / 32767.0;
float mZ_s = ((float)magZ * 2.0) / 32767.0;

// Applicazione della formula di calibrazione
h_calib[0] = (mX_s - CAL_PARAM[0]) / CAL_PARAM[3];
h_calib[1] = ((mY_s - CAL_PARAM[1]) / CAL_PARAM[4] - ((mX_s - CAL_PARAM[0]) / CAL_PARAM[3]) * sin(CAL_PARAM[6])) / cos(CAL_PARAM[6]);
h_calib[2] = ((mZ_s - CAL_PARAM[2]) / CAL_PARAM[5] - ((mX_s - CAL_PARAM[0]) / CAL_PARAM[3]) * sin(CAL_PARAM[7]) * cos(CAL_PARAM[8]) - (sin(CAL_PARAM[8]) / cos(CAL_PARAM[6])) * ((mY_s - CAL_PARAM[1]) / CAL_PARAM[4] - (mX_s - CAL_PARAM[0]) / CAL_PARAM[3]) * sin(CAL_PARAM[6])) / (cos(CAL_PARAM[7]) * cos(CAL_PARAM[8]));

/*MAHORY UPDATE*/
  now = micros();
  deltat = (now - last) * 1.0e-6; //seconds since last update
  last = now;
  
  MahonyQuaternionUpdate(accX_calibrated, accY_calibrated, accZ_calibrated, gyroX_calibrated, gyroY_calibrated, gyroZ_calibrated,
                         h_calib[0], h_calib[1], h_calib[2], deltat);
  
  roll  = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
  pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
  yaw   = atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - ( q[2] * q[2] + q[3] * q[3]));
  // to degrees
  yaw   *= 180.0 / PI;
  pitch *= 180.0 / PI;
  roll *= 180.0 / PI;

  // http://www.ngdc.noaa.gov/geomag-web/#declination
  //conventional nav, yaw increases CW from North, corrected for local magnetic declination

  yaw = -yaw + declination;
  if(yaw<0) yaw += 360.0;
  if(yaw>360.0) yaw -= 360.0;

  now_ms = millis(); //time to print?
  if (now_ms - last_ms >= print_ms) {
    last_ms = now_ms;
    // print angles for serial plotter...
    //  Serial.print("ypr ");
    Serial.print(yaw, 0);
    Serial.print(", ");
    Serial.print(pitch, 0);
    Serial.print(", ");
    Serial.println(roll, 0);
  }

}

void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat){
  
  // Vector to hold integral error for Mahony method
  static float eInt[3] = {0.0, 0.0, 0.0};
  
  // short name local variable for readability
  float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
  float norm;
  float hx, hy, bx, bz;
  float vx, vy, vz, wx, wy, wz;
  float ex, ey, ez;

  // Auxiliary variables to avoid repeated arithmetic
  float q1q1 = q1 * q1;
  float q1q2 = q1 * q2;
  float q1q3 = q1 * q3;
  float q1q4 = q1 * q4;
  float q2q2 = q2 * q2;
  float q2q3 = q2 * q3;
  float q2q4 = q2 * q4;
  float q3q3 = q3 * q3;
  float q3q4 = q3 * q4;
  float q4q4 = q4 * q4;
  /*
    // already done in loop()

    // Normalise accelerometer measurement
    norm = sqrt(ax * ax + ay * ay + az * az);
    if (norm == 0.0f) return; // Handle NaN
    norm = 1.0f / norm;       // Use reciprocal for division
    ax *= norm;
    ay *= norm;
    az *= norm;

    // Normalise magnetometer measurement
    norm = sqrt(mx * mx + my * my + mz * mz);
    if (norm == 0.0f) return; // Handle NaN
    norm = 1.0f / norm;       // Use reciprocal for division
    mx *= norm;
    my *= norm;
    mz *= norm;
  */
  // Reference direction of Earth's magnetic field
  hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
  hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
  bx = sqrt((hx * hx) + (hy * hy));
  bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);

  // Estimated direction of gravity and magnetic field
  vx = 2.0f * (q2q4 - q1q3);
  vy = 2.0f * (q1q2 + q3q4);
  vz = q1q1 - q2q2 - q3q3 + q4q4;
  wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
  wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
  wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);

  // Error is cross product between estimated direction and measured direction of the reference vectors
  ex = (ay * vz - az * vy) + (my * wz - mz * wy);
  ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
  ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
  if (Ki > 0.0f)
  {
    eInt[0] += ex;      // accumulate integral error
    eInt[1] += ey;
    eInt[2] += ez;
    // Apply I feedback
    gx += Ki*eInt[0];
    gy += Ki*eInt[1];
    gz += Ki*eInt[2];
  }


  // Apply P feedback
  gx = gx + Kp * ex; 
  gy = gy + Kp * ey;
  gz = gz + Kp * ez;

 // Integrate rate of change of quaternion
 // small correction 1/11/2022, see https://github.com/kriswiner/MPU9250/issues/447
 gx = gx * (0.5*deltat); // pre-multiply common factors 
 gy = gy * (0.5*deltat);
 gz = gz * (0.5*deltat);
 float qa = q1;
 float qb = q2;
 float qc = q3;
 q1 += (-qb * gx - qc * gy - q4 * gz);
 q2 += (qa * gx + qc * gz - q4 * gy);
 q3 += (qa * gy - qb * gz + q4 * gx);
 q4 += (qa * gz + qb * gy - qc * gx);

  // Normalise quaternion
  norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
  norm = 1.0f / norm;
  q[0] = q1 * norm;
  q[1] = q2 * norm;
  q[2] = q3 * norm;
  q[3] = q4 * norm;
}

float vector_dot(float a[3], float b[3]){
  return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

void vector_normalize(float a[3]){
  float mag = sqrt(vector_dot(a, a));
  a[0] /= mag;
  a[1] /= mag;
  a[2] /= mag;
}

void calibrateMPU6050() {
  
  delay(2000);
  digitalWrite(2, HIGH);
  delay(100);
  digitalWrite(2, LOW);
  delay(100);

  const int numSamples = 1000;
  long accX_sum = 0, accY_sum = 0, accZ_sum = 0;
  long gyroX_sum = 0, gyroY_sum = 0, gyroZ_sum = 0;

  for (int i = 0; i < numSamples; ++i) {
    int16_t accX, accY, accZ;
    int16_t gyroX, gyroY, gyroZ;

    // Read accelerometer data
    Wire.beginTransmission(MPU6050_ADDRESS);
    Wire.write(MPU6050_REG_ACCEL_XOUT_H);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU6050_ADDRESS, 6);
    if (Wire.available() >= 6) {
      accX = Wire.read() << 8 | Wire.read();
      accY = Wire.read() << 8 | Wire.read();
      accZ = Wire.read() << 8 | Wire.read();
    }

    // Read gyroscope data
    Wire.beginTransmission(MPU6050_ADDRESS);
    Wire.write(MPU6050_REG_GYRO_XOUT_H);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU6050_ADDRESS, 6);
    if (Wire.available() >= 6) {
      gyroX = Wire.read() << 8 | Wire.read();
      gyroY = Wire.read() << 8 | Wire.read();
      gyroZ = Wire.read() << 8 | Wire.read();
    }

    accX_sum += accX;
    accY_sum += accY;
    accZ_sum += accZ;
    gyroX_sum += gyroX;
    gyroY_sum += gyroY;
    gyroZ_sum += gyroZ;

    delay(10);
  }

  for (int i = 0; i < 2; i++) {
    digitalWrite(2, HIGH);
    delay(100);
    digitalWrite(2, LOW);
    delay(100);
  }

  accBiasX = (float)accX_sum / numSamples;
  accBiasY = (float)accY_sum / numSamples;
  accBiasZ = (float)accZ_sum / numSamples;
  gyroBiasX = (float)gyroX_sum / numSamples;
  gyroBiasY = (float)gyroY_sum / numSamples;
  gyroBiasZ = (float)gyroZ_sum / numSamples;
}

andreaarcarisi · May 8, 2024, 4:42pm

I also tried this version but still nothing...

 #include <Wire.h>
 #include <math.h>

/*MPU6050*/
 #define MPU6050_ADDRESS 0x68
 #define MPU6050_REG_ACCEL_XOUT_H 0x3B
 #define MPU6050_REG_GYRO_XOUT_H 0x43
 float accBiasX = 0, accBiasY = 0, accBiasZ = 0;
 float gyroBiasX = 0, gyroBiasY = 0, gyroBiasZ = 0;

/*QMC5883L*/
 #define QMC5883L_ADDRESS 0x0D
 #define QMC5883L_REG_X_LSB 0x00
 #define QMC5883L_REG_Y_LSB 0x02
 #define QMC5883L_REG_Z_LSB 0x04
// Definizione dei valori di CAL_PARAM
 float CAL_PARAM[9] = {-0.0150011772491854,-0.0106985402435778,0.0355930495987404,0.610897253682805,0.582287592807394,0.592696731175137,0.0762073179374682,0.0361950160491302,0.0452707353405034};
 float h_calib[3]; // Vettore per i risultati della calibrazione


/*MAHONY*/
 #define Kp 10.0
 #define Ki 0.00
static float declination = 3.7;

 unsigned long now = 0, last = 0; //micros() timers
 float deltat = 0;  //loop time in seconds
 unsigned long now_ms, last_ms = 0; //millis() timers
 unsigned long print_ms = 100; //print every "print_ms" milliseconds
 static float q[4] = {1.0, 0.0, 0.0, 0.0};
 static float yaw, pitch, roll; //Euler angle output
 

void setup() {

 /*SET GLOBAL*/
  Wire.begin();
  Serial.begin(115200);
  pinMode(2, OUTPUT);

/*SET MPU6050*/
  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(0x6B); // Power Management register
  Wire.write(0); // Accendi il dispositivo (wakes up the MPU-6050)
  Wire.endTransmission(true);

/*SET QMC5883L*/
  Wire.beginTransmission(QMC5883L_ADDRESS);
  Wire.write(0x09); // Registri di controllo
  Wire.write(0x0D); // Modalità continuativa con 50Hz
  Wire.endTransmission();
  
/*CALIB MPU6050*/
  calibrateMPU6050();
}

void loop() {

/* DATA MPU6050*/
  int16_t accX, accY, accZ;
  int16_t gyroX, gyroY, gyroZ;

  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(MPU6050_REG_ACCEL_XOUT_H);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDRESS, 6);
  if (Wire.available() >= 6) {
    accX = Wire.read() << 8 | Wire.read();
    accY = Wire.read() << 8 | Wire.read();
    accZ = Wire.read() << 8 | Wire.read();
  }

  // Leggi i dati del giroscopio dal MPU6050
  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(MPU6050_REG_GYRO_XOUT_H);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDRESS, 6);
  if (Wire.available() >= 6) {
    gyroX = Wire.read() << 8 | Wire.read();
    gyroY = Wire.read() << 8 | Wire.read();
    gyroZ = Wire.read() << 8 | Wire.read();
  }

/*CALIB MPU6050*/
  float accX_calibrated = ((float)accX - accBiasX) / 16384.0 * 9.81;
  float accY_calibrated = ((float)accY - accBiasY) / 16384.0 * 9.81;
  float accZ_calibrated = ((float)accZ - accBiasZ) / 16384.0 * 9.81;
  float gyroX_calibrated = ((float)gyroX - gyroBiasX) / 131.0;
  float gyroY_calibrated = ((float)gyroY - gyroBiasY) / 131.0;
  float gyroZ_calibrated = ((float)gyroZ - gyroBiasZ) / 131.0;

/*DATA QMC5883L*/
  int16_t magX, magY, magZ;

  Wire.beginTransmission(QMC5883L_ADDRESS);
  Wire.write(QMC5883L_REG_X_LSB);
  Wire.endTransmission(false);
  Wire.requestFrom(QMC5883L_ADDRESS, 6);

  if (Wire.available() >= 6) {
    magX = Wire.read() | (Wire.read() << 8); // legge dato, poi legge segno e sposta sx
    magY = Wire.read() | (Wire.read() << 8);
    magZ = Wire.read() | (Wire.read() << 8);
  }


/*CALIB QMC5883L*/
 /*  
  float magX_scaled = ((float)magX * 2.0) / 32767.0;
  float magY_scaled = ((float)magY * 2.0) / 32767.0;
  float magZ_scaled = ((float)magZ * 2.0) / 32767.0;
*/

// Definizione dei valori mX_s, mY_s, mZ_s
float mX_s = ((float)magX * 2.0) / 32767.0;
float mY_s = ((float)magY * 2.0) / 32767.0;
float mZ_s = ((float)magZ * 2.0) / 32767.0;

// Applicazione della formula di calibrazione
h_calib[0] = (mX_s - CAL_PARAM[0]) / CAL_PARAM[3];
h_calib[1] = ((mY_s - CAL_PARAM[1]) / CAL_PARAM[4] - ((mX_s - CAL_PARAM[0]) / CAL_PARAM[3]) * sin(CAL_PARAM[6])) / cos(CAL_PARAM[6]);
h_calib[2] = ((mZ_s - CAL_PARAM[2]) / CAL_PARAM[5] - ((mX_s - CAL_PARAM[0]) / CAL_PARAM[3]) * sin(CAL_PARAM[7]) * cos(CAL_PARAM[8]) - (sin(CAL_PARAM[8]) / cos(CAL_PARAM[6])) * ((mY_s - CAL_PARAM[1]) / CAL_PARAM[4] - (mX_s - CAL_PARAM[0]) / CAL_PARAM[3]) * sin(CAL_PARAM[6])) / (cos(CAL_PARAM[7]) * cos(CAL_PARAM[8]));

/*MAHORY UPDATE*/
  now = micros();
  deltat = (now - last) * 1.0e-6; //seconds since last update
  last = now;
  
  MahonyQuaternionUpdate(accX_calibrated, accY_calibrated, accZ_calibrated, gyroX_calibrated, gyroY_calibrated, gyroZ_calibrated,
                         h_calib[0], h_calib[1], h_calib[2], deltat);
  
  roll  = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
  pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
  yaw   = atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - ( q[2] * q[2] + q[3] * q[3]));
  // to degrees
  yaw   *= 180.0 / PI;
  pitch *= 180.0 / PI;
  roll *= 180.0 / PI;

  // http://www.ngdc.noaa.gov/geomag-web/#declination
  //conventional nav, yaw increases CW from North, corrected for local magnetic declination

  yaw = -(yaw + declination);
  if(yaw<0) yaw += 360.0;
  if(yaw>=360.0) yaw -= 360.0;

  now_ms = millis(); //time to print?
  if (now_ms - last_ms >= print_ms) {
    last_ms = now_ms;
    // print angles for serial plotter...
    //  Serial.print("ypr ");
    Serial.print(yaw, 0);
    Serial.print(", ");
    Serial.print(pitch, 0);
    Serial.print(", ");
    Serial.println(roll, 0);
  }

}

void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)  // Vector to hold integral error for Mahony method
 {
  static float eInt[3] = {0.0, 0.0, 0.0};
  // short name local variable for readability
  float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
  float norm;
  float hx, hy, hz;  //observed West horizon vector W = AxM
  float ux, uy, uz, wx, wy, wz; //calculated A (Up) and W in body frame
  float ex, ey, ez;
  float pa, pb, pc;

  // Auxiliary variables to avoid repeated arithmetic
  float q1q1 = q1 * q1;
  float q1q2 = q1 * q2;
  float q1q3 = q1 * q3;
  float q1q4 = q1 * q4;
  float q2q2 = q2 * q2;
  float q2q3 = q2 * q3;
  float q2q4 = q2 * q4;
  float q3q3 = q3 * q3;
  float q3q4 = q3 * q4;
  float q4q4 = q4 * q4;

  // Measured horizon vector = a x m (in body frame)
  hx = ay * mz - az * my;
  hy = az * mx - ax * mz;
  hz = ax * my - ay * mx;
  // Normalise horizon vector
  norm = sqrt(hx * hx + hy * hy + hz * hz);
  if (norm == 0.0f) return; // Handle div by zero

  norm = 1.0f / norm;
  hx *= norm;
  hy *= norm;
  hz *= norm;

  // Estimated direction of Up reference vector
  ux = 2.0f * (q2q4 - q1q3);
  uy = 2.0f * (q1q2 + q3q4);
  uz = q1q1 - q2q2 - q3q3 + q4q4;

  // estimated direction of horizon (West) reference vector
  wx = 2.0f * (q2q3 + q1q4);
  wy = q1q1 - q2q2 + q3q3 - q4q4;
  wz = 2.0f * (q3q4 - q1q2);

  // Error is the summed cross products of estimated and measured directions of the reference vectors
  // It is assumed small, so sin(theta) ~ theta IS the angle required to correct the orientation error.

  ex = (ay * uz - az * uy) + (hy * wz - hz * wy);
  ey = (az * ux - ax * uz) + (hz * wx - hx * wz);
  ez = (ax * uy - ay * ux) + (hx * wy - hy * wx);

  if (Ki > 0.0f)
  {
    eInt[0] += ex;      // accumulate integral error
    eInt[1] += ey;
    eInt[2] += ez;
    // Apply I feedback
    gx += Ki * eInt[0];
    gy += Ki * eInt[1];
    gz += Ki * eInt[2];
  }


  // Apply P feedback
  gx = gx + Kp * ex;
  gy = gy + Kp * ey;
  gz = gz + Kp * ez;


 //update quaternion with integrated contribution
 // small correction 1/11/2022, see https://github.com/kriswiner/MPU9250/issues/447
gx = gx * (0.5*deltat); // pre-multiply common factors
gy = gy * (0.5*deltat);
gz = gz * (0.5*deltat);
float qa = q1;
float qb = q2;
float qc = q3;
q1 += (-qb * gx - qc * gy - q4 * gz);
q2 += (qa * gx + qc * gz - q4 * gy);
q3 += (qa * gy - qb * gz + q4 * gx);
q4 += (qa * gz + qb * gy - qc * gx);

  // Normalise quaternion
  norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
  norm = 1.0f / norm;
  q[0] = q1 * norm;
  q[1] = q2 * norm;
  q[2] = q3 * norm;
  q[3] = q4 * norm;
}

float vector_dot(float a[3], float b[3]){
  return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

void vector_normalize(float a[3]){
  float mag = sqrt(vector_dot(a, a));
  a[0] /= mag;
  a[1] /= mag;
  a[2] /= mag;
}

void calibrateMPU6050() {
  
  delay(2000);
  digitalWrite(2, HIGH);
  delay(100);
  digitalWrite(2, LOW);
  delay(100);

  const int numSamples = 1000;
  long accX_sum = 0, accY_sum = 0, accZ_sum = 0;
  long gyroX_sum = 0, gyroY_sum = 0, gyroZ_sum = 0;

  for (int i = 0; i < numSamples; ++i) {
    int16_t accX, accY, accZ;
    int16_t gyroX, gyroY, gyroZ;

    // Read accelerometer data
    Wire.beginTransmission(MPU6050_ADDRESS);
    Wire.write(MPU6050_REG_ACCEL_XOUT_H);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU6050_ADDRESS, 6);
    if (Wire.available() >= 6) {
      accX = Wire.read() << 8 | Wire.read();
      accY = Wire.read() << 8 | Wire.read();
      accZ = Wire.read() << 8 | Wire.read();
    }

    // Read gyroscope data
    Wire.beginTransmission(MPU6050_ADDRESS);
    Wire.write(MPU6050_REG_GYRO_XOUT_H);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU6050_ADDRESS, 6);
    if (Wire.available() >= 6) {
      gyroX = Wire.read() << 8 | Wire.read();
      gyroY = Wire.read() << 8 | Wire.read();
      gyroZ = Wire.read() << 8 | Wire.read();
    }

    accX_sum += accX;
    accY_sum += accY;
    accZ_sum += accZ;
    gyroX_sum += gyroX;
    gyroY_sum += gyroY;
    gyroZ_sum += gyroZ;

    delay(10);
  }

  for (int i = 0; i < 2; i++) {
    digitalWrite(2, HIGH);
    delay(100);
    digitalWrite(2, LOW);
    delay(100);
  }

  accBiasX = (float)accX_sum / numSamples;
  accBiasY = (float)accY_sum / numSamples;
  accBiasZ = (float)accZ_sum / numSamples;
  gyroBiasX = (float)gyroX_sum / numSamples;
  gyroBiasY = (float)gyroY_sum / numSamples;
  gyroBiasZ = (float)gyroZ_sum / numSamples;
}

system · November 4, 2024, 4:42pm

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Mahony with calibration Programming	14	616	October 27, 2024
Spikes when using Mahony filter Programming	6	1021	September 23, 2023
Selfmade Tilt compensated Compass Pitch, Roll, Yaw Exhibition	28	39059	May 6, 2021
compass heading changes on tilt of device. Sensors	27	5922	May 6, 2021
Mpu6050 gives me different values for same angular rotation ... Sensors	18	7924	May 6, 2021

Mahony calibration problem

Related topics