[paparazzi-commits] [4692] playing around

[antoine drouin]

Revision: 4692
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4692
Author:   poine
Date:     2010-03-15 15:43:16 +0000 (Mon, 15 Mar 2010)
Log Message:
-----------
playing around

Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/scilab/q6d/povray/Makefile
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_algebra.sci
    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/q6d_sensors.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce

Added Paths:
-----------
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_ctl.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_io.sci

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/povray/Makefile
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/povray/Makefile    2010-03-15 
15:42:10 UTC (rev 4691)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/povray/Makefile    2010-03-15 
15:43:16 UTC (rev 4692)
@@ -1,3 +1,5 @@
+test:
+       povray test.pov +Oimg001.png Display=false +W800 +H600 +Q9 +A0.3 +R5
 
 clean:
-       rm -f *.png
\ No newline at end of file
+       rm -f *~ img*.png test.avi q3d.pov 

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_algebra.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_algebra.sci    2010-03-15 
15:42:10 UTC (rev 4691)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_algebra.sci    2010-03-15 
15:43:16 UTC (rev 4692)
@@ -131,6 +131,13 @@
 //
 //
 //
+function [qo] = quat_normalize(qi)
+  qo = qi / norm(qi); 
+endfunction
+
+//
+//
+//
 function [vo] = quat_vect_mult(q, vi)
   dcm = dcm_of_quat(q);
   vo = dcm * vi;

