Issues reading Gyro Data, and issues with my own PID Controller

Hello, I am a MET student who is trying to implement some control theory with an Arduino micro. I am using an LSM9DS1 sensor with the SparkFun i2c library as a basis for my code. The complimentary and moving average filter that I used on the MPU6050 is having trouble giving me accurate gyro values once I have added in my own PID code for some reason. I attempted to isolate the gyro values to diagnose by integrating them with a small delay dt, but the gyro values seem to be giving nonsense and floating around 0. My D term for my PID controller also does not really work, which leads me to believe that something with dt value is very wrong. Does anyone have any advice?

This is my second Arduino project, and im trying to avoid using libraries that make the control theory easier so that I can learn as much as possible.

#include <Wire.h>
#include <SPI.h>
#include <SparkFunLSM9DS1.h>
#include <Servo.h>
LSM9DS1 imu;
Servo R;
Servo L;

void print_values();

float timer,timer2;
int i=0;
int ip=0;
float Az,Ay,Ax,Gz,Gx,Gy,Gangle,Aroll,Apitch,ApitchAvg,pitch_store,pitch,pitchp;
float error,Perror,PID_p,PID_i,PID_ip,PID_d,Output;
float Rout,Lout;
float CF = .96;
float SetPoint=0;
float kp =.3;
float kd =25;
float ki = 0.0;

float Min = 35;

void setup() {

   //initializes imu (not sure how but without it no values can be grabbed)
    if (imu.begin() == false) // with no arguments, this uses default addresses (AG:0x6B, M:0x1E) and i2c port (Wire).
    Serial.println("Somthing is very wrong, I am a computer, so you will have to figure out what it is");

float dt = .009;

void loop() {
   Serial.print(" ");
   if ( imu.accelAvailable() ){
    //Grabs acceleration values 
    //I chose not to go with the given conversion to Gs 
    //calculated the pitch and roll, and converts to degrees
    Aroll = atan2(Ay,Az)*180.0 / PI;
    Apitch = -(atan2(Ax, sqrt(Ay*Ay+(Az*Az))))*180.0 / PI;
   if ( imu.gyroAvailable() ){
    //grabs gyro data and converts to degrees/s
    Gz = (imu.calcGyro(imu.gz));
    Gx = (imu.calcGyro(imu.gx));
    Gy = (imu.calcGyro(;
  //Moving Average Filter (taken out to diagnose)
  while (i <= 6)
    pitch_store = pitch_store + Apitch;
    i +=1;
    if (i == 6){
      pitch_store = 0;

  pitch = (Cf*(pitch +(Gx*dt))+(1-CF*ApitchAvg));\

  Gangle = Gangle + (Gx*dt);
  error = SetPoint - pitch;
  PID_p = kp*error;
  PID_d = kd*((error-Perror)/(dt));
  PID_i = PID_i+(ki*(error*(dt))); 
  Output = PID_p+ PID_i + PID_d;
  Perror = error;
   //prints current needed

 //writes to a balance beam with two brushless motors and props that i made
  Rout = Min+Output;
  Lout = Min-Output;
  delay((dt*1000) - (millis() - timer));

void print_values(){

What do you mean by "accurate gyro values" (the gyro measures rate of rotation), and what are you trying to control?

If you want 3D orientation angles, these days almost everyone uses Madgwick or Mahony fusion filters. Here is a Mahony filter for the LSM9DS1.

I have a balance beam with two props, motors, and escs on either side. I am attempting to get the system to balance on this scale before moving to my thrust vectoring project. I assumed that by integrating the gyro values I could get a theta reading. I will definitely look into the Mahoney filter as that seems to be what im missing for calculating the offset of the gyro. This still does not explain why my gyro always sits around 0 while integrated with time.

This still does not explain why my gyro always sits around 0 while integrated with time.

There are so many problems with the code that it is hard to say which might be causing the issue(s).

Where do you subtract the gyro offsets?

What is the data rate setting for the gyro?

What are the units of rate values returned by the gyro sensor, and how are those scaled to sensible values?

Where does "dt" come from?

What is the purpose of this line? An arbitrary delay can make nonsense of time integration.

  delay((dt*1000) - (millis() - timer));

Usually one integrates the data as soon as all sensors have been updated, and as quickly as possible, so using delay() would be a mistake.

Thank your Re-reply, dt was set at .009 secounds(top of the code). The delay was an attempt to mitigate noise however from your reply this sounds like it will not work. Essentially delaying any time that does not add up to .009

As far as the offset, I’m ignoring it for now as I’m just attempting to gain an angle from its starting point. Data rate setting I am ignorant on as well, I belive the sparkfun library takes care of that. It gives a calculated gyro value in d/s.

Additionally I would love to know the other issues with the code!

The code linked in reply #1 actually works.

dt was set at .009 secounds

Yes, I saw that. Why did you choose that particular value?

Data rate setting I am ignorant on as well, I belive the sparkfun library takes care of that.

Sparkfun does not relieve you of the responsibility of knowing the value. You cannot integrate data faster than it arrives.

There are three sensors on the LSM9DS1 module, all with different data rates.

And the concern for the gyro's reading 0. When are the gyro's measuring 0?

Those gyro's are known as rate gyro's. A rate gyro gives the rate of change.

"Those gyro's are known as rate gyro's. A rate gyro gives the rate of change."

Yup! This is why im attempting to integrate the values

"Yes, I saw that. Why did you choose that particular value?"

I chose .009 as it was 3-4 milliseconds slower than then my testing of how fast it was refreshing the data. I wanted to get a time value that was definitely slower than the gyro outputs. To test it, I recorded the time after the gyro was sent to the output, then right before it again. Found the dt (it was around 5-6 milliseconds)

"There are three sensors on the LSM9DS1 module, all with different data rates."

Looking at the data sheet,I do see that. It looks like there are multiple settings for the data output, rangine from .6hz to 80hz (quite the range). The default setting is 000 for the gyro which indicates no cutoff hz? I am very confused!

pg 46

This is what the LSM9DS1 data sheet says. The question that really should concern you is: to what value does the Sparkfun library set the gyro ODR?



Neither the Sparkfun nor the factory lsm library designates ODR or data rate selection. I believe it is in the factory setting.

The Sparkfun LSM9DS1 library sets the gyro output data rate (ODR) using the following code, in SparkfunLSM9DS1.cpp

	// gyro sample rate: value between 1-6
	// 1 = 14.9    4 = 238
	// 2 = 59.5    5 = 476
	// 3 = 119     6 = 952	
settings.gyro.sampleRate = 6;

Best of luck with your project.

Thank you! There was a setting file that I somehow missed. I see this now. The current setting was around 3, much faster than my delay. strange.

Do I use that sample rate to integrate with the gyro ie :

Gangle = Gangle + (Gx*(1/238)); with a setting of 4?

This gives me just a straight 0 for gyro

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.