Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Corrected bug in q6d_diff_flatness.sci, function df_state_of_fo and added

Browse Source 
povray utilities. Seems to work in open loop.
This commit is contained in:
Gustavo Oliveira Violato
2010-02-25 02:14:45 +00:00
parent 829c5180b7
commit 1ad25b94fa
4 changed files with 46 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q3d/Makefile
+1 -1
View File
@@ -7,4 +7,4 @@ out.mpg:
ffmpeg -f image2 -i images/frame_%04d.ppm out.mpg
clean:
rm images/*
rm -f *~ \#*\#
sw/simulator/scilab/q6d/q6d_diff_flatness.sci
+1 -1
View File
@@ -78,7 +78,7 @@ function [state] = df_state_of_fo(fo)
adv = (axpsi*adxpsi + z2dmg*fo(3,4))/av;
phid = sign(z2dmg)*(adypsi*av-adv*aypsi)/(aypsi^2+av^2);
thetad = (adxpsi*z2dmg-z3d*aypsi)/(axpsi^2+z2dmg^2);
thetad = (adxpsi*z2dmg-z3d*axpsi)/(axpsi^2+z2dmg^2);
cphi = cos(state(DF_REF_PHI));
sphi = sin(state(DF_REF_PHI));
sw/simulator/scilab/q6d/q6d_povray.sci
+34
View File
@@ -0,0 +1,34 @@
function povray_draw( time, diff_flat_ref )
dt_display = 1/25;
t_idx = 1;
f_idx = 1;
t = time(t_idx);
while (t_idx<length(time))
printf('drawing %d (%f)\n', f_idx, time(t_idx));
x = -1000*diff_flat_ref(DF_REF_X,t_idx);
y = 1000*diff_flat_ref(DF_REF_Y,t_idx);
z = -1000*diff_flat_ref(DF_REF_Z,t_idx);
phi = deg_of_rad(-diff_flat_ref(DF_REF_PHI,t_idx));
theta = deg_of_rad(-diff_flat_ref(DF_REF_THETA,t_idx));
psi = deg_of_rad(-diff_flat_ref(DF_REF_PSI,t_idx));
// printf('( %f %f %f)\n', x, y, alpha);
fid = mopen('povray/q3d.pov', "w");
mfprintf(fid, "#include ""povray/q3d.inc""\n");
mfprintf(fid, "object { Q3D() rotate <%f,%f,%f> translate <%f,%f,%f>}",-phi,psi,-theta,x,y,z);
mclose(fid);
cmd = sprintf('povray povray/q3d.pov +Opovray/foo%04d.png Display=false +W800 +H600 +Q9 +A0.3 +R5', f_idx);
a = unix_g(cmd);
while (t_idx<length(time) & time(t_idx) < f_idx*dt_display)
t_idx = t_idx + 1;
end
f_idx = f_idx + 1;
end
mplayer_cmd = "mencoder ""mf://povray/foo*.png"" -mf fps=25 -o povray/test.avi -ovc lavc -lavcopts vcodec=msmpeg4v2:vbitrate=800";
unix_g(mplayer_cmd);
endfunction
sw/simulator/scilab/q6d/test_stop_stop.sce
+10 -6
View File
@@ -6,9 +6,10 @@ exec('q6d_diff_flatness.sci');
exec('q6d_fdm.sci');
exec('q6d_algebra.sci');
exec('q6d_display.sci');
exec('q6d_povray.sci');
t0 = 0;
t1 = 8.;
t1 = 10.;
dt = 1/512;
time = t0:dt:t1;
@@ -19,9 +20,10 @@ 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 = [ 1 -1 -1 ];
//[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
[fo_traj] = fo_traj_circle(time);
b1 = [ 0 0 5 ];
[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
[traj] = fo_traj_circle(time);
fo_traj(1:2,:,:) = traj(1:2,:,:);
set("current_figure",0);
clf();
@@ -62,7 +64,7 @@ X0 = [diff_flat_ref(DF_REF_X,1) ; diff_flat_ref(DF_REF_Y,1) ; diff_flat_ref(DF_
quat_of_euler([diff_flat_ref(DF_REF_PHI,1) diff_flat_ref(DF_REF_THETA,1) diff_flat_ref(DF_REF_PSI,1)])
diff_flat_ref(DF_REF_P,1) ; diff_flat_ref(DF_REF_Q,1) ; diff_flat_ref(DF_REF_R,1)
];
fdm_init(time, X0);
for i=2:length(time)
fdm_run(i, motor_cmd(:,i-1));
@@ -71,4 +73,6 @@ end
set("current_figure",3);
clf();
//display_fdm(time, fdm_state, fdm_euler)
display_control(time, fdm_state, fdm_euler, diff_flat_ref);
display_control(time, fdm_state, fdm_euler, diff_flat_ref);
povray_draw(time,diff_flat_ref);