[paparazzi-commits] [4588]

[antoine drouin]

Revision: 4588
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4588
Author:   poine
Date:     2010-02-24 16:05:23 +0000 (Wed, 24 Feb 2010)
Log Message:
-----------


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

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

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-02-24 14:41:22 UTC (rev 4587)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-02-24 16:05:23 UTC (rev 4588)
@@ -1,3 +1,9 @@
+DF_FO_X   = 1;
+DF_FO_Y   = 2;
+DF_FO_Z   = 3;
+DF_FO_PSI = 4;
+DF_FO_SIZE = 4;
+DF_FO_ORDER = 5;
 
 DF_REF_X      = 1;
 DF_REF_Y      = 2;
@@ -25,15 +31,15 @@
 function [state] = df_state_of_fo(fo)
 
   state = zeros(DF_REF_SIZE, 1);
-  state(DF_REF_X)      = fo(1,1);
-  state(DF_REF_Y)      = fo(2,1);
-  state(DF_REF_Z)      = fo(3,1);
+  state(DF_REF_X)      = fo(DF_FO_X,1);
+  state(DF_REF_Y)      = fo(DF_FO_Y,1);
+  state(DF_REF_Z)      = fo(DF_FO_Z,1);
 
-  state(DF_REF_XD)     = fo(1,2);
-  state(DF_REF_YD)     = fo(2,2);
-  state(DF_REF_ZD)     = fo(3,2);
+  state(DF_REF_XD)     = fo(DF_FO_X,2);
+  state(DF_REF_YD)     = fo(DF_FO_Y,2);
+  state(DF_REF_ZD)     = fo(DF_FO_Z,2);
 
-  state(DF_REF_PSI)    = fo(4,1);
+  state(DF_REF_PSI)    = fo(DF_FO_PSI,1);
   
   psi = state(DF_REF_PSI);
   cpsi = cos(psi);

Added: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci               
                (rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci       
2010-02-24 16:05:23 UTC (rev 4588)
@@ -0,0 +1,31 @@
+
+
+function [fo_traj] = fo_traj_circle(time)
+
+  fo_traj = zeros(DF_FO_SIZE, DF_FO_ORDER, length(time));
+
+  radius = 3;
+  omega = rad_of_deg(60);
+  c = [0 0]';
+  
+  for i=1:length(time)
+
+    fo_traj(DF_FO_X,1,i) = c(1) + radius*cos(omega*time(i));
+    fo_traj(DF_FO_Y,1,i) = c(2) + radius*sin(omega*time(i));
+
+    fo_traj(DF_FO_X,2,i) = c(1) - omega*radius*sin(omega*time(i));
+    fo_traj(DF_FO_Y,2,i) = c(2) + omega*radius*cos(omega*time(i));
+  
+    fo_traj(DF_FO_X,3,i) = c(1) - omega^2*radius*cos(omega*time(i));
+    fo_traj(DF_FO_Y,3,i) = c(2) - omega^2*radius*sin(omega*time(i));
+    
+    fo_traj(DF_FO_X,4,i) = c(1) + omega^3*radius*sin(omega*time(i));
+    fo_traj(DF_FO_Y,4,i) = c(2) - omega^3*radius*cos(omega*time(i));
+    
+    fo_traj(DF_FO_X,5,i) = c(1) + omega^4*radius*cos(omega*time(i));
+    fo_traj(DF_FO_Y,5,i) = c(2) + omega^4*radius*sin(omega*time(i));
+  
+  end
+    
+endfunction
+

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci        2010-02-24 
14:41:22 UTC (rev 4587)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci        2010-02-24 
16:05:23 UTC (rev 4588)
@@ -4,6 +4,8 @@
 //
 //
 
+
+
 SBB_TOLERANCE = 0.025;
 
 function [pulse_dt, pulse_ampl, traj_dt] = compute_step(dist, dyn, max_accel, 
max_speed)
@@ -36,16 +38,14 @@
 
 function [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1)
 
-  n_comp = 4;  // x, y, z, psi
-  order = 5;   
-  fo_traj = zeros(n_comp, order, length(time));
+  fo_traj = zeros(DF_FO_SIZE, DF_FO_ORDER, length(time));
 
   // psi
   omega = rad_of_deg(45);
   for i=1:length(time)
-    fo_traj(4,1,i) = sin(omega*time(i));
-    fo_traj(4,2,i) = omega*cos(omega*time(i));
-    fo_traj(4,3,i) = -omega^2*sin(omega*time(i));
+    fo_traj(DF_FO_PSI, 1, i) =          sin(omega*time(i));
+    fo_traj(DF_FO_PSI, 2, i) =  omega*  cos(omega*time(i));
+    fo_traj(DF_FO_PSI, 3, i) = -omega^2*sin(omega*time(i));
   end
   
   // x and y

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-24 
14:41:22 UTC (rev 4587)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-24 
16:05:23 UTC (rev 4588)
@@ -1,6 +1,7 @@
 clear();
 
 exec('q6d_sbb.sci');
+exec('q6d_fo_traj_misc.sci');
 exec('q6d_diff_flatness.sci');
 exec('q6d_fdm.sci');
 exec('q6d_algebra.sci');
@@ -19,9 +20,9 @@
 //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] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
+[fo_traj] = fo_traj_circle(time);
 
-
 set("current_figure",0);
 clf();
 display_fo_traj(time, fo_traj);