[paparazzi-commits] [4621]

[antoine drouin]

Revision: 4621
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4621
Author:   poine
Date:     2010-03-03 20:53:39 +0000 (Wed, 03 Mar 2010)
Log Message:
-----------


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

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci        2010-03-03 
17:56:28 UTC (rev 4620)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci        2010-03-03 
20:53:39 UTC (rev 4621)
@@ -43,10 +43,10 @@
 fdm_wind = [0 0]';
 
 global fdm_time;
-global fdm_state;
-global fdm_accel;
+global fdm_state;                    // state
+global fdm_accel;                    // aceleration (used for sensors)
+global fdm_perturb;                  // perturbation
 
-
 function fdm_init(time, fdm_X0, cmd0) 
 
   global fdm_time;
@@ -56,24 +56,27 @@
   fdm_state(:,1) = fdm_X0;
   global fdm_accel;
   fdm_accel = zeros(FDM_ASIZE, length(fdm_time));
-  accel = fdm_get_derivatives(time(1), fdm_state(:,1), cmd0);
+  accel = fdm_get_derivatives(time(1), fdm_state(:,1), cmd0, [0;0;0]);
   fdm_accel(:,1) = accel(FDM_SXD:FDM_STHETAD);
-
+  global fdm_perturb;
+  fdm_perturb = zeros(FDM_ASIZE, length(fdm_time));
 endfunction
 
+
+// propagate dynamic model from step i-1 to step i
 function fdm_run(i, cmd)
  
   cmd = trim_vect(cmd, fdm_min_thrust, fdm_max_thrust);
   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_time;
+  fdm_state(:,i) = ode(fdm_state(:,i-1), fdm_time(i-1), fdm_time(i), 
list(fdm_get_derivatives, cmd, fdm_perturb(:,i-1)));
   global fdm_accel;
-  accel = fdm_get_derivatives(fdm_time(i), fdm_state(:,i), cmd);
+  accel = fdm_get_derivatives(fdm_time(i), fdm_state(:,i), cmd, 
fdm_perturb(:,i));
   fdm_accel(:,i) = accel(FDM_SXD:FDM_STHETAD);
 
 endfunction
 
-function [Xdot] = fdm_get_derivatives(t, X, U)
+function [Xdot] = fdm_get_derivatives(t, X, U, perturb)
 
   Xdot = zeros(length(X),1);
   
@@ -98,5 +101,7 @@
   // moments
   Xdot(FDM_STHETAD) = fdm_la * fdm_Ct0 / fdm_inertia*(U(FDM_MOTOR_LEFT) - 
U(FDM_MOTOR_RIGHT));
   
+  // add perturbation
+  Xdot(FDM_SXD:FDM_STHETAD) = Xdot(FDM_SXD:FDM_STHETAD)+perturb;
 endfunction
 

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce     2010-03-03 
17:56:28 UTC (rev 4620)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce     2010-03-03 
20:53:39 UTC (rev 4621)
@@ -1,4 +1,5 @@
 clear();
+clearglobal();
 
 exec('q3d_utils.sci');
 
@@ -23,7 +24,7 @@
 
 // trajectory generation
 if 1
-  dyn = [rad_of_deg(500) 0.7; rad_of_deg(500) 0.7];
+  dyn = [rad_of_deg(400) 0.7; rad_of_deg(400) 0.7];
   max_speed = [ 5                        2.5];
   max_accel = [ 9.81*tan(rad_of_deg(30)) 0.5*9.81];
   b0 = [-5  0];
@@ -40,6 +41,15 @@
 
 fdm_init(time, df_state_of_fo(fo_traj(:,:,1)), [0.25 0.25]');
 
+//pause
+global fdm_perturb;
+fdm_perturb(:,2562) = [15;15;15];
+fdm_perturb(:,2563) = [15;15;15];
+fdm_perturb(:,2564) = [15;15;15];
+fdm_perturb(:,2565) = [15;15;15];
+fdm_perturb(:,2566) = [15;15;15];
+fdm_perturb(:,2567) = [15;15;15];
+
 ctl_init(time);
 
 diff_flat_cmd = zeros(2,length(time));