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 29 Packages Projects Releases Wiki Activity

Browse Source
This commit is contained in:
Antoine Drouin
2010-02-22 08:22:47 +00:00
parent 19b9d727ed
commit d6100f01d2
2 changed files with 63 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q3d/q3d_sbb.sci
+52 -2
View File
@@ -1,12 +1,62 @@
//
//
// Smooth bang bang
// Smoothed bang bang trajectory
//
//
function [fo_traj] = sbb_gen_traj(time, vmax, theta_max, b0, b1)
sbb_omega = rad_of_deg(500);
sbb_ksi = 0.70;
sbb_tolerance = 0.05;
function [fo_traj] = sbb_gen_traj(time, max_xd, max_theta, b0, b1)
n_comp = 2;
order = 5;
fo_traj = zeros(n_comp, order, length(time));
// compute trajectory caracteristics
dx = b1(1)-b0(1);
step_dt = -log(sbb_tolerance)/(sbb_ksi*sbb_omega*sqrt(1-sbb_ksi^2));
step_xdd = dx / (2*step_dt^2);
max_xdd = 9.81 * tan(max_theta);
if step_xdd < max_xdd
if step_xdd > max_xd/step_dt
step_xdd = max_xd/step_dt;
end
else
step_xdd = max_xdd;
if step_xdd <= max_xd/step_dt
step_dt = max_xd/step_xdd;
else
step_xdd = max_xd/step_dt;
end
end
t_tot = (dx - 2*step_dt*(step_xdd*step_dt))/(step_xdd*step_dt) + 4*step_dt;
printf('dx :%f\n', dx);
printf('step_dt :%f\n', step_dt);
printf('step_xdd :%f\n', step_xdd);
printf('total time:%f\n', t_tot);
fo_traj(1,1,1) = b0(1);
for i=2:length(time)
if time(i) < step_dt
sp = step_xdd;
elseif time(i) < t_tot - step_dt & time(i) >= t_tot - 2 * step_dt
sp = -step_xdd;
else
sp = 0;
end
fo_traj(1,:,i) = propagate_traj(fo_traj(1,:,i-1), sp, time(i) - time(i-1));
end
endfunction
function [Xi1] = propagate_traj(Xi, sp, dt)
Xi1 = zeros(1,5);
Xi1(1:4) = Xi(1:4) + Xi(2:5)*dt;
Xi1(5) = -2*sbb_ksi*sbb_omega*Xi1(4)-sbb_omega^2*(Xi1(3)-sp);
endfunction
sw/simulator/scilab/q3d/test_stop_stop.sce
+11 -5
View File
@@ -7,22 +7,25 @@ exec('q3d_sbb.sci');
exec('q3d_diff_flatness.sci');
exec('q3d_fdm.sci');
exec('q3d_display.sci');
exec('q3d_povray.sci');
t0 = 0;
t1 = 2.;
t1 = 4.5;
dt = 0.01;
time = t0:dt:t1;
if (0)
// polynomials
b0 = [0 0 0 0 0; 0 0 0 0 0];
b1 = [5 0 0 0 0; 0 0 0 0 0];
b1 = [2 0 0 0 0; 0 0 0 0 0];
[coefs] = poly_get_coef_from_bound(time, b0, b1);
[fo_traj] = poly_gen_traj(time, coefs);
else
// differential equation
[fo_traj] = sbb_gen_traj(time, 5, rad_of_deg(30), [0 0] [20 0]);
[fo_traj] = sbb_gen_traj(time, 5, rad_of_deg(29.983325), [0 0], [5 0]);
printf('xfinal:%f\n',fo_traj(1,1,$));
end
diff_flat_cmd = zeros(2,length(time));
@@ -32,7 +35,7 @@ for i=1:length(time)
diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
end
fdm_init(time, df_state_of_fo(b0) );
fdm_init(time, df_state_of_fo(fo_traj(:,:,1)));
for i=2:length(time)
u1 = diff_flat_cmd(1,i-1);
u2 = diff_flat_cmd(2,i-1);
@@ -40,7 +43,10 @@ for i=2:length(time)
m2 = 0.5*(u1-u2);
fdm_run(i, [m1 m2]')
end
//povray_draw(time, diff_flat_ref);
set("current_figure",0);
clf();
f=get("current_figure");