Home e-Manual

OpenCM and MPU6050

Hi everyone, I am working with MPU6050 sensor connected with OPenCM9.0 4 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)+ -1 AccErrorX;
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI)-1
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.

It is certainly strange that the same code on 2 different processor gives different results.

The only 2 different things that I can think that might be causing this is :

  1. the Uno is 5v and the OpenCv is 3.3v so the ADCs on the MPU6050 will have different ref voltages but I would doubt this would cause this problem.

  2. I do believe the Ardunio port for STM32 treats floats as 64 bit floats where as Ardunio on Uno uses 32 bit floats so not sure if this is causing a bit misalignment when the data is being read from the MPU6050

Mind neither of these thing would explain why reading all the other MPU6050 register works correctly and only the 2 bytes for GyroX and the 2 bytes for GyroZ are incorrect.

To maybe find the problem write simple code just to read 2 bytes from the GyroX regiter and print out those 2 bytes and run it on both boards and see if the raw 2 bytes are really different

I apologise for late reply, somehow my this forum account was on hold so wasn’t able to check this. And here is the answer from another forum:

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.

The full answer and code can be found here

1 Like

So if I read the difference in the code between the 2 they made 2 changes 1. changing from using floats to int16_t and 2. they read 1 byte at a time rather than 2 bytes at a time.

It would interesting to see which if the 2 changes solved the problem. Try using int16_t but read 2 bytes at a time then try using floats but using 1 byte at a time and see which 1 works.

I suspect it was the reading bytes from the MPU6050 in to float that was causing your problem as I am pretty sure Ardunio on AVR chip uses 32 bit floats and Ardunio on STM32 uses 64 bit floats.

1 Like
 int16_t t = Wire.read();
  AcX = (t << 8) | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = (t << 8) | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
 float 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();

Hi thanks for analysing the problem and is this code correct to do that… please guide me.

using int16_t but reading 2 bytes at a time

int16_t Acx;
Acx = (Wire.read() << 8 | Wire.read());

using floats but reading 1 byte at a time
float Acx;
float t;
t = Wire.read();
acx = (t << 8) | Wire.read();

1 Like

This is working normal but

showing error (for AcX,Y,Z and for GyroX,Y,Z) while uploading the sketch

error: invalid operands of types ‘float’ and ‘int’ to binary ‘operator<<’
AcX = (t << 8) | Wire.read();

OK compare

int16_t Acx;
Acx = (Wire.read() << 8 | Wire.read());

with

float_t Acx;
Acx = (Wire.read() << 8 | Wire.read());

and see the difference running both codes on a ardunio based board and a stm32 based board then you will see floats are treated different between the 2

1 Like

Thanks again, I will try it.