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

proper fdm initialisation

Browse Source
This commit is contained in:
Antoine Drouin
2010-02-24 14:35:03 +00:00
parent e24830156f
commit a334a1b92a
3 changed files with 21 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q6d/q6d_fdm.sci
+6 -5
View File
@@ -54,9 +54,9 @@ fdm_inertia = [0.0078 0. 0. // inertia tensor
0. 0. 0.0137 ];
fdm_Ct0 = 4. * fdm_mass * fdm_g / 4; // thrust coefficient
//fdm_V0 = 7.; //
fdm_V0 = 1000.; //
//fdm_Cd = 0.01; // drag coefficient
fdm_Cd = 0.000001; // drag coefficient
fdm_V0 = 1e6; //
//fdm_Cd = 0.01; // drag coefficient
fdm_Cd = 1e-6; // drag coefficient
fdm_la = 0.25; // arm length
fdm_Cm = fdm_Ct0 / 10; // torque coefficient
@@ -74,14 +74,15 @@ global fdm_accel;
global fdm_raccel;
function fdm_init(time)
function fdm_init(time, X0)
global fdm_time;
fdm_time = time;
global fdm_state;
fdm_state = zeros(FDM_SSIZE, length(fdm_time));
fdm_state(FDM_SQI, 1) = 1.;
fdm_state(:,1) = X0;
global fdm_euler;
fdm_euler = zeros(AXIS_NB, length(fdm_time));
fdm_euler(:,1) = euler_of_quat(fdm_state(FDM_SQI:FDM_SQZ,1));
global fdm_accel;
fdm_accel = zeros(AXIS_NB, length(fdm_time));
global fdm_raccel;
sw/simulator/scilab/q6d/q6d_sbb.sci
+7
View File
@@ -39,7 +39,13 @@ 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));
// psi
for i=1:length(time)
fo_traj(4,1,i) = rad_of_deg(30);
end
// x and y
disp_xy = b1(1:2) - b0(1:2);
[pulse_dt, pulse_ampl, traj_dt] = compute_step(norm(disp_xy), dyn(1,:), max_accel(1), max_speed(1));
fo_traj(1:2,1,1) = b0(1:2);
@@ -61,6 +67,7 @@ function [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1)
fo_traj(1:2,5,i) = -2*dyn(1,2)*dyn(1,1)*fo_traj(1:2,4,i)-dyn(1,1)^2*(fo_traj(1:2,3,i)-sp*u);
end
// z
disp_z = b1(3) - b0(3);
[pulse_dt, pulse_ampl, traj_dt] = compute_step(norm(disp_z), dyn(2,:), max_accel(2), max_speed(2));
fo_traj(3,1,1) = b0(3);
sw/simulator/scilab/q6d/test_stop_stop.sce
+8 -2
View File
@@ -18,7 +18,7 @@ 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 = [ 1 0 0 ];
b1 = [ 1 0 -1 ];
[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
@@ -56,7 +56,13 @@ end
display_motor_cmd(time, motor_cmd);
fdm_init(time);
X0 = [diff_flat_ref(DF_REF_X,1) ; diff_flat_ref(DF_REF_Y,1) ; diff_flat_ref(DF_REF_Z,1)
diff_flat_ref(DF_REF_XD,1) ; diff_flat_ref(DF_REF_YD,1); diff_flat_ref(DF_REF_ZD,1)
quat_of_euler([diff_flat_ref(DF_REF_PHI,1) diff_flat_ref(DF_REF_THETA,1) diff_flat_ref(DF_REF_PSI,1)])
diff_flat_ref(DF_REF_P,1) ; diff_flat_ref(DF_REF_Q,1) ; diff_flat_ref(DF_REF_R,1)
];
fdm_init(time, X0);
for i=2:length(time)
fdm_run(i, motor_cmd(:,i-1));
end