diff --git a/sw/simulator/scilab/q6d/q6d_diff_flatness.sci b/sw/simulator/scilab/q6d/q6d_diff_flatness.sci index 980bd3377b..04afc8cbf5 100644 --- a/sw/simulator/scilab/q6d/q6d_diff_flatness.sci +++ b/sw/simulator/scilab/q6d/q6d_diff_flatness.sci @@ -15,7 +15,11 @@ DF_REF_SIZE = 12; DF_G = 9.81; DF_MASS = 0.5; - +DF_JXX = 0.0078; +DF_JYY = 0.0078; +DF_JZZ = 0.0137; +DF_L = 0.25; +DF_CM_OV_CT = 0.1; // state from flat output function [state] = df_state_of_fo(fo) @@ -30,37 +34,61 @@ function [state] = df_state_of_fo(fo) state(DF_REF_ZD) = fo(3,2); state(DF_REF_PSI) = fo(4,1); - - axpsi = cos(state(DF_REF_PSI))*fo(1,3) + sin(state(DF_REF_PSI))*fo(2,3); - aypsi = sin(state(DF_REF_PSI))*fo(1,3) - cos(state(DF_REF_PSI))*fo(2,3); - zddmg = fo(3,3) - DF_G; - av = sqrt(axpsi^2 + zddmg^2); + + psi = state(DF_REF_PSI); + cpsi = cos(psi); + spsi = sin(psi); + + x2d = fo(1,3); + y2d = fo(2,3); + z2d = fo(2,3); + + axpsi = cpsi*x2d + spsi*y2d; + aypsi = spsi*x2d - cpsi*y2d; + z2dmg = z2d - DF_G; + av = sqrt(axpsi^2 + z2dmg^2); state(DF_REF_PHI) = atan(aypsi/av); - state(DF_REF_THETA) = atan(axpsi/zddmg); + state(DF_REF_THETA) = atan(axpsi/z2dmg); - jxpsi = cos(state(DF_REF_PSI))*fo(1,4) + sin(state(DF_REF_PSI))*fo(2,4); - jypsi = sin(state(DF_REF_PSI))*fo(1,4) - cos(state(DF_REF_PSI))*fo(2,4); + x3d = fo(1,4); + y3d = fo(2,4); + z3d = fo(3,4); + + jxpsi = cpsi*x3d + spsi*y3d; + jypsi = spsi*x3d - cpsi*y3d; - kxpsi = cos(state(DF_REF_PSI))*fo(1,5) + sin(state(DF_REF_PSI))*fo(2,5); - kypsi = sin(state(DF_REF_PSI))*fo(1,5) - cos(state(DF_REF_PSI))*fo(2,5); + x4d = fo(1,5); + y4d = fo(2,5); + + kxpsi = cpsi*x4d + spsi*y4d; + kypsi = spsi*x4d - cpsi*y4d; psid = fo(4,2); adxpsi = -psid*aypsi + jxpsi; adypsi = psid*axpsi + jypsi; - adv = (axpsi*adxpsi + zddmg*fo(3,4))/av; + adv = (axpsi*adxpsi + z2dmg*fo(3,4))/av; phid = (adypsi*av-adv*aypsi)/(aypsi^2+av^2); - thetad = (adxpsi*zddmg-fo(3,4)*aypsi)/(axpsi^2+zddmg^2); + thetad = (adxpsi*z2dmg-z3d*aypsi)/(axpsi^2+z2dmg^2); - state(DF_REF_P) = phid - sin(state(DF_REF_THETA))*psid; - state(DF_REF_Q) = cos(state(DF_REF_PHI))*thetad + sin(state(DF_REF_PHI))*cos(state(DF_REF_THETA))*psid; - state(DF_REF_R) = -sin(state(DF_REF_PHI))*thetad + cos(state(DF_REF_PHI))*cos(state(DF_REF_THETA))*psid; + cphi = cos(state(DF_REF_PHI)); + sphi = sin(state(DF_REF_PHI)); + + ctheta = cos(state(DF_REF_THETA)); + stheta = sin(state(DF_REF_THETA)); + + state(DF_REF_P) = phid - stheta*psid; + state(DF_REF_Q) = cphi*thetad + sphi*ctheta*psid; + state(DF_REF_R) = -sphi*thetad + cphi*ctheta*psid; endfunction + + + // control input from flat output function [inp] = df_input_of_fo(fo) @@ -71,25 +99,69 @@ function [inp] = df_input_of_fo(fo) zddmg = fo(3,3) - DF_G; inp(1) = DF_MASS * sqrt(xdd^2+ydd^2+zddmg^2); - psi = fo(4,1); - + psi = fo(4,1); + psid = fo(4,2); + psidd = fo(4,3); + axpsi = cos(psi)*xdd + sin(psi)*ydd; aypsi = sin(psi)*xdd - cos(psi)*ydd; - zddmg = fo(3,3) - DF_G; - av = sqrt(axpsi^2 + zddmg^2); + xddd = fo(1,4); + yddd = fo(2,4); + + jxpsi = cos(psi)*xddd + sin(psi)*yddd; + jypsi = sin(psi)*xddd - cos(psi)*yddd; + + xdddd = fo(1,5); + ydddd = fo(2,5); + + kxpsi = cos(psi)*xdddd + sin(psi)*ydddd; + kypsi = sin(psi)*xdddd - cos(psi)*ydddd; + + adxpsi = -psid*aypsi + jxpsi; + adypsi = psid*axpsi + jypsi; + + addxpsi = -psidd*aypsi - psid^2*axpsi - 2*psid*jypsi + kxpsi; + addypsi = psidd*axpsi - psid^2*aypsi + 2*psid*jxpsi + kypsi; + + av = sqrt(axpsi^2 + zddmg^2); + zddd = fo(3,4); + adv = (axpsi*adxpsi+zddmg*zddd)/av; + zdddd = fo(3,5); + a = (axpsi*addxpsi + adxpsi^2 + (zddmg)*zdddd +zddd)*av; + b = -adv*(axpsi*adxpsi + zddmg*zddd); + addv = (a+b)/av^2; + + phi = atan(aypsi/av); + theta = atan(axpsi/zddmg); + + phid = (adypsi*av-adv*aypsi)/(aypsi^2+av^2); + thetad = (adxpsi*zddmg-zddd*aypsi)/(axpsi^2+zddmg^2); + a = (addypsi*av + adv*(adypsi-aypsi)-addv*aypsi)*(aypsi^2+av^2); b = -2*(aypsi*adypsi+av*adv)*(adypsi*av-adv*aypsi); c = (aypsi^2+av^2)^2; - phidd = (a + b)/c; + phidd = (a+b)/c; a = (addxpsi*zddmg+fo(3,4)*(adxpsi - axpsi) - fo(3,5)*axpsi)*(axpsi^2+zddmg^2); b = -2*(axpsi*adxpsi+zddmg*fo(3,4))*(adxpsi*zddmg-fo(3,4)*axpsi); c = (axpsi^2+zddmg^2)^2; thetadd = (a+b)/c; - psidd = fo(4,3); + p = phid - sin(theta)*psid; + q = cos(phi)*thetad + sin(phi)*cos(theta)*psid; + r = -sin(phi)*thetad + cos(phi)*cos(theta)*psid; + pd = phidd - cos(theta)*thetad*psid - sin(theta)*psidd; + a = -sin(phi)*phid*thetad + cos(phi)*thetadd + cos(phi)*cos(theta)*phid*psid; + b = -sin(phi)*sin(theta)*thetad*psid + sin(phi)*cos(theta)*psidd; + qd = a+b; + a = -cos(phi)*phid*thetad - sin(phi)*thetadd - sin(phi)*cos(theta)*phid*psid; + b = -cos(phi)*sin(theta)*thetad*psid + cos(phi)*cos(theta)*psidd; + rd = a+b; + inp(2) = DF_JXX/DF_L*pd + (DF_JZZ-DF_JYY)*q*r; + inp(3) = DF_JYY/DF_L*qd + (DF_JXX-DF_JZZ)*p*r; + inp(4) = DF_JZZ/DF_CM_OV_CT*rd + (DF_JYY-DF_JXX)*p*q; endfunction \ No newline at end of file diff --git a/sw/simulator/scilab/q6d/q6d_display.sci b/sw/simulator/scilab/q6d/q6d_display.sci index 28d31e2b6b..73c6172112 100644 --- a/sw/simulator/scilab/q6d/q6d_display.sci +++ b/sw/simulator/scilab/q6d/q6d_display.sci @@ -152,7 +152,184 @@ function display_df_ref(time, diff_flat_ref) plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_R,:))); xtitle('R'); - +endfunction + + +function display_df_cmd(time, diff_flat_cmd) + + f=get("current_figure"); + f.figure_name="Command"; + + subplot(2,4,1); + plot2d(time, diff_flat_cmd(1,:)); + xtitle('Ut'); + + subplot(2,4,2); + plot2d(time, diff_flat_cmd(2,:)); + xtitle('Up'); + + subplot(2,4,3); + plot2d(time, diff_flat_cmd(3,:)); + xtitle('Uq'); + + subplot(2,4,4); + plot2d(time, diff_flat_cmd(4,:)); + xtitle('Ur'); + +endfunction + + +function display_motor_cmd(time, motor_cmd) + + subplot(2,4,5); + plot2d(time, motor_cmd(1,:)); + xtitle('F1'); + + subplot(2,4,6); + plot2d(time, motor_cmd(2,:)); + xtitle('F2'); + + subplot(2,4,7); + plot2d(time, motor_cmd(3,:)); + xtitle('F3'); + + subplot(2,4,8); + plot2d(time, motor_cmd(4,:)); + xtitle('F4'); + +endfunction + + +function display_fdm(time, state, euler) + + f=get("current_figure"); + f.figure_name="FDM"; + + subplot(4,3,1); + plot2d(time, state(FDM_SX,:)); + xtitle('X'); + + subplot(4,3,2); + plot2d(time, state(FDM_SY,:)); + xtitle('Y'); + + subplot(4,3,3); + plot2d(time, state(FDM_SZ,:)); + xtitle('Z'); + + + subplot(4,3,4); + plot2d(time, state(FDM_SXD,:)); + xtitle('Xd'); + + subplot(4,3,5); + plot2d(time, state(FDM_SYD,:)); + xtitle('Yd'); + + subplot(4,3,6); + plot2d(time, state(FDM_SZD,:)); + xtitle('Zd'); + + + subplot(4,3,7); + plot2d(time, deg_of_rad(euler(FDM_EPHI,:))); + xtitle('Phi'); + + subplot(4,3,8); + plot2d(time, deg_of_rad(euler(FDM_ETHETA,:))); + xtitle('Theta'); + + subplot(4,3,9); + plot2d(time, deg_of_rad(euler(FDM_EPSI,:))); + xtitle('Psi'); + + + subplot(4,3,10); + plot2d(time, deg_of_rad(state(FDM_SP,:))); + xtitle('p'); + + subplot(4,3,11); + plot2d(time, deg_of_rad(state(FDM_SQ,:))); + xtitle('q'); + + subplot(4,3,12); + plot2d(time, deg_of_rad(state(FDM_SR,:))); + xtitle('r'); + + +endfunction + + +function display_control(time, fdm_state, fdm_euler, diff_flat_ref) + + f=get("current_figure"); + f.figure_name="Control"; + + subplot(4,3,1); + plot2d(time, fdm_state(FDM_SX,:), 2); + plot2d(time, diff_flat_ref(DF_REF_X,:),3); + xtitle('X'); + legends(["fdm", "ref"],[2 3], with_box=%f, opt="ul"); + + subplot(4,3,2); + plot2d(time, fdm_state(FDM_SY,:), 2); + plot2d(time, diff_flat_ref(DF_REF_Y,:),3); + xtitle('Y'); + + subplot(4,3,3); + plot2d(time, fdm_state(FDM_SZ,:), 2); + plot2d(time, diff_flat_ref(DF_REF_Z,:),3); + xtitle('Z'); + + + subplot(4,3,4); + plot2d(time, fdm_state(FDM_SXD,:), 2); + plot2d(time, diff_flat_ref(DF_REF_XD,:),3); + xtitle('Xd'); + + subplot(4,3,5); + plot2d(time, fdm_state(FDM_SYD,:), 2); + plot2d(time, diff_flat_ref(DF_REF_YD,:),3); + xtitle('Yd'); + + subplot(4,3,6); + plot2d(time, fdm_state(FDM_SZD,:), 2); + plot2d(time, diff_flat_ref(DF_REF_ZD,:),3); + xtitle('Zd'); + + + subplot(4,3,7); + plot2d(time, deg_of_rad(fdm_euler(FDM_EPHI,:)), 2); + plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_PHI,:)), 3); + xtitle('Phi'); + + subplot(4,3,8); + plot2d(time, deg_of_rad(fdm_euler(FDM_ETHETA,:)), 2); + plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_THETA,:)), 3); + xtitle('Theta'); + + subplot(4,3,9); + plot2d(time, deg_of_rad(fdm_euler(FDM_EPSI,:)), 2); + plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_PSI,:)), 3); + xtitle('Psi'); + + + subplot(4,3,10); + plot2d(time, deg_of_rad(fdm_state(FDM_SP,:)), 2); + plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_P,:)), 3); + xtitle('p'); + + subplot(4,3,11); + plot2d(time, deg_of_rad(fdm_state(FDM_SQ,:)), 2); + plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_Q,:)), 3); + xtitle('q'); + + subplot(4,3,12); + plot2d(time, deg_of_rad(fdm_state(FDM_SR,:)), 2); + plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_R,:)), 3); + xtitle('r'); + + endfunction diff --git a/sw/simulator/scilab/q6d/q6d_fdm.sci b/sw/simulator/scilab/q6d/q6d_fdm.sci index 806310f40c..1478f571b1 100644 --- a/sw/simulator/scilab/q6d/q6d_fdm.sci +++ b/sw/simulator/scilab/q6d/q6d_fdm.sci @@ -30,6 +30,23 @@ FDM_MOTOR_BACK = 3; FDM_MOTOR_LEFT = 4; FDM_MOTOR_NB = 4; + +// +// By Products +// +FDM_EPHI = 1; // euler angles +FDM_ETHETA = 2; +FDM_EPSI = 3; + +FDM_AXDD = 1; // linear accelerations +FDM_AYDD = 2; +FDM_AZDD = 3; + +FDM_RAPD = 1; // rotational accelerations +FDM_RAQD = 2; +FDM_RARD = 3; + + fdm_g = 9.81; // gravitational acceleration fdm_mass = 0.5; // mass in kg fdm_inertia = [0.0078 0. 0. // inertia tensor @@ -55,9 +72,9 @@ global fdm_accel; global fdm_raccel; -function fdm_init(t_start, t_stop) +function fdm_init(time) global fdm_time; - fdm_time = t_start:fdm_dt:t_stop; + fdm_time = time; global fdm_state; fdm_state = zeros(FDM_SSIZE, length(fdm_time)); fdm_state(FDM_SQI, 1) = 1.; diff --git a/sw/simulator/scilab/q6d/q6d_sbb.sci b/sw/simulator/scilab/q6d/q6d_sbb.sci index 7d752ca0a1..bb686fd540 100644 --- a/sw/simulator/scilab/q6d/q6d_sbb.sci +++ b/sw/simulator/scilab/q6d/q6d_sbb.sci @@ -10,6 +10,7 @@ function [step_dt, step_ampl, traj_dt] = compute_step(dist, dyn, max_accel, max_ if (dist < 0.01) step_dt = 0; step_ampl = 0; + traj_dt = 1; else omega = dyn(1); xsi = dyn(2); diff --git a/sw/simulator/scilab/q6d/test_stop_stop.sce b/sw/simulator/scilab/q6d/test_stop_stop.sce index 405e315293..4385aa3826 100644 --- a/sw/simulator/scilab/q6d/test_stop_stop.sce +++ b/sw/simulator/scilab/q6d/test_stop_stop.sce @@ -2,6 +2,7 @@ clear(); exec('q6d_sbb.sci'); exec('q6d_diff_flatness.sci'); +exec('q6d_fdm.sci'); exec('q6d_algebra.sci'); exec('q6d_display.sci'); @@ -14,8 +15,10 @@ 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(30)) 0.5*9.81]; -b0 = [ 0 0 0]; -b1 = [-10 1 -2]; +//b0 = [ 0 0 0]; +//b1 = [-10 1 -2]; +b0 = [ 0 0 0]; +b1 = [ 0 5 0]; [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1); @@ -27,10 +30,38 @@ display_fo_traj(time, fo_traj); diff_flat_cmd = zeros(4,length(time)); diff_flat_ref = zeros(DF_REF_SIZE, length(time)); for i=1:length(time) -// diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i)); + diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i)); diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i)); end -set("current_figure",2); +set("current_figure",1); clf(); display_df_ref(time, diff_flat_ref); + +set("current_figure",2); +clf(); +display_df_cmd(time, diff_flat_cmd) + + +motor_of_cmd = [ 0.25 0. 0.5 -0.25 + 0.25 -0.5 0. 0.25 + 0.25 0. -0.5 -0.25 + 0.25 0.5 0. 0.25 ]; + +motor_cmd = zeros(4,length(time)); + +for i=1:length(time) + motor_cmd(:,i) = 1/fdm_Ct0 * motor_of_cmd * diff_flat_cmd(:,i); +end + +display_motor_cmd(time, motor_cmd); + +fdm_init(time); +for i=2:length(time) + fdm_run(i, motor_cmd(:,i-1)); +end + +set("current_figure",3); +clf(); +//display_fdm(time, fdm_state, fdm_euler) +display_control(time, fdm_state, fdm_euler, diff_flat_ref); \ No newline at end of file