RPY 좌표계 소스

X축 각도(가속) = arctan(Y축 가속 / Z축 가속) x 180 / PI

Y축 각도(가속) = arctan(X축 가속 / Z축 가속) x 180 / PI

[출처] mpu6050 각도 계산 이게 맞는가요 ? (당근이의 AVR 갖구 놀기) |작성자 꾸용

void compFilter(void)
{
getGyroDegree();
getAcclDegree();


xTmp1 = (-y_aResult) + (float)xFilterAngle;
xIntTmp1 = (float)xIntTmp1 + (xTmp1 * 0.01);
xTmp2 = (-kp * xTmp1) + (-ki * (float)xIntTmp1) + x_gResult;
xFilterAngle = xFilterAngle + (xTmp2 * 0.01);
pitch = (int)(xFilterAngle * 180 / PI);

yTmp1 = (-x_aResult) + (float)yFilterAngle;
yIntTmp1 = (float)yIntTmp1 + (yTmp1 * 0.01);
yTmp2 = (-kp * yTmp1) + (-ki * (float)yIntTmp1) + y_gResult;
yFilterAngle = yFilterAngle + (yTmp2 * 0.01);
roll = (int)(yFilterAngle * 180 / PI);
}

[출처] mpu6050 각도 계산 이게 맞는가요 ? (당근이의 AVR 갖구 놀기) |작성자 꾸용