Help with IMU Sensor tilt calibration

Using Arduino Project Guidance
1

Hello everyone,
I am trying to make my pmodnav show the heading my remote controlled car is going. I am using this formula for the tilt calibration:
Xhorizontal = Xcos(pitch) + Ysin(roll)sin(pitch) – Zcos(roll)sin(pitch)
Yhorizontal = Ycos(roll) + Z*sin(roll)
It works for correcting for the pitch (within 3 degrees), but it the value is way off the roll.
Could you tell me what I am doing wrong?
Here is the code for the tilt calibration (I use the nav.h library):

void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
{
  float roll = atan2(ay, az); //calculate roll
  float pitch = atan2(-ax, sqrt(ay * ay + az * az));  //calculate pitch
  float heading;  //variable for heading
  //calculate heading
  if (my == 0) {
    heading = (mx < 0) ? PI : 0;
  }
  else {
    float Xhor = imu.calcMag(imu.mx)*cos(pitch)+imu.calcMag(imu.my)*sin(roll)*sin(pitch)-imu.calcMag(imu.mz)*cos(roll)*sin(pitch);
    float Yhor = imu.calcMag(imu.my)*cos(roll)+imu.calcMag(imu.mz)*sin(roll);
    heading = atan2(Yhor, Xhor);
  }

  //correct heading according to declination
  heading -= DECLINATION * PI / 180;
  if (heading > PI) {
    heading -= (2 * PI);
  }
  else if (heading < -PI) {
    heading += (2 * PI);
  }
  else if (heading < 0) {
    heading += 2 * PI;
  }
  
  //convert values in degree
  heading *= 180.0 / PI;
  pitch *= 180.0 / PI;
  roll *= 180.0 / PI;
 
  //display calculated data
  Serial.print("Heading: ");
  Serial.print(heading, 2);
  Serial.println(" °");
}

Every suggestion is appreciated :slight_smile:

2

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