Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2009-03-19 17:34:43 +00:00
parent 260f16e463
commit 256666cdc2
3 changed files with 57 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q3d/q3d_ref_misc.sci
+36 -6
View File
@@ -28,6 +28,37 @@ function [time, Xref] = get_reference_circle()
endfunction
function [time, Xref] = get_reference_looping(t0, duration, center, radius)
dt = 1/512;
time = t0:dt:t0+duration;
delta_dot = %pi/duration;
delta = delta_dot*time;
X0 = radius * sin(2*delta) + center(AXIS_X);
Z0 = -radius * (cos(2*delta)-1) + center(AXIS_Z);
X1 = +2*radius*delta_dot.*cos(2*delta);
Z1 = +2*radius*delta_dot.*sin(2*delta);
X2 = -4*radius*(delta_dot^2).*sin(2*delta);
Z2 = +4*radius*(delta_dot^2).*cos(2*delta);
X3 = -8*radius*(delta_dot^3).*cos(2*delta);
Z3 = -8*radius*(delta_dot^3).*sin(2*delta);
X4 = +16*radius*(delta_dot^4).*sin(2*delta);
Z4 = -16*radius*(delta_dot^4).*cos(2*delta);
Xref = [X0;Z0;X1;Z1;X2;Z2;X3;Z3;X4;Z4];
endfunction
//http://www.tsplines.com/resources/class_notes/Bezier_curves.pdf
function [time, Xref] = get_reference_poly(duration, a, b)
@@ -106,7 +137,6 @@ function [time, Xref] = get_reference_poly2(duration, a, b)
p0_5 = 1/6*(p1_5 - p0_6);
p1_4 = p0_4 + p0_5;
// p2_4 = ;
// p2_3 = ;
@@ -128,11 +158,11 @@ function [time, Xref] = get_reference_poly2(duration, a, b)
Xref = zeros(10, length(time));
for i=1:length(time)
t = time(i);
Xref(1:2,i) = p0_0*(1-t)^9 + p0_1*t*(1-t)^8 + p0_2*t^2*(1-t)^7 + p0_3*t^3*(1-t)^6 + p0_4*t^4*(1-t)^5 + p0_5*t^5*(1-t)^4 + p0_6*t^6*(1-t)^3 + p0_7*t^7*(1-t)^2 + p0_8*t^8*(1-t)^1 + p0_9*t^9;
Xref(3:4,i) = p1_0*(1-t)^8 + p1_1*t*(1-t)^7 + p1_2*t^2*(1-t)^6 + p1_3*t^3*(1-t)^5 + p1_4*t^4*(1-t)^4 + p1_5*t^5*(1-t)^3 + p1_6*t^6*(1-t)^2 + p1_7*t^7*(1-t)^1 + p1_8*t^8;
Xref(5:6,i) = p2_0*(1-t)^7 + p2_1*t*(1-t)^6 + p2_2*t^2*(1-t)^5 + p2_3*t^3*(1-t)^4 + p2_4*t^4*(1-t)^3 + p2_5*t^5*(1-t)^2 + p2_6*t^6*(1-t)^1 + p2_7*t^7;
Xref(7:8,i) = p3_0*(1-t)^6 + p3_1*t*(1-t)^5 + p3_2*t^2*(1-t)^4 + p3_3*t^3*(1-t)^3 + p3_4*t^4*(1-t)^2 + p3_5*t^5*(1-t)^1 + p3_6*t^6;
Xref(9:10,i) = p4_0*(1-t)^5 + p4_1*t*(1-t)^4 + p4_2*t^2*(1-t)^3 + p4_3*t^3*(1-t)^2 + p4_4*t^4*(1-t)^1 + p4_5*t^5;
// Xref(1:2,i) = p0_0*(1-t)^9 + p0_1*t*(1-t)^8 + p0_2*t^2*(1-t)^7 + p0_3*t^3*(1-t)^6 + p0_4*t^4*(1-t)^5 + p0_5*t^5*(1-t)^4 + p0_6*t^6*(1-t)^3 + p0_7*t^7*(1-t)^2 + p0_8*t^8*(1-t)^1 + p0_9*t^9;
// Xref(3:4,i) = p1_0*(1-t)^8 + p1_1*t*(1-t)^7 + p1_2*t^2*(1-t)^6 + p1_3*t^3*(1-t)^5 + p1_4*t^4*(1-t)^4 + p1_5*t^5*(1-t)^3 + p1_6*t^6*(1-t)^2 + p1_7*t^7*(1-t)^1 + p1_8*t^8;
// Xref(5:6,i) = p2_0*(1-t)^7 + p2_1*t*(1-t)^6 + p2_2*t^2*(1-t)^5 + p2_3*t^3*(1-t)^4 + p2_4*t^4*(1-t)^3 + p2_5*t^5*(1-t)^2 + p2_6*t^6*(1-t)^1 + p2_7*t^7;
// Xref(7:8,i) = p3_0*(1-t)^6 + p3_1*t*(1-t)^5 + p3_2*t^2*(1-t)^4 + p3_3*t^3*(1-t)^3 + p3_4*t^4*(1-t)^2 + p3_5*t^5*(1-t)^1 + p3_6*t^6*;
// Xref(9:10,i) = p4_0*(1-t)^5 + p4_1*t*(1-t)^4 + p4_2*t^2*(1-t)^3 + p4_3*t^3*(1-t)^2 + p4_4*t^4*(1-t)^1 + p4_5*t^5;
end
endfunction
sw/simulator/scilab/q3d/test_6.sce
+20 -12
View File
@@ -10,26 +10,34 @@ exec('poly_utils.sci');
global fdm_state
[time_loop, ref_loop] = get_reference_circle();
a = [-1 0 0 0 0;
0 0 0 0 0];
0 0 0 0 0];
b = [0 1.2566371 0 -1.9844017 0;
0 0 1.5791367 0 -2.4936727];
[time_foo, ref_foo] = get_reference_poly3(1,a ,a);
start_loop = [ref_loop(1,1) ref_loop(3,1) ref_loop(5,1) ref_loop(7,1) ref_loop(9,1)
ref_loop(2,1) ref_loop(4,1) ref_loop(6,1) ref_loop(8,1) ref_loop(10,1)];
[time1, Xref1] = get_reference_poly3(5, a, b);
[time2, Xref2] = get_reference_circle();
[time_intro, ref_intro] = get_reference_poly3(0.9, a, start_loop);
c = [Xref2(1,$),Xref2(3,$),Xref2(5,$),Xref2(7,$),Xref2(9,$);
Xref2(2,$),Xref2(4,$),Xref2(6,$),Xref2(8,$),Xref2(10,$)];
a(1,1) = 3;
a(2,1) = 1;
end_loop = [ref_loop(1,$),ref_loop(3,$),ref_loop(5,$),ref_loop(7,$),ref_loop(9,$);
ref_loop(2,$),ref_loop(4,$),ref_loop(6,$),ref_loop(8,$),ref_loop(10,$)];
[time3, Xref3] = get_reference_poly3(4,c,a);
c = [ 1 0 0 0 0;
0 0 0 0 0];
Xref = [Xref1 Xref2(:,2:$) Xref3(:,2:$)];
[time_outro, ref_outro] = get_reference_poly3(0.9,end_loop,c);
[time_bar, ref_bar] = get_reference_poly3(1,c ,c);
fdm_init(0,time1($)+time2($)+time3($));
Xref = [ref_foo ref_intro(:,2:$) ref_loop(:,2:$) ref_outro(:,2:$) ref_bar(:,2:$)];
fdm_init(0,time_foo($)+time_intro($)+time_loop($)+time_outro($)+time_bar($));
fdm_state(FDM_SX,1) = -1;
ctl_init();
sw/simulator/scilab/q3d/test_7.sce
+1 -1
View File
@@ -17,7 +17,7 @@ b = [ 0 ; 0
0 ; 1.5791367
-1.9844017; 0
0 ; -2.4936727 ];
[time, Xref] = get_reference_poly(1, a, b);
[time, Xref] = get_reference_looping(0, 4, [0;0], 2);
clf();