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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2009-03-13 01:35:58 +00:00
parent 86aaa45365
commit 3d09e8a185
6 changed files with 113 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q1d/q1d_ctl_common.sci
+22 -24
View File
@@ -17,10 +17,12 @@ ctl_min_cmd = 0.01; // actuators low saturation
ref_omega = rad_of_deg(200.);// second order linear dynamics
ref_zeta = 0.85;
ref_saturate = 1; // with saturations
ref_max_accel = 2. * fdm_g; // m/s2 - aka (fdm_max_thrust/fdm_mass - 1) fdm_g
ref_min_accel = -0.9 * fdm_g;// m/s2
ref_max_speed = 3.; // m/s
ref_max_accel = 2. * 9.81; // m/s2 - aka (fdm_max_thrust/fdm_mass - 1) fdm_g
ref_min_accel = -0.9 * 9.81; // m/s2
ref_max_speed = 3.; // m/s
ref_min_speed = -3.; // m/s
ref_thau_z = 0.35;
ref_thau_zd = 0.12;
//
// propagation of a second order linear model with saturations
@@ -29,29 +31,25 @@ function [Xrefi1] = ctl_update_ref(Xrefi, Xspi, dt)
Xrefi1 = zeros(FDM_SIZE, 1);
// second order ref model
Xrefi1(REF_Z) = Xrefi(REF_Z) + dt * Xrefi(REF_ZD);
Xrefi1(REF_ZD) = Xrefi(REF_ZD) + dt * Xrefi(REF_ZDD);
Xrefi1(REF_ZDD) = -2*ref_zeta*ref_omega * Xrefi(REF_ZD) ...
-ref_omega^2 * ( Xrefi(REF_Z) - Xspi);
if ref_saturate
// saturate acceleration
if Xrefi1(REF_ZDD) > ref_max_accel
Xrefi1(REF_ZDD) = ref_max_accel;
elseif Xrefi1(REF_ZDD) < ref_min_accel
Xrefi1(REF_ZDD) = ref_min_accel;
end
// saturate speed
if Xrefi1(REF_ZD) >= ref_max_speed & Xrefi1(REF_ZDD) >= 0
Xrefi1(REF_ZD) = ref_max_speed;
Xrefi1(REF_ZDD) = 0;
elseif Xrefi1(REF_ZD) <= -ref_max_speed & Xrefi1(REF_ZDD) <= 0
Xrefi1(REF_ZD) = -ref_max_speed;
Xrefi1(REF_ZDD) = 0;
end
end
err_z = Xrefi1(REF_Z) - Xspi;
sp_zd = -1/ref_thau_z*err_z;
if sp_zd >= 0
sp_zd = min(sp_zd, ref_max_speed);
else
sp_zd = max(sp_zd, ref_min_speed);
end
err_zd = Xrefi1(REF_ZD) - sp_zd;
sp_zdd = -1/ref_thau_zd*err_zd;
if sp_zdd >= 0
sp_zdd = min(sp_zdd, ref_max_accel);
else
sp_zdd = max(sp_zdd, ref_min_accel);
end
Xrefi1(REF_ZDD) = sp_zdd;
endfunction
//
sw/simulator/scilab/q1d/q1d_run.sce
+7 -7
View File
@@ -1,12 +1,12 @@
clear();
exec("tvc_utils.sci");
exec("tvc_fdm.sci");
exec("tvc_sensors.sci");
exec("tvc_ins.sci");
exec("tvc_ctl_common.sci");
exec("tvc_ctl_miac.sci");
exec("tvc_ctl_mrac.sci");
exec("q1d_utils.sci");
exec("q1d_fdm.sci");
exec("q1d_sensors.sci");
exec("q1d_ins.sci");
exec("q1d_ctl_common.sci");
exec("q1d_ctl_miac.sci");
exec("q1d_ctl_mrac.sci");
//
sw/simulator/scilab/q3d/q3d_ctl.sci
+17 -11
View File
@@ -61,7 +61,7 @@ function ctl_run(i)
if fdm_time(i) < 4
ctl_sp_pos(:,i)= [ 1; 0];
else
ctl_sp_pos(:,i)= [ 0; 0];
ctl_sp_pos(:,i)= [ 0; 1];
end
ctl_update_ref_4th_order(i);
@@ -81,11 +81,11 @@ function ctl_run(i)
b = z2p1^2+x2^2;
c = 2 * (z2p1*z3 + x2*x3);
d = x3*z2p1-z3*x2;
ctl_cmd(CMD_DF,i) = ctl_inertia * ( a/b - c*d/b^2);
ctl_cmd(CMD_DF,i) = -ctl_inertia * ( a/b - c*d/b^2);
global ctl_motor;
A2M = 0.5 * [ 1 1
1 -1 ];
1 -1 ];
ctl_motor(:,i) = A2M * ctl_cmd(:,i);
endfunction
@@ -126,9 +126,9 @@ function ctl_update_ref_4th_order(i)
ctl_ref_4(:,i) = -a3 .* ctl_ref_3(:,i) -a2 .* ctl_ref_2(:,i) -a1 .* ctl_ref_1(:,i) -a0.*err_pos;
global ctl_ref_theta;
ctl_ref_theta(i) = atan(ctl_ref_2(AXIS_X,i), 9.81 + ctl_ref_2(AXIS_Z,i));
ctl_ref_theta(i) = -atan(ctl_ref_2(AXIS_X,i), 9.81 + ctl_ref_2(AXIS_Z,i));
global ctl_ref_thetad;
ctl_ref_thetad(i) = ((9.81 + ctl_ref_2(AXIS_Z,i))*ctl_ref_3(AXIS_X,i) - ctl_ref_2(AXIS_X,i)*ctl_ref_3(AXIS_Z,i)) / ...
ctl_ref_thetad(i) = -((9.81 + ctl_ref_2(AXIS_Z,i))*ctl_ref_3(AXIS_X,i) - ctl_ref_2(AXIS_X,i)*ctl_ref_3(AXIS_Z,i)) / ...
((9.81 + ctl_ref_2(AXIS_Z,i))^2+ctl_ref_2(AXIS_X,i)^2);
@@ -148,40 +148,46 @@ function ctl_display()
nr = 5;
nc = 3;
subplot(nr,nc,1);
plot2d(fdm_time, fdm_state(FDM_SX,:),2);
plot_with_min_rect(fdm_time, fdm_state(FDM_SX,:),2, -0.5, 0.5);
// plot2d(fdm_time, fdm_state(FDM_SX,:),2);
plot2d(fdm_time, ctl_ref_0(AXIS_X,:),3);
plot2d(fdm_time, ctl_sp_pos(AXIS_X,:),5);
legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
xtitle('X(0)');
subplot(nr,nc,2);
plot2d(fdm_time, fdm_state(FDM_SZ,:),2);
plot_with_min_rect(fdm_time, fdm_state(FDM_SZ,:),2, -0.5, 0.5);
// plot2d(fdm_time, fdm_state(FDM_SZ,:),2);
plot2d(fdm_time, ctl_ref_0(AXIS_Z,:),3);
plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5);
legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
xtitle('Z(0)');
subplot(nr,nc,3);
plot2d(fdm_time, deg_of_rad(fdm_state(FDM_STHETA,:)),2);
plot_with_min_rect(fdm_time, deg_of_rad(fdm_state(FDM_STHETA,:)),2, -1., 1.);
// plot2d(fdm_time, deg_of_rad(fdm_state(FDM_STHETA,:)),2);
plot2d(fdm_time, deg_of_rad(ctl_ref_theta),3);
// plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5);
legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
xtitle('Theta(0)');
subplot(nr,nc,4);
plot2d(fdm_time, fdm_state(FDM_SXD,:),2);
plot_with_min_rect(fdm_time, fdm_state(FDM_SXD,:),2, -0.5, 0.5);
// plot2d(fdm_time, fdm_state(FDM_SXD,:),2);
plot2d(fdm_time, ctl_ref_1(AXIS_X,:),3);
legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
xtitle('X(1)');
subplot(nr,nc,5);
plot2d(fdm_time, fdm_state(FDM_SZD,:),2);
plot_with_min_rect(fdm_time, fdm_state(FDM_SZD,:),2, -0.5, 0.5);
// plot2d(fdm_time, fdm_state(FDM_SZD,:),2);
plot2d(fdm_time, ctl_ref_1(AXIS_Z,:),3);
legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
xtitle('Z(1)');
subplot(nr,nc,6);
plot2d(fdm_time, deg_of_rad(fdm_state(FDM_STHETAD,:)),2);
plot_with_min_rect(fdm_time, deg_of_rad(fdm_state(FDM_STHETAD,:)),2, -1., 1.);
//plot2d(fdm_time, deg_of_rad(fdm_state(FDM_STHETAD,:)),2);
plot2d(fdm_time, deg_of_rad(ctl_ref_thetad),3);
// plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5);
legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
sw/simulator/scilab/q3d/q3d_fdm.sci
+1 -1
View File
@@ -58,7 +58,7 @@ function [Xdot] = fdm_get_derivatives(t, X, U)
Xdot(FDM_SX) = X(FDM_SXD);
Xdot(FDM_SZ) = X(FDM_SZD);
Xdot(FDM_STHETA) = X(FDM_STHETAD);
Xdot(FDM_SXD) = sum(U)/fdm_mass*sin(X(FDM_STHETA));
Xdot(FDM_SXD) = -sum(U)/fdm_mass*sin(X(FDM_STHETA));
Xdot(FDM_SZD) = 1/fdm_mass*(sum(U)*cos(X(FDM_STHETA))-fdm_mass*fdm_g);
Xdot(FDM_STHETAD) = 1/fdm_inertia*(U(FDM_MOTOR_RIGHT) - U(FDM_MOTOR_LEFT));
sw/simulator/scilab/q3d/q3d_utils.sci
+54
View File
@@ -32,3 +32,57 @@ function [_o] = trim_vect(_i,_min, _max)
end
end
endfunction
function plot_with_min_rect(t, v, c, min_y, max_y)
max_y = max(max_y, max(v));
min_y = min(min_y, min(v));
_rect = [t(1) min_y, t($), max_y];
plot2d(t, v, c, rect=_rect);
endfunction
function draw_quad(i)
global fdm_state;
body_lines = list([-0.5 0; 0.5 0], [-0.8 0.08; -0.2 0.08], [0.2 0.08; 0.8 0.08]);
dcmt = [ cos(fdm_state(FDM_STHETA,i)) -sin(fdm_state(FDM_STHETA,i))
sin(fdm_state(FDM_STHETA,i)) cos(fdm_state(FDM_STHETA,i)) ];
_rect = [ -1 -1 1 1];
earth_lines = list();
plot2d(fdm_state(FDM_SX,i), fdm_state(FDM_SZ,i),3);
for j=1:length(body_lines)
earth_lines(j) = dcmt*body_lines(j)';
plot2d(earth_lines(j)(1,:)+fdm_state(FDM_SX,i),earth_lines(j)(2,:)+fdm_state(FDM_SZ,i),1, rect=_rect);
end
endfunction
function gen_video()
dt_display = 1/25;
time_display = 0;
for i=1:length(fdm_time)
if fdm_time(i) >= time_display
set("current_figure",0);
f=get("current_figure");
f.figure_name="CTL";
clf();
drawlater();
draw_quad(i);
// filename = sprintf('images/frame_%03d.gif',i);
filename = sprintf('images/frame_%03d.ppm',i);
// xs2gif(0, filename);
xs2ppm(0, filename);
drawnow();
pause
time_display = time_display + dt_display;
end
end
endfunction
sw/simulator/scilab/q3d/test_2.sce
+12 -7
View File
@@ -8,7 +8,7 @@ exec('q3d_ctl.sci');
fdm_init(0,8.0);
fdm_init(0,7.0);
ctl_init();
global ctl_motor;
@@ -21,10 +21,15 @@ for i=1:length(fdm_time)-1
end
set("current_figure",0);
//set("current_figure",0);
clf();
f=get("current_figure");
f.figure_name="CTL";
drawlater();
ctl_display();
drawnow();
//f=get("current_figure");
//f.figure_name="CTL";
if 1
drawlater();
ctl_display();
drawnow();
end
pause
gen_video();