[paparazzi-commits] [4584] more gustavo's bug hunt

[antoine drouin]

Revision: 4584
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4584
Author:   poine
Date:     2010-02-24 14:14:31 +0000 (Wed, 24 Feb 2010)
Log Message:
-----------
more gustavo's bug hunt

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

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-02-24 13:10:13 UTC (rev 4583)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-02-24 14:14:31 UTC (rev 4584)
@@ -133,7 +133,7 @@
   adv = (axpsi*adxpsi + z2dmg*z3d)/av;
   z4d = fo(3,5);
   a = (axpsi*a2dxpsi + adxpsi^2 + (z2dmg)*z4d +z3d^2)*av;
-//  a = (axpsi*a2dxpsi + adxpsi^2 + (z2dmg)*z4d +z3d)*av;
+  //a = (axpsi*a2dxpsi + adxpsi^2 + (z2dmg)*z4d +z3d)*av;
   b = -adv*(axpsi*adxpsi + z2dmg*z3d);
   a2dv = (a+b)/av^2;
   
@@ -144,12 +144,12 @@
   //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);
+  a = (a2dypsi*av-a2dv*aypsi)*(aypsi^2+av^2);
   b = -2*(aypsi*adypsi+av*adv)*(adypsi*av-adv*aypsi);
   c = (aypsi^2+av^2)^2;
   phi2d = sign(z2dmg)*(a+b)/c;
   
-  a = (a2dxpsi*z2dmg+z3d*(adxpsi - axpsi) - z4d*axpsi)*(axpsi^2+z2dmg^2);
+  a = (a2dxpsi*z2dmg-z4d*axpsi)*(axpsi^2+z2dmg^2);
   b = -2*(axpsi*adxpsi+z2dmg*z3d)*(adxpsi*z2dmg-z3d*axpsi);
   c = (axpsi^2+z2dmg^2)^2;
   theta2d = (a+b)/c;

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci        2010-02-24 
13:10:13 UTC (rev 4583)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci        2010-02-24 
14:14:31 UTC (rev 4584)
@@ -53,8 +53,10 @@
                0.     0.0078 0.
               0.     0.     0.0137 ];
 fdm_Ct0 = 4. * fdm_mass * fdm_g / 4;   // thrust coefficient
-fdm_V0  = 7.;                         // 
-fdm_Cd  = 0.01;                        // drag coefficient
+//fdm_V0  = 7.;                               // 
+fdm_V0  = 1000.;                              // 
+//fdm_Cd  = 0.01;                        // drag coefficient
+fdm_Cd  = 0.000001;                        // drag coefficient
 fdm_la  = 0.25;                        // arm length
 fdm_Cm  = fdm_Ct0 / 10;                // torque coefficient
 
@@ -137,8 +139,7 @@
   airspeed_ltp = gspeed_ltp - fdm_wind;
   quat = X(FDM_SQI:FDM_SQZ);
   airspeed_body = quat_vect_mult(quat, airspeed_ltp);
-
-  lift_body = [0; 0; -sum(U) * fdm_Ct0 * ( 1 - 1/fdm_V0 * 
airspeed_body(AXIS_Z))];
+  lift_body = [0; 0; -sum(U) * fdm_Ct0 * ( 1 - abs(1/fdm_V0 * 
airspeed_body(AXIS_Z)))];
   lift_ltp = quat_vect_inv_mult(quat, lift_body);
   
   weight_ltp = [0; 0; fdm_g * fdm_mass];

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-24 
13:10:13 UTC (rev 4583)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-24 
14:14:31 UTC (rev 4584)
@@ -18,7 +18,7 @@
 //b0 = [ 0    0   0];
 //b1 = [-10   1  -2];
 b0 = [ 0   0   0 ];
-b1 = [ 0   0   -1 ];
+b1 = [ 1   0   0 ];
 [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);