[paparazzi-commits] [4548]

[antoine drouin]

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


Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci
    paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_utils.sci

Added Paths:
-----------
    paparazzi3/trunk/sw/simulator/scilab/q3d/test_polynomial.sce

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci        2010-02-09 
13:01:15 UTC (rev 4547)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci        2010-02-10 
18:18:18 UTC (rev 4548)
@@ -14,6 +14,14 @@
 FDM_SSIZE   = 6;
 
 //
+// Accelerations
+//
+FDM_AX     = 1;
+FDM_AZ     = 2;
+FDM_ATHETA = 3;
+FDM_ASIZE  = 3;
+
+//
 // Actuators
 //
 FDM_MOTOR_RIGHT = 1;
@@ -24,8 +32,8 @@
 fdm_mass    = 0.25;
 fdm_inertia = 0.0078;
 
-fdm_min_thrust =  0.5 * 0.1 * fdm_mass * fdm_g;
-fdm_max_thrust =  0.5 * 4.0 * fdm_mass * fdm_g;
+fdm_min_thrust =  0.5 * 0.1 * fdm_mass * fdm_g; // 10%  of hovering power for 
each motor
+fdm_max_thrust =  0.5 * 4.0 * fdm_mass * fdm_g; // 400% of hovering power for 
each motor
 
 fdm_dt = 1./512.;
 
@@ -34,13 +42,15 @@
 global fdm_accel;
 
 
-function fdm_init(time_ref, ref) 
+function fdm_init(time, fdm_X0) 
 
   global fdm_time;
-  fdm_time = time_ref;
+  fdm_time = time;
   global fdm_state;
   fdm_state = zeros(FDM_SSIZE, length(fdm_time));
-  fdm_state(:,1) = ctl_state_of_flat_out(ref);
+  fdm_state(:,1) = fdm_X0;
+  global fdm_accel;
+  fdm_accel = zeros(FDM_ASIZE, length(fdm_time));
   
 endfunction
 
@@ -50,7 +60,10 @@
   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));
-  
+  global fdm_accel;
+  accel = fdm_get_derivatives(fdm_time(i), fdm_state(:,i), cmd);
+  fdm_accel(:,i) = accel(FDM_SXD:FDM_STHETAD);
+
 endfunction
 
 function [Xdot] = fdm_get_derivatives(t, X, U)
@@ -63,6 +76,5 @@
   Xdot(FDM_SZD) = 1/fdm_mass*(sum(U)*cos(X(FDM_STHETA))-fdm_mass*fdm_g);
   Xdot(FDM_STHETAD) = 1/fdm_inertia*(U(FDM_MOTOR_RIGHT) - U(FDM_MOTOR_LEFT));
   
-  
 endfunction
 

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_utils.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_utils.sci      2010-02-09 
13:01:15 UTC (rev 4547)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_utils.sci      2010-02-10 
18:18:18 UTC (rev 4548)
@@ -82,7 +82,7 @@
       drawlater();
       draw_quad(i, _rect);
       drawnow();
-      if 1
+      if 0
        filename = sprintf('images/frame_%04d.ppm',frame_idx);
        xs2ppm(0, filename, 1);
       else

Added: paparazzi3/trunk/sw/simulator/scilab/q3d/test_polynomial.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_polynomial.sce                
                (rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_polynomial.sce        
2010-02-10 18:18:18 UTC (rev 4548)
@@ -0,0 +1,144 @@
+clear()
+
+//
+// traj is a n_compo*n_order*n_sample vector
+// n_compo components
+// n_order succesive time derivatives
+// n_sample time values
+//
+function poly_display_traj(time, traj)
+
+  [n_compo, n_order, n_sample] = size(traj); 
+ 
+  for compo=1:n_compo
+    for order=1:n_order
+      subplot(n_order, n_compo, compo+(order-1)*n_compo);
+      plot2d(time, matrix(traj(compo,order,:), n_sample, 1));
+      xtitle(sprintf('$X^{%d}_{%d}$', order-1, compo));
+    end
+  end
+  
+endfunction
+
+
+//
+// compute the values of a set of polynomials along a time vector
+// 
+//
+function [traj] = poly_gen_traj(time, coefs)
+ 
+  [n_comp, n_order, n_coef] = size(coefs);
+ 
+  traj = zeros(n_comp, n_coef/2, length(time));
+  for compo=1:n_comp 
+    for order=1:n_order
+      for i=1:length(time)
+       traj(compo, order, i) = ...
+           poly_compute_val(matrix(coefs(compo,order, :),1,n_coef), time(1), 
time(i));
+      end
+    end
+  end
+
+endfunction
+
+
+//
+// compute v = a_{n}*(t-t_0)^{n} + a_{n-1}*(t-t_0)^{n-1} + ... + a_{0}
+//
+//
+function [v] = poly_compute_val(coefs, t0, t)
+  dt = t-t0;
+  v = coefs($);
+  for i=1:length(coefs)-1
+    v = v * dt;
+    v = v + coefs(length(coefs)-i); // assume coef(1) = a_0
+  end
+  
+endfunction
+
+
+//
+//  compute coefficients for a set of polynomials
+//  having bond values b0 and b1
+//
+//
+function [coefs] = poly_get_coef_from_bound(time, b0,b1)
+
+  [n_comp, n_order] = size(b0);
+  n_coef = 2*n_order
+  coefs = zeros(n_comp, n_order, n_coef); 
+  
+  // refer to paper for notations
+  
+  for compo=1:n_comp
+
+    // invert of the top left corner block  
+    invA1 = zeros(n_order, n_order);
+    for i=1:n_order
+      invA1(i,i) = 1/Arr(i-1,i-1);
+    end
+    // first half of the coefficients
+    coefs(compo, 1, 1:n_order) = (invA1*b0(compo,:)')';
+    
+    // bottom right block : triangular
+    A3 = zeros(n_order, n_order);
+    dt = time($) - time(1);
+    for i=1:n_order
+      for j=i:n_order
+       A3(i,j) = Arr(i-1,j-1) * dt^(j-1);
+      end
+    end
+    // bottom left block 
+    A4 = zeros(n_order, n_order);
+    for i=1:n_order
+      for j=1:n_order
+       A4(i,j) = Arr(i-1,j-1+n_order) * dt^(j-1+n_order);
+      end
+    end
+    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  
+    for order=2:n_order
+      for pow=1:2*n_order-order
+       coefs(compo, order, pow+1) = Arr(order-1,pow-1+order)*coefs(compo, 1, 
pow+order);
+      end
+    end
+  end
+  
+endfunction
+
+
+//
+// Arrangement
+//
+//
+function [akn] = Arr(k,n)
+  akn = factorial(n)/factorial(n-k);
+endfunction
+
+
+b0 = [1 0 0 0; 1 0 0 0];
+b1 = [2 0 0 0;-1 0 0 0];
+t0 = 1;
+t1 = 2;
+dt = 0.01;
+time = t0:dt:t1;
+if 1
+[coefs] = poly_get_coef_from_bound(time, b0, b1);
+[traj] = poly_gen_traj(time, coefs);
+
+clf();
+poly_display_traj(time, traj);
+end
+
+if 0
+coefs = [0 1 0 ];
+time = 0:dt:1;
+[traj] = poly_gen_traj(time, coefs);
+
+clf();
+poly_display_traj(time, traj);
+end
+
+
+// 1 0 0 10 -15 6
\ No newline at end of file