mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
This commit is contained in:
Antoine Drouin
@@ -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
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -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.;
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
Reference in New Issue
Block a user