MPU9255 gyro data to roll pitch?

Question

So I have -32768 to +32768 coming out from MPU9255 (gx,gy,gz) which is converted to 0-250 dps(degrees per second) using 131 which is Gyro's sensitivity.

My question would be how do you use this data to convert it into Roll and Pitch?

I am trying to make a stabiliser. I have tried using $$ \theta = \sum^n_n\omega*\delta(t) $$

where n = infinity I don't if my equation is wrong or not here is my code:

dt = now_c - pr_dt;
pr_dt = dt;
Pitch_gyro += Gxyz[1]*(dt/1000000.0);
Roll_gyro += Gxyz[0]*(dt/1000000.0); 

This is my function:

Gxyz[0] = (double)(gx/131);//131;
Gxyz[1] = (double)(gy/131);//131;// 250/32768.0;
Gxyz[2] = (double)(gz/131);//131;// 250/32768.0;

Any guide as to how to solve this I have looked into euler angles. I still don't understand how you get the angles given angular velocity which is from gyro.

Answer 1

The problem you have is that the gyro is outputting angular velocity. Taking the integral of the angular velocity you only get the angle offset from some constant c, normally you would deal with this by initializing the system with some known orientation, typically by letting it rest flat for a moment before using it to track angle.

But there is another problem, gyros have some implicit bias in them. This means that when it is not moving it will still report that it is, you have to take this into account when you build your system saving them when you boot up during that time when your device is not moving. millisecond So your code might look like this:

init(){
    bias_x = Gxyz[0];
    bias_y = Gxyz[1];
    bias_z = Gxyz[2];
}

loop(){
    dt = now_c - pr_dt;
    pr_dt = dt;
    // x, y, z gyro are in deg/millisecond
    x_gyro += (Gxyz[0] + bias_x)*(dt/1000.0);
    y_gyro += (Gxyz[1] + bias_y)*(dt/1000.0); 
    z_gyro += (Gxyz[2] + bias_z)*(dt/1000.0); 
}

But there is another problem the measured angle will diverge over time from the actual angle becouse of the accuracy of the gyroscope, $0.1^\circ/s$ after 5 minutes the gyro will read $ 30^\circ$ off where it should be, and after an hour it will be $ 360^\circ$ off the original position, you will have no information on the orientation at all.

Answer 2

For those of you looking for How to read gyro data from MPU9255. I manage to solve it some what. Like the answer above said you need the gyro bias to be initialised. I have looped it 2000 times to get reading from gyro then averaged it to find a good value for the bias. I myself don't understand it properly why it work. But to put it blandly, to get the angular rate you simply divide it by 131 given that you are using 250 dps check the link to MPU9255 register maps sheet. Then you further divide it by your sample rate which should be 8000 by default again that is if you are using 250 dps it is also the default setting when you initialise MPU9255. I was just tinker around with the code to give me the right values and it seems to do that. I do have a problem with the drift. So if anyone can point me in the right direction. I don't know if I am implementing complementary filter the right way or not, I came across a normal filterish don't know what it's called that seems to work pretty well. However it assumes that the value from accel is absolute or it gives pretty precise.

#include "Arduino.h"
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU9250.h"

MPU9250 accelgyro;
volatile float Gxyz[3] = {0,0,0};
volatile float Axyz[3] = {0,0,0};
//raw acc, gyro, compass values
int16_t ax,ay,az,gx,gy,gz,mx,my,mz = 0;
//gyro calibration
volatile float G_cal_x,G_cal_y,G_cal_z = 0;
volatile float angle_x,d_angle_x,angle_y,d_angle_y = 0;
//accel and complimentary variables
volatile float Roll, Pitch, Roll_output, Pitch_output = 0;
//PID compute variables
volatile float Kp = 0;
volatile float Output,error,Set_point,Input_angle = 0;

void get_gyro();
void get_accel();
void PID_compute();

void setup()
{
  Wire.begin();
  //initialise the clock speed, gyroscope, accel,
  accelgyro.initialize();
  //serial communication begin
  for(int i = 0; i < 2000; i++)
     {
       get_gyro();
       G_cal_x += Gxyz[0];
       G_cal_y += Gxyz[1];
       G_cal_z += Gxyz[2];
       delay(3);
     }
  G_cal_x /= 2000;
  G_cal_y /= 2000;
  G_cal_z /= 2000;
  Serial.begin(115200);
}

void loop()
{
 //gets gyro and accelerometer values
  get_accel();
  get_gyro();

  //gyro to roll and pitch calculation d_angle_x and d_angle_y are roll and pitch
  Gxyz[0] = Gxyz[0] - G_cal_x;
  Gxyz[1] = Gxyz[1] - G_cal_y;
 //here is the sample rate and 131 value which is 0.000000954 it seem
 //when you call accel function as well you need to multiply it by 2
  angle_x += Gxyz[0]*(0.000000954*2);
  angle_y += Gxyz[1]*(0.000000954*2);

  d_angle_x = angle_x*180;
  d_angle_y = angle_y*180;
  //accel calculation
  Roll = (float)atan2(Axyz[1],Axyz[2])*(180/3.1415);
  Pitch = (float)(-atan2(-Axyz[0], sqrt((Axyz[1] * Axyz[1]) + (Axyz[2] * Axyz[2]))) * (180 / 3.1415));
  //complimentary filter
  Roll_output = 0.98*(0.1*Roll_output+0.9*d_angle_x) + 0.02*Roll;
  Pitch_output = 0.98*(0.1*Roll_output+0.9*d_angle_y) + 0.02*Pitch;

  //Normal filter
  // Roll_output = d_angle_x + 0.75(Roll-d_angle_x);
  // Pitch_output = d_angle_y + 0.75(Pitch-d_angle_y);

  // printing to see the values
  Serial.print(d_angle_x);
  Serial.print(" ");
  Serial.print(d_angle_y);
  Serial.print(" ");
  Serial.print(Roll_output);
  Serial.print(" ");
  Serial.print(Pitch_output);
  Serial.println();
}

//initialising or getting the gyro data
void get_gyro()
{
  accelgyro.getMotion9(&ax,&ay,&az,&gx,&gy,&gz,&mx,&my,&mz);
  Gxyz[0] = gx;
  Gxyz[1] = gy;
  Gxyz[2] = gz;
}

void get_accel()
{
  accelgyro.getMotion9(&ax,&ay,&az,&gx,&gy,&gz,&mx,&my,&mz);
  Axyz[0] = (float)ax/16384;
  Axyz[1] = (float)ay/16384;
  Axyz[2] = (float)az/16384;
}
// Proportional Intergal and differentiate
void PID_compute()
{
 error = Set_point - Input_angle;
 Output = Kp * error;
}

https://stanford.edu/class/ee267/misc/MPU-9255-Register-Map.pdf