Fix roll range

PaulStoffregen · PaulStoffregen · commit 0a202045b339 · 2016-03-24T08:53:57.000-07:00
diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp
@@ -243,12 +243,9 @@ float Madgwick::invSqrt(float x) {
 
 void Madgwick::computeAngles()
 {
-	float x = 2.0f * (q1*q3 - q0*q2);
-	float y = 2.0f * (q0*q1 + q2*q3);
-	float z = q0*q0 - q1*q1 - q2*q2 + q3*q3;
-	pitch = atan2f(x, sqrtf(y*y + z*z));
-	roll = atan2f(y, sqrtf(x*x + z*z));
-	yaw = atan2f(2.0f*q1*q2 - 2.0f*q0*q3, 2.0f*q0*q0 + 2.0f*q1*q1 - 1.0f);
+	roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
+	pitch = asinf(-2.0f * (q1*q3 - q0*q2));
+	yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
 	anglesComputed = 1;
 }
 

Original file line number	Diff line number	Diff line change
`@@ -243,12 +243,9 @@ float Madgwick::invSqrt(float x) {`
`243`	`243`
`244`	`244`	`void Madgwick::computeAngles()`
`245`	`245`	`{`
`246`		`- float x = 2.0f * (q1q3 - q0q2);`
`247`		`- float y = 2.0f * (q0q1 + q2q3);`
`248`		`- float z = q0q0 - q1q1 - q2q2 + q3q3;`
`249`		`- pitch = atan2f(x, sqrtf(yy + zz));`
`250`		`- roll = atan2f(y, sqrtf(xx + zz));`
`251`		`- yaw = atan2f(2.0fq1q2 - 2.0fq0q3, 2.0fq0q0 + 2.0fq1q1 - 1.0f);`
	`246`	`+ roll = atan2f(q0q1 + q2q3, 0.5f - q1q1 - q2q2);`
	`247`	`+ pitch = asinf(-2.0f * (q1q3 - q0q2));`
	`248`	`+ yaw = atan2f(q1q2 + q0q3, 0.5f - q2q2 - q3q3);`
`252`	`249`	`anglesComputed = 1;`
`253`	`250`	`}`
`254`	`251`