Out of bounds FilteredAngY and Z (possibly X too) ? about tinympu6050 HOT 9 CLOSED

Hey again @foxabilo!

Yeah, these are definitely out of bounds. The convention for this library is -179.99~179.99. Could you please post the code you used, for error reproducing purposes?

from tinympu6050.

No problem will do, also I found this after leaving the system running over night
16:40:40.871 -> AccelAngX = 179.60
16:40:40.871 -> AccelAngY = -179.59
16:40:40.871 -> AccelAngZ = 135.49

The vales were fixed at those numbers, no change on movement, a quick reset later and all was well again, but it seems to loose min and max extents as time passes, almost as if the Delta T was being incorrectly processed.

changes to the .h file were the deadbands we talked about previously, this is the code I used , just your code with just the data I was interested in.

/*
 *  Mandatory includes
 */
#include <Arduino.h>
#include <TinyMPU6050.h>
#include <Wire.h>

/*
 *  Constructing MPU-6050
 */
MPU6050 mpu (Wire);

/*
 *  Method that prints everything
 */
void PrintGets () {
  mpu.Execute();
  Serial.println("--- Accel angles:");
  Serial.print("AccelAngX = ");
  Serial.println(mpu.GetAngAccX());
  Serial.print("AccelAngY = ");
  Serial.println(mpu.GetAngAccY());
  Serial.print("AccelAngZ = ");
  Serial.println(mpu.GetAngAccZ());

  Serial.println("--- Filtered angles:");
  Serial.print("FilteredAngX = ");
  Serial.println(mpu.GetAngX());
  Serial.print("FilteredAngY = ");
  Serial.println(mpu.GetAngY());
  Serial.print("FilteredAngZ = ");
  Serial.println(mpu.GetAngZ());

}

/*
 *  Setup
 */
void setup() {

  // Initialization
  Wire.begin(4,15);
  mpu.Initialize();

  // Calibration
  Serial.begin(115200);
  Serial.println("=====================================");
  Serial.println("Starting calibration...");
  mpu.Calibrate();
  Serial.println("Calibration complete!");
}

/*
 *  Loop
 */
void loop() {
  
  PrintGets();
  delay(1000); // 30 sec delay

}

My6050Project.zip

from tinympu6050.

it seems to loose min and max extents as time passes

That's something interesting, I'll perform some tests on my ESP32 (later, as it's occupied now) and will let you know about it. If you notice something suspicious, please let me know.

Now that you've mentioned this, I remember that there were some mistakes for the angle computing (orientation stuff) I had to fix, maybe it's something about that.

Anyway, I'll be working on it.

from tinympu6050.

It is not surprising to get out-of-bounds angles. If, as a first approximation, we neglect filterAccelCoeff, then the angles are updated by periodically running, within MPU6050::Execute(), something equivalent to

angX += gyroX * dt;

and so forth for angY and angZ. If the IMU experiences continuous rotation, the computed angle will grow unbounded. In order to get angles within ±180°, out of bounds angles have to be explicitly wrapped into the valid range.

Then, there is the issue of filterAccelCoeff and filterGyroCoeff. These coefficients are used to compute a weighted average between two estimates of the angles, but the average is computed in an incorrect way. Angles should not be averaged like regular numbers. For example, the average between the numbers +179 and −179 is 0, but the average between the angles +179° and −179° should be evaluated to ±180°.

A simple way to get the correct angle average is to wrap the difference between the angles. In this example,

• the raw difference between +179° and −179° is −358°
• the wrapped difference is +2°
• the angle −179° is thus equivalent to +179°+2°
• the average between +179° and (+179°+2°) is +180°

Here is how I would compute the filtered, correctly wrapped angles:

/*
 *	Wrap an angle to the interval [-180, +180]
 */
static float wrap(float angle)
{
	while (angle > +180) angle -= 360;
	while (angle < -180) angle += 360;
	return angle;
}

/*
 *	Private method used to filter the angles
 */
float MPU6050::filterAngle(float angle, float gyro, float angAcc)
{
	angle += gyro * dt;
	angle += filterAccelCoeff * wrap(angAcc - angle);
	return wrap(angle);
}

void MPU6050::Execute() {
	// ...

	// Computing complementary filter angles
	angX = filterAngle(angX, gyroX, angAccX);
	angY = filterAngle(angY, gyroY, angAccY);
	angZ = filterAngle(angZ, gyroZ, angAccZ);

	// ...
}

Note that filterGyroCoeff is not used in this code, as it is implicitly assumed to be equal to 1-filterAccelCoeff. Any other value would make no sense.

from tinympu6050.

@edgar-bonet I'll implement this as said.
@foxabilo Sorry for the delay, I'll release 0.5.0 ASAP.

Thanks everybody.

from tinympu6050.

Release 0.5.0 is now out here.

I'll close this for now. If you continue to have issues please contact. Thanks!

from tinympu6050.

These expressions are not quite right:

angGyroX += wrap(gyroX * dt);

For once, unless something is very wrong (huge dt), gyroX * dt is a small angle. Thus, the call to wrap() is almost guaranteed to have no effect. On the other hand, if the IMU is continuously rotating, repeatedly adding small angles will end up giving an out-of-range sum.

The fix is to wrap the sum rather than the small increments:

angGyroX = wrap(angGyroX + gyroX * dt);

And then there is this:

angX = wrap((filterAccelCoeff * angAccX) + (filterGyroCoeff * (angX + gyroX * dt)));

As I tried to explain before, one should not average angles like plain numbers. Try to think what would happen if angAccX is −179° and angX + gyroX * dt is +179°: you would get a value for angX that is way off.

The solution is, as I wrote before, to wrap the difference between the numbers one wants to average:

angX = wrap(filterAccelCoeff * angAccX
     + filterGyroCoeff * (angAccX + wrap(angX + gyroX * dt - angAccX)));

A weighted_average() helper function could make the intent of the code a little bit clearer:

/*
 *	Compute the weighted average of a (with weight wa)
 *	and b (weight wb), treating both a and b as angles.
 *	It is assumed the sum of the weights is 1.
 */
static float weighted_average(float wa, float a, float wb, float b)
{
	return wrap(wa * a + wb * (a + wrap(b-a)));
}

void MPU6050::Execute() {
	// ...
	angX = weighted_average(filterAccelCoeff, angAccX,
	                        filterGyroCoeff, angX + gyroX * dt);
	// ...
}

from tinympu6050.

@edgar-bonet you're absolutely right. I was too focused on delivering the new release ASAP I haven't noticed that stuff.

Is it of your interest to call for a pull request for those things (as well as things here #12 (comment)) or do you want me to implement it myself? Any help from outside is welcome and you could as well become a contributor to this repository, what do you think?

from tinympu6050.

OK, I will try to open a couple of pull requests. It's always fun. :-)

Keep in mind, though, that my interest in the project is quite limited. I do not have an MPU to play with and test. Instead, I am a regular contributor to the Arduino Stackexchange site, and I have seen there quite a few instances of things like

the_16_bit_value = Wire.read() << 8 | Wire.read();

Since the users posting this sort of code are most of the time copying it from examples seen on the Web, I though it would be useful to hunt these idioms on GitHub and file issues. This is how I stumbled upon this repo.

from tinympu6050.