[paparazzi-commits] [4568]

[antoine drouin]

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


Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_display.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-02-24 07:56:04 UTC (rev 4567)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-02-24 09:34:02 UTC (rev 4568)
@@ -15,8 +15,12 @@
 
 DF_G = 9.81;
 DF_MASS = 0.5;
+DF_JXX = 0.0078;
+DF_JYY = 0.0078;
+DF_JZZ = 0.0137;
+DF_L = 0.25;
+DF_CM_OV_CT = 0.1;
 
-
 // state from flat output
 function [state] = df_state_of_fo(fo)
 
@@ -30,37 +34,61 @@
   state(DF_REF_ZD)     = fo(3,2);
 
   state(DF_REF_PSI)    = fo(4,1);
-    
-  axpsi = cos(state(DF_REF_PSI))*fo(1,3) + sin(state(DF_REF_PSI))*fo(2,3);
-  aypsi = sin(state(DF_REF_PSI))*fo(1,3) - cos(state(DF_REF_PSI))*fo(2,3);
-  zddmg = fo(3,3) - DF_G;
-  av = sqrt(axpsi^2 + zddmg^2);
   
+  psi = state(DF_REF_PSI);
+  cpsi = cos(psi);
+  spsi = sin(psi);
+  
+  x2d = fo(1,3);
+  y2d = fo(2,3);
+  z2d = fo(2,3);
+  
+  axpsi = cpsi*x2d + spsi*y2d;
+  aypsi = spsi*x2d - cpsi*y2d;
+  z2dmg = z2d - DF_G;
+  av = sqrt(axpsi^2 + z2dmg^2);
+  
   state(DF_REF_PHI) = atan(aypsi/av);
-  state(DF_REF_THETA) = atan(axpsi/zddmg);
+  state(DF_REF_THETA) = atan(axpsi/z2dmg);
   
-  jxpsi = cos(state(DF_REF_PSI))*fo(1,4) + sin(state(DF_REF_PSI))*fo(2,4);
-  jypsi = sin(state(DF_REF_PSI))*fo(1,4) - cos(state(DF_REF_PSI))*fo(2,4);
+  x3d = fo(1,4);
+  y3d = fo(2,4);
+  z3d = fo(3,4);
+  
+  jxpsi = cpsi*x3d + spsi*y3d;
+  jypsi = spsi*x3d - cpsi*y3d;
    
-  kxpsi = cos(state(DF_REF_PSI))*fo(1,5) + sin(state(DF_REF_PSI))*fo(2,5);
-  kypsi = sin(state(DF_REF_PSI))*fo(1,5) - cos(state(DF_REF_PSI))*fo(2,5);
+  x4d = fo(1,5);
+  y4d = fo(2,5);
+  
+  kxpsi = cpsi*x4d + spsi*y4d;
+  kypsi = spsi*x4d - cpsi*y4d;
 
   psid = fo(4,2);
   
   adxpsi = -psid*aypsi + jxpsi;
   adypsi =  psid*axpsi + jypsi;
 
-  adv = (axpsi*adxpsi + zddmg*fo(3,4))/av;
+  adv = (axpsi*adxpsi + z2dmg*fo(3,4))/av;
   
   phid   = (adypsi*av-adv*aypsi)/(aypsi^2+av^2);
-  thetad = (adxpsi*zddmg-fo(3,4)*aypsi)/(axpsi^2+zddmg^2);
+  thetad = (adxpsi*z2dmg-z3d*aypsi)/(axpsi^2+z2dmg^2);
   
-  state(DF_REF_P) =  phid - sin(state(DF_REF_THETA))*psid;
-  state(DF_REF_Q) =  cos(state(DF_REF_PHI))*thetad +   
sin(state(DF_REF_PHI))*cos(state(DF_REF_THETA))*psid;
-  state(DF_REF_R) = -sin(state(DF_REF_PHI))*thetad +   
cos(state(DF_REF_PHI))*cos(state(DF_REF_THETA))*psid;
+  cphi = cos(state(DF_REF_PHI));
+  sphi = sin(state(DF_REF_PHI));
+
+  ctheta = cos(state(DF_REF_THETA));
+  stheta = sin(state(DF_REF_THETA));
   
+  state(DF_REF_P) =  phid - stheta*psid;
+  state(DF_REF_Q) =  cphi*thetad + sphi*ctheta*psid;
+  state(DF_REF_R) = -sphi*thetad + cphi*ctheta*psid;
+  
 endfunction
 
+
+
+
 // control input from flat output
 function [inp] = df_input_of_fo(fo)
 
@@ -71,25 +99,69 @@
   zddmg = fo(3,3) - DF_G;
   inp(1) = DF_MASS * sqrt(xdd^2+ydd^2+zddmg^2);
   
