[paparazzi-commits] [4573]

[antoine drouin]

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


Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.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 09:49:58 UTC (rev 4572)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-02-24 09:52:45 UTC (rev 4573)
@@ -48,7 +48,7 @@
   z2dmg = z2d - DF_G;
   av = sqrt(axpsi^2 + z2dmg^2);
   
-  state(DF_REF_PHI) = atan(aypsi/av);
+  state(DF_REF_PHI) = sign(z2dmg)*atan(aypsi/av);
   state(DF_REF_THETA) = atan(axpsi/z2dmg);
   
   x3d = fo(1,4);
@@ -132,7 +132,7 @@
   b = -adv*(axpsi*adxpsi + zddmg*zddd);
   addv = (a+b)/av^2;
   
-  phi = atan(aypsi/av);
+  phi = sign(zddmg)*atan(aypsi/av);
   theta = atan(axpsi/zddmg);
   
   phid   = (adypsi*av-adv*aypsi)/(aypsi^2+av^2);