diff --git a/sw/simulator/scilab/q6d/q6d_diff_flatness.sci b/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
index 458e51bf71..ea45430cfe 100644
--- a/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
+++ b/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
@@ -170,8 +170,8 @@ function [inp] = df_input_of_fo(fo)
   b  = -cphi*stheta*thetad*psid + cphi*ctheta*psi2d; 
   rd = a+b;
   
-  inp(2) = DF_JXX/DF_L*pd + (DF_JZZ-DF_JYY)*q*r;
-  inp(3) = DF_JYY/DF_L*qd + (DF_JXX-DF_JZZ)*p*r;
-  inp(4) = DF_JZZ/DF_CM_OV_CT*rd + (DF_JYY-DF_JXX)*p*q;
+  inp(2) = 1/DF_L*(DF_JXX*pd + (DF_JZZ-DF_JYY)*q*r);
+  inp(3) = 1/DF_L*(DF_JYY*qd + (DF_JXX-DF_JZZ)*p*r);
+  inp(4) = 1/DF_CM_OV_CT*(DF_JZZ*rd + (DF_JYY-DF_JXX)*p*q);
   
 endfunction
\ No newline at end of file
diff --git a/sw/simulator/scilab/q6d/test_stop_stop.sce b/sw/simulator/scilab/q6d/test_stop_stop.sce
index 4385aa3826..f730b86425 100644
--- a/sw/simulator/scilab/q6d/test_stop_stop.sce
+++ b/sw/simulator/scilab/q6d/test_stop_stop.sce
@@ -18,7 +18,7 @@ max_accel = [ 9.81*tan(rad_of_deg(30)) 0.5*9.81];
 //b0 = [ 0    0   0];
 //b1 = [-10   1  -2];
 b0 = [ 0   0   0];
-b1 = [ 0   5   0];
+b1 = [ 1   0   5];
 [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);