[paparazzi-commits] [4561]

[antoine drouin]

Revision: 4561
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4561
Author:   poine
Date:     2010-02-23 17:37:29 +0000 (Tue, 23 Feb 2010)
Log Message:
-----------


Added Paths:
-----------
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_display.sci

Added: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci              
                (rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-02-23 17:37:29 UTC (rev 4561)
@@ -0,0 +1,95 @@
+
+DF_REF_X      = 1;
+DF_REF_Y      = 2;
+DF_REF_Z      = 3;
+DF_REF_PHI    = 4;
+DF_REF_THETA  = 5;
+DF_REF_PSI    = 6;
+DF_REF_XD     = 7;
+DF_REF_YD     = 8;
+DF_REF_ZD     = 9;
+DF_REF_P      = 10;
+DF_REF_Q      = 11;
+DF_REF_R      = 12;
+DF_REF_SIZE   = 12;
+
+DF_G = 9.81;
+DF_MASS = 0.5;
+
+
+// state from flat output
+function [state] = df_state_of_fo(fo)
+
+  state = zeros(DF_REF_SIZE, 1);
+  state(DF_REF_X)      = fo(1,1);
+  state(DF_REF_Y)      = fo(2,1);
+  state(DF_REF_Z)      = fo(3,1);
+
+  state(DF_REF_XD)     = fo(1,2);
+  state(DF_REF_YD)     = fo(2,2);
+  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);
+  
+  state(DF_REF_PHI) = atan(aypsi/av);
+  state(DF_REF_THETA) = atan(axpsi/zddmg);
+  
+  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);
+   
+  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);
+
+  psid = fo(4,2);
+  
+  adxpsi = -psid*aypsi + jxpsi;
+  adypsi =  psid*axpsi + jypsi;
+
+  adv = (axpsi*adxpsi + zddmg*fo(3,4))/av;
+  
+  phid   = (adypsi*av-adv*aypsi)/(aypsi^2+av^2);
+  thetad = (adxpsi*zddmg-fo(3,4)*aypsi)/(axpsi^2+zddmg^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;
+  
+endfunction
+
+// control input from flat output
+function [inp] = df_input_of_fo(fo)
+
+  inp = zeros(4,1);
+  
+  xdd = fo(1,3);
+  ydd = fo(2,3);
+  zddmg = fo(3,3) - DF_G;
+  inp(1) = DF_MASS * sqrt(xdd^2+ydd^2+zddmg^2);
+  
+  psi =  fo(4,1);
+  
+  axpsi = cos(psi)*xdd + sin(psi)*ydd;
+  aypsi = sin(psi)*xdd - cos(psi)*ydd;
+  zddmg = fo(3,3) - DF_G;
+  av = sqrt(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;
+  
+  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);
+  
+  
+  
+endfunction
\ No newline at end of file

Added: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_display.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_display.sci                    
        (rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_display.sci    2010-02-23 
17:37:29 UTC (rev 4561)
@@ -0,0 +1,158 @@
+
+function display_fo_traj(time, fo_traj)
+  
+  f=get("current_figure");
+  f.figure_name="Flat Outputs Trajectory";
+
+  subplot(5,4,1);
+  plot2d(time, matrix(fo_traj(1,1,:), 1, length(time)));
+  xtitle('X(0)');  
+  
+  subplot(5,4,2);
+  plot2d(time, matrix(fo_traj(2,1,:), 1, length(time)));
+  xtitle('Y(0)');
+  
+  subplot(5,4,3);
+  plot2d(time, matrix(fo_traj(3,1,:), 1, length(time)));
+  xtitle('Z(0)');
+  
+  subplot(5,4,4);
+  plot2d(time, deg_of_rad(matrix(fo_traj(4,1,:), 1, length(time))));
+  xtitle('PSI(0)');
+
+  
+  
+  subplot(5,4,5);
+  plot2d(time, matrix(fo_traj(1,2,:), 1, length(time)));
+  xtitle('X(1)');  
+  
+  subplot(5,4,6);
+  plot2d(time, matrix(fo_traj(2,2,:), 1, length(time)));
+  xtitle('Y(1)');
+  
+  subplot(5,4,7);
+  plot2d(time, matrix(fo_traj(3,2,:), 1, length(time)));
+  xtitle('Z(1)');
+  
+  subplot(5,4,8);
+  plot2d(time, deg_of_rad(matrix(fo_traj(4,2,:), 1, length(time))));
+  xtitle('PSI(1)');
+  
+  
+  
+  subplot(5,4,9);
+  plot2d(time, matrix(fo_traj(1,3,:), 1, length(time)));
+  xtitle('X(2)');  
+  
+  subplot(5,4,10);
+  plot2d(time, matrix(fo_traj(2,3,:), 1, length(time)));
+  xtitle('Y(2)');
+  
+  subplot(5,4,11);
+  plot2d(time, matrix(fo_traj(3,3,:), 1, length(time)));
+  xtitle('Z(2)');
+  
+  subplot(5,4,12);
+  plot2d(time, deg_of_rad(matrix(fo_traj(4,3,:), 1, length(time))));
+  xtitle('PSI(2)');
+  
+  
+  
+  subplot(5,4,13);
+  plot2d(time, matrix(fo_traj(1,4,:), 1, length(time)));
+  xtitle('X(3)');  
+  
+  subplot(5,4,14);
+  plot2d(time, matrix(fo_traj(2,4,:), 1, length(time)));
+  xtitle('Y(3)');
+  
+  subplot(5,4,15);
+  plot2d(time, matrix(fo_traj(3,4,:), 1, length(time)));
+  xtitle('Z(3)');
+  
+//  subplot(5,4,16);
+//  plot2d(time, deg_of_rad(matrix(fo_traj(4,4,:), 1, length(time))));
+//  xtitle('PSI(3)');
+   
+  
+  
+  subplot(5,4,17);
+  plot2d(time, matrix(fo_traj(1,5,:), 1, length(time)));
+  xtitle('X(4)');  
+  
+  subplot(5,4,18);
+  plot2d(time, matrix(fo_traj(2,5,:), 1, length(time)));
+  xtitle('Y(4)');
+  
+  subplot(5,4,19);
+  plot2d(time, matrix(fo_traj(3,5,:), 1, length(time)));
+  xtitle('Z(4)');
+  
+//  subplot(5,4,20);
+//  plot2d(time, deg_of_rad(matrix(fo_traj(4,5,:), 1, length(time))));
+//  xtitle('PSI(4)');
+
+  
+endfunction
+
+
+function display_df_ref(time, diff_flat_ref)
+
+  f=get("current_figure");
+  f.figure_name="Reference";
+ 
+  subplot(6,2,1);
+  plot2d(time, diff_flat_ref(DF_REF_X,:));
+  xtitle('X');  
+  
+  subplot(6,2,3);
+  plot2d(time, diff_flat_ref(DF_REF_Y,:));
+  xtitle('Y');  
+
+  subplot(6,2,5);
+  plot2d(time, diff_flat_ref(DF_REF_Z,:));
+  xtitle('Z');  
+
+
+  subplot(6,2,7);
+  plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_PHI,:)));
+  xtitle('PHI');  
+  
+  subplot(6,2,9);
+  plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_THETA,:)));
+  xtitle('THETA');  
+
+  subplot(6,2,11);
+  plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_PSI,:)));
+  xtitle('PSI');  
+
+  
+  subplot(6,2,2);
+  plot2d(time, diff_flat_ref(DF_REF_XD,:));
+  xtitle('XD');  
+  
+  subplot(6,2,4);
+  plot2d(time, diff_flat_ref(DF_REF_YD,:));
+  xtitle('YD');  
+
+  subplot(6,2,6);
+  plot2d(time, diff_flat_ref(DF_REF_ZD,:));
+  xtitle('ZD');  
+
+
+  subplot(6,2,8);
+  plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_P,:)));
+  xtitle('P');  
+  
+  subplot(6,2,10);
+  plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_Q,:)));
+  xtitle('Q');  
+
+  subplot(6,2,12);
+  plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_R,:)));
+  xtitle('R');  
+
+  
+endfunction
+
+