diff --git a/sw/simulator/scilab/q6d/q6d_diff_flatness.sci b/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
index 5fa9bc4b5c..b7275ff83e 100644
--- a/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
+++ b/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
@@ -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 @@ DF_CM_OV_CT = 0.1;
 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);
diff --git a/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci b/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci
new file mode 100644
index 0000000000..d843642115
--- /dev/null
+++ b/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci
@@ -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
+
diff --git a/sw/simulator/scilab/q6d/q6d_sbb.sci b/sw/simulator/scilab/q6d/q6d_sbb.sci
index 8ab8efdfa6..92df15ad92 100644
--- a/sw/simulator/scilab/q6d/q6d_sbb.sci
+++ b/sw/simulator/scilab/q6d/q6d_sbb.sci
@@ -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 @@ endfunction
 
 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
diff --git a/sw/simulator/scilab/q6d/test_stop_stop.sce b/sw/simulator/scilab/q6d/test_stop_stop.sce
index b456a7d4ee..03cafd0131 100644
--- a/sw/simulator/scilab/q6d/test_stop_stop.sce
+++ b/sw/simulator/scilab/q6d/test_stop_stop.sce
@@ -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,8 +20,8 @@ max_accel = [ 9.81*tan(rad_of_deg(30)) 0.5*9.81];
 //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();