-  psi =  fo(4,1);
-  
+  psi   = fo(4,1);
+  psid  = fo(4,2);
+  psidd = fo(4,3);
+   
   axpsi = cos(psi)*xdd + sin(psi)*ydd;
   aypsi = sin(psi)*xdd - cos(psi)*ydd;
-  zddmg = fo(3,3) - DF_G;
+  
+  xddd = fo(1,4);
+  yddd = fo(2,4);
+  
+  jxpsi = cos(psi)*xddd + sin(psi)*yddd;
+  jypsi = sin(psi)*xddd - cos(psi)*yddd;
+  
+  xdddd = fo(1,5);
+  ydddd = fo(2,5);
+  
+  kxpsi = cos(psi)*xdddd + sin(psi)*ydddd;
+  kypsi = sin(psi)*xdddd - cos(psi)*ydddd;
+  
+  adxpsi = -psid*aypsi + jxpsi;
+  adypsi =  psid*axpsi + jypsi;
+  
+  addxpsi = -psidd*aypsi - psid^2*axpsi - 2*psid*jypsi + kxpsi;
+  addypsi =  psidd*axpsi - psid^2*aypsi + 2*psid*jxpsi + kypsi;
+  
   av = sqrt(axpsi^2 + zddmg^2);
+  zddd = fo(3,4);
+  adv = (axpsi*adxpsi+zddmg*zddd)/av;
+  zdddd = fo(3,5);
+  a = (axpsi*addxpsi + adxpsi^2 + (zddmg)*zdddd +zddd)*av;
+  b = -adv*(axpsi*adxpsi + zddmg*zddd);
+  addv = (a+b)/av^2;
   
+  phi = atan(aypsi/av);
+  theta = atan(axpsi/zddmg);
+  
+  phid   = (adypsi*av-adv*aypsi)/(aypsi^2+av^2);
+  thetad = (adxpsi*zddmg-zddd*aypsi)/(axpsi^2+zddmg^2);
+    
   a = (addypsi*av + adv*(adypsi-aypsi)-addv*aypsi)*(aypsi^2+av^2);
   b = -2*(aypsi*adypsi+av*adv)*(adypsi*av-adv*aypsi);
   c = (aypsi^2+av^2)^2;
-  phidd = (a + b)/c;
+  phidd = (a+b)/c;
   
   a = (addxpsi*zddmg+fo(3,4)*(adxpsi - axpsi) - 
fo(3,5)*axpsi)*(axpsi^2+zddmg^2);
   b = -2*(axpsi*adxpsi+zddmg*fo(3,4))*(adxpsi*zddmg-fo(3,4)*axpsi);
   c = (axpsi^2+zddmg^2)^2;
   thetadd = (a+b)/c;
    
-  psidd = fo(4,3);
+  p =  phid - sin(theta)*psid;
+  q =  cos(phi)*thetad + sin(phi)*cos(theta)*psid;
+  r = -sin(phi)*thetad + cos(phi)*cos(theta)*psid;
   
+  pd = phidd - cos(theta)*thetad*psid - sin(theta)*psidd;
+  a  = -sin(phi)*phid*thetad + cos(phi)*thetadd + 
cos(phi)*cos(theta)*phid*psid;
+  b  = -sin(phi)*sin(theta)*thetad*psid + sin(phi)*cos(theta)*psidd;
+  qd =  a+b;
+  a  = -cos(phi)*phid*thetad - sin(phi)*thetadd - 
sin(phi)*cos(theta)*phid*psid;
+  b  = -cos(phi)*sin(theta)*thetad*psid + cos(phi)*cos(theta)*psidd; 
+  rd = a+b;
   
+  inp(2) = DF_JXX/DF_L*pd + (DF_JZZ-DF_JYY)*q*r;
+  inp(3) = DF_JYY/DF_L*qd + (DF_JXX-DF_JZZ)*p*r;
+  inp(4) = DF_JZZ/DF_CM_OV_CT*rd + (DF_JYY-DF_JXX)*p*q;
   
 endfunction
\ No newline at end of file

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_display.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_display.sci    2010-02-24 
07:56:04 UTC (rev 4567)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_display.sci    2010-02-24 
09:34:02 UTC (rev 4568)
@@ -152,7 +152,184 @@
   plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_R,:)));
   xtitle('R');  
 
