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

cleaning

Browse Source
This commit is contained in:
Antoine Drouin
2010-03-03 22:29:50 +00:00
parent 96dc4b660f
commit 0821a5da5c
2 changed files with 34 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q3d/q3d_ctl.sci
+31 -12
View File
@@ -2,11 +2,11 @@ CTL_UT = 1;
CTL_UD = 2;
CTL_USIZE = 2;
global ctl_diff_flat_cmd;
global ctl_diff_flat_ref;
global ctl_fb_cmd;
global ctl_u;
// X Z Theta Xd Zd Theta_d
//ctl_gain = [ 0 -1 0 0 -1 0
// -2 0 5 -2 0 5 ];
global ctl_motor_cmd;
fb_o_x = rad_of_deg(250);
fb_x_x = 0.9;
@@ -27,12 +27,37 @@ ctl_gain = [ 0 -1 0 0 -1 0
end
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(FDM_SSIZE, 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(2,length(time));
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), 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));
global ctl_u;
ctl_u(:,i) = ctl_diff_flat_cmd(:,i) + ctl_fb_cmd(:,i);
MotorsOfCmds = 0.5*[1 -1 ; 1 1];
global ctl_motor_cmd;
ctl_motor_cmd(:,i) = MotorsOfCmds * ctl_u(:,i);
endfunction
function [fb_cmd] = ctl_compute_feeback(fdm_state, s_ref, u_ref, a, b)
state_err = fdm_state - s_ref;
@@ -47,12 +72,6 @@ function [fb_cmd] = ctl_compute_feeback(fdm_state, s_ref, u_ref, a, b)
xo2_t = 2*fb_x_t*fb_o_t;
ut = u_ref(1);
if ut == 0
pause
end
// gain = [ o2_x*st/a -o2_z*ct/a 0 xo2_x*st/a -xo2_z*ct/a 0
// o2_t/b*o2_x*ct/a/ut o2_t/b*o2_z*st/a/ut -o2_t/b -o2_t/b*xo2_x*ct/a/ut -o2_t/b*xo2_z*st/a/ut -xo2_t/b];
gain = [ o2_x*st/a -o2_z*ct/a 0 xo2_x*st/a -xo2_z*ct/a 0
o2_t/b*o2_x*ct/a/ut o2_t/b*o2_z*st/a/ut -o2_t/b o2_t/b*xo2_x*ct/a/ut o2_t/b*xo2_z*st/a/ut -xo2_t/b];
sw/simulator/scilab/q3d/test_adapt.sce
+3 -18
View File
@@ -46,34 +46,19 @@ k=find(time > 5 & time < 5.05);
fdm_perturb(FDM_AX,k) = 10*ones(1,length(k));
ctl_init(time);
diff_flat_cmd = zeros(2,length(time));
diff_flat_ref = zeros(FDM_SSIZE, length(time));
diff_flat_ref(:,1) = df_state_of_fo(fo_traj(:,:,1));
fb_cmd = zeros(2,length(time));
motor_cmd = zeros(2,length(time));
adp_init(time, [15 100]', []);
sensors_init(time)
for i=2:length(time)
// diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i), fdm_Ct0/fdm_mass, fdm_la*fdm_Ct0/fdm_inertia);
diff_flat_cmd(:,i-1) = df_input_of_fo(fo_traj(:,:,i-1), adp_est(1,i-1), adp_est(2,i-1));
diff_flat_ref(:,i-1) = df_state_of_fo(fo_traj(:,:,i-1));
fb_cmd(:,i-1) = ctl_compute_feeback(fdm_state(:,i-1),diff_flat_ref(:,i-1), diff_flat_cmd(:,i-1), adp_est(1,i-1), adp_est(2,i-1));
global ctl_u;
ctl_u(:,i-1) = diff_flat_cmd(:,i-1) + fb_cmd(:,i-1);
MotorsOfCmds = 0.5*[1 -1 ; 1 1];
motor_cmd(:,i-1) = MotorsOfCmds * ctl_u(:,i-1);
fdm_run(i, motor_cmd(:,i-1));
ctl_run(i-1);
fdm_run(i, ctl_motor_cmd(:,i-1));
sensors_run(i);
adp_run(i);
end
set("current_figure",1);
display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd, motor_cmd );
display_control(time, ctl_diff_flat_ref, fdm_state, ctl_diff_flat_cmd, ctl_fb_cmd, ctl_motor_cmd );
set("current_figure",2);
display_adaptation();