[paparazzi-commits] [4555]

[antoine drouin]

Revision: 4555
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4555
Author:   poine
Date:     2010-02-18 18:12:05 +0000 (Thu, 18 Feb 2010)
Log Message:
-----------


Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_polynomials.sci
    paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_algebra.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_stabilization.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/test1.sce

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_polynomials.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_polynomials.sci        
2010-02-18 16:42:44 UTC (rev 4554)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_polynomials.sci        
2010-02-18 18:12:05 UTC (rev 4555)
@@ -96,8 +96,6 @@
     coefs(compo, 1, n_order+1:2*n_order) = ...
        (inv(A4)*(b1(compo,:)' - A3*matrix(coefs(compo,1, 1:n_order), n_order, 
1)))';
     // fill in the coefficients for the succesive time derivatives  
-//    pause
-    
     for order=2:n_order
       for pow=0:2*n_order-order
        coefs(compo, order, pow+1) = Arr(order-1,pow-1+order)*coefs(compo, 1, 
pow+order);

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce 2010-02-18 
16:42:44 UTC (rev 4554)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce 2010-02-18 
18:12:05 UTC (rev 4555)
@@ -1,25 +1,30 @@
 clear()
 exec('q3d_utils.sci');
+
 exec('q3d_polynomials.sci');
+exec('q3d_sbb.sci');
+
 exec('q3d_diff_flatness.sci');
 exec('q3d_fdm.sci');
 exec('q3d_display.sci');
 
-b0 = [0 0 0 0 0; 0 0 0 0 0];
-b1 = [5 0 0 0 0; 0 0 0 0 0];
+
 t0 = 0;
 t1 = 2.;
 dt = 0.01;
 time = t0:dt:t1;
 
-[coefs] = poly_get_coef_from_bound(time, b0, b1);
+if (0)
+  // polynomials
+  b0 = [0 0 0 0 0; 0 0 0 0 0];
+  b1 = [5 0 0 0 0; 0 0 0 0 0];
+  [coefs] = poly_get_coef_from_bound(time, b0, b1);
+  [fo_traj] = poly_gen_traj(time, coefs);
+else
+// differential equation
+  [fo_traj] = sbb_gen_traj(time, 5, rad_of_deg(30), [0 0] [20 0]);
+end
 
-//coefs = zeros(2,5,10);
-//coefs(1,1,2) = 1;
-//coefs(1,2,1) = 1;
-
-[fo_traj] = poly_gen_traj(time, coefs);
-
 diff_flat_cmd = zeros(2,length(time));
 diff_flat_ref = zeros(FDM_SSIZE, length(time));
 for i=1:length(time)

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_algebra.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_algebra.sci    2010-02-18 
16:42:44 UTC (rev 4554)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_algebra.sci    2010-02-18 
18:12:05 UTC (rev 4555)
@@ -7,6 +7,19 @@
   end
 endfunction
 
+function [o] = trim_vect(i,_min, _max)
+  o = i;
+  for j=1:length(i)
+    if i(j) > _max
+      o(j) = _max
+    elseif i(j) < _min
+      o(j) = _min
+    end
+  end
+endfunction
+
+
+
 function [rad] = rad_of_deg(deg)
   rad = deg / 180 * %pi;
 endfunction
@@ -33,9 +46,17 @@
 Q_QZ = 4;
 Q_SIZE = 4;
 
+
 //
+// 
 //
+function [quat] = quat_null()
+  quat = [1 0 0 0]';
+endfunction
+
 //
+//
+//
 function [dcm] = dcm_of_quat(q)
 
   qi2  = q(Q_QI)*q(Q_QI);

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci        2010-02-18 
16:42:44 UTC (rev 4554)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci        2010-02-18 
18:12:05 UTC (rev 4555)
@@ -45,6 +45,9 @@
 
 fdm_dt = 1/512;
 
+fdm_max_u = 1.0; // 100%
+fdm_min_u = 0.1; // 10%
+
 global fdm_time;
 global fdm_state;
 global fdm_euler;
@@ -71,6 +74,7 @@
 
 function fdm_run(i, cmd)
 
+  cmd = trim_vect(cmd,fdm_min_u, fdm_max_u);
   global fdm_state;
   global fdm_time;
   fdm_state(:,i) = ode(fdm_state(:,i-1), fdm_time(i-1), fdm_time(i), 
list(fdm_get_derivatives, cmd));

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_stabilization.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_stabilization.sci      
2010-02-18 16:42:44 UTC (rev 4554)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_stabilization.sci      
2010-02-18 18:12:05 UTC (rev 4555)
@@ -16,7 +16,7 @@
 global stabilization_cmd_motors;
 
 
-stab_omega_ref = [ rad_of_deg(1440); rad_of_deg(1440); rad_of_deg(720) ]; 
+stab_omega_ref = [ rad_of_deg(720); rad_of_deg(720); rad_of_deg(720) ]; 
 stab_zeta_ref  = [ 0.8; 0.8; 0.8 ]; 
 
 

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/test1.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/test1.sce  2010-02-18 16:42:44 UTC 
(rev 4554)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/test1.sce  2010-02-18 18:12:05 UTC 
(rev 4555)
@@ -27,7 +27,15 @@
   sensors_run(i+1);
   ahrs_propagate(i+1);
   ins_propagate(i+1);
-  guidance_run(i+1);
+//  guidance_run(i+1);
+
+  if (fdm_time(i)>1 & fdm_time(i)<2)
+    stabilization_sp_quat(:,i+1) = quat_of_euler([rad_of_deg(30) 0 0]');
+  else
+    stabilization_sp_quat(:,i+1) = quat_null();
+  end
+  stabilization_sp_thrust(i+1) = guidance_mass / guidance_Ct0 * 9.81;
+
   stabilization_run(i+1);
   
 end