[paparazzi-commits] [4623] cleaning

[antoine drouin]

Revision: 4623
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4623
Author:   poine
Date:     2010-03-03 22:29:50 +0000 (Wed, 03 Mar 2010)
Log Message:
-----------
cleaning

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

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci        2010-03-03 
21:56:29 UTC (rev 4622)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci        2010-03-03 
22:29:50 UTC (rev 4623)
@@ -2,12 +2,12 @@
 CTL_UD    = 2;
 CTL_USIZE = 2;
 
+global ctl_diff_flat_cmd;
+global ctl_diff_flat_ref;
+global ctl_fb_cmd;
 global ctl_u;
+global ctl_motor_cmd;
 
-//           X    Z  Theta   Xd    Zd   Theta_d
-//ctl_gain = [ 0   -1    0      0    -1    0
-//            -2    0    5     -2     0    5 ];
-
 fb_o_x = rad_of_deg(250);
 fb_x_x = 0.9;
 
@@ -27,12 +27,37 @@
 end
 
 function ctl_init(time)
-
+  global ctl_diff_flat_cmd;
+  ctl_diff_flat_cmd = zeros(CTL_USIZE,length(time));
+  global ctl_diff_flat_ref;
+  ctl_diff_flat_ref = zeros(FDM_SSIZE, length(time));
+  global ctl_fb_cmd;
+  ctl_fb_cmd = zeros(CTL_USIZE,length(time));
   global ctl_u;
   ctl_u = zeros(CTL_USIZE, length(time));
-  
+  global ctl_motor_cmd;
+  ctl_motor_cmd = zeros(2,length(time));
 endfunction
 
+function ctl_run(i)
+
+  global ctl_diff_flat_cmd;
+  //  ctl_diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i), 
fdm_Ct0/fdm_mass, fdm_la*fdm_Ct0/fdm_inertia);
+  ctl_diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i), adp_est(1,i), 
adp_est(2,i));
+  global ctl_diff_flat_ref;
+  ctl_diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
+  global ctl_fb_cmd;
+  ctl_fb_cmd(:,i) = ctl_compute_feeback(fdm_state(:,i), 
ctl_diff_flat_ref(:,i), ctl_diff_flat_cmd(:,i), adp_est(1,i), adp_est(2,i)); 
+  global ctl_u;
+  ctl_u(:,i) = ctl_diff_flat_cmd(:,i) + ctl_fb_cmd(:,i);
+  MotorsOfCmds = 0.5*[1 -1 ; 1 1];
+  global ctl_motor_cmd;
+  ctl_motor_cmd(:,i) =  MotorsOfCmds * ctl_u(:,i);
+
+endfunction
+
+
+
 function [fb_cmd] = ctl_compute_feeback(fdm_state, s_ref, u_ref, a, b)
 
   state_err = fdm_state - s_ref;
@@ -47,13 +72,7 @@
   xo2_t = 2*fb_x_t*fb_o_t;
   ut = u_ref(1);
    
-  if ut == 0
-    pause
-  end
   
-//  gain = [ o2_x*st/a            -o2_z*ct/a                0      xo2_x*st/a  
           -xo2_z*ct/a                   0
-//           o2_t/b*o2_x*ct/a/ut   o2_t/b*o2_z*st/a/ut   -o2_t/b   
-o2_t/b*xo2_x*ct/a/ut  -o2_t/b*xo2_z*st/a/ut    -xo2_t/b];
-  
   gain = [ o2_x*st/a            -o2_z*ct/a                0      xo2_x*st/a    
         -xo2_z*ct/a                   0
            o2_t/b*o2_x*ct/a/ut   o2_t/b*o2_z*st/a/ut   -o2_t/b   
o2_t/b*xo2_x*ct/a/ut   o2_t/b*xo2_z*st/a/ut    -xo2_t/b];
 

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce     2010-03-03 
21:56:29 UTC (rev 4622)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce     2010-03-03 
22:29:50 UTC (rev 4623)
@@ -46,34 +46,19 @@
 fdm_perturb(FDM_AX,k) = 10*ones(1,length(k));
 
 ctl_init(time);
-
-diff_flat_cmd = zeros(2,length(time));
-diff_flat_ref = zeros(FDM_SSIZE, length(time));
-diff_flat_ref(:,1) = df_state_of_fo(fo_traj(:,:,1));
-
-fb_cmd = zeros(2,length(time));
-motor_cmd = zeros(2,length(time));
-
 adp_init(time, [15 100]', []);
 sensors_init(time)
 
 for i=2:length(time)
-//  diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i), fdm_Ct0/fdm_mass, 
fdm_la*fdm_Ct0/fdm_inertia);
-  diff_flat_cmd(:,i-1) = df_input_of_fo(fo_traj(:,:,i-1), adp_est(1,i-1), 
adp_est(2,i-1));
-  diff_flat_ref(:,i-1) = df_state_of_fo(fo_traj(:,:,i-1));
-  fb_cmd(:,i-1) = ctl_compute_feeback(fdm_state(:,i-1),diff_flat_ref(:,i-1), 
diff_flat_cmd(:,i-1),  adp_est(1,i-1), adp_est(2,i-1)); 
-  global ctl_u;
-  ctl_u(:,i-1) = diff_flat_cmd(:,i-1) + fb_cmd(:,i-1);
-  MotorsOfCmds = 0.5*[1 -1 ; 1 1];
-  motor_cmd(:,i-1) =  MotorsOfCmds * ctl_u(:,i-1);
-  fdm_run(i, motor_cmd(:,i-1));
+  ctl_run(i-1); 
+  fdm_run(i, ctl_motor_cmd(:,i-1));
   sensors_run(i);
   adp_run(i);
 end
 
   
 set("current_figure",1);
-display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd, 
motor_cmd );
+display_control(time, ctl_diff_flat_ref, fdm_state, ctl_diff_flat_cmd, 
ctl_fb_cmd, ctl_motor_cmd );
 
 set("current_figure",2);
 display_adaptation();