[paparazzi-commits] [4559]

[antoine drouin]

Revision: 4559
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4559
Author:   poine
Date:     2010-02-22 08:22:47 +0000 (Mon, 22 Feb 2010)
Log Message:
-----------


Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sbb.sci
    paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sbb.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sbb.sci        2010-02-18 
23:29:54 UTC (rev 4558)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sbb.sci        2010-02-22 
08:22:47 UTC (rev 4559)
@@ -1,12 +1,62 @@
 //
 //
-// Smooth bang bang
+// Smoothed bang bang trajectory
 //
 //
 
-function [fo_traj] = sbb_gen_traj(time, vmax, theta_max, b0, b1)
+sbb_omega = rad_of_deg(500);
+sbb_ksi = 0.70;
+sbb_tolerance = 0.05;
 
+function [fo_traj] = sbb_gen_traj(time, max_xd, max_theta, b0, b1)
+  n_comp = 2;
+  order = 5;
+  fo_traj = zeros(n_comp, order, length(time));
 
+  // compute trajectory caracteristics
+  dx = b1(1)-b0(1);
+  step_dt = -log(sbb_tolerance)/(sbb_ksi*sbb_omega*sqrt(1-sbb_ksi^2));
+  step_xdd = dx / (2*step_dt^2);
+  max_xdd = 9.81 * tan(max_theta);
+  
+  if step_xdd < max_xdd
+    if step_xdd > max_xd/step_dt
+      step_xdd = max_xd/step_dt;
+    end
+  else
+    step_xdd = max_xdd;
+    if step_xdd <= max_xd/step_dt
+      step_dt = max_xd/step_xdd;
+    else
+      step_xdd = max_xd/step_dt;
+    end
+  end
 
+  t_tot = (dx - 2*step_dt*(step_xdd*step_dt))/(step_xdd*step_dt) + 4*step_dt;
 
+  printf('dx        :%f\n', dx);
+  printf('step_dt   :%f\n', step_dt);
+  printf('step_xdd  :%f\n', step_xdd);
+  printf('total time:%f\n', t_tot);
+  
+  
+  fo_traj(1,1,1) = b0(1);
+  for i=2:length(time)
+    if time(i) < step_dt
+      sp = step_xdd;      
+    elseif time(i) < t_tot - step_dt & time(i) >= t_tot -  2 * step_dt
+      sp = -step_xdd;      
+    else
+      sp = 0;
+    end
+    fo_traj(1,:,i) = propagate_traj(fo_traj(1,:,i-1), sp, time(i) - 
time(i-1)); 
+  end
+
 endfunction
+
+
+function [Xi1] = propagate_traj(Xi, sp, dt)
+  Xi1 = zeros(1,5);
+  Xi1(1:4) = Xi(1:4) +  Xi(2:5)*dt;
+  Xi1(5) = -2*sbb_ksi*sbb_omega*Xi1(4)-sbb_omega^2*(Xi1(3)-sp);
+endfunction

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce 2010-02-18 
23:29:54 UTC (rev 4558)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce 2010-02-22 
08:22:47 UTC (rev 4559)
@@ -7,22 +7,25 @@
 exec('q3d_diff_flatness.sci');
 exec('q3d_fdm.sci');
 exec('q3d_display.sci');
+exec('q3d_povray.sci');
 
 
 t0 = 0;
-t1 = 2.;
+t1 = 4.5;
 dt = 0.01;
 time = t0:dt:t1;
 
 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];
+  b1 = [2 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]);
+  [fo_traj] = sbb_gen_traj(time, 5, rad_of_deg(29.983325), [0 0], [5 0]);
+  printf('xfinal:%f\n',fo_traj(1,1,$)); 
+
 end
 
 diff_flat_cmd = zeros(2,length(time));
@@ -32,7 +35,7 @@
   diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
 end
 
-fdm_init(time, df_state_of_fo(b0) ); 
+fdm_init(time, df_state_of_fo(fo_traj(:,:,1))); 
 for i=2:length(time)
   u1 = diff_flat_cmd(1,i-1);
   u2 = diff_flat_cmd(2,i-1);
@@ -40,7 +43,10 @@
   m2 = 0.5*(u1-u2);
   fdm_run(i, [m1 m2]')
 end
-  
+
+
+//povray_draw(time, diff_flat_ref);
+
 set("current_figure",0);
 clf();
 f=get("current_figure");