[paparazzi-commits] [4590] Corrected bug in q6d_diff_flatness.sci, function df_state_of_fo and added

[Gustavo Oliveira Violato]

Revision: 4590
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4590
Author:   gov
Date:     2010-02-25 02:14:45 +0000 (Thu, 25 Feb 2010)
Log Message:
-----------
Corrected bug in q6d_diff_flatness.sci, function df_state_of_fo and added
povray utilities. Seems to work in open loop.

Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/scilab/q3d/Makefile
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce

Added Paths:
-----------
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_povray.sci

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/Makefile
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/Makefile   2010-02-24 17:00:39 UTC 
(rev 4589)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/Makefile   2010-02-25 02:14:45 UTC 
(rev 4590)
@@ -7,4 +7,4 @@
        ffmpeg -f image2 -i images/frame_%04d.ppm out.mpg
 
 clean:
-       rm images/*
\ No newline at end of file
+       rm -f *~ \#*\#
\ No newline at end of file

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-02-24 17:00:39 UTC (rev 4589)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-02-25 02:14:45 UTC (rev 4590)
@@ -78,7 +78,7 @@
   adv = (axpsi*adxpsi + z2dmg*fo(3,4))/av;
   
   phid   = sign(z2dmg)*(adypsi*av-adv*aypsi)/(aypsi^2+av^2);
-  thetad = (adxpsi*z2dmg-z3d*aypsi)/(axpsi^2+z2dmg^2);
+  thetad = (adxpsi*z2dmg-z3d*axpsi)/(axpsi^2+z2dmg^2);
   
   cphi = cos(state(DF_REF_PHI));
   sphi = sin(state(DF_REF_PHI));

Added: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_povray.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_povray.sci                     
        (rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_povray.sci     2010-02-25 
02:14:45 UTC (rev 4590)
@@ -0,0 +1,34 @@
+
+function povray_draw( time, diff_flat_ref )
+  
+  dt_display = 1/25;
+
+  t_idx = 1;
+  f_idx = 1;
+  t = time(t_idx);
+  while (t_idx<length(time))
+    printf('drawing %d (%f)\n', f_idx, time(t_idx));
+    x = -1000*diff_flat_ref(DF_REF_X,t_idx);
+    y = 1000*diff_flat_ref(DF_REF_Y,t_idx);
+    z = -1000*diff_flat_ref(DF_REF_Z,t_idx);
+    phi =  deg_of_rad(-diff_flat_ref(DF_REF_PHI,t_idx));
+    theta =  deg_of_rad(-diff_flat_ref(DF_REF_THETA,t_idx));
+    psi =  deg_of_rad(-diff_flat_ref(DF_REF_PSI,t_idx));
+//    printf('( %f %f %f)\n', x, y, alpha);
+    fid = mopen('povray/q3d.pov', "w");
+    mfprintf(fid, "#include ""povray/q3d.inc""\n");
+    mfprintf(fid, "object { Q3D()  rotate <%f,%f,%f> translate 
<%f,%f,%f>}",-phi,psi,-theta,x,y,z);
+    mclose(fid);
+    cmd = sprintf('povray povray/q3d.pov +Opovray/foo%04d.png Display=false 
+W800 +H600 +Q9 +A0.3 +R5', f_idx);
+    a = unix_g(cmd);
+    while (t_idx<length(time) & time(t_idx) < f_idx*dt_display)
+      t_idx = t_idx + 1;
+    end
+    f_idx = f_idx + 1;
+  end
+
+  mplayer_cmd = "mencoder ""mf://povray/foo*.png"" -mf fps=25 -o 
povray/test.avi -ovc lavc -lavcopts vcodec=msmpeg4v2:vbitrate=800";
+  unix_g(mplayer_cmd);
+  
+endfunction
+

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-24 
17:00:39 UTC (rev 4589)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-25 
02:14:45 UTC (rev 4590)
@@ -6,9 +6,10 @@
 exec('q6d_fdm.sci');
 exec('q6d_algebra.sci');
 exec('q6d_display.sci');
+exec('q6d_povray.sci');
 
 t0 = 0;
-t1 = 8.;
+t1 = 10.;
 dt = 1/512;
 time = t0:dt:t1;
 
@@ -19,9 +20,10 @@
 //b0 = [ 0    0   0];
 //b1 = [-10   1  -2];
 b0 = [ 0   0   0 ];
-b1 = [ 1  -1  -1 ];
-//[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
-[fo_traj] = fo_traj_circle(time);
+b1 = [ 0   0  5 ];
+[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
+[traj] = fo_traj_circle(time);
+fo_traj(1:2,:,:) = traj(1:2,:,:);
 
 set("current_figure",0);
 clf();
@@ -62,7 +64,7 @@
       quat_of_euler([diff_flat_ref(DF_REF_PHI,1) diff_flat_ref(DF_REF_THETA,1) 
diff_flat_ref(DF_REF_PSI,1)])
       diff_flat_ref(DF_REF_P,1)  ; diff_flat_ref(DF_REF_Q,1) ; 
diff_flat_ref(DF_REF_R,1) 
       ];
-  
+
 fdm_init(time, X0);
 for i=2:length(time)
   fdm_run(i, motor_cmd(:,i-1));
@@ -71,4 +73,6 @@
 set("current_figure",3);
 clf();
 //display_fdm(time, fdm_state, fdm_euler)
-display_control(time, fdm_state, fdm_euler, diff_flat_ref);
\ No newline at end of file
+display_control(time, fdm_state, fdm_euler, diff_flat_ref);
+
+povray_draw(time,diff_flat_ref);
\ No newline at end of file