diff --git a/sw/simulator/scilab/q6d/q6d_diff_flatness.sci b/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
index ea45430cfe..3178b9f9a8 100644
--- a/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
+++ b/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
@@ -132,7 +132,8 @@ function [inp] = df_input_of_fo(fo)
   z3d = fo(3,4);
   adv = (axpsi*adxpsi + z2dmg*z3d)/av;
   z4d = fo(3,5);
-  a = (axpsi*a2dxpsi + adxpsi^2 + (z2dmg)*z4d +z3d)*av;
+  a = (axpsi*a2dxpsi + adxpsi^2 + (z2dmg)*z4d +z3d^2)*av;
+//  a = (axpsi*a2dxpsi + adxpsi^2 + (z2dmg)*z4d +z3d)*av;
   b = -adv*(axpsi*adxpsi + z2dmg*z3d);
   a2dv = (a+b)/av^2;
   
@@ -140,7 +141,8 @@ function [inp] = df_input_of_fo(fo)
   theta = atan(axpsi/z2dmg);
   
   phid   = sign(z2dmg)*(adypsi*av-adv*aypsi)/(aypsi^2+av^2);
-  thetad = (adxpsi*z2dmg-z3d*aypsi)/(axpsi^2+z2dmg^2);
+  //thetad = (adxpsi*z2dmg-z3d*aypsi)/(axpsi^2+z2dmg^2);
+  thetad = (adxpsi*z2dmg-z3d*axpsi)/(axpsi^2+z2dmg^2);
     
   a = (a2dypsi*av + adv*(adypsi-aypsi)-a2dv*aypsi)*(aypsi^2+av^2);
   b = -2*(aypsi*adypsi+av*adv)*(adypsi*av-adv*aypsi);