PROBLEM WITH PITCH , ROLL CALCULATION

Hi,
I’m using mpu9250 sensor with arduino uno.
My problem is that something is wrong with pitch calculation. It prints values from 82 to 89 and between of them some zeros. My roll calculations works fine but I dont know what’s wrong with pitch.
Here is code:

#include <Wire.h>


#define    MPU9250_ADDRESS            0x68
#define    MAG_ADDRESS                0x0C

#define    GYRO_FULL_SCALE_250_DPS    0x00  
#define    GYRO_FULL_SCALE_500_DPS    0x08
#define    GYRO_FULL_SCALE_1000_DPS   0x10
#define    GYRO_FULL_SCALE_2000_DPS   0x18

#define    ACC_FULL_SCALE_2_G        0x00  
#define    ACC_FULL_SCALE_4_G        0x08
#define    ACC_FULL_SCALE_8_G        0x10
#define    ACC_FULL_SCALE_16_G       0x18


// This function read Nbytes bytes from I2C device at address Address. 
// Put read bytes starting at register Register in the Data array. 
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
{
  // Set register address
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.endTransmission();
  
  // Read Nbytes
  Wire.requestFrom(Address, Nbytes); 
  uint8_t index=0;
  while (Wire.available())
    Data[index++]=Wire.read();
}


// Write a byte (Data) in device (Address) at register (Register)
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
  // Set register address
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.write(Data);
  Wire.endTransmission();
}


// Initializations
void setup()
{
  // Arduino initializations
  Wire.begin();
  Serial.begin(115200);

  
  // Set accelerometers low pass filter at 5Hz
  I2CwriteByte(MPU9250_ADDRESS,29,0x06);
  // Set gyroscope low pass filter at 5Hz
  I2CwriteByte(MPU9250_ADDRESS,26,0x06);
 
   
  // Configure gyroscope range
  I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_1000_DPS);
  // Configure accelerometers range
  I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_4_G);
  // Set by pass mode for the magnetometers
  I2CwriteByte(MPU9250_ADDRESS,0x37,0x02);
  
  // Request continuous magnetometer measurements in 16 bits
  I2CwriteByte(MAG_ADDRESS,0x0A,0x16);
  
   pinMode(13, OUTPUT);
  
  
  
}


// Main loop, read and display data
void loop()
{
  
 
 
  // ____________________________________
  // :::  accelerometer and gyroscope ::: 

  // Read accelerometer and gyroscope
  uint8_t Buf[14];
  I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
  
  // Create 16 bits values from 8 bits data
  
  // Accelerometer
  int16_t ax=-(Buf[0]<<8 | Buf[1]);
  int16_t ay=-(Buf[2]<<8 | Buf[3]);
  int16_t az=Buf[4]<<8 | Buf[5];

  // Gyroscope
  int16_t gx=-(Buf[8]<<8 | Buf[9]);
  int16_t gy=-(Buf[10]<<8 | Buf[11]);
  int16_t gz=Buf[12]<<8 | Buf[13];
int Roll, Pitch, sroll;\
Serial.print("X=");
Serial.print(ax);

Roll = atan2(ay, az) * 180/PI;
Serial.print("ROLL=");
Serial.print(Roll);
Serial.print("\t");



delay(150);
Pitch = atan2(ax, sqrt(ay*ay + az*az))* 180/PI ;
Serial.print("Pitch=");
Serial.print(Pitch);
Serial.print("|");
Serial.print("\t");




  
    // Display values
  
  // Accelerometer
  Serial.print (ax,DEC); 
  Serial.print ("\t");
  Serial.print (ay,DEC);
  Serial.print ("\t");
  Serial.print (az,DEC);  
  Serial.print ("\t");
  
  // Gyroscope
  Serial.print (gx,DEC); 
  Serial.print ("\t");
  Serial.print (gy,DEC);
  Serial.print ("\t");
  Serial.print (gz,DEC);  
  Serial.print ("\t");

  
  // _____________________
  // :::  Magnetometer ::: 

  
  // Read register Status 1 and wait for the DRDY: Data Ready
  
  uint8_t ST1;
  do
  {
    I2Cread(MAG_ADDRESS,0x02,1,&ST1);
  }
  while (!(ST1&0x01));

  // Read magnetometer data  
  uint8_t Mag[7];  
  I2Cread(MAG_ADDRESS,0x03,7,Mag);
  

  // Create 16 bits values from 8 bits data
  
  // Magnetometer
  int16_t mx=-(Mag[3]<<8 | Mag[2]);
  int16_t my=-(Mag[1]<<8 | Mag[0]);
  int16_t mz=-(Mag[5]<<8 | Mag[4]);
  Serial.print("mx=");
  Serial.print(mx);
  // Magnetometer
  float heading_in_radians = atan2(my,mz);

  if(heading_in_radians < 0)
    heading_in_radians += 2*PI;
  float headingdegrees = (heading_in_radians * 180) / M_PI;
  
  
  Serial.print("\t");
  Serial.print (mx+200,DEC); 
  Serial.print ("\t");
  Serial.print (my-70,DEC);
  Serial.print ("\t");
  Serial.print (mz-700,DEC);  
  Serial.print ("\t");

  
  
  // End of line
  Serial.println("");
//  delay(100);    
}

Use equations 26 and 28 in this PDF document.

Here are the equations I used in a camera platform stabilizer. You only need to scale the output from radians to degrees with a #define DEG_PER_RAD 57.3.

// Platform Stabilizer Equations
// Ax, Ay, Az are acceleration in pitch forward, roll down-right, and vertical down directions
// pitch = atan( Ax / sqrt( ( Ay * Ay ) + ( Az * Az ) )
// roll  = atan( Ay / sqrt( ( Ax * Ax ) + ( Az * Az ) )

How I could calibrate data fron accelerometer into degrees? the range is from -8300 to 8300

The accelerometer measures [u]acceleration[/u], not degrees.

Do you mean "g" values? If so, consult the data sheet.

I have found on web that pitch and roll calculated from this formula: Roll = atan2(AcY, AcZ)*180/PI; Pitch = atan2(AcX, sqrt(AcY*AcY + AcZ*AcZ))*180/PI;

which AcY, AcX, AcZ are data from accelerometer. Roll calculation works fine and measure degrees. Pitch displays me stange values something like this

Pitch = -89 Pitch = -88 Pitch = 0 Pitch = 0 Pitch = 0 Pitch = -83 Pitch = 0 Pitch = 0 Pitch = 0

and these values are with sensor immobilised. Thanks for all replies but I can't find the problem.

and these values are with sensor immobilised.

You are probably confused about which directions in space correspond to "X", "Y" and "Z" on the sensor. The data sheet will tell you how these axes are arranged on the sensor.

You should experiment with the sensor and print out the raw values of the X, Y and Z acceleration values. Hold the sensor immobilized in various orientations, with X, Y and Z held straight up or down. You should be measuring the value of "g" [u]along the axis that is pointing up[/u] and about zero for the axes that are parallel to the floor.

You may also need to change the sign on or more of the terms inside the call to atan2().