Some errors in the code and questions about the filter's operation

Hello @mikeshub! When adapting this code to the LSM303 and L3DG20 sensors, I discovered a completely ineffective code. By reviewing the lines of code https://github.com/mikeshub/Pololu_Open_IMU/blob/e57681a01a09616f91e1404ec9a054b99002efc5/Pololu_Open_IMU.ino#L191 and https://github.com/mikeshub/Pololu_Open_IMU/blob/e57681a01a09616f91e1404ec9a054b99002efc5/Pololu_Open_IMU.ino#L277 an oddity was found in this line and replaced with the following code.

gyroSumX += groRead[X_];
gyroSumY += groRead[Y_];
gyroSumZ += groRead[Z_];

and

acc_x = accScaled[X_];
acc_y = accScaled[Y_];
acc_z = accScaled[Z_];

respectively. As a result, I compiled the code into firmware and output it to the plotter, I got reasonable pitch and roll angles, but I have drift. Also, when changing the roll angle, there is no effect on the pitch angle. But when I change the pitch angle more than 80 or -80 degrees, the roll angle begins to change. I also found differences in calculating pitch and roll angles in radians based on quaternions. Source: https://github.com/mikeshub/Pololu_Open_IMU/blob/e57681a01a09616f91e1404ec9a054b99002efc5/Pololu_Open_IMU.ino#L313 https://github.com/mikeshub/Pololu_Open_IMU/blob/e57681a01a09616f91e1404ec9a054b99002efc5/Pololu_Open_IMU.ino#L319 https://github.com/mikeshub/Pololu_Open_IMU/blob/e57681a01a09616f91e1404ec9a054b99002efc5/Pololu_Open_IMU.ino#L324 My corrected code:

void GetPitch() {
    // pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
        // pitchInRadians = asin(2.0 * (q0 * q2 - q3 * q1));
      pitchInRadians = asin(2.0 * (q0 * q2 - q1 * q3));
      pitchInDegrees =  pitchInRadians * RAD_TO_DEG;

}

void GetRoll() {
    // roll  = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
    // rollInRadians = atan2(2 * (q0 * q1 + q2 * q3),1 - 2.0 * (q1 * q1 + q2 * q2));
    rollInRadians = atan2((q0 * q1 + q2 * q3), 0.5 - (q1 * q1 + q2 * q2));
    rollInDegrees = rollInRadians * RAD_TO_DEG;
}

void GetYaw() {
    // yaw   = -atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - (q[2] * q[2] + q[3] * q[3]));
    // yawInRadians = atan2(2.0 * (q0 * q3 + q1 * q2),1 - 2.0 * (q2 * q2 + q3 * q3));
    yawInRadians = -atan2((q1 * q1q2 + q0 * q3), 0.5 - (q2 * q2 + q3 * q3));
    yawInDegrees = yawInRadians * RAD_TO_DEG;

     if (yawInDegrees < 0){
     yawInDegrees +=360;
     }
     if (yawInDegrees > 360){
     yawInDegrees -=360;
     }
}
void GetEuler() {
    GetPitch();
    GetRoll();
    GetYaw();
}

As far as I understand, the author of the code does not support and does not respond to requests... this is of course very bad when a person’s work is forgotten and becomes moldy. I wanted the code and the project to continue to live and benefit others.

mikeshub / Pololu_Open_IMU

Some errors in the code and questions about the filter's operation #9