MPU6050 example code with Arduino Uno and OpenCM

[danaishrathore]

Hi everyone, I am working with MPU6050 sensor connected with OPenCM9.04 board and using Arduino IDE to upload the code. I had used the same MPU6050 code (attached below) on Arduino Uno and it worked fine, but now the problem is when I uploaded the same code on OpenCM and opened the serial monitor the data from accelerometer is fine but the gyro data is some different (serial monitor outputcan be seen after code).

Hardware setup:
MPU6050 OpenCM
MPU SCL -> pin24 (SCL)
MPU SDA -> pin25 (SDA)
MPU VIN -> 3V
MPU GND -> GND

MPU6050 code

#include <Wire.h>
#include <math.h>
const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float phiFold=0, phiFnew,thetaFold=0, thetaFnew;
float roll, pitch, yaw;
float AccErrorX=0, AccErrorY=0, GyroErrorX=0, GyroErrorY=0, GyroErrorZ=0;
float elapsedTime, currentTime, previousTime;
int c = 0;
int sample=1000;
float AcceX=0, AcceY=0, AcceZ=0;

/--------------------------------//void setup//----------------------------
void setup() {
Serial.begin(115200);
while(!Serial);
Wire.begin(); // Initialize communication
Wire.beginTransmission(MPU); // Start communication with MPU6050 // MPU=0x68
Wire.write(0x6B); // Talk to the register 6B
Wire.write(0x00); // Make reset - place a 0 into the 6B register
Wire.endTransmission(true); //end the transmission
calculate_IMU_error();
delay(20);
}

//--------------------------------//void loop//----------------------------
void loop() {
// === Read accelerometer data === //
Wire.beginTransmission(MPU);
Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 14, true);

AccX = (Wire.read() << 8 | Wire.read()) / 16384.0;
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;

// Calculating Roll and Pitch from the accelerometer data
accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI)+ -1<em>AccErrorX;
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI)-1</em>AccErrorY;

//low filter for accelerometer data
phiFnew=.9 * phiFold + .1 * accAngleX;
thetaFnew=.9 * thetaFold + .1 * accAngleY;

// === Read gyroscope data === //
previousTime = currentTime;
currentTime = millis();
elapsedTime = (currentTime - previousTime) / 1000;

Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);

GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

// Correct the outputs with the calculated error values
GyroX = GyroX -1 * GyroErrorX;
GyroY = GyroY -1 * GyroErrorY;
GyroZ = GyroZ -1 * GyroErrorZ;

// Currently the raw values are in degrees per seconds, deg/s, we need to multiply by sendonds (s) to get the angle in degrees
gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
gyroAngleY = gyroAngleY + GyroY * elapsedTime;
yaw = yaw + GyroZ * elapsedTime;

// Complementary filter - combine acceleromter and gyro angle values
gyroAngleX = 0.96 * gyroAngleX + 0.04 * phiFnew;
gyroAngleY = 0.96 * gyroAngleY + 0.04 * thetaFnew;

roll = gyroAngleX;
pitch = gyroAngleY;
float Yaw = yaw;

phiFold=phiFnew;
thetaFold=thetaFnew;

Serial.print(roll); Serial.print(" ");Serial.print(pitch); Serial.print(" ");Serial.println(Yaw);
delay (10);
}

//---------------------------------//Calculate IMU error//---------------------------------
void calculate_IMU_error() {

c=0;
while (c < sample) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);

 AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
 AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
 AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;

// Sum all readings
 AcceX = AcceX+AccX*9.8;
 AcceY = AcceY+AccY*9.8;
 AcceZ = AcceZ+AccZ*9.8;
c++;

}

//Divide the sum by 200 to get the error value
AcceX = AcceX / sample;
AcceY = AcceY / sample;
AcceZ = AcceZ / sample;

Serial.print("AcceX: ");
Serial.println(AcceX);
Serial.print("AcceY: ");
Serial.println(AcceY);
Serial.print("AcceZ: ");
Serial.println(AcceZ);
Serial.println(" ");
delay (1000);

// Read accelerometer values 200 times to calculate the accelerometer data error
c=0;
while (c < sample) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);

AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;

// Sum all readings
AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
c++;

}

//Divide the sum by 200 to get the error value
AccErrorX = AccErrorX / sample;
AccErrorY = AccErrorY / sample;

// Read gyro values 200 times to calculate the gyro data error
c = 0;
while (c < sample) {
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);


GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

