[paparazzi-commits] [4585] proper fdm initialisation

[antoine drouin]

Revision: 4585
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4585
Author:   poine
Date:     2010-02-24 14:35:03 +0000 (Wed, 24 Feb 2010)
Log Message:
-----------
proper fdm initialisation

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

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci        2010-02-24 
14:14:31 UTC (rev 4584)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci        2010-02-24 
14:35:03 UTC (rev 4585)
@@ -54,9 +54,9 @@
               0.     0.     0.0137 ];
 fdm_Ct0 = 4. * fdm_mass * fdm_g / 4;   // thrust coefficient
 //fdm_V0  = 7.;                               // 
-fdm_V0  = 1000.;                              // 
-//fdm_Cd  = 0.01;                        // drag coefficient
-fdm_Cd  = 0.000001;                        // drag coefficient
+fdm_V0  = 1e6;                        // 
+//fdm_Cd  = 0.01;                      // drag coefficient
+fdm_Cd  = 1e-6;                        // drag coefficient
 fdm_la  = 0.25;                        // arm length
 fdm_Cm  = fdm_Ct0 / 10;                // torque coefficient
 
@@ -74,14 +74,15 @@
 global fdm_raccel;
 
 
-function fdm_init(time) 
+function fdm_init(time, X0) 
   global fdm_time;
   fdm_time = time;
   global fdm_state;
   fdm_state = zeros(FDM_SSIZE, length(fdm_time));
-  fdm_state(FDM_SQI, 1) = 1.;
+  fdm_state(:,1) = X0;
   global fdm_euler;
   fdm_euler = zeros(AXIS_NB, length(fdm_time));
+  fdm_euler(:,1) = euler_of_quat(fdm_state(FDM_SQI:FDM_SQZ,1));
   global fdm_accel;
   fdm_accel = zeros(AXIS_NB, length(fdm_time));
   global fdm_raccel;

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci        2010-02-24 
14:14:31 UTC (rev 4584)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci        2010-02-24 
14:35:03 UTC (rev 4585)
@@ -39,7 +39,13 @@
   n_comp = 4;  // x, y, z, psi
   order = 5;   
   fo_traj = zeros(n_comp, order, length(time));
+
+  // psi
+  for i=1:length(time)
+    fo_traj(4,1,i) = rad_of_deg(30);
+  end
   
+  // x and y
   disp_xy = b1(1:2) - b0(1:2);
   [pulse_dt, pulse_ampl, traj_dt] = compute_step(norm(disp_xy), dyn(1,:), 
max_accel(1), max_speed(1));
   fo_traj(1:2,1,1) = b0(1:2);
@@ -61,6 +67,7 @@
     fo_traj(1:2,5,i) = 
-2*dyn(1,2)*dyn(1,1)*fo_traj(1:2,4,i)-dyn(1,1)^2*(fo_traj(1:2,3,i)-sp*u);  
   end
   
+  // z
   disp_z = b1(3) - b0(3);
   [pulse_dt, pulse_ampl, traj_dt] = compute_step(norm(disp_z), dyn(2,:), 
max_accel(2), max_speed(2));
   fo_traj(3,1,1) = b0(3);

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-24 
14:14:31 UTC (rev 4584)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-24 
14:35:03 UTC (rev 4585)
@@ -18,7 +18,7 @@
 //b0 = [ 0    0   0];
 //b1 = [-10   1  -2];
 b0 = [ 0   0   0 ];
-b1 = [ 1   0   0 ];
+b1 = [ 1   0  -1 ];
 [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
 
 
@@ -56,7 +56,13 @@
 
 display_motor_cmd(time, motor_cmd);
 
-fdm_init(time);
+X0 = [diff_flat_ref(DF_REF_X,1)  ; diff_flat_ref(DF_REF_Y,1) ; 
diff_flat_ref(DF_REF_Z,1)
+      diff_flat_ref(DF_REF_XD,1) ; diff_flat_ref(DF_REF_YD,1); 
diff_flat_ref(DF_REF_ZD,1)
+      quat_of_euler([diff_flat_ref(DF_REF_PHI,1) diff_flat_ref(DF_REF_THETA,1) 
diff_flat_ref(DF_REF_PSI,1)])
+      diff_flat_ref(DF_REF_P,1)  ; diff_flat_ref(DF_REF_Q,1) ; 
diff_flat_ref(DF_REF_R,1) 
+      ];
+  
+fdm_init(time, X0);
 for i=2:length(time)
   fdm_run(i, motor_cmd(:,i-1));
 end