[paparazzi-commits] [4634]

[antoine drouin]

Revision: 4634
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4634
Author:   poine
Date:     2010-03-05 19:57:53 +0000 (Fri, 05 Mar 2010)
Log Message:
-----------


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

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci        2010-03-05 
00:28:59 UTC (rev 4633)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci        2010-03-05 
19:57:53 UTC (rev 4634)
@@ -36,19 +36,17 @@
   global ctl_u;
   ctl_u = zeros(CTL_USIZE, length(time));
   global ctl_motor_cmd;
-  ctl_motor_cmd = zeros(2,length(time));
+  ctl_motor_cmd = zeros(CTL_USIZE,length(time));
 endfunction
 
-function ctl_run(i)
+function ctl_run(i, model_a, model_b)
 
   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));
+  ctl_diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i), model_a, model_b);
   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)); 
-//  ctl_fb_cmd(:,i) = ctl_compute_feeback(fdm_state(:,i), 
ctl_diff_flat_ref(:,i), ctl_diff_flat_cmd(:,i), fdm_Ct0/fdm_mass,  
fdm_la*fdm_Ct0/fdm_inertia); 
+  ctl_fb_cmd(:,i) = ctl_compute_feeback(fdm_state(:,i), 
ctl_diff_flat_ref(:,i), ctl_diff_flat_cmd(:,i), model_a, model_b); 
   global ctl_u;
   ctl_u(:,i) = ctl_diff_flat_cmd(:,i) + ctl_fb_cmd(:,i);
   MotorsOfCmds = 0.5*[1 -1 ; 1 1];

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce     2010-03-05 
00:28:59 UTC (rev 4633)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce     2010-03-05 
19:57:53 UTC (rev 4634)
@@ -46,11 +46,11 @@
 fdm_perturb(FDM_AX,k) = 10*ones(1,length(k));
 
 ctl_init(time);
-adp_init(time, [15 100]', []);
+adp_init(time, [19.5 157]', []);
 sensors_init(time)
 
 for i=2:length(time)
-  ctl_run(i-1); 
+  ctl_run(i-1, adp_est(1,i-1), adp_est(2,i-1)); 
   fdm_run(i, ctl_motor_cmd(:,i-1));
   sensors_run(i);
   adp_run(i);

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce 2010-03-05 
00:28:59 UTC (rev 4633)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce 2010-03-05 
19:57:53 UTC (rev 4634)
@@ -13,7 +13,7 @@
 
 
 t0 = 0;
-t1 = 3.;
+t1 = 5.;
 dt = 1/512;
 time = t0:dt:t1;
 
@@ -40,21 +40,12 @@
 end
  
  
-diff_flat_cmd = zeros(2,length(time));
-diff_flat_ref = zeros(FDM_SSIZE, length(time));
-for i=1: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_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
-end
-
 fdm_init(time, df_state_of_fo(fo_traj(:,:,1)), [0.25 0.25]');
-fb_cmd = zeros(2,length(time));
-motor_cmd = zeros(2,length(time));
+ctl_init(time);
+
 for i=2:length(time)
-  fb_cmd(:,i-1) = ctl_compute_feeback(fdm_state(:,i-1),diff_flat_ref(:,i-1)); 
-  MotorsOfCmds = 0.5*[1 -1 ; 1 1];
-  motor_cmd(:,i-1) =  MotorsOfCmds * ( diff_flat_cmd(:,i-1) + fb_cmd(:,i-1));
-  fdm_run(i, motor_cmd(:,i-1));
+  ctl_run(i-1, fdm_Ct0/fdm_mass, fdm_la*fdm_Ct0/fdm_inertia); 
+  fdm_run(i, ctl_motor_cmd(:,i-1));
 end
 
 set("current_figure",0);
@@ -84,6 +75,6 @@
 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 );
 //povray_draw(time, diff_flat_ref);