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 fine but the gyro data is some different (photo attached).
Hardware setup:
MPU6050 OpenCM
MPU SCL -> pin24
MPU SDA -> pin25
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)+ -1AccErrorX;
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI)-1AccErrorY;
//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.
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent