I do not know why roll angle is not correct. But system looks okay

1 files changed, 38 insertions, 34 deletions

diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
index 81a5e5b07..a8561a078 100755
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
@@ -44,8 +44,8 @@ extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[])
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
 static int attitude_estimator_so3_comp_task;				/**< Handle of deamon task / thread */
-volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;					// quaternion of sensor frame relative to auxiliary frame
-volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// integral error terms scaled by Ki
+static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;					// quaternion of sensor frame relative to auxiliary frame
+static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// integral error terms scaled by Ki
 
 /**
  * Mainloop of attitude_estimator_so3_comp.
@@ -135,7 +135,6 @@ void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float
 	float recipNorm;
 	float halfvx, halfvy, halfvz;
 	float halfex, halfey, halfez;
-	float qa, qb, qc;
 
 	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
 	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
@@ -181,13 +180,10 @@ void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float
 	gx *= (0.5f * dt);		// pre-multiply common factors
 	gy *= (0.5f * dt);
 	gz *= (0.5f * dt);
-	qa = q0;
-	qb = q1;
-	qc = q2;
-	q0 += (-qb * gx - qc * gy - q3 * gz);
-	q1 += (qa * gx + qc * gz - q3 * gy);
-	q2 += (qa * gy - qb * gz + q3 * gx);
-	q3 += (qa * gz + qb * gy - qc * gx); 
+	q0 += (-q1 * gx - q2 * gy - q3 * gz);
+	q1 += (q0 * gx + q2 * gz - q3 * gy);
+	q2 += (q0 * gy - q1 * gz + q3 * gx);
+	q3 += (q0 * gz + q1 * gy - q2 * gx); 
 	
 	// Normalise quaternion
 	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
@@ -203,7 +199,6 @@ void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az
 	float hx, hy, bx, bz;
 	float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
 	float halfex, halfey, halfez;
-	float qa, qb, qc;
 
 	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
 	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
@@ -282,13 +277,10 @@ void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az
 	gx *= (0.5f * dt);		// pre-multiply common factors
 	gy *= (0.5f * dt);
 	gz *= (0.5f * dt);
-	qa = q0;
-	qb = q1;
-	qc = q2;
-	q0 += (-qb * gx - qc * gy - q3 * gz);
-	q1 += (qa * gx + qc * gz - q3 * gy);
-	q2 += (qa * gy - qb * gz + q3 * gx);
-	q3 += (qa * gz + qb * gy - qc * gx); 
+	q0 += (-q1 * gx - q2 * gy - q3 * gz);
+	q1 += (q0 * gx + q2 * gz - q3 * gy);
+	q2 += (q0 * gy - q1 * gz + q3 * gx);
+	q3 += (q0 * gz + q1 * gy - q2 * gx); 
 	
 	// Normalise quaternion
 	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
@@ -532,19 +524,19 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					float c = q2;
 					float d = q3;
 
-					Rot_matrix[0] = aSq + bSq - cSq - dSq;
-        				Rot_matrix[1] = 2.0 * (b * c - a * d);
-        				Rot_matrix[2] = 2.0 * (a * c + b * d);
-        				Rot_matrix[3] = 2.0 * (b * c + a * d);
-        				Rot_matrix[4] = aSq - bSq + cSq - dSq;
-        				Rot_matrix[5] = 2.0 * (c * d - a * b);
-        				Rot_matrix[6] = 2.0 * (b * d - a * c);
-        				Rot_matrix[7] = 2.0 * (a * b + c * d);
-        				Rot_matrix[8] = aSq - bSq - cSq + dSq;
+					Rot_matrix[0] = aSq + bSq - cSq - dSq;	// 11
+        				Rot_matrix[1] = 2.0 * (b * c - a * d);	// 12
+        				Rot_matrix[2] = 2.0 * (a * c + b * d);	// 13
+        				Rot_matrix[3] = 2.0 * (b * c + a * d);	// 21
+        				Rot_matrix[4] = aSq - bSq + cSq - dSq;	// 22
+        				Rot_matrix[5] = 2.0 * (c * d - a * b);	// 23
+        				Rot_matrix[6] = 2.0 * (b * d - a * c);	// 31
+        				Rot_matrix[7] = 2.0 * (a * b + c * d);	// 32
+        				Rot_matrix[8] = aSq - bSq - cSq + dSq;	// 33
 
-					/* Compute Euler angle */
-					float theta = asinf(-Rot_matrix[6]);
-					euler[1] = theta;
+					/* FIXME : Work around this later...
+					float theta = asinf(-Rot_matrix[6]);	// -r_{31}
+					euler[1] = theta;			// pitch angle
 
 					if(fabsf(theta - M_PI_2_F) < 1.0e-3f){
 						euler[0] = 0.0f;
@@ -556,6 +548,16 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						euler[0] = atan2f(Rot_matrix[7], Rot_matrix[8]);
 						euler[2] = atan2f(Rot_matrix[3], Rot_matrix[0]);
 					}
+					*/
+
+					float q1q1 = q1*q1;
+					float q2q2 = q2*q2;
+					float q3q3 = q3*q3;
+
+					euler[0] = atan2f(2*(q0*q1 + q2*q3),1-2*(q1q1+q2q2));	// roll
+					euler[1] = asinf(2*(q0*q2 - q3*q1)); // pitch
+					euler[2] = atan2f(2*(q0*q3 + q1*q2),1-2*(q2q2 + q3q3)); // yaw
+					
 
 					/* swap values for next iteration, check for fatal inputs */
 					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
@@ -577,10 +579,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					att.pitch = euler[1] - so3_comp_params.pitch_off;
 					att.yaw = euler[2] - so3_comp_params.yaw_off;
 
-					/* FIXME : This can be a problem for rate controller. Rate in body or inertial?
-					att.rollspeed = x_aposteriori[0];
-					att.pitchspeed = x_aposteriori[1];
-					att.yawspeed = x_aposteriori[2];
+					/* FIXME : This can be a problem for rate controller. Rate in body or inertial? */
+					att.rollspeed = q1;
+					att.pitchspeed = q2;
+					att.yawspeed = q3;
+
+					/*
 					att.rollacc = x_aposteriori[3];
 					att.pitchacc = x_aposteriori[4];
 					att.yawacc = x_aposteriori[5];