// Sum all readings
GyroErrorX = GyroErrorX + (GyroX);
GyroErrorY = GyroErrorY + (GyroY);
GyroErrorZ = GyroErrorZ + (GyroZ);
c++;

}

Serial.print("GyroX: ");
Serial.println(GyroX);
Serial.print("GyroY: ");
Serial.println(GyroY);
Serial.print("GyroZ: ");
Serial.println(GyroZ);
Serial.println(" ");
delay (2000);

//Divide the sum by 200 to get the error value
GyroErrorX = GyroErrorX / sample;
GyroErrorY = GyroErrorY / sample;
GyroErrorZ = GyroErrorZ / sample;

// Print the error values on the Serial Monitor
Serial.print("AccErrorX: ");
Serial.println(AccErrorX);
Serial.print("AccErrorY: ");
Serial.println(AccErrorY);
Serial.print("GyroErrorX: ");
Serial.println(GyroErrorX);
Serial.print("GyroErrorY: ");
Serial.println(GyroErrorY);
Serial.print("GyroErrorZ: ");
Serial.println(GyroErrorZ);
Serial.println(" ");
delay (2000);
}

//---------------------------------//Serial monitor//---------------------------------

Arduino Uno (MPU data):

AcceX: 0.51
AcceY: -0.21
AcceZ: 9.09

GyroX: -1.92
GyroY: 1.18
GyroZ: -2.05

AccErrorX: -1.35
AccErrorY: -3.23
GyroErrorX: -1.70
GyroErrorY: 0.98
GyroErrorZ: -2.01

roll pitch yaw
0.33 -0.34 -1.42
0.31 -0.33 -1.42
0.30 -0.32 -1.42
0.29 -0.31 -1.42
0.28 -0.29 -1.42

OpenCM (MPU data):

AcceX: 0.39
AcceY: 0.61
AcceZ: 9.03

> GyroX: 499.52

GyroY: 1.24

> GyroZ: 499.30

AccErrorX: 3.85
AccErrorY: -2.46
GyroErrorX: 499.43
GyroErrorY: 1.29
GyroErrorZ: 499.36

roll pitch yaw
0.01 3.82 -1.52
0.01 3.67 -1.52
0.01 3.52 -1.52

I don't know what is wrong here using MPU with OpenCM... why the gyro values (OpenCM) are so different from those of arduino Uno data! these gyro values are not correct therefore I am not getting the pitch, yaw and roll correctly. I tried a lot but couldn't solve the problem and even I don't know where I am doing the mistake using OpenCM.
It would be great if any one can help and thanks in advance.

[jremington]

Please edit your post to add code tags, as described in the how to use the forum post.

[jremington]

GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;

The OPENCM board you are using is 32 bits, and the default size for int variables is probably 32 bits. The above construction is not guaranteed to do what you want, because C/C++ compilers are not required to evaluate the Wire.read() statements in any particular order.

You might try this code, which allows no freedom regarding the data read order and explicitly types the data as 16 bit integers.

// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.h>
const int MPU_addr = 0x68; // I2C address of the MPU-6050
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}

void loop() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14); // request a total of 14 registers
  int16_t t = Wire.read();
  AcX = (t << 8) | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  t = Wire.read();
  AcY = (t << 8) | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  t = Wire.read();
  AcZ = (t << 8) | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  t = Wire.read();
  Tmp = (t << 8) | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  t = Wire.read();
  GyX = (t << 8) | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  t = Wire.read();
  GyY = (t << 8) | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  t = Wire.read();
  GyZ = (t << 8) | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  t = Wire.read();
  Serial.print("AcX = "); Serial.print(AcX);
  Serial.print(" | AcY = "); Serial.print(AcY);
  Serial.print(" | AcZ = "); Serial.print(AcZ);
  Serial.print(" | Tmp = "); Serial.print(Tmp / 340.00 + 36.53); //equation for temperature in degrees C from datasheet
  Serial.print(" | GyX = "); Serial.print(GyX);
  Serial.print(" | GyY = "); Serial.print(GyY);
  Serial.print(" | GyZ = "); Serial.println(GyZ);
  delay(333);
}

Incidentally, one of these two equations is wrong, plus you copied it incorrectly from somewhere.

// Calculating Roll and Pitch from the accelerometer data
accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI)+ -1<em>AccErrorX;
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI)-1</em>AccErrorY;

[danaishrathore]

Hi @jremington,

Thanks for the information and sharing the example code. It worked for me again many thanks.