mathematic for MPU-6050 / GY-521

Other Hardware Sensors
1

I am completely null in math (and english)

With MPU-6050 / GY-521 I get raw values x, y, z.

I convert them into degrees from 0 to 360.

X = 0 or x = 180 or x = 360
and
Y = 0 or y = 180 or y = 360
being horizontal perfect.

I transform them from -180 to +180.

For values from -90 to 90, I adapt them to my screen from 0 to 240, displaying only values between 0 and 240.

Is it possible to simplify these calculations?

  AcX=Wire.read()<<8|Wire.read();
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();

    int xAng = map(AcX,minVal,maxVal,-90,90);
    int yAng = map(AcY,minVal,maxVal,-90,90);
    int zAng = map(AcZ,minVal,maxVal,-90,90);

    double x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
    double y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);

x = x>180 ? x-360 : x;
y = y>180 ? y-360 : y;

x= map(x,-90,90,0,240);
y= map(y,-90,90,0,240);
2

It looks like you want to sense tilt. If so, you could try doing the calculations correctly.

Google "accelerometer tilt sensing" for the details.

3

jremington:
you could try doing the calculations correctly.

Can you explain me please ?

4

Can you explain me please ?

For tilt measurements, the calculations done in the code you posted are incorrect.

For example, the following makes no sense at all.

    int xAng = map(AcX,minVal,maxVal,-90,90);
    int yAng = map(AcY,minVal,maxVal,-90,90);
    int zAng = map(AcZ,minVal,maxVal,-90,90);
5

I think this is what you're looking for.

If you want example code, below is a sketch I use for laser altimeter stabilization on one of my RC airplanes and works correctly (except that I'm using an MPU9250 instead):

#include <Wire.h>
#include <FaBo9Axis_MPU9250.h>
#include <Servo.h>

#define M_PI 3.14159265359

FaBo9Axis fabo_9axis;

float offsetax = 0;
float offsetay = 0;
float ax, ay, az;
float gx, gy, gz;
float integrated_gx = 0;
float scaled_gx = 0;
float integrated_gy = 0;
float scaled_gy = 0;
float angleRoll = 0;
float anglePitch = 0;
float A_ = 0.7;
float dt = 0.01;
float angleRollAcc = 0;
float anglePitchAcc = 0;
byte gyroSensitivity = 131; //16 bit ADC with +-250dps --> sensitivity = 2^16 / 500 = 131

double currentTime = millis();
double timeBench = currentTime;
double samplePeriod = 10;  //sample period in ms

Servo horizontal;
Servo vertical;



void setup()
{
  Serial.begin(115200);

  if (fabo_9axis.begin())
  {
    Serial.println("configured FaBo 9Axis I2C Brick");
  }
  else
  {
    Serial.println("device error");
    while (1);
  }

  horizontal.attach(30);
  vertical.attach(31);

  horizontal.write(90);
  vertical.write(90);
}



void loop()
{
  //prime the timer
  currentTime = millis();

  //execute once per sample time-period
  if ((currentTime - timeBench) >= samplePeriod)
  {
    //reset timer
    timeBench = currentTime;

    //get data
    fabo_9axis.readAccelXYZ(&ax, &ay, &az);
    fabo_9axis.readGyroXYZ(&gx, &gy, &gz);

    //remove offset
    ax = ax - offsetax;
    ay = ay - offsetay;

    //scale by sensitivity
    scaled_gx = gx / gyroSensitivity;
    scaled_gx = gy / gyroSensitivity;

    //integrate gyro readings
    integrated_gx = scaled_gx * dt;
    integrated_gx = scaled_gx * dt;

    angleRollAcc = -atan2(ax, sqrt((ay * ay) + (az * az))) * 180 / M_PI;
    anglePitchAcc = atan2(ay, sqrt((ax * ax) + (az * az))) * 180 / M_PI;

    angleRoll = A_ * (angleRoll + integrated_gx) + (1 - A_) * (angleRollAcc);
    anglePitch = A_ * (anglePitch + integrated_gx) + (1 - A_) * (anglePitchAcc);

    //print for debugging////
    Serial.print("angleRoll: ");
    Serial.print(angleRoll);
    Serial.print(" anglePitch: ");
    Serial.print(anglePitch);
    Serial.println();
    /////////////////////////

    /*Serial.print("angleRoll: ");
      Serial.print((byte)map(angleRoll,-90,90,0,180));
      Serial.print(" anglePitch: ");
      Serial.println((byte)map(anglePitch,-90,90,180,0));*/

    horizontal.write(map(angleRoll, -90, 90, 0, 180));
    vertical.write(map(anglePitch, -90, 90, 0, 180));
  }
}
6

What are you trying do?

7

jremington:
For tilt measurements, the calculations done in the code you posted are incorrect.

For example, the following makes no sense at all.

    int xAng = map(AcX,minVal,maxVal,-90,90);

int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);

sketch come from http://www.instructables.com/id/How-to-Measure-Angle-With-MPU-6050GY-521/

Power_Broker:
I think this is what you're looking for.

If you want example code, below is a sketch I use for laser altimeter stabilization on one of my RC airplanes and works correctly (except that I'm using an MPU9250 instead):

#include <Wire.h>

#include <FaBo9Axis_MPU9250.h>
#include <Servo.h>

#define M_PI 3.14159265359

FaBo9Axis fabo_9axis;

float offsetax = 0;
float offsetay = 0;
float ax, ay, az;
float gx, gy, gz;
float integrated_gx = 0;
float scaled_gx = 0;
float integrated_gy = 0;
float scaled_gy = 0;
float angleRoll = 0;
float anglePitch = 0;
float A_ = 0.7;
float dt = 0.01;
float angleRollAcc = 0;
float anglePitchAcc = 0;
byte gyroSensitivity = 131; //16 bit ADC with +-250dps --> sensitivity = 2^16 / 500 = 131

double currentTime = millis();
double timeBench = currentTime;
double samplePeriod = 10; //sample period in ms

Servo horizontal;
Servo vertical;

void setup()
{
Serial.begin(115200);

if (fabo_9axis.begin())
{
Serial.println("configured FaBo 9Axis I2C Brick");
}
else
{
Serial.println("device error");
while (1);
}

horizontal.attach(30);
vertical.attach(31);

horizontal.write(90);
vertical.write(90);
}

void loop()
{
//prime the timer
currentTime = millis();

//execute once per sample time-period
if ((currentTime - timeBench) >= samplePeriod)
{
//reset timer
timeBench = currentTime;

//get data
fabo_9axis.readAccelXYZ(&ax, &ay, &az);
fabo_9axis.readGyroXYZ(&gx, &gy, &gz);

//remove offset
ax = ax - offsetax;
ay = ay - offsetay;

//scale by sensitivity
scaled_gx = gx / gyroSensitivity;
scaled_gx = gy / gyroSensitivity;

//integrate gyro readings
integrated_gx = scaled_gx * dt;
integrated_gx = scaled_gx * dt;

angleRollAcc = -atan2(ax, sqrt((ay * ay) + (az * az))) * 180 / M_PI;
anglePitchAcc = atan2(ay, sqrt((ax * ax) + (az * az))) * 180 / M_PI;

angleRoll = A_ * (angleRoll + integrated_gx) + (1 - A_) * (angleRollAcc);
anglePitch = A_ * (anglePitch + integrated_gx) + (1 - A_) * (anglePitchAcc);

//print for debugging////
Serial.print("angleRoll: ");
Serial.print(angleRoll);
Serial.print(" anglePitch: ");
Serial.print(anglePitch);
Serial.println();
/////////////////////////

/*Serial.print("angleRoll: ");
  Serial.print((byte)map(angleRoll,-90,90,0,180));
  Serial.print(" anglePitch: ");
  Serial.println((byte)map(anglePitch,-90,90,180,0));*/

horizontal.write(map(angleRoll, -90, 90, 0, 180));
vertical.write(map(anglePitch, -90, 90, 0, 180));

}
}

Thank you for your sketch.
I will try to adapt it to MPU-6050 / GY-521.
If you unclude wire.h, do you need #define M_PI 3.14159265359 ?

jremington:
What are you trying do?

with raw data from MPU-6050 / GY-521 I want display on my tft screen something like that :

506×900 352 KB

8

So, you do want to measure tilt.

If you don't have time or know how to use Google as suggested in reply #1, here is a simple guide: How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing-DFRobot

Avoid Instructables. They are almost always crap.

9

jremington:
So, you do want to measure tilt.

If you don't have time or know how to use Google as suggested in reply #1, here is a simple guide: How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing-DFRobot

Before writing here, I read a lot of informaton about roll, pitch, yaw, gravity, etc.
I see many mathematic formul as this

but I am completly null with mathematic.

10

mybrain_iq55:
Before writing here, I read a lot of informaton about roll, pitch, yaw, gravity, etc.
I see many mathematic formul as this

but I am completly null with mathematic.

Did you not see my implementation of (similar) equations in my example code?

11

but I am completly null with mathematic.

That can be fixed with some study. You won't get very far with computers if you don't bother.

12

jremington:
That can be fixed with some study. You won't get very far with computers if you don't bother.

LOL ! you are so fun !