From 2132115f9461e11edf65325c9048566db72004db Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Wed, 3 Mar 2010 16:26:29 +0000 Subject: [PATCH] --- sw/simulator/scilab/q3d/q3d_ctl.sci | 36 +++++++++++++++++++++++--- sw/simulator/scilab/q3d/test_adapt.sce | 3 ++- 2 files changed, 35 insertions(+), 4 deletions(-) diff --git a/sw/simulator/scilab/q3d/q3d_ctl.sci b/sw/simulator/scilab/q3d/q3d_ctl.sci index 3198150e72..49c0e3fed9 100644 --- a/sw/simulator/scilab/q3d/q3d_ctl.sci +++ b/sw/simulator/scilab/q3d/q3d_ctl.sci @@ -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 diff --git a/sw/simulator/scilab/q3d/test_adapt.sce b/sw/simulator/scilab/q3d/test_adapt.sce index 28800915af..da94dd0eb8 100644 --- a/sw/simulator/scilab/q3d/test_adapt.sce +++ b/sw/simulator/scilab/q3d/test_adapt.sce @@ -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];