ZHomeSlice
commented
5 years ago I have updated the MPU6050_6Axis_MotionApps20.h code to include a very fast PID based calibration routine. Make a backup of the old MPU6050 library just to be safe. The example code has the calibration routine and provides offsets for you to use.
you may want to try these equations out:
// Included with the library Gravity only Based pitch and roll
void GetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
#ifdef USE_OLD_GETYAWPITCHROLL
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
// pitch: (nose up/down, about Y axis)
data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
// roll: (tilt left/right, about X axis)
data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
#else
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
// pitch: (nose up/down, about Y axis)
data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
// roll: (tilt left/right, about X axis)
data[2] = atan2(gravity -> y , gravity -> z);
if (gravity -> z < 0) {
if(data[1] > 0) {
data[1] = PI - data[1];
} else {
data[1] = -PI - data[1];
}
}
#endif
}
// New and a possible new addition to this library. Try it out.
// DMP integrated Quaternion which I believe has Gyro and gravity based Pitch and Roll
void GetYawPitchRoll(float *data, Quaternion *q) {
data[0] = atan2(2.0f * (q -> x*q -> y + q -> w * q -> z), q -> w * q -> w + q -> x * q -> x - q -> y * q -> y - q -> z * q -> z);
data[1] = -asin(2.0f * (q -> x * q -> z - q -> w * q -> y));
data[2] = atan2(2.0f * (q -> w * q -> x + q -> y * q -> z), q -> w * q -> w - q -> x * q -> x - q -> y * q -> y + q -> z * q -> z);
}The first function has 2 options each react differently to the use of acceleration and gravity with unique output ranges.
The second function (Not in the mpu6050.c code) uses the DMP Quaternion only. I believe the DMP already integrates the acceleration values with the Gyro portion for the Quaternion calculations that are used with Pitch and Roll so I believe this to be the most accurate way to handle pitch and roll. Yaw has no other source to prevent drift completely until we can figure out the best way to integrate a magnetometer with the values.
Each of these functions generates different value ranges like pitch =+-90 while yaw and roll are -+180 (on the second function). See what fits.
In addition to the new calibration routines, I have added the code for MPU6050_6Axis_MotionApps_V6_12.h which uses the latest instance of the DMP firmware. Although it is bigger it contains an auto-calibration routine for gyro which further stabilizes drift after several seconds of no motion.
Z
bk4nt
Hello,
I'm using MPU6050_6Axis_MotionApps20.h plus main lines of MPU6050_DMP6 to get roll out of the MPU6050. That works fine as long as pitch = zero. Code being that of the example:
Callibration is done, using offsets provided by IMU_ZERO sketch (adding extra setX/YAccelOffset lines, but that made no difference):
Works, but when pitch is different than zero, roll gets affected. For example, extrem, if pitch (MPU6050 chip orientation) is 90°, the roll output of MPU6050 is some 80-85° for 45° chip roll.
With a roll of 24°, if pitch varies between +/-30°, roll output shifts from 24° to 27-28°. With a roll of 36°, if pitch varies between +/-30°, roll output shifts from 36° to 42-43°.
Do I miss use MPU6050_6Axis_MotionApps20.h and MPU6050 or is any exta math required when both roll and pitch are different from zero?
Best regards