Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Browse Source
This commit is contained in:
Antoine Drouin
2010-03-04 14:49:49 +00:00
parent 74b2fd5582
commit 8a63ef575f
2 changed files with 22 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q3d/q3d_ctl.sci
+2 -1
View File
@@ -42,12 +42,13 @@ endfunction
function ctl_run(i)
global ctl_diff_flat_cmd;
// ctl_diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i), fdm_Ct0/fdm_mass, fdm_la*fdm_Ct0/fdm_inertia);
// ctl_diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i), fdm_Ct0/fdm_mass, fdm_la*fdm_Ct0/fdm_inertia);
ctl_diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i), adp_est(1,i), adp_est(2,i));
global ctl_diff_flat_ref;
ctl_diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
global ctl_fb_cmd;
ctl_fb_cmd(:,i) = ctl_compute_feeback(fdm_state(:,i), ctl_diff_flat_ref(:,i), ctl_diff_flat_cmd(:,i), adp_est(1,i), adp_est(2,i));
// ctl_fb_cmd(:,i) = ctl_compute_feeback(fdm_state(:,i), ctl_diff_flat_ref(:,i), ctl_diff_flat_cmd(:,i), fdm_Ct0/fdm_mass, fdm_la*fdm_Ct0/fdm_inertia);
global ctl_u;
ctl_u(:,i) = ctl_diff_flat_cmd(:,i) + ctl_fb_cmd(:,i);
MotorsOfCmds = 0.5*[1 -1 ; 1 1];
sw/simulator/scilab/q3d/q3d_display.sci
+20 -20
View File
@@ -137,59 +137,59 @@ function display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd,
clf();
subplot(4,3,1);
plot2d(time, diff_flat_ref(FDM_SX, :), 3);
plot2d(time, fdm_state(FDM_SX, :), 2);
plot2d(time(2:$-1), ctl_diff_flat_ref(FDM_SX, 2:$-1), 3);
plot2d(time(2:$-1), fdm_state(FDM_SX, 2:$-1), 2);
legends(["fdm", "ref"],[2 3], with_box=%f, opt="ul");
xtitle('X');
subplot(4,3,2);
plot2d(time, diff_flat_ref(FDM_SZ, :), 3);
plot2d(time, fdm_state(FDM_SZ, :), 2);
plot2d(time(2:$-1), ctl_diff_flat_ref(FDM_SZ, 2:$-1), 3);
plot2d(time(2:$-1), fdm_state(FDM_SZ, 2:$-1), 2);
xtitle('Z');
subplot(4,3,3);
plot2d(time, deg_of_rad(diff_flat_ref(FDM_STHETA, :)), 3);
plot2d(time, deg_of_rad(fdm_state(FDM_STHETA, :)), 2);
plot2d(time(2:$-1), deg_of_rad(ctl_diff_flat_ref(FDM_STHETA, 2:$-1)), 3);
plot2d(time(2:$-1), deg_of_rad(fdm_state(FDM_STHETA, 2:$-1)), 2);
xtitle('Theta');
subplot(4,3,4);
plot2d(time, diff_flat_ref(FDM_SXD, :), 3);
plot2d(time, fdm_state(FDM_SXD, :), 2);
plot2d(time(2:$-1), ctl_diff_flat_ref(FDM_SXD, 2:$-1), 3);
plot2d(time(2:$-1), fdm_state(FDM_SXD, 2:$-1), 2);
xtitle('Xd');
subplot(4,3,5);
plot2d(time, diff_flat_ref(FDM_SZD, :), 3);
plot2d(time, fdm_state(FDM_SZD, :), 2);
plot2d(time(2:$-1), diff_flat_ref(FDM_SZD, 2:$-1), 3);
plot2d(time(2:$-1), fdm_state(FDM_SZD, 2:$-1), 2);
xtitle('Zd');
subplot(4,3,6);
plot2d(time, deg_of_rad(diff_flat_ref(FDM_STHETAD, :)), 3);
plot2d(time, deg_of_rad(fdm_state(FDM_STHETAD, :)), 2);
plot2d(time(2:$-1), deg_of_rad(diff_flat_ref(FDM_STHETAD, 2:$-1)), 3);
plot2d(time(2:$-1), deg_of_rad(fdm_state(FDM_STHETAD, 2:$-1)), 2);
xtitle('Thetad');
subplot(4,3,7);
xset("color",5);
foo = diff_flat_cmd(1,:) + fb_cmd(1,:);
xfpoly([time time($:-1:1)], [diff_flat_cmd(1,:) foo($:-1:1)]);
xfpoly([time(2:$-1) time($-1:-1:2)], [diff_flat_cmd(1,2:$-1) ctl_u(1,$-1:-1:2)]);
xset("color",1);
plot2d(time, diff_flat_cmd(1,:), 2);
plot2d(time(2:$-1), ctl_u(1,2:$-1), 5);
plot2d(time(2:$-1), diff_flat_cmd(1,2:$-1), 2);
xtitle('u_t');
subplot(4,3,8);
tot_cmd = diff_flat_cmd(2,:) + fb_cmd(2,:);
xset("color",5);
xfpoly([time time($:-1:1)], [diff_flat_cmd(2,:) tot_cmd($:-1:1)]);
xfpoly([time(2:$-1) time($-1:-1:2)], [diff_flat_cmd(2,2:$-1) ctl_u(2,$-1:-1:2)]);
xset("color",1);
plot2d(time, diff_flat_cmd(2,:), 2);
plot2d(time(2:$-1), ctl_u(2,2:$-1), 5);
plot2d(time(2:$-1), diff_flat_cmd(2,2:$-1), 2);
xtitle('u_d');
subplot(4,3,10);
plot2d(time, motor_cmd(1,:), 2);
plot2d(time(2:$-1), motor_cmd(1,2:$-1), 2);
xtitle('u1');
subplot(4,3,11);
plot2d(time, motor_cmd(2,:), 2);
plot2d(time(2:$-1), motor_cmd(2,2:$-1), 2);
xtitle('u2');