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 25 Packages Projects Releases Wiki Activity

Browse Source
This commit is contained in:
Antoine Drouin
2010-02-24 16:05:23 +00:00
parent c1c0ab5cae
commit 7137154467
4 changed files with 53 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q6d/q6d_diff_flatness.sci
+13 -7
View File
@@ -1,3 +1,9 @@
DF_FO_X = 1;
DF_FO_Y = 2;
DF_FO_Z = 3;
DF_FO_PSI = 4;
DF_FO_SIZE = 4;
DF_FO_ORDER = 5;
DF_REF_X = 1;
DF_REF_Y = 2;
@@ -25,15 +31,15 @@ DF_CM_OV_CT = 0.1;
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_X) = fo(DF_FO_X,1);
state(DF_REF_Y) = fo(DF_FO_Y,1);
state(DF_REF_Z) = fo(DF_FO_Z,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_XD) = fo(DF_FO_X,2);
state(DF_REF_YD) = fo(DF_FO_Y,2);
state(DF_REF_ZD) = fo(DF_FO_Z,2);
state(DF_REF_PSI) = fo(4,1);
state(DF_REF_PSI) = fo(DF_FO_PSI,1);
psi = state(DF_REF_PSI);
cpsi = cos(psi);
sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci
+31
View File
@@ -0,0 +1,31 @@
function [fo_traj] = fo_traj_circle(time)
fo_traj = zeros(DF_FO_SIZE, DF_FO_ORDER, length(time));
radius = 3;
omega = rad_of_deg(60);
c = [0 0]';
for i=1:length(time)
fo_traj(DF_FO_X,1,i) = c(1) + radius*cos(omega*time(i));
fo_traj(DF_FO_Y,1,i) = c(2) + radius*sin(omega*time(i));
fo_traj(DF_FO_X,2,i) = c(1) - omega*radius*sin(omega*time(i));
fo_traj(DF_FO_Y,2,i) = c(2) + omega*radius*cos(omega*time(i));
fo_traj(DF_FO_X,3,i) = c(1) - omega^2*radius*cos(omega*time(i));
fo_traj(DF_FO_Y,3,i) = c(2) - omega^2*radius*sin(omega*time(i));
fo_traj(DF_FO_X,4,i) = c(1) + omega^3*radius*sin(omega*time(i));
fo_traj(DF_FO_Y,4,i) = c(2) - omega^3*radius*cos(omega*time(i));
fo_traj(DF_FO_X,5,i) = c(1) + omega^4*radius*cos(omega*time(i));
fo_traj(DF_FO_Y,5,i) = c(2) + omega^4*radius*sin(omega*time(i));
end
endfunction
sw/simulator/scilab/q6d/q6d_sbb.sci
+6 -6
View File
@@ -4,6 +4,8 @@
//
//
SBB_TOLERANCE = 0.025;
function [pulse_dt, pulse_ampl, traj_dt] = compute_step(dist, dyn, max_accel, max_speed)
@@ -36,16 +38,14 @@ endfunction
function [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1)
n_comp = 4; // x, y, z, psi
order = 5;
fo_traj = zeros(n_comp, order, length(time));
fo_traj = zeros(DF_FO_SIZE, DF_FO_ORDER, length(time));
// psi
omega = rad_of_deg(45);
for i=1:length(time)
fo_traj(4,1,i) = sin(omega*time(i));
fo_traj(4,2,i) = omega*cos(omega*time(i));
fo_traj(4,3,i) = -omega^2*sin(omega*time(i));
fo_traj(DF_FO_PSI, 1, i) = sin(omega*time(i));
fo_traj(DF_FO_PSI, 2, i) = omega* cos(omega*time(i));
fo_traj(DF_FO_PSI, 3, i) = -omega^2*sin(omega*time(i));
end
// x and y
sw/simulator/scilab/q6d/test_stop_stop.sce
+3 -2
View File
@@ -1,6 +1,7 @@
clear();
exec('q6d_sbb.sci');
exec('q6d_fo_traj_misc.sci');
exec('q6d_diff_flatness.sci');
exec('q6d_fdm.sci');
exec('q6d_algebra.sci');
@@ -19,8 +20,8 @@ max_accel = [ 9.81*tan(rad_of_deg(30)) 0.5*9.81];
//b1 = [-10 1 -2];
b0 = [ 0 0 0 ];
b1 = [ 1 -1 -1 ];
[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
//[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
[fo_traj] = fo_traj_circle(time);
set("current_figure",0);
clf();