diff --git a/sw/simulator/scilab/q3d/q3d_ctl.sci b/sw/simulator/scilab/q3d/q3d_ctl.sci index af2419b9cc..471998c346 100644 --- a/sw/simulator/scilab/q3d/q3d_ctl.sci +++ b/sw/simulator/scilab/q3d/q3d_ctl.sci @@ -1,274 +1,23 @@ +// X Z Theta Xd Zd Theta_d +//ctl_gain = [ 0 -1 0 0 -1 0 +// -2 0 5 -2 0 5 ]; - -global ctl_sp_pos; -global ctl_ref_0; -global ctl_ref_1; -global ctl_ref_2; -global ctl_ref_3; -global ctl_ref_4; - -global ctl_ref_theta; -global ctl_ref_thetad; - -CMD_SF = 1; -CMD_DF = 2; - -global ctl_cmd; -global ctl_motor; - - - - -function ctl_init() - - global fdm_time; - - global ctl_sp_pos; - ctl_sp_pos = zeros(AXIS_NB, length(fdm_time)); - global ctl_ref_0; - ctl_ref_0 = zeros(AXIS_NB, length(fdm_time)); - global ctl_ref_1; - ctl_ref_1 = zeros(AXIS_NB, length(fdm_time)); - global ctl_ref_2; - ctl_ref_2 = zeros(AXIS_NB, length(fdm_time)); - global ctl_ref_3; - ctl_ref_3 = zeros(AXIS_NB, length(fdm_time)); - global ctl_ref_4; - ctl_ref_4 = zeros(AXIS_NB, length(fdm_time)); - - global ctl_ref_theta; - ctl_ref_theta = zeros(1, length(fdm_time)); - global ctl_ref_thetad; - ctl_ref_thetad = zeros(1, length(fdm_time)); - - - global ctl_cmd; - ctl_cmd = zeros(FDM_MOTOR_NB, length(fdm_time)); - global ctl_motor; - ctl_motor = zeros(FDM_MOTOR_NB, length(fdm_time)); - -endfunction - - - -ctl_mass = 0.25; -ctl_inertia = 0.0078; - -function ctl_run(i, sp) - - global ctl_sp_pos; - ctl_sp_pos(:,i) = sp; - - ctl_update_ref_4th_order(i); - - ctl_run_flatness(i); +if 0 +ctl_gain = [ 0 0 0 0 0 0 + 0 0 0 0 0 0 ]; +else +ctl_gain = [ 0 -1 0 0 -1 0 + -0.95 0 3 -1.2 0 3 ]; -endfunction +end +function [fb_cmd] = ctl_compute_feeback(fdm_state, ref) - - -function ctl_run_flatness(i) - - - global ctl_ref_theta; - 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)) / ... - ((9.81 + ctl_ref_2(AXIS_Z,i))^2+ctl_ref_2(AXIS_X,i)^2); - - global ctl_cmd; - global ctl_ref_2; - ctl_cmd(CMD_SF,i) = ctl_mass * sqrt(ctl_ref_2(AXIS_X,i)^2 +... - (ctl_ref_2(AXIS_Z,i)+9.81)^2); - - x2 = ctl_ref_2(AXIS_X,i); - z2p1 = ctl_ref_2(AXIS_Z,i)+9.81; - x3 = ctl_ref_3(AXIS_X,i); - z3 = ctl_ref_3(AXIS_Z,i); - x4 = ctl_ref_4(AXIS_X,i); - z4 = ctl_ref_4(AXIS_Z,i); - a = x4*z2p1 - z4*x2; - 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); - - global ctl_motor; - A2M = 0.5 * [ 1 1 - 1 -1 ]; - ctl_motor(:,i) = A2M * ctl_cmd(:,i); - -endfunction - - -function [state] = ctl_state_of_flat_out(ref) - - state = zeros(FDM_SSIZE, 1); - state(FDM_SX) = ref(1); - state(FDM_SZ) = ref(2); - state(FDM_SXD) = ref(3); - state(FDM_SZD) = ref(4); - theta = -atan(ref(5), 9.81 + ref(6)); - thetad = -((9.81 + ref(6))*ref(7) - ref(5)*ref(8)) / ... - ((9.81 + ref(6))^2+ref(5)^2); - state(FDM_STHETA) = theta; - state(FDM_STHETAD) = thetad; + state_err = fdm_state - ref; + fb_cmd = ctl_gain * state_err; endfunction - -ctl_omega_ref1 = [ rad_of_deg(90); rad_of_deg(90)]; -ctl_zeta_ref1 = [ 0.9; 0.9 ]; - -ctl_omega_ref2 = [ rad_of_deg(1440); rad_of_deg(1440)]; -ctl_zeta_ref2 = [ 0.9; 0.9 ]; - -a0 = ctl_omega_ref1^2 .* ctl_omega_ref2^2; -a1 = 2 * ctl_zeta_ref1 .* ctl_omega_ref1 .* ctl_omega_ref2^2 + ... - 2 * ctl_zeta_ref2 .* ctl_omega_ref2 .* ctl_omega_ref1^2; -a2 = ctl_omega_ref1^2 + ... - 2 * ctl_zeta_ref1 .* ctl_omega_ref1 .* ctl_zeta_ref2 .* ctl_omega_ref2 + ... - ctl_omega_ref2^2; -a3 = 2 * ctl_zeta_ref1 .* ctl_omega_ref1 + 2 * ctl_zeta_ref2 .* ctl_omega_ref2; -//a4 = [1;1]; - -function ctl_update_ref_4th_order(i) - global ctl_sp_pos; - global ctl_ref_0; - global ctl_ref_1; - global ctl_ref_2; - global ctl_ref_3; - global ctl_ref_4; - - dt = 1/512; - ctl_ref_3(:,i) = ctl_ref_3(:,i-1) + dt * ctl_ref_4(:,i-1); - ctl_ref_2(:,i) = ctl_ref_2(:,i-1) + dt * ctl_ref_3(:,i-1); - ctl_ref_1(:,i) = ctl_ref_1(:,i-1) + dt * ctl_ref_2(:,i-1); - ctl_ref_0(:,i) = ctl_ref_0(:,i-1) + dt * ctl_ref_1(:,i-1); - - err_pos = ctl_ref_0(:,i) - ctl_sp_pos(:,i); - ctl_ref_4(:,i) = -a3 .* ctl_ref_3(:,i) -a2 .* ctl_ref_2(:,i) -a1 .* ctl_ref_1(:,i) -a0.*err_pos; - - - - -endfunction - - -function ctl_update_ref_1st_order() - - - -endfunction - - - -function ctl_display() - nr = 5; - nc = 3; - subplot(nr,nc,1); - 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); - 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); - 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); - 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); - 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); - 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"); - xtitle('Theta(1)'); - - subplot(nr,nc,7); -// plot2d(fdm_time, ins_state(INS_SX,:),2); - plot2d(fdm_time, ctl_ref_2(AXIS_X,:),3); - legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); - xtitle('X(2)'); - - subplot(nr,nc,8); -// plot2d(fdm_time, ins_state(INS_SX,:),2); - plot2d(fdm_time, ctl_ref_2(AXIS_Z,:),3); - legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); - xtitle('Z(2)'); - - subplot(nr,nc,10); -// plot2d(fdm_time, ins_state(INS_SX,:),2); - plot2d(fdm_time, ctl_ref_3(AXIS_X,:),3); - legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); - xtitle('X(3)'); - - subplot(nr,nc,11); -// plot2d(fdm_time, ins_state(INS_SX,:),2); - plot2d(fdm_time, ctl_ref_3(AXIS_Z,:),3); - legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); - xtitle('Z(3)'); - - subplot(nr,nc,13); -// plot2d(fdm_time, ins_state(INS_SX,:),2); - plot2d(fdm_time, ctl_ref_4(AXIS_X,:),3); - legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); - xtitle('X(4)'); - - subplot(nr,nc,14); -// plot2d(fdm_time, ins_state(INS_SX,:),2); - plot2d(fdm_time, ctl_ref_4(AXIS_Z,:),3); - legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); - xtitle('Z(4)'); - - subplot(nr,nc,9); - plot2d(fdm_time, ctl_cmd(CMD_SF,:),3); -// legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); - xtitle('U_sigma'); - - subplot(nr,nc,12); - plot2d(fdm_time, ctl_cmd(CMD_DF,:),3); -// legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur"); - xtitle('U_delta'); - - subplot(nr,nc,15); - plot2d(fdm_time, ctl_motor(FDM_MOTOR_RIGHT,:),2); - plot2d(fdm_time, ctl_motor(FDM_MOTOR_LEFT,:),3); - plot2d(fdm_time, fdm_min_thrust * ones(1,length(fdm_time)),5); - plot2d(fdm_time, fdm_max_thrust * ones(1,length(fdm_time)),5); -// legends(["setpoint", "RIGHT", "LEFT"],[5 2 3], with_box=%f, opt="ur"); - xtitle('U_motors'); - -endfunction - diff --git a/sw/simulator/scilab/q3d/q3d_diff_flatness.sci b/sw/simulator/scilab/q3d/q3d_diff_flatness.sci index 16b26007c9..84820b1aa1 100644 --- a/sw/simulator/scilab/q3d/q3d_diff_flatness.sci +++ b/sw/simulator/scilab/q3d/q3d_diff_flatness.sci @@ -10,7 +10,9 @@ DF_ORANK = 5; // Number of time derivative needed fo_g = 9.81; fo_mass = 0.25; -fo_inertia = 0.0078; +fo_J = 0.0078; +fo_Ct0 = 4. * fo_mass * fo_g / 2; +fo_la = 0.25; global fo_traj; @@ -32,19 +34,19 @@ endfunction function [inp] = df_input_of_fo(fo) x2 = fo(1,3); - z2p1 = fo(2,3)+9.81; + z2pg = fo(2,3)+9.81; - u1 = fo_mass * sqrt((x2)^2 + (z2p1)^2); + u1 = fo_mass / fo_Ct0 * sqrt((x2)^2 + (z2pg)^2); x3 = fo(1,4); z3 = fo(2,4); x4 = fo(1,5); z4 = fo(2,5); - a = x4*z2p1 - z4*x2; - b = z2p1^2+x2^2; - c = 2 * (z2p1*z3 + x2*x3); - d = x3*z2p1-z3*x2; - u2 = -fo_inertia * ( a/b - c*d/b^2); + a = x4*z2pg - z4*x2; + b = z2pg^2+x2^2; + c = 2 * (z2pg*z3 + x2*x3); + d = x3*z2pg-z3*x2; + u2 = fo_J / fo_la /fo_Ct0 * ( a/b - c*d/b^2); inp = [u1; u2]; diff --git a/sw/simulator/scilab/q3d/q3d_display.sci b/sw/simulator/scilab/q3d/q3d_display.sci index 4575a59785..6a46a7931b 100644 --- a/sw/simulator/scilab/q3d/q3d_display.sci +++ b/sw/simulator/scilab/q3d/q3d_display.sci @@ -127,4 +127,72 @@ function display_fo_ref(time, diff_flat_ref) plot2d(time, deg_of_rad(diff_flat_ref(FDM_STHETAD, :))); xtitle('Thetad'); - endfunction \ No newline at end of file +endfunction + +function display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd, motor_cmd ); + + f=get("current_figure"); + f.figure_name="Control"; + + clf(); + + subplot(4,3,1); + plot2d(time, diff_flat_ref(FDM_SX, :), 3); + plot2d(time, fdm_state(FDM_SX, :), 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); + 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); + xtitle('Theta'); + + subplot(4,3,4); + plot2d(time, diff_flat_ref(FDM_SXD, :), 3); + plot2d(time, fdm_state(FDM_SXD, :), 2); + xtitle('Xd'); + + subplot(4,3,5); + plot2d(time, diff_flat_ref(FDM_SZD, :), 3); + plot2d(time, fdm_state(FDM_SZD, :), 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); + xtitle('Thetad'); + + + subplot(4,2,5); +// plot2d(time, diff_flat_cmd(1,:)+ fb_cmd(1,:), 5); + xset("color",5); + foo = diff_flat_cmd(1,:) + fb_cmd(1,:); + xfpoly([time time($:-1:1)], [diff_flat_cmd(1,:) foo($:-1:1)]); + xset("color",1); + plot2d(time, diff_flat_cmd(1,:), 2); + xtitle('u_t'); + + subplot(4,2,6); + 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)]); + xset("color",1); + plot2d(time, diff_flat_cmd(2,:), 2); + xtitle('u_d'); + + subplot(4,2,7); + plot2d(time, motor_cmd(1,:), 2); + xtitle('u1'); + + subplot(4,2,8); + plot2d(time, motor_cmd(2,:), 2); + xtitle('u2'); + + +endfunction + diff --git a/sw/simulator/scilab/q3d/q3d_fdm.sci b/sw/simulator/scilab/q3d/q3d_fdm.sci index c463e250c3..479c31fbb5 100644 --- a/sw/simulator/scilab/q3d/q3d_fdm.sci +++ b/sw/simulator/scilab/q3d/q3d_fdm.sci @@ -31,11 +31,16 @@ FDM_MOTOR_NB = 2; fdm_g = 9.81; fdm_mass = 0.25; fdm_inertia = 0.0078; +fdm_la = 0.25; // arm length -fdm_min_thrust = 0.5 * 0.1 * fdm_mass * fdm_g; // 10% of hovering power for each motor -fdm_max_thrust = 0.5 * 4.0 * fdm_mass * fdm_g; // 400% of hovering power for each motor +fdm_Ct0 = 4. * fdm_mass * fdm_g / 2; // thrust coefficient +fdm_V0 = 1e9; // +fdm_Cd = 1e-2; // drag coefficient -fdm_dt = 1./512.; +fdm_min_thrust = 0.05; // 5% +fdm_max_thrust = 1.0; // 400% + +fdm_wind = [0 0]'; global fdm_time; global fdm_state; @@ -69,12 +74,29 @@ endfunction function [Xdot] = fdm_get_derivatives(t, X, U) Xdot = zeros(length(X),1); - 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_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)); + + Xdot(FDM_SX:FDM_STHETA) = X(FDM_SXD:FDM_STHETAD); + + // forces : + gspeed_ltp = X(FDM_SXD:FDM_SZD); + airspeed_ltp = gspeed_ltp - fdm_wind; + + stheta = sin(X(FDM_STHETA)); + ctheta = cos(X(FDM_STHETA)); + + DCM = [ctheta stheta ; -stheta ctheta]; + airspeed_body = DCM * airspeed_ltp; + + lift_body = [0; sum(U) * fdm_Ct0 * ( 1 - abs(1/fdm_V0 * airspeed_body(AXIS_Z)))]; + lift_ltp = DCM'*lift_body; + weight_ltp = [0; -fdm_g * fdm_mass]; + drag_ltp = -fdm_Cd * norm(airspeed_ltp) * airspeed_ltp; + Xdot(FDM_SXD:FDM_SZD) = 1/fdm_mass*(lift_ltp+weight_ltp+drag_ltp); + + // moments + Xdot(FDM_STHETAD) = fdm_la * fdm_Ct0 / fdm_inertia*(U(FDM_MOTOR_RIGHT) - U(FDM_MOTOR_LEFT)); + +// pause endfunction diff --git a/sw/simulator/scilab/q3d/test_stop_stop.sce b/sw/simulator/scilab/q3d/test_stop_stop.sce index 70a628eb0f..64069e2f47 100644 --- a/sw/simulator/scilab/q3d/test_stop_stop.sce +++ b/sw/simulator/scilab/q3d/test_stop_stop.sce @@ -6,13 +6,14 @@ exec('q3d_sbb.sci'); exec('q3d_fo_traj_misc.sci'); exec('q3d_diff_flatness.sci'); +exec('q3d_ctl.sci'); exec('q3d_fdm.sci'); exec('q3d_display.sci'); exec('q3d_povray.sci'); t0 = 0; -t1 = 10.; +t1 = 3.; dt = 1/512; time = t0:dt:t1; @@ -23,18 +24,19 @@ if (0) [coefs] = poly_get_coef_from_bound(time, b0, b1); [fo_traj] = poly_gen_traj(time, coefs); end -if (0) -// differential equation +if (1) + // differential equation dyn = [rad_of_deg(500) 0.7; rad_of_deg(500) 0.7]; max_speed = [5 2.5]; max_accel = [ 9.81*tan(rad_of_deg(29.983325)) 0.5*9.81]; b0 = [-5 0]; - b1 = [ 5 -2]; + b1 = [ 5 -5]; [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1); printf('xfinal %f, zfinal:%f\n',fo_traj(1,1,$), fo_traj(2,1,$)); end -if (1) - [fo_traj] = fo_traj_circle(time, [0 0], 2, rad_of_deg(45)); +if (0) + // analytic definition + [fo_traj] = fo_traj_circle(time, [0 0], 2, rad_of_deg(45)); end @@ -45,13 +47,14 @@ for i=1:length(time) diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i)); end -fdm_init(time, df_state_of_fo(fo_traj(:,:,1))); +fdm_init(time, df_state_of_fo(fo_traj(:,:,1))); +fb_cmd = zeros(2,length(time)); +motor_cmd = zeros(2,length(time)); for i=2:length(time) - u1 = diff_flat_cmd(1,i-1); - u2 = diff_flat_cmd(2,i-1); - m1 = 0.5*(u1+u2); - m2 = 0.5*(u1-u2); - fdm_run(i, [m1 m2]') + fb_cmd(:,i-1) = ctl_compute_feeback(fdm_state(:,i-1),diff_flat_ref(:,i-1)); + MotorsOfCmds = 0.5*[1 -1 ; 1 1]; + motor_cmd(:,i-1) = MotorsOfCmds * ( diff_flat_cmd(:,i-1) + fb_cmd(:,i-1)); + fdm_run(i, motor_cmd(:,i-1)); end set("current_figure",0); @@ -60,23 +63,27 @@ f=get("current_figure"); f.figure_name="Flat Outputs Trajectory"; display_fo(time, fo_traj); +if 0 + set("current_figure",1); + clf(); + f=get("current_figure"); + f.figure_name="Commands"; + display_commands(time, diff_flat_cmd); + + set("current_figure",2); + clf(); + f=get("current_figure"); + f.figure_name="Reference"; + display_fo_ref(time, diff_flat_ref); + + set("current_figure",3); + clf(); + f=get("current_figure"); + f.figure_name="FDM"; + display_fdm(); +end + set("current_figure",1); -clf(); -f=get("current_figure"); -f.figure_name="Commands"; -display_commands(time, diff_flat_cmd); +display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd, motor_cmd ); -set("current_figure",2); -clf(); -f=get("current_figure"); -f.figure_name="Reference"; -display_fo_ref(time, diff_flat_ref); - -set("current_figure",3); -clf(); -f=get("current_figure"); -f.figure_name="FDM"; -display_fdm(); - - -povray_draw(time, diff_flat_ref); +//povray_draw(time, diff_flat_ref);