added rapid "pid cont" in pidcont.*

mculoop/geometry.c

@@ -34,10 +34,10 @@ void eulerangle_norm(eulerangle_t* a) {
 }
 
 void quaternion_init(quaternion_t* q) {
-    q->w = 1.0f;
-    q->x = 0.0f;
-    q->y = 0.0f;
-    q->z = 0.0f;
+    q->w = 1.0;
+    q->x = 0.0;
+    q->y = 0.0;
+    q->z = 0.0;
 }
 
 void quaternion_toeuler(quaternion_t* q, eulerangle_t* a) {
@@ -47,22 +47,30 @@ void quaternion_toeuler(quaternion_t* q, eulerangle_t* a) {
     double z = q->z;
     double w = q->w;
 
+    double ax = 0.0;
+    double ay = 0.0;
+    double az = 0.0;
+
     double t0 = (x + z)*(x - z);     // x^2-z^2
     double t1 = (w + y)*(w - y);     // w^2-y^2
 
-    double xx = 0.5 * (t0 + t1);     // 1/2 x of x'
-    double xy = x*y + w*z;           // 1/2 y of x'
-    double xz = w*y - x*z;           // 1/2 z of x'
+    double n1 = 0.5 * (t0 + t1);     // 1/2 x of x'
+    double n2 = x*y + w*z;           // 1/2 y of x'
+    double n3 = w*y - x*z;           // 1/2 z of x'
 
-    double t  = xx*xx + xy*xy;       // cos(theta)^2
-    double yz = 2.0 * (y*z + w*x);   // z of y'
+    double t  = n1*n1 + n2*n2;       // cos(theta)^2
+    double n4 = 2.0 * (y*z + w*x);   // z of y'
 
-    a->z = atan2(xy, xx);             // yaw   (psi)
-    a->y = atan(xz /sqrt(t));         // pitch (theta)
+    az = atan2(n2, n1);              // yaw   (psi)
+    ay = atan(n3 / sqrt(t));         // pitch (theta)
 
-    if (t != 0) {
-        a->x = atan2(yz, t1 - t0);
+    if (t != 0.0) {                  // roll
+        ax = atan2(n4, t1 - t0);
     } else {
-        a->x = (2.0*atan2(x, w) - copysign(1.0, xz)*a->z);
+        ax = (2.0 * atan2(x, w) - copysign(1.0, n3) * az);
     }
+
+    a->x = ax;
+    a->y = ay;
+    a->z = az;
 }