2
$\begingroup$

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.

$\endgroup$
10
  • $\begingroup$ Would you mind explaining what the variables are in the summation? Also, you should check the bounds on the summation. $\endgroup$
    – JSycamore
    Jun 15 '17 at 11:27
  • $\begingroup$ omega is angular velocity $\endgroup$
    – Sad.coder
    Jun 15 '17 at 23:38
  • $\begingroup$ d(t) is difference in time $\endgroup$
    – Sad.coder
    Jun 15 '17 at 23:39
  • $\begingroup$ and theta is well degrees or radians $\endgroup$
    – Sad.coder
    Jun 15 '17 at 23:39
  • $\begingroup$ The summation is looking even more suspect. Is $\delta$ a fixed time step? If so, then it is definitely not a continuous function of time. Also, nothing in the summation depends on $n.$ Are you trying write the difference equation: $\theta [k] = \theta[k-1]+\omega \delta_t$? $\endgroup$
    – JSycamore
    Jun 16 '17 at 13:55
1
$\begingroup$

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.

$\endgroup$
7
  • $\begingroup$ Hi sorry for the late reply, yes so wouldn't you use a complementary filter for it. $\endgroup$
    – Sad.coder
    Jun 15 '17 at 8:19
  • $\begingroup$ I am kinda of having problem with the complementary filter as well. $\endgroup$
    – Sad.coder
    Jun 15 '17 at 8:20
  • $\begingroup$ Roll_output = 0.98*(0.1*Roll_output+0.9*d_angle_x) + 0.02*Roll; $\endgroup$
    – Sad.coder
    Jun 15 '17 at 8:20
  • $\begingroup$ it seems that the gyro drift isn't eliminated with just gyro and accel data. However, I have seen people make it work I don't know if they are using fake values or not $\endgroup$
    – Sad.coder
    Jun 15 '17 at 8:22
  • 1
    $\begingroup$ Sure, you could use a complementary filter, but it's accuracy is still limited by the IMU you will always have drift $\endgroup$
    – Mark Omo
    Jun 15 '17 at 14:26
0
$\begingroup$

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

$\endgroup$
2
  • 1
    $\begingroup$ Arduino is an AVR without a floating point unit. Using float would make your calculations MUCH slower than they should really be. I strongly recommend working out the precision and range you need and use fixed point numbers. For example, if the precision you need is 1% of a degree, then you can have GxyzDeciDegree, which would be Gxyz * 100 (but integer). You might end up with different variables with different "units", which means your final calculations may require additional constants to fix up the final unit. $\endgroup$
    – Shahbaz
    Oct 13 '17 at 13:15
  • $\begingroup$ Also, with fixed point, pay attention to the order of your operations so you don't overflow your integers or lose your precision too much. $\endgroup$
    – Shahbaz
    Oct 13 '17 at 13:16

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.