+endfunction
+
+
+function display_df_cmd(time, diff_flat_cmd)
+
+  f=get("current_figure");
+  f.figure_name="Command";
+
+  subplot(2,4,1);
+  plot2d(time, diff_flat_cmd(1,:));
+  xtitle('Ut');  
+
+  subplot(2,4,2);
+  plot2d(time, diff_flat_cmd(2,:));
+  xtitle('Up');  
+
+  subplot(2,4,3);
+  plot2d(time, diff_flat_cmd(3,:));
+  xtitle('Uq');  
+
+  subplot(2,4,4);
+  plot2d(time, diff_flat_cmd(4,:));
+  xtitle('Ur');  
+
+endfunction
+
+
+function display_motor_cmd(time, motor_cmd)
+
+  subplot(2,4,5);
+  plot2d(time, motor_cmd(1,:));
+  xtitle('F1');  
+
+  subplot(2,4,6);
+  plot2d(time, motor_cmd(2,:));
+  xtitle('F2');  
+
+  subplot(2,4,7);
+  plot2d(time, motor_cmd(3,:));
+  xtitle('F3');  
+
+  subplot(2,4,8);
+  plot2d(time, motor_cmd(4,:));
+  xtitle('F4');  
+
+endfunction
+
+
+function display_fdm(time, state, euler)
+
+  f=get("current_figure");
+  f.figure_name="FDM";
+
+  subplot(4,3,1);
+  plot2d(time, state(FDM_SX,:));
+  xtitle('X');
   
+  subplot(4,3,2);
+  plot2d(time, state(FDM_SY,:));
+  xtitle('Y');
+
+  subplot(4,3,3);
+  plot2d(time, state(FDM_SZ,:));
+  xtitle('Z');
+
+  
+  subplot(4,3,4);
+  plot2d(time, state(FDM_SXD,:));
+  xtitle('Xd');
+  
+  subplot(4,3,5);
+  plot2d(time, state(FDM_SYD,:));
+  xtitle('Yd');
+
+  subplot(4,3,6);
+  plot2d(time, state(FDM_SZD,:));
+  xtitle('Zd');
+
+  
+  subplot(4,3,7);
+  plot2d(time, deg_of_rad(euler(FDM_EPHI,:)));
+  xtitle('Phi');
+  
+  subplot(4,3,8);
+  plot2d(time, deg_of_rad(euler(FDM_ETHETA,:)));
+  xtitle('Theta');
+
+  subplot(4,3,9);
+  plot2d(time, deg_of_rad(euler(FDM_EPSI,:)));
+  xtitle('Psi');
+
+
+  subplot(4,3,10);
+  plot2d(time, deg_of_rad(state(FDM_SP,:)));
+  xtitle('p');
+  
+  subplot(4,3,11);
+  plot2d(time, deg_of_rad(state(FDM_SQ,:)));
+  xtitle('q');
+
+  subplot(4,3,12);
+  plot2d(time, deg_of_rad(state(FDM_SR,:)));
+  xtitle('r');
+
+
 endfunction
 
 
+function display_control(time, fdm_state, fdm_euler, diff_flat_ref)
+
+  f=get("current_figure");
+  f.figure_name="Control";
+
+  subplot(4,3,1);
+  plot2d(time, fdm_state(FDM_SX,:), 2);
+  plot2d(time, diff_flat_ref(DF_REF_X,:),3);
+  xtitle('X');
+  legends(["fdm", "ref"],[2 3], with_box=%f, opt="ul"); 
+
+  subplot(4,3,2);
+  plot2d(time, fdm_state(FDM_SY,:), 2);
+  plot2d(time, diff_flat_ref(DF_REF_Y,:),3);
+  xtitle('Y');
+
+  subplot(4,3,3);
+  plot2d(time, fdm_state(FDM_SZ,:), 2);
+  plot2d(time, diff_flat_ref(DF_REF_Z,:),3);
+  xtitle('Z');
+
+  
+  subplot(4,3,4);
+  plot2d(time, fdm_state(FDM_SXD,:), 2);
+  plot2d(time, diff_flat_ref(DF_REF_XD,:),3);
+  xtitle('Xd');
+  
+  subplot(4,3,5);
+  plot2d(time, fdm_state(FDM_SYD,:), 2);
+  plot2d(time, diff_flat_ref(DF_REF_YD,:),3);
+  xtitle('Yd');
+
+  subplot(4,3,6);
+  plot2d(time, fdm_state(FDM_SZD,:), 2);
+  plot2d(time, diff_flat_ref(DF_REF_ZD,:),3);
+  xtitle('Zd');
+
+  
+  subplot(4,3,7);
+  plot2d(time, deg_of_rad(fdm_euler(FDM_EPHI,:)), 2);
+  plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_PHI,:)), 3);
+  xtitle('Phi');
+  
+  subplot(4,3,8);
+  plot2d(time, deg_of_rad(fdm_euler(FDM_ETHETA,:)), 2);
+  plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_THETA,:)), 3);
+  xtitle('Theta');
+
+  subplot(4,3,9);
+  plot2d(time, deg_of_rad(fdm_euler(FDM_EPSI,:)), 2);
+  plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_PSI,:)), 3);
+  xtitle('Psi');
+
+
+  subplot(4,3,10);
+  plot2d(time, deg_of_rad(fdm_state(FDM_SP,:)), 2);
+  plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_P,:)), 3);
+  xtitle('p');
+  
+  subplot(4,3,11);
+  plot2d(time, deg_of_rad(fdm_state(FDM_SQ,:)), 2);
+  plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_Q,:)), 3);
+  xtitle('q');
+
+  subplot(4,3,12);
+  plot2d(time, deg_of_rad(fdm_state(FDM_SR,:)), 2);
+  plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_R,:)), 3);
+  xtitle('r');
+
+
+endfunction
+
+

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci        2010-02-24 
07:56:04 UTC (rev 4567)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci        2010-02-24 
09:34:02 UTC (rev 4568)
@@ -30,6 +30,23 @@
 FDM_MOTOR_LEFT  = 4;
 FDM_MOTOR_NB    = 4;
 
+
+//
+// By Products
+//
+FDM_EPHI   = 1;  // euler angles
+FDM_ETHETA = 2;
+FDM_EPSI   = 3;
+
+FDM_AXDD   = 1;  // linear accelerations
+FDM_AYDD   = 2;
+FDM_AZDD   = 3;
+
+FDM_RAPD   = 1;  // rotational accelerations
+FDM_RAQD   = 2;
+FDM_RARD   = 3;
+
+
 fdm_g = 9.81;                          // gravitational acceleration
 fdm_mass = 0.5;                        // mass in kg
 fdm_inertia = [0.0078 0.     0.        // inertia tensor
@@ -55,9 +72,9 @@
 global fdm_raccel;
 
 
-function fdm_init(t_start, t_stop) 
+function fdm_init(time) 
   global fdm_time;
-  fdm_time = t_start:fdm_dt:t_stop;
+  fdm_time = time;
   global fdm_state;
   fdm_state = zeros(FDM_SSIZE, length(fdm_time));
   fdm_state(FDM_SQI, 1) = 1.;

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci        2010-02-24 
07:56:04 UTC (rev 4567)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci        2010-02-24 
09:34:02 UTC (rev 4568)
@@ -10,6 +10,7 @@
   if (dist < 0.01)
     step_dt = 0;
     step_ampl = 0;
+    traj_dt = 1;
   else
     omega = dyn(1);
     xsi = dyn(2);

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-24 
07:56:04 UTC (rev 4567)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-24 
09:34:02 UTC (rev 4568)
@@ -2,6 +2,7 @@
 
 exec('q6d_sbb.sci');
 exec('q6d_diff_flatness.sci');
+exec('q6d_fdm.sci');
 exec('q6d_algebra.sci');
 exec('q6d_display.sci');
 
@@ -14,8 +15,10 @@
 max_speed = [5 2.5];
 max_accel = [ 9.81*tan(rad_of_deg(30)) 0.5*9.81];
 
-b0 = [ 0    0   0];
-b1 = [-10   1  -2];
+//b0 = [ 0    0   0];
+//b1 = [-10   1  -2];
+b0 = [ 0   0   0];
+b1 = [ 0   5   0];
 [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
 
 
@@ -27,10 +30,38 @@
 diff_flat_cmd = zeros(4,length(time));
 diff_flat_ref = zeros(DF_REF_SIZE, length(time));
 for i=1:length(time)
-//  diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i));
+  diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i));
   diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
 end
 
+set("current_figure",1);
+clf();
+display_df_ref(time, diff_flat_ref);
+
 set("current_figure",2);
 clf();
-display_df_ref(time, diff_flat_ref);
+display_df_cmd(time, diff_flat_cmd)
+
+
+motor_of_cmd = [ 0.25    0.     0.5   -0.25  
+                 0.25   -0.5    0.     0.25  
+                 0.25    0.    -0.5   -0.25  
+                 0.25    0.5    0.     0.25  ];
+
+motor_cmd = zeros(4,length(time));         
+
+for i=1:length(time)
+  motor_cmd(:,i) = 1/fdm_Ct0 * motor_of_cmd * diff_flat_cmd(:,i);
+end
+
+display_motor_cmd(time, motor_cmd);
+
+fdm_init(time);
+for i=2:length(time)
+  fdm_run(i, motor_cmd(:,i-1));
+end
+
+set("current_figure",3);
+clf();
+//display_fdm(time, fdm_state, fdm_euler)
+display_control(time, fdm_state, fdm_euler, diff_flat_ref);
\ No newline at end of file