Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Browse Source
This commit is contained in:
Antoine Drouin
2010-02-23 17:37:29 +00:00
parent b46d62c4c1
commit 0e169b6a3f
2 changed files with 253 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q6d/q6d_diff_flatness.sci
+95
View File
@@ -0,0 +1,95 @@
DF_REF_X = 1;
DF_REF_Y = 2;
DF_REF_Z = 3;
DF_REF_PHI = 4;
DF_REF_THETA = 5;
DF_REF_PSI = 6;
DF_REF_XD = 7;
DF_REF_YD = 8;
DF_REF_ZD = 9;
DF_REF_P = 10;
DF_REF_Q = 11;
DF_REF_R = 12;
DF_REF_SIZE = 12;
DF_G = 9.81;
DF_MASS = 0.5;
// state from flat output
function [state] = df_state_of_fo(fo)
state = zeros(DF_REF_SIZE, 1);
state(DF_REF_X) = fo(1,1);
state(DF_REF_Y) = fo(2,1);
state(DF_REF_Z) = fo(3,1);
state(DF_REF_XD) = fo(1,2);
state(DF_REF_YD) = fo(2,2);
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);
state(DF_REF_PHI) = atan(aypsi/av);
state(DF_REF_THETA) = atan(axpsi/zddmg);
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);
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);
psid = fo(4,2);
adxpsi = -psid*aypsi + jxpsi;
adypsi = psid*axpsi + jypsi;
adv = (axpsi*adxpsi + zddmg*fo(3,4))/av;
phid = (adypsi*av-adv*aypsi)/(aypsi^2+av^2);
thetad = (adxpsi*zddmg-fo(3,4)*aypsi)/(axpsi^2+zddmg^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;
endfunction
// control input from flat output
function [inp] = df_input_of_fo(fo)
inp = zeros(4,1);
xdd = fo(1,3);
ydd = fo(2,3);
zddmg = fo(3,3) - DF_G;
inp(1) = DF_MASS * sqrt(xdd^2+ydd^2+zddmg^2);
psi = fo(4,1);
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);
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;
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);
endfunction
sw/simulator/scilab/q6d/q6d_display.sci
+158
View File
@@ -0,0 +1,158 @@
function display_fo_traj(time, fo_traj)
f=get("current_figure");
f.figure_name="Flat Outputs Trajectory";
subplot(5,4,1);
plot2d(time, matrix(fo_traj(1,1,:), 1, length(time)));
xtitle('X(0)');
subplot(5,4,2);
plot2d(time, matrix(fo_traj(2,1,:), 1, length(time)));
xtitle('Y(0)');
subplot(5,4,3);
plot2d(time, matrix(fo_traj(3,1,:), 1, length(time)));
xtitle('Z(0)');
subplot(5,4,4);
plot2d(time, deg_of_rad(matrix(fo_traj(4,1,:), 1, length(time))));
xtitle('PSI(0)');
subplot(5,4,5);
plot2d(time, matrix(fo_traj(1,2,:), 1, length(time)));
xtitle('X(1)');
subplot(5,4,6);
plot2d(time, matrix(fo_traj(2,2,:), 1, length(time)));
xtitle('Y(1)');
subplot(5,4,7);
plot2d(time, matrix(fo_traj(3,2,:), 1, length(time)));
xtitle('Z(1)');
subplot(5,4,8);
plot2d(time, deg_of_rad(matrix(fo_traj(4,2,:), 1, length(time))));
xtitle('PSI(1)');
subplot(5,4,9);
plot2d(time, matrix(fo_traj(1,3,:), 1, length(time)));
xtitle('X(2)');
subplot(5,4,10);
plot2d(time, matrix(fo_traj(2,3,:), 1, length(time)));
xtitle('Y(2)');
subplot(5,4,11);
plot2d(time, matrix(fo_traj(3,3,:), 1, length(time)));
xtitle('Z(2)');
subplot(5,4,12);
plot2d(time, deg_of_rad(matrix(fo_traj(4,3,:), 1, length(time))));
xtitle('PSI(2)');
subplot(5,4,13);
plot2d(time, matrix(fo_traj(1,4,:), 1, length(time)));
xtitle('X(3)');
subplot(5,4,14);
plot2d(time, matrix(fo_traj(2,4,:), 1, length(time)));
xtitle('Y(3)');
subplot(5,4,15);
plot2d(time, matrix(fo_traj(3,4,:), 1, length(time)));
xtitle('Z(3)');
// subplot(5,4,16);
// plot2d(time, deg_of_rad(matrix(fo_traj(4,4,:), 1, length(time))));
// xtitle('PSI(3)');
subplot(5,4,17);
plot2d(time, matrix(fo_traj(1,5,:), 1, length(time)));
xtitle('X(4)');
subplot(5,4,18);
plot2d(time, matrix(fo_traj(2,5,:), 1, length(time)));
xtitle('Y(4)');
subplot(5,4,19);
plot2d(time, matrix(fo_traj(3,5,:), 1, length(time)));
xtitle('Z(4)');
// subplot(5,4,20);
// plot2d(time, deg_of_rad(matrix(fo_traj(4,5,:), 1, length(time))));
// xtitle('PSI(4)');
endfunction
function display_df_ref(time, diff_flat_ref)
f=get("current_figure");
f.figure_name="Reference";
subplot(6,2,1);
plot2d(time, diff_flat_ref(DF_REF_X,:));
xtitle('X');
subplot(6,2,3);
plot2d(time, diff_flat_ref(DF_REF_Y,:));
xtitle('Y');
subplot(6,2,5);
plot2d(time, diff_flat_ref(DF_REF_Z,:));
xtitle('Z');
subplot(6,2,7);
plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_PHI,:)));
xtitle('PHI');
subplot(6,2,9);
plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_THETA,:)));
xtitle('THETA');
subplot(6,2,11);
plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_PSI,:)));
xtitle('PSI');
subplot(6,2,2);
plot2d(time, diff_flat_ref(DF_REF_XD,:));
xtitle('XD');
subplot(6,2,4);
plot2d(time, diff_flat_ref(DF_REF_YD,:));
xtitle('YD');
subplot(6,2,6);
plot2d(time, diff_flat_ref(DF_REF_ZD,:));
xtitle('ZD');
subplot(6,2,8);
plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_P,:)));
xtitle('P');
subplot(6,2,10);
plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_Q,:)));
xtitle('Q');
subplot(6,2,12);
plot2d(time, deg_of_rad(diff_flat_ref(DF_REF_R,:)));
xtitle('R');
endfunction