Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 17:49:49 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Browse Source
This commit is contained in:
Antoine Drouin
2010-03-03 20:53:39 +00:00
parent dc97c7cc48
commit f18dcce148
2 changed files with 25 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q3d/q3d_fdm.sci
+14 -9
View File
@@ -43,9 +43,9 @@ fdm_max_thrust = 1.00; // 100%
fdm_wind = [0 0]';
global fdm_time;
global fdm_state;
global fdm_accel;
global fdm_state; // state
global fdm_accel; // aceleration (used for sensors)
global fdm_perturb; // perturbation
function fdm_init(time, fdm_X0, cmd0)
@@ -56,24 +56,27 @@ function fdm_init(time, fdm_X0, cmd0)
fdm_state(:,1) = fdm_X0;
global fdm_accel;
fdm_accel = zeros(FDM_ASIZE, length(fdm_time));
accel = fdm_get_derivatives(time(1), fdm_state(:,1), cmd0);
accel = fdm_get_derivatives(time(1), fdm_state(:,1), cmd0, [0;0;0]);
fdm_accel(:,1) = accel(FDM_SXD:FDM_STHETAD);
global fdm_perturb;
fdm_perturb = zeros(FDM_ASIZE, length(fdm_time));
endfunction
// propagate dynamic model from step i-1 to step i
function fdm_run(i, cmd)
cmd = trim_vect(cmd, fdm_min_thrust, fdm_max_thrust);
global fdm_state;
global fdm_time;
fdm_state(:,i) = ode(fdm_state(:,i-1), fdm_time(i-1), fdm_time(i), list(fdm_get_derivatives, cmd));
// global fdm_time;
fdm_state(:,i) = ode(fdm_state(:,i-1), fdm_time(i-1), fdm_time(i), list(fdm_get_derivatives, cmd, fdm_perturb(:,i-1)));
global fdm_accel;
accel = fdm_get_derivatives(fdm_time(i), fdm_state(:,i), cmd);
accel = fdm_get_derivatives(fdm_time(i), fdm_state(:,i), cmd, fdm_perturb(:,i));
fdm_accel(:,i) = accel(FDM_SXD:FDM_STHETAD);
endfunction
function [Xdot] = fdm_get_derivatives(t, X, U)
function [Xdot] = fdm_get_derivatives(t, X, U, perturb)
Xdot = zeros(length(X),1);
@@ -98,5 +101,7 @@ function [Xdot] = fdm_get_derivatives(t, X, U)
// moments
Xdot(FDM_STHETAD) = fdm_la * fdm_Ct0 / fdm_inertia*(U(FDM_MOTOR_LEFT) - U(FDM_MOTOR_RIGHT));
// add perturbation
Xdot(FDM_SXD:FDM_STHETAD) = Xdot(FDM_SXD:FDM_STHETAD)+perturb;
endfunction
sw/simulator/scilab/q3d/test_adapt.sce
+11 -1
View File
@@ -1,4 +1,5 @@
clear();
clearglobal();
exec('q3d_utils.sci');
@@ -23,7 +24,7 @@ time2 = t1:dt:t2;
// trajectory generation
if 1
dyn = [rad_of_deg(500) 0.7; rad_of_deg(500) 0.7];
dyn = [rad_of_deg(400) 0.7; rad_of_deg(400) 0.7];
max_speed = [ 5 2.5];
max_accel = [ 9.81*tan(rad_of_deg(30)) 0.5*9.81];
b0 = [-5 0];
@@ -40,6 +41,15 @@ end
fdm_init(time, df_state_of_fo(fo_traj(:,:,1)), [0.25 0.25]');
//pause
global fdm_perturb;
fdm_perturb(:,2562) = [15;15;15];
fdm_perturb(:,2563) = [15;15;15];
fdm_perturb(:,2564) = [15;15;15];
fdm_perturb(:,2565) = [15;15;15];
fdm_perturb(:,2566) = [15;15;15];
fdm_perturb(:,2567) = [15;15;15];
ctl_init(time);
diff_flat_cmd = zeros(2,length(time));