Diff: main.cpp

Revision:

5:4c03a1a0ad98

Parent:

3:e6d345973797

diff -r 52d2d31a7347 -r 4c03a1a0ad98 main.cpp
--- a/main.cpp	Thu Dec 05 09:09:31 2019 +0000
+++ b/main.cpp	Tue Mar 10 09:14:00 2020 +0000
@@ -29,6 +29,7 @@
 
 // user defined functions
 double quat2psi(double qw, double qx, double qy, double qz);
+void   quat2rpy(double qw, double qx, double qy, double qz, double *phi, double *theta, double *psi);
 double getShortRotation(double ang);
 
 const int N = 40;
@@ -75,11 +76,18 @@
         double e_ang = getShortRotation(target_angle - phiR);
         // update angle controller
         */
-        // double qw = 0.984005578673161;
-        // double qx = 0.009063618716137;
-        // double qy = 0.177899336159591;
-        // double qz = 0.001629344754243;
+        double qw = 0.984005578673161;
+        double qx = 0.009063618716137;
+        double qy = 0.177899336159591;
+        double qz = 0.001629344754243;
+        double phi, theta, psi;
+        quat2rpy(qw, qx, qy, qz, &phi, &theta, &psi);
+        if(i++ == 0) {
+            pc.printf("%.15f; %.15f; %.15f; \r\n", phi, theta, psi);
+        }
         
+        
+        /*
         if(i < 200) {
             // pc.printf("%i; %.12f; %.12f; %0.12f; %.6f;\r\n", i, quat2psi(qw, qx, qy, qz), qx*qy + qw*qz, 0.5 - (qy*qy + qz*qz), dt);
             // pc.printf("%i; %i; %.6f;\r\n", i, target_planer.returnPathLength(), dt);
@@ -96,24 +104,32 @@
         
         i++;        
         wait_us(33333);
+        */
     }
 }
 
-/*
-function psi = quat2psi(q)
-% q = [qw qx qy qz] (= [q0, q1, q2, q3] = [qr, qi, qj, qk])
-    for i = 1:length(q) % restore norm
-        q(i,:) = q(i,:) / sqrt(q(i,1)^2 + q(i,2)^2 + q(i,3)^2 + q(i,4)^2);
-    end
-    psi   = atan2( (q(:,2).*q(:,3) + q(:,1).*q(:,4) ), 1/2 - (q(:,3).^2 + q(:,4).^2) );
-end
-*/
 double quat2psi(double qw, double qx, double qy, double qz)
 {
     double s = 1/(qw*qw + qx*qx + qy*qy + qz*qz);    
     return atan2(s*(qx*qy + qw*qz), 0.5 - s*(qy*qy + qz*qz));
 }
 
+void quat2rpy(double qw, double qx, double qy, double qz, double *phi, double *theta, double *psi)
+{
+    double s = 1/(qw*qw + qx*qx + qy*qy + qz*qz);
+    
+    *phi = atan2(s*(qy*qz + qw*qx), 0.5 - s*(qx*qx + qy*qy));
+    
+    *theta = -2.0*s*(qx*qz - qw*qy);
+    if(*theta >  1.0) *theta =  1.0;
+    if(*theta < -1.0) *theta = -1.0;
+    *theta = asin(*theta);
+    
+    *psi = atan2(s*(qx*qy + qw*qz), 0.5 - s*(qy*qy + qz*qz));
+    
+    return;
+}
+
 double getShortRotation(double ang)
 {   
     // we maybe need to introduce a threshold in case the angle is noisy and around +/-pi (180° rotation)