tomasstio_as:
Hello all,
using the angle measured by the gyro.
Thanks for your help
You are going to find that the gyro is really a rate gyro. The gyro is going to give you the rate of precession. When the gyro stops its precession the gyro will read 0. The accelerometers will give you tilt angle. Gyros drift and accelerometers are noisy. Integrating the readings from the gyros and accelerometers filters the data. If you are going to use an Arduino Uno, the best you can do is to use a complementary filter, which will work fairly well.
If you need better output results then your choices are a Kalman filter or a Quaternion/Euler Angle filter, these will not do very well on a Uno.
I have had great results with the MahonyQuaternionUpdate filter. There is, also the MadgwickUpdateFilter. You can get the code for the Quaternion Filters here: https://os.mbed.com/users/onehorse/code/MPU9250AHRS/file/4e59a37182df/MPU9250.h/
You will need to define these values for yourself based on your research into the MMU that you use:
float deltat = 0.0f; // integration interval for both filter schemes
#define Kp 7.50f
// #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 1.7f
The values, of Kp and Ki were found through lots of digging into the data of the MPU9250 and then fine tuning those numbers based upon 100 hour runs per number tweak.
It is better to use a value for Kp and Ki as they adjust, via eInt over time to compensate for various drift factors with the Mahony filter:
float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method
Conversion of the Quaternions into Euler Angles is done by the following formulas:
MahonyQuaternionUpdate ( ax[0], ay[0], az[0], gx[0], gy[0], gz[0], mx[0], my[0], mz[0] );
float yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
float pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
float roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
The value of float deltat = 0.0f; needs to be done by your own routines. I do the following:
unsigned long TimePast = micros();
unsigned long TimeNow = micros();
TimeNow = micros();
deltat = ((TimeNow - TimePast) / 1000000.0f);
Read the gyros, accelerometers, do the Mahony, do the Euler angle conversion, do any other number crunching, and the last thing to do is TimePast = micros();. The closer to the beginning and end of your code processing you can record time now and time past the more accurate your results will be; I hope that makes sense. deltat must be preserved between each iteration of your calculation. There is a easy way to use nano seconds, with a ESP32 but you'll get overflows in about 28 seconds that you have to compensate for. Which adds more code which increases the time between reads and calculations. I found it was not worth the effort.
A Arduino Due and a ESP32 has enough arse, from what I have used, to handle the calculations. The ESP32 takes about 290uS to do the Mahony, Euler, and other processing. I get great results with an ESP32, and reading the MPU9250 FIFO buffer, and processing the MPU info at 10 times a second. I get a bit less than 30 FIFO samples per read, I average the contents of the FIFO buffer before sending the data to the Mahony.
I use the MPU data to create servo torque values to maintain a X/Y platform to level.
After all that, you can get fair results using a complementary filter and a Uno with data reads of about 1X or 2X a second. A search the net for a mmu complementary filter will get you the code.
A complementary (with low pass filter) can result in oscillating values, you can adjust those oscillations out, the K and K1 values, but you reduce sensitivity by doing so.
I had a few runs with the MahonyQuaternionUpdate where data oscillations occurred when I used a 0 for Ki.