AP_Math: added quaternion->matrix and earth frame routines

this will be used for 3d acceleration

libraries/AP_Math/AP_Math.cpp

@@ -56,7 +56,7 @@ void rotation_matrix_from_euler(Matrix3f &m, float roll, float pitch, float yaw)
 
 // calculate euler angles from a rotation matrix
 // this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
-void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw)
+void calculate_euler_angles(const Matrix3f &m, float *roll, float *pitch, float *yaw)
 {
 	if (pitch != NULL) {
 		*pitch	= -safe_asin(m.c.x);
@@ -73,14 +73,14 @@ void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw)
 // create a quaternion from Euler angles
 void quaternion_from_euler(Quaternion &q, float roll, float pitch, float yaw)
 {
-	float cr2 = cos(roll/2);
-	float cp2 = cos(pitch/2);
-	float cy2 = cos(yaw/2);
+	float cr2 = cos(roll*0.5);
+	float cp2 = cos(pitch*0.5);
+	float cy2 = cos(yaw*0.5);
 	// the sign reversal here is due to the different conventions
 	// in the madgwick quaternion code and the rest of APM
-	float sr2 = -sin(roll/2);
-	float sp2 = -sin(pitch/2);
-	float sy2 = sin(yaw/2);
+	float sr2 = -sin(roll*0.5);
+	float sp2 = -sin(pitch*0.5);
+	float sy2 = sin(yaw*0.5);
 
 	q.q1 = cr2*cp2*cy2 + sr2*sp2*sy2;
 	q.q2 = sr2*cp2*cy2 - cr2*sp2*sy2;
@@ -89,7 +89,7 @@ void quaternion_from_euler(Quaternion &q, float roll, float pitch, float yaw)
 }
 
 // create eulers from a quaternion
-void euler_from_quaternion(Quaternion &q, float *roll, float *pitch, float *yaw)
+void euler_from_quaternion(const Quaternion &q, float *roll, float *pitch, float *yaw)
 {
 	*roll = -(atan2(2.0*(q.q1*q.q2 + q.q3*q.q4),
 			1 - 2.0*(q.q2*q.q2 + q.q3*q.q3)));
@@ -98,3 +98,40 @@ void euler_from_quaternion(Quaternion &q, float *roll, float *pitch, float *yaw)
 	*yaw = atan2(2.0*(q.q1*q.q4 + q.q2*q.q3),
 		     1 - 2.0*(q.q3*q.q3 + q.q4*q.q4));
 }
+
+// convert a quaternion to a rotation matrix
+void quaternion_to_rotation_matrix(const Quaternion &q, Matrix3f &m)
+{
+	float q3q3 = q.q3 * q.q3;
+	float q3q4 = q.q3 * q.q4;
+	float q2q2 = q.q2 * q.q2;
+	float q2q3 = q.q2 * q.q3;
+	float q2q4 = q.q2 * q.q4;
+	float q1q2 = q.q1 * q.q2;
+	float q1q3 = q.q1 * q.q3;
+	float q1q4 = q.q1 * q.q4;
+	float q4q4 = q.q4 * q.q4;
+
+	m.a.x = 1-2*(q3q3 + q4q4);
+	m.a.y =   2*(q2q3 - q1q4);
+	m.a.z = - 2*(q2q4 + q1q3);
+	m.b.x =   2*(q2q3 + q1q4);
+	m.b.y = 1-2*(q2q2 + q4q4);
+	m.b.z =  -2*(q3q4 - q1q2);
+	m.c.x =  -2*(q2q4 - q1q3);
+	m.c.y =  -2*(q3q4 + q1q2);
+	m.c.z = 1-2*(q2q2 + q3q3);
+}
+
+// convert a vector in earth frame to a vector in body frame,
+// assuming body current rotation is given by a quaternion
+void quaternion_earth_to_body(const Quaternion &q, Vector3f &v)
+{
+	Matrix3f m;
+	// we reverse z before and afterwards because of the differing
+	// quaternion conventions from APM conventions.
+	v.z = -v.z;
+	quaternion_to_rotation_matrix(q, m);
+	v = m * v;
+	v.z = -v.z;
+}

libraries/AP_Math/AP_Math.h

@@ -24,10 +24,17 @@ float safe_sqrt(float v);
 void rotation_matrix_from_euler(Matrix3f &m, float roll, float pitch, float yaw);
 
 // calculate euler angles from a rotation matrix
-void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw);
+void calculate_euler_angles(const Matrix3f &m, float *roll, float *pitch, float *yaw);
 
 // create a quaternion from Euler angles
 void quaternion_from_euler(Quaternion &q, float roll, float pitch, float yaw);
 
 // create eulers from a quaternion
-void euler_from_quaternion(Quaternion &q, float *roll, float *pitch, float *yaw);
+void euler_from_quaternion(const Quaternion &q, float *roll, float *pitch, float *yaw);
+
+// convert a quaternion to a rotation matrix
+void quaternion_to_rotation_matrix(const Quaternion &q, Matrix3f &m);
+
+// convert a vector in earth frame to a vector in body frame,
+// assuming body current rotation is given by a quaternion
+void quaternion_earth_to_body(const Quaternion &q, Vector3f &v);