Very strange Uno with MPU9250 IMU Issue

Hi,

I ran into a very strange issue. I'm trying to do a gyro angle drift compensation. Before the compensation, I saw there was about 67 degree drifting every hour. But after I did the compensation, the drifting became crazy, it's 30 degrees per second. >:(

Here is related code,

loop()
{

....
read gz from register

static unsigned long old_time = 0;
unsigned long new_time;
new_time = millis();
angle += gz/1000.0 * (new_time - old_time);
old_time = new_time;

...
// gyro dift compensation:
// we saw 67.6 degrees incremental in 1 hour (60 minutes * 60 seconds * 1000ms)
orig_angle = angle;
float m = (67.6/3600.0)/1000.0;
Serial.println((float)m, 6);
uint32_t k = millis() - start_time;
float angle1 = m * ((float)k) ;
angle = angle - angle1; <<<<<<<<<<<<<<<< if I commented this line out, I got qW reading reasonable. Otherwise, qW reading went crazy.

// AutoX IMU msg format
Serial.println("<qW:" + String(angle, 4)

  • ",qX:" + String(orig_angle, 4)
  • ",qY:" + String(0.0, 4)
  • ",qZ:" + String(0.0, 4)
  • ",eX:" + String(yaw, 4)
  • ",eY:" + String(pitch, 4)
  • ",eZ:" + String(roll, 4)
  • ">\r\n");
    }

the only line of code causes trouble was in red. I really don't understand why. Could anyone help?

thanks a lot and have a good night,

Steve

In every loop you do the compensation for the complete running time again. This sums up very fast.

hmm... I also suspected the computing power could be the issue. But with Arduino UNO, I don't know any way to check its CPU usage.

I even tried a Arduino Due board which is 32 bit and supposed to be much more powerful. Well, I ran into similar issue, probably even worse. :slight_smile:

In general it is much easier to find problems if you post your whole code, or at least a shortened version that still exhibits the problem.
Make sure you post it in code tags.

Your code will also be easier to read if you use more meaningful variable names, e.g.

float driftPerMillisecond = (67.6/3600.0)/1000.0;
uint32_t timeSinceStart = millis() - start_time;
float driftSinceStart = driftPerMillisecond  * ((float)timeSinceStart) ;

But the real problem is that each loop you are adding the change in angle based on the gyro reading and the recent time interval, and then subtracting the total expected drift that has occurred from the start, not just the expected drift from the latest interval.

hmm... I also suspected the computing power could be the issue. But with Arduino UNO, I don't know any way to check its CPU usage.

I wrote nothing about computing power! You overcompensate in your code!

But with Arduino UNO, I don't know any way to check its CPU usage.

No need, it is always 100%.