Added: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_ctl.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_ctl.sci                        
        (rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_ctl.sci        2010-03-15 
15:43:16 UTC (rev 4692)
@@ -0,0 +1,46 @@
+CTL_UT    = 1;
+CTL_UP    = 2;
+CTL_UQ    = 3;
+CTL_UR    = 4;
+CTL_USIZE = 4;
+
+
+global ctl_diff_flat_cmd;
+global ctl_diff_flat_ref;
+global ctl_fb_cmd;
+global ctl_u;
+global ctl_motor_cmd;
+
+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(DF_REF_SIZE, 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(CTL_USIZE,length(time));
+
+endfunction
+
+
+
+function ctl_run(i, fo_tra)
+
+  global ctl_diff_flat_cmd;
+  ctl_diff_flat_cmd(:,i) = df_input_of_fo(fo_tra);
+  global ctl_diff_flat_ref;
+  ctl_diff_flat_ref(:,i) = df_ref_of_fo(fo_tra);
+  global ctl_u;
+  ctl_u(:,i) = ctl_diff_flat_cmd(:,i);
+  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 ];
+  global ctl_motor_cmd;
+  ctl_motor_cmd(:,i) =  1/fdm_Ct0 * motor_of_cmd * ctl_u(:,i);
+
+endfunction

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-03-15 15:42:10 UTC (rev 4691)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-03-15 15:43:16 UTC (rev 4692)
@@ -28,20 +28,20 @@
 DF_CM_OV_CT = 0.1;
 
 // state from flat output
-function [state] = df_state_of_fo(fo)
+function [ref] = df_ref_of_fo(fo)
 
-  state = zeros(DF_REF_SIZE, 1);
-  state(DF_REF_X)      = fo(DF_FO_X,1);
-  state(DF_REF_Y)      = fo(DF_FO_Y,1);
-  state(DF_REF_Z)      = fo(DF_FO_Z,1);
+  ref = zeros(DF_REF_SIZE, 1);
+  ref(DF_REF_X)  = fo(DF_FO_X,1);
+  ref(DF_REF_Y)  = fo(DF_FO_Y,1);
+  ref(DF_REF_Z)  = fo(DF_FO_Z,1);
 
-  state(DF_REF_XD)     = fo(DF_FO_X,2);
-  state(DF_REF_YD)     = fo(DF_FO_Y,2);
-  state(DF_REF_ZD)     = fo(DF_FO_Z,2);
+  ref(DF_REF_XD) = fo(DF_FO_X,2);
+  ref(DF_REF_YD) = fo(DF_FO_Y,2);
+  ref(DF_REF_ZD) = fo(DF_FO_Z,2);
 
-  state(DF_REF_PSI)    = fo(DF_FO_PSI,1);
+  ref(DF_REF_PSI) = fo(DF_FO_PSI,1);
   
-  psi = state(DF_REF_PSI);
+  psi = ref(DF_REF_PSI);
   cpsi = cos(psi);
   spsi = sin(psi);
   
@@ -54,8 +54,8 @@
   z2dmg = z2d - DF_G;
   av = sqrt(axpsi^2 + z2dmg^2);
   
-  state(DF_REF_PHI) = sign(z2dmg)*atan(aypsi/av);
-  state(DF_REF_THETA) = atan(axpsi/z2dmg);
+  ref(DF_REF_PHI) = sign(z2dmg)*atan(aypsi/av);
+  ref(DF_REF_THETA) = atan(axpsi/z2dmg);
   
   x3d = fo(1,4);
   y3d = fo(2,4);
@@ -80,14 +80,14 @@
   phid   = sign(z2dmg)*(adypsi*av-adv*aypsi)/(aypsi^2+av^2);
   thetad = (adxpsi*z2dmg-z3d*axpsi)/(axpsi^2+z2dmg^2);
 
-  cphi = cos(state(DF_REF_PHI));
-  sphi = sin(state(DF_REF_PHI));
-  ctheta = cos(state(DF_REF_THETA));
-  stheta = sin(state(DF_REF_THETA));
+  cphi = cos(ref(DF_REF_PHI));
+  sphi = sin(ref(DF_REF_PHI));
+  ctheta = cos(ref(DF_REF_THETA));
+  stheta = sin(ref(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;
+  ref(DF_REF_P) =  phid - stheta*psid;
+  ref(DF_REF_Q) =  cphi*thetad + sphi*ctheta*psid;
+  ref(DF_REF_R) = -sphi*thetad + cphi*ctheta*psid;
   
 endfunction
 

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_display.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_display.sci    2010-03-15 
15:42:10 UTC (rev 4691)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_display.sci    2010-03-15 
15:43:16 UTC (rev 4692)
@@ -333,3 +333,47 @@
 endfunction
 
 
+function display_sensors(time)
+
+  f=get("current_figure");
+  f.figure_name="Sensors";
+
+  subplot(3,3,1);
+  plot2d(time, deg_of_rad(sensor_gyro(1,:)), 2);
+  xtitle('gyro p');
+ 
+  subplot(3,3,2);
+  plot2d(time, deg_of_rad(sensor_gyro(2,:)), 2);
+  xtitle('gyro q');
+
+  subplot(3,3,3);
+  plot2d(time, deg_of_rad(sensor_gyro(3,:)), 2);
+  xtitle('gyro r');
+
+  
+  subplot(3,3,4);
+  plot2d(time, sensor_accel(1,:), 2);
+  xtitle('accel x');
+  
+  subplot(3,3,5);
+  plot2d(time, sensor_accel(2,:), 2);
+  xtitle('accel y');
+
+  subplot(3,3,6);
+  plot2d(time, sensor_accel(3,:), 2);
+  xtitle('accel z');
+
+  subplot(3,3,7);
+  plot2d(time, sensor_mag(1,:), 2);
+  xtitle('mag x');
+  
+  subplot(3,3,8);
+  plot2d(time, sensor_mag(2,:), 2);
+  xtitle('mag y');
+
+  subplot(3,3,9);
+  plot2d(time, sensor_mag(3,:), 2);
+  xtitle('mag z');
+
+endfunction
+

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci        2010-03-15 
15:42:10 UTC (rev 4691)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fdm.sci        2010-03-15 
15:43:16 UTC (rev 4692)
@@ -75,6 +75,7 @@
 
 
 function fdm_init(time, X0) 
+
   global fdm_time;
   fdm_time = time;
   global fdm_state;

Added: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_io.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_io.sci                         
(rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_io.sci 2010-03-15 15:43:16 UTC 
(rev 4692)
@@ -0,0 +1,20 @@
+
+
+
+function io_dump_fdm_sensor_dat(time, filename)
+
+  fid = mopen(filename+'.txt', "w");
+  for i=1:length(time)
+    mfprintf(fid, "%f [%.16f %.16f %.16f %.16f] [%.16f %.16f %.16f] [%.16f 
%.16f %.16f] [%.16f %.16f %.16f] [%.16f %.16f %.16f]\n", ...
+            time(i),...
+            fdm_state(FDM_SQI,i), fdm_state(FDM_SQX,i), fdm_state(FDM_SQY,i), 
fdm_state(FDM_SQZ,i),...
+            fdm_state(FDM_SP,i), fdm_state(FDM_SQ,i), fdm_state(FDM_SR,i),...
+            sensor_gyro(1,i), sensor_gyro(2,i), sensor_gyro(3,i),...
+            sensor_accel(1,i), sensor_accel(2,i), sensor_accel(3,i),...
+            sensor_mag(1,i), sensor_mag(2,i), sensor_mag(3,i) );
+  end
+  mclose(fid);
+  save(filename+'.dat',time, fdm_state, sensor_gyro, sensor_accel, sensor_mag);
+
+endfunction
+

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci        2010-03-15 
15:42:10 UTC (rev 4691)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sbb.sci        2010-03-15 
15:43:16 UTC (rev 4692)
@@ -41,13 +41,14 @@
   fo_traj = zeros(DF_FO_SIZE, DF_FO_ORDER, length(time));
 
   // psi
-  omega = rad_of_deg(45);
+if 1
+ omega = rad_of_deg(45);
   for i=1:length(time)
     fo_traj(DF_FO_PSI, 1, i) =          sin(omega*time(i));
     fo_traj(DF_FO_PSI, 2, i) =  omega*  cos(omega*time(i));
     fo_traj(DF_FO_PSI, 3, i) = -omega^2*sin(omega*time(i));
   end
-  
+end  
   // x and y
   disp_xy = b1(1:2) - b0(1:2);
   [pulse_dt, pulse_ampl, traj_dt] = compute_step(norm(disp_xy), dyn(1,:), 
max_accel(1), max_speed(1));

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sensors.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sensors.sci    2010-03-15 
15:42:10 UTC (rev 4691)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_sensors.sci    2010-03-15 
15:43:16 UTC (rev 4692)
@@ -2,29 +2,36 @@
 // Sensors model
 //
 
+sensor_noise_gyro = rad_of_deg(3);
+sensor_bias_gyro = [rad_of_deg(2.5) rad_of_deg(-2.5) rad_of_deg(-1.25)]';
+//sensor_bias_gyro = [rad_of_deg(0.5) rad_of_deg(-0.5) rad_of_deg(0.25)]';
+//sensor_bias_gyro = [rad_of_deg(0) rad_of_deg(0) rad_of_deg(0)]';
+sensor_noise_accel = 1.;
+sensor_noise_mag = 0.5;
 
 
-
 global sensor_gyro;
 global sensor_accel;
+global sensor_mag;
 global sensor_baro;
 global sensor_gps_pos;
 global sensor_gps_speed;
 
 
-function sensors_init() 
+function sensors_init(time) 
 
-  global fdm_time;
+  global sensor_gyro;
+  sensor_gyro = sensor_noise_gyro * rand(AXIS_NB, length(time),'normal'); 
   global sensor_accel;
-  sensor_accel = zeros(AXIS_NB, length(fdm_time));
-  global sensor_gyro;
-  sensor_gyro = zeros(AXIS_NB, length(fdm_time)); 
+  sensor_accel = sensor_noise_accel * rand(AXIS_NB, length(time),'normal'); 
+  global sensor_mag;
+  sensor_mag = sensor_noise_mag * rand(AXIS_NB, length(time),'normal'); 
   global sensor_baro;
-  sensor_baro = zeros(1, length(fdm_time)); 
+  sensor_baro = zeros(1, length(time)); 
   global sensor_gps_pos;
-  sensor_gps_pos = zeros(AXIS_NB, length(fdm_time)); 
+  sensor_gps_pos = zeros(AXIS_NB, length(time)); 
   global sensor_gps_speed;
-  sensor_gps_speed = zeros(AXIS_NB, length(fdm_time)); 
+  sensor_gps_speed = zeros(AXIS_NB, length(time)); 
   
 endfunction
 
@@ -36,14 +43,22 @@
   global fdm_accel;
   global sensor_gyro;
   global sensor_accel;
+  global sensor_mag;
   global sensor_baro;
   global sensor_gps_pos;
   global sensor_gps_speed;
 
-  sensor_gyro(:,i) = fdm_state(FDM_SP:FDM_SR, i);
+  sensor_gyro(:,i) = sensor_gyro(:,i) + fdm_state(FDM_SP:FDM_SR, i) + 
sensor_bias_gyro;
 
-  sensor_accel(:,i) = quat_vect_mult(fdm_state(FDM_SQI:FDM_SQZ,i), 
(fdm_accel(:,i)-[0; 0; 9.81]));
+  accel_earth = fdm_accel(:,i)-[0; 0; 9.81];
+  accel_body = quat_vect_mult(fdm_state(FDM_SQI:FDM_SQZ,i), accel_earth);
+  sensor_accel(:,i) = sensor_accel(:,i) + accel_body;
 
+//  mag_earth = [1 0 1]';
+  mag_earth = [0.4912 0.1225 0.8624]';
+  mag_body = quat_vect_mult(fdm_state(FDM_SQI:FDM_SQZ,i), mag_earth);
+  sensor_mag(:,i) = sensor_mag(:,i) + mag_body;
+  
   sensor_gps_pos(:,i) = fdm_state(FDM_SX:FDM_SZ, i);
 
   sensor_gps_speed(:,i) = fdm_state(FDM_SXD:FDM_SZD, i);

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-03-15 
15:42:10 UTC (rev 4691)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-03-15 
15:43:16 UTC (rev 4692)
@@ -21,8 +21,8 @@
 b1 = [  5    0   0];
 //b0 = [ 0   -5   0 ];
 //b1 = [ 0    5  0 ];
-//[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
-[fo_traj] = fo_traj_circle(time);
+[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
+//[fo_traj] = fo_traj_circle(time);
 
 set("current_figure",0);
 clf();
@@ -33,14 +33,14 @@
 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_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
+  diff_flat_ref(:,i) = df_ref_of_fo(fo_traj(:,:,i));
 end
 
 set("current_figure",1);
 clf();
 display_df_ref(time, diff_flat_ref);
 
-povray_draw( time, diff_flat_ref );
+//povray_draw( time, diff_flat_ref );
 
 set("current_figure",2);
 clf();
@@ -76,4 +76,4 @@
 //display_fdm(time, fdm_state, fdm_euler)
 display_control(time, fdm_state, fdm_euler, diff_flat_ref);
 
-povray_draw(time,diff_flat_ref);
\ No newline at end of file
+//povray_draw(time,diff_flat_ref);
\ No newline at end of file