[paparazzi-commits] [4617]

[antoine drouin]

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


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 
13:49:58 UTC (rev 4616)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci        2010-03-03 
16:26:29 UTC (rev 4617)
@@ -8,6 +8,15 @@
 //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;
+
+fb_o_z = rad_of_deg(250);
+fb_x_z = 0.9;
+
+fb_o_t = rad_of_deg(1050);
+fb_x_t = 0.9;
+
 if 0
 ctl_gain = [ 0    0    0      0     0    0
              0    0    0      0     0    0 ];
@@ -24,11 +33,32 @@
   
 endfunction
 
-function [fb_cmd] = ctl_compute_feeback(fdm_state, ref)
+function [fb_cmd] = ctl_compute_feeback(fdm_state, s_ref, u_ref, a, b)
 
-  state_err = fdm_state - ref;
-  fb_cmd = ctl_gain * state_err;
+  state_err = fdm_state - s_ref;
 
+  st = sin(s_ref(FDM_STHETA));
+  ct = cos(s_ref(FDM_STHETA));
+  o2_x = fb_o_x^2;
+  xo2_x = 2*fb_x_x*fb_o_x;
+  o2_z = fb_o_z^2;
+  xo2_z = 2*fb_x_z*fb_o_z;
+  o2_t = fb_o_t^2;
+  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];
+
+  fb_cmd = gain * state_err;
+
 endfunction
 
 

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce     2010-03-03 
13:49:58 UTC (rev 4616)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_adapt.sce     2010-03-03 
16:26:29 UTC (rev 4617)
@@ -30,6 +30,7 @@
   b1 = [ 5 -5];
   b2 = [ 0 -5];
   [fo_traj1] = sbb_gen_traj(time1, dyn, max_speed, max_accel, b0, b1);
+  b1 = [fo_traj1(1,1,$) fo_traj1(2,1,$)];
   [fo_traj2] = sbb_gen_traj(time2, dyn, max_speed, max_accel, b1, b2);
   [time, fo_traj] = merge_traj(list(time1, time2), list(fo_traj1, fo_traj2));
 else
@@ -55,7 +56,7 @@
 //  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)); 
+  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];