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

Browse Source
This commit is contained in:
Antoine Drouin
2010-03-03 16:26:29 +00:00
parent 330be5c8bf
commit 2132115f94
2 changed files with 35 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q3d/q3d_ctl.sci
+33 -3
View File
@@ -8,6 +8,15 @@ global ctl_u;
//ctl_gain = [ 0 -1 0 0 -1 0
// -2 0 5 -2 0 5 ];
fb_o_x = rad_of_deg(250);
fb_x_x = 0.9;
fb_o_z = rad_of_deg(250);
fb_x_z = 0.9;
fb_o_t = rad_of_deg(1050);
fb_x_t = 0.9;
if 0
ctl_gain = [ 0 0 0 0 0 0
0 0 0 0 0 0 ];
@@ -24,10 +33,31 @@ function ctl_init(time)
endfunction
function [fb_cmd] = ctl_compute_feeback(fdm_state, ref)
function [fb_cmd] = ctl_compute_feeback(fdm_state, s_ref, u_ref, a, b)
state_err = fdm_state - ref;
fb_cmd = ctl_gain * state_err;
state_err = fdm_state - s_ref;
st = sin(s_ref(FDM_STHETA));
ct = cos(s_ref(FDM_STHETA));
o2_x = fb_o_x^2;
xo2_x = 2*fb_x_x*fb_o_x;
o2_z = fb_o_z^2;
xo2_z = 2*fb_x_z*fb_o_z;
o2_t = fb_o_t^2;
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];
fb_cmd = gain * state_err;
endfunction
sw/simulator/scilab/q3d/test_adapt.sce
+2 -1
View File
@@ -30,6 +30,7 @@ if 1
b1 = [ 5 -5];
b2 = [ 0 -5];
[fo_traj1] = sbb_gen_traj(time1, dyn, max_speed, max_accel, b0, b1);
b1 = [fo_traj1(1,1,$) fo_traj1(2,1,$)];
[fo_traj2] = sbb_gen_traj(time2, dyn, max_speed, max_accel, b1, b2);
[time, fo_traj] = merge_traj(list(time1, time2), list(fo_traj1, fo_traj2));
else
@@ -55,7 +56,7 @@ 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));
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];