[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4601]
|
From: |
antoine drouin |
|
Subject: |
[paparazzi-commits] [4601] |
|
Date: |
Mon, 01 Mar 2010 17:06:47 +0000 |
Revision: 4601
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4601
Author: poine
Date: 2010-03-01 17:06:47 +0000 (Mon, 01 Mar 2010)
Log Message:
-----------
Modified Paths:
--------------
paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci
paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_diff_flatness.sci
paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_display.sci
paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci
paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce
Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci 2010-02-28
23:07:09 UTC (rev 4600)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_ctl.sci 2010-03-01
17:06:47 UTC (rev 4601)
@@ -1,274 +1,23 @@
+// X Z Theta Xd Zd Theta_d
+//ctl_gain = [ 0 -1 0 0 -1 0
+// -2 0 5 -2 0 5 ];
-
-global ctl_sp_pos;
-global ctl_ref_0;
-global ctl_ref_1;
-global ctl_ref_2;
-global ctl_ref_3;
-global ctl_ref_4;
-
-global ctl_ref_theta;
-global ctl_ref_thetad;
-
-CMD_SF = 1;
-CMD_DF = 2;
-
-global ctl_cmd;
-global ctl_motor;
-
-
-
-
-function ctl_init()
-
- global fdm_time;
-
- global ctl_sp_pos;
- ctl_sp_pos = zeros(AXIS_NB, length(fdm_time));
- global ctl_ref_0;
- ctl_ref_0 = zeros(AXIS_NB, length(fdm_time));
- global ctl_ref_1;
- ctl_ref_1 = zeros(AXIS_NB, length(fdm_time));
- global ctl_ref_2;
- ctl_ref_2 = zeros(AXIS_NB, length(fdm_time));
- global ctl_ref_3;
- ctl_ref_3 = zeros(AXIS_NB, length(fdm_time));
- global ctl_ref_4;
- ctl_ref_4 = zeros(AXIS_NB, length(fdm_time));
-
- global ctl_ref_theta;
- ctl_ref_theta = zeros(1, length(fdm_time));
- global ctl_ref_thetad;
- ctl_ref_thetad = zeros(1, length(fdm_time));
-
-
- global ctl_cmd;
- ctl_cmd = zeros(FDM_MOTOR_NB, length(fdm_time));
- global ctl_motor;
- ctl_motor = zeros(FDM_MOTOR_NB, length(fdm_time));
-
-endfunction
-
-
-
-ctl_mass = 0.25;
-ctl_inertia = 0.0078;
-
-function ctl_run(i, sp)
-
- global ctl_sp_pos;
- ctl_sp_pos(:,i) = sp;
-
- ctl_update_ref_4th_order(i);
-
- ctl_run_flatness(i);
+if 0
+ctl_gain = [ 0 0 0 0 0 0
+ 0 0 0 0 0 0 ];
+else
+ctl_gain = [ 0 -1 0 0 -1 0
+ -0.95 0 3 -1.2 0 3 ];
-endfunction
+end
+function [fb_cmd] = ctl_compute_feeback(fdm_state, ref)
+ state_err = fdm_state - ref;
+ fb_cmd = ctl_gain * state_err;
-
-function ctl_run_flatness(i)
-
-
- global ctl_ref_theta;
- ctl_ref_theta(i) = -atan(ctl_ref_2(AXIS_X,i), 9.81 + ctl_ref_2(AXIS_Z,i));
- global ctl_ref_thetad;
- ctl_ref_thetad(i) = -((9.81 + ctl_ref_2(AXIS_Z,i))*ctl_ref_3(AXIS_X,i) -
ctl_ref_2(AXIS_X,i)*ctl_ref_3(AXIS_Z,i)) / ...
- ((9.81 + ctl_ref_2(AXIS_Z,i))^2+ctl_ref_2(AXIS_X,i)^2);
-
- global ctl_cmd;
- global ctl_ref_2;
- ctl_cmd(CMD_SF,i) = ctl_mass * sqrt(ctl_ref_2(AXIS_X,i)^2 +...
- (ctl_ref_2(AXIS_Z,i)+9.81)^2);
-
- x2 = ctl_ref_2(AXIS_X,i);
- z2p1 = ctl_ref_2(AXIS_Z,i)+9.81;
- x3 = ctl_ref_3(AXIS_X,i);
- z3 = ctl_ref_3(AXIS_Z,i);
- x4 = ctl_ref_4(AXIS_X,i);
- z4 = ctl_ref_4(AXIS_Z,i);
- a = x4*z2p1 - z4*x2;
- b = z2p1^2+x2^2;
- c = 2 * (z2p1*z3 + x2*x3);
- d = x3*z2p1-z3*x2;
- ctl_cmd(CMD_DF,i) = -ctl_inertia * ( a/b - c*d/b^2);
-
- global ctl_motor;
- A2M = 0.5 * [ 1 1
- 1 -1 ];
- ctl_motor(:,i) = A2M * ctl_cmd(:,i);
-
endfunction
-function [state] = ctl_state_of_flat_out(ref)
- state = zeros(FDM_SSIZE, 1);
- state(FDM_SX) = ref(1);
- state(FDM_SZ) = ref(2);
- state(FDM_SXD) = ref(3);
- state(FDM_SZD) = ref(4);
- theta = -atan(ref(5), 9.81 + ref(6));
- thetad = -((9.81 + ref(6))*ref(7) - ref(5)*ref(8)) / ...
- ((9.81 + ref(6))^2+ref(5)^2);
- state(FDM_STHETA) = theta;
- state(FDM_STHETAD) = thetad;
-endfunction
-
-
-
-
-
-ctl_omega_ref1 = [ rad_of_deg(90); rad_of_deg(90)];
-ctl_zeta_ref1 = [ 0.9; 0.9 ];
-
-ctl_omega_ref2 = [ rad_of_deg(1440); rad_of_deg(1440)];
-ctl_zeta_ref2 = [ 0.9; 0.9 ];
-
-a0 = ctl_omega_ref1^2 .* ctl_omega_ref2^2;
-a1 = 2 * ctl_zeta_ref1 .* ctl_omega_ref1 .* ctl_omega_ref2^2 + ...
- 2 * ctl_zeta_ref2 .* ctl_omega_ref2 .* ctl_omega_ref1^2;
-a2 = ctl_omega_ref1^2 + ...
- 2 * ctl_zeta_ref1 .* ctl_omega_ref1 .* ctl_zeta_ref2 .* ctl_omega_ref2 +
...
- ctl_omega_ref2^2;
-a3 = 2 * ctl_zeta_ref1 .* ctl_omega_ref1 + 2 * ctl_zeta_ref2 .* ctl_omega_ref2;
-//a4 = [1;1];
-
-function ctl_update_ref_4th_order(i)
- global ctl_sp_pos;
- global ctl_ref_0;
- global ctl_ref_1;
- global ctl_ref_2;
- global ctl_ref_3;
- global ctl_ref_4;
-
- dt = 1/512;
- ctl_ref_3(:,i) = ctl_ref_3(:,i-1) + dt * ctl_ref_4(:,i-1);
- ctl_ref_2(:,i) = ctl_ref_2(:,i-1) + dt * ctl_ref_3(:,i-1);
- ctl_ref_1(:,i) = ctl_ref_1(:,i-1) + dt * ctl_ref_2(:,i-1);
- ctl_ref_0(:,i) = ctl_ref_0(:,i-1) + dt * ctl_ref_1(:,i-1);
-
- err_pos = ctl_ref_0(:,i) - ctl_sp_pos(:,i);
- ctl_ref_4(:,i) = -a3 .* ctl_ref_3(:,i) -a2 .* ctl_ref_2(:,i) -a1 .*
ctl_ref_1(:,i) -a0.*err_pos;
-
-
-
-
-endfunction
-
-
-function ctl_update_ref_1st_order()
-
-
-
-endfunction
-
-
-
-function ctl_display()
- nr = 5;
- nc = 3;
- subplot(nr,nc,1);
- plot_with_min_rect(fdm_time, fdm_state(FDM_SX,:),2, -0.5, 0.5);
-// plot2d(fdm_time, fdm_state(FDM_SX,:),2);
- plot2d(fdm_time, ctl_ref_0(AXIS_X,:),3);
- plot2d(fdm_time, ctl_sp_pos(AXIS_X,:),5);
- legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('X(0)');
-
- subplot(nr,nc,2);
- plot_with_min_rect(fdm_time, fdm_state(FDM_SZ,:),2, -0.5, 0.5);
-// plot2d(fdm_time, fdm_state(FDM_SZ,:),2);
- plot2d(fdm_time, ctl_ref_0(AXIS_Z,:),3);
- plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5);
- legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('Z(0)');
-
- subplot(nr,nc,3);
- plot_with_min_rect(fdm_time, deg_of_rad(fdm_state(FDM_STHETA,:)),2, -1., 1.);
-// plot2d(fdm_time, deg_of_rad(fdm_state(FDM_STHETA,:)),2);
- plot2d(fdm_time, deg_of_rad(ctl_ref_theta),3);
-// plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5);
- legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('Theta(0)');
-
- subplot(nr,nc,4);
- plot_with_min_rect(fdm_time, fdm_state(FDM_SXD,:),2, -0.5, 0.5);
-// plot2d(fdm_time, fdm_state(FDM_SXD,:),2);
- plot2d(fdm_time, ctl_ref_1(AXIS_X,:),3);
- legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('X(1)');
-
- subplot(nr,nc,5);
- plot_with_min_rect(fdm_time, fdm_state(FDM_SZD,:),2, -0.5, 0.5);
-// plot2d(fdm_time, fdm_state(FDM_SZD,:),2);
- plot2d(fdm_time, ctl_ref_1(AXIS_Z,:),3);
- legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('Z(1)');
-
- subplot(nr,nc,6);
- plot_with_min_rect(fdm_time, deg_of_rad(fdm_state(FDM_STHETAD,:)),2, -1.,
1.);
- //plot2d(fdm_time, deg_of_rad(fdm_state(FDM_STHETAD,:)),2);
- plot2d(fdm_time, deg_of_rad(ctl_ref_thetad),3);
-// plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5);
- legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('Theta(1)');
-
- subplot(nr,nc,7);
-// plot2d(fdm_time, ins_state(INS_SX,:),2);
- plot2d(fdm_time, ctl_ref_2(AXIS_X,:),3);
- legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('X(2)');
-
- subplot(nr,nc,8);
-// plot2d(fdm_time, ins_state(INS_SX,:),2);
- plot2d(fdm_time, ctl_ref_2(AXIS_Z,:),3);
- legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('Z(2)');
-
- subplot(nr,nc,10);
-// plot2d(fdm_time, ins_state(INS_SX,:),2);
- plot2d(fdm_time, ctl_ref_3(AXIS_X,:),3);
- legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('X(3)');
-
- subplot(nr,nc,11);
-// plot2d(fdm_time, ins_state(INS_SX,:),2);
- plot2d(fdm_time, ctl_ref_3(AXIS_Z,:),3);
- legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('Z(3)');
-
- subplot(nr,nc,13);
-// plot2d(fdm_time, ins_state(INS_SX,:),2);
- plot2d(fdm_time, ctl_ref_4(AXIS_X,:),3);
- legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('X(4)');
-
- subplot(nr,nc,14);
-// plot2d(fdm_time, ins_state(INS_SX,:),2);
- plot2d(fdm_time, ctl_ref_4(AXIS_Z,:),3);
- legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('Z(4)');
-
- subplot(nr,nc,9);
- plot2d(fdm_time, ctl_cmd(CMD_SF,:),3);
-// legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('U_sigma');
-
- subplot(nr,nc,12);
- plot2d(fdm_time, ctl_cmd(CMD_DF,:),3);
-// legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
- xtitle('U_delta');
-
- subplot(nr,nc,15);
- plot2d(fdm_time, ctl_motor(FDM_MOTOR_RIGHT,:),2);
- plot2d(fdm_time, ctl_motor(FDM_MOTOR_LEFT,:),3);
- plot2d(fdm_time, fdm_min_thrust * ones(1,length(fdm_time)),5);
- plot2d(fdm_time, fdm_max_thrust * ones(1,length(fdm_time)),5);
-// legends(["setpoint", "RIGHT", "LEFT"],[5 2 3], with_box=%f, opt="ur");
- xtitle('U_motors');
-
-endfunction
-
Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_diff_flatness.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_diff_flatness.sci
2010-02-28 23:07:09 UTC (rev 4600)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_diff_flatness.sci
2010-03-01 17:06:47 UTC (rev 4601)
@@ -10,7 +10,9 @@
fo_g = 9.81;
fo_mass = 0.25;
-fo_inertia = 0.0078;
+fo_J = 0.0078;
+fo_Ct0 = 4. * fo_mass * fo_g / 2;
+fo_la = 0.25;
global fo_traj;
@@ -32,19 +34,19 @@
function [inp] = df_input_of_fo(fo)
x2 = fo(1,3);
- z2p1 = fo(2,3)+9.81;
+ z2pg = fo(2,3)+9.81;
- u1 = fo_mass * sqrt((x2)^2 + (z2p1)^2);
+ u1 = fo_mass / fo_Ct0 * sqrt((x2)^2 + (z2pg)^2);
x3 = fo(1,4);
z3 = fo(2,4);
x4 = fo(1,5);
z4 = fo(2,5);
- a = x4*z2p1 - z4*x2;
- b = z2p1^2+x2^2;
- c = 2 * (z2p1*z3 + x2*x3);
- d = x3*z2p1-z3*x2;
- u2 = -fo_inertia * ( a/b - c*d/b^2);
+ a = x4*z2pg - z4*x2;
+ b = z2pg^2+x2^2;
+ c = 2 * (z2pg*z3 + x2*x3);
+ d = x3*z2pg-z3*x2;
+ u2 = fo_J / fo_la /fo_Ct0 * ( a/b - c*d/b^2);
inp = [u1; u2];
Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_display.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_display.sci 2010-02-28
23:07:09 UTC (rev 4600)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_display.sci 2010-03-01
17:06:47 UTC (rev 4601)
@@ -127,4 +127,72 @@
plot2d(time, deg_of_rad(diff_flat_ref(FDM_STHETAD, :)));
xtitle('Thetad');
- endfunction
\ No newline at end of file
+endfunction
+
+function display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd,
fb_cmd, motor_cmd );
+
+ f=get("current_figure");
+ f.figure_name="Control";
+
+ clf();
+
+ subplot(4,3,1);
+ plot2d(time, diff_flat_ref(FDM_SX, :), 3);
+ plot2d(time, fdm_state(FDM_SX, :), 2);
+ legends(["fdm", "ref"],[2 3], with_box=%f, opt="ul");
+ xtitle('X');
+
+ subplot(4,3,2);
+ plot2d(time, diff_flat_ref(FDM_SZ, :), 3);
+ plot2d(time, fdm_state(FDM_SZ, :), 2);
+ xtitle('Z');
+
+ subplot(4,3,3);
+ plot2d(time, deg_of_rad(diff_flat_ref(FDM_STHETA, :)), 3);
+ plot2d(time, deg_of_rad(fdm_state(FDM_STHETA, :)), 2);
+ xtitle('Theta');
+
+ subplot(4,3,4);
+ plot2d(time, diff_flat_ref(FDM_SXD, :), 3);
+ plot2d(time, fdm_state(FDM_SXD, :), 2);
+ xtitle('Xd');
+
+ subplot(4,3,5);
+ plot2d(time, diff_flat_ref(FDM_SZD, :), 3);
+ plot2d(time, fdm_state(FDM_SZD, :), 2);
+ xtitle('Zd');
+
+ subplot(4,3,6);
+ plot2d(time, deg_of_rad(diff_flat_ref(FDM_STHETAD, :)), 3);
+ plot2d(time, deg_of_rad(fdm_state(FDM_STHETAD, :)), 2);
+ xtitle('Thetad');
+
+
+ subplot(4,2,5);
+// plot2d(time, diff_flat_cmd(1,:)+ fb_cmd(1,:), 5);
+ xset("color",5);
+ foo = diff_flat_cmd(1,:) + fb_cmd(1,:);
+ xfpoly([time time($:-1:1)], [diff_flat_cmd(1,:) foo($:-1:1)]);
+ xset("color",1);
+ plot2d(time, diff_flat_cmd(1,:), 2);
+ xtitle('u_t');
+
+ subplot(4,2,6);
+ tot_cmd = diff_flat_cmd(2,:) + fb_cmd(2,:);
+ xset("color",5);
+ xfpoly([time time($:-1:1)], [diff_flat_cmd(2,:) tot_cmd($:-1:1)]);
+ xset("color",1);
+ plot2d(time, diff_flat_cmd(2,:), 2);
+ xtitle('u_d');
+
+ subplot(4,2,7);
+ plot2d(time, motor_cmd(1,:), 2);
+ xtitle('u1');
+
+ subplot(4,2,8);
+ plot2d(time, motor_cmd(2,:), 2);
+ xtitle('u2');
+
+
+endfunction
+
Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci 2010-02-28
23:07:09 UTC (rev 4600)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fdm.sci 2010-03-01
17:06:47 UTC (rev 4601)
@@ -31,12 +31,17 @@
fdm_g = 9.81;
fdm_mass = 0.25;
fdm_inertia = 0.0078;
+fdm_la = 0.25; // arm length
-fdm_min_thrust = 0.5 * 0.1 * fdm_mass * fdm_g; // 10% of hovering power for
each motor
-fdm_max_thrust = 0.5 * 4.0 * fdm_mass * fdm_g; // 400% of hovering power for
each motor
+fdm_Ct0 = 4. * fdm_mass * fdm_g / 2; // thrust coefficient
+fdm_V0 = 1e9; //
+fdm_Cd = 1e-2; // drag coefficient
-fdm_dt = 1./512.;
+fdm_min_thrust = 0.05; // 5%
+fdm_max_thrust = 1.0; // 400%
+fdm_wind = [0 0]';
+
global fdm_time;
global fdm_state;
global fdm_accel;
@@ -69,12 +74,29 @@
function [Xdot] = fdm_get_derivatives(t, X, U)
Xdot = zeros(length(X),1);
- Xdot(FDM_SX) = X(FDM_SXD);
- Xdot(FDM_SZ) = X(FDM_SZD);
- Xdot(FDM_STHETA) = X(FDM_STHETAD);
- Xdot(FDM_SXD) = -sum(U)/fdm_mass*sin(X(FDM_STHETA));
- Xdot(FDM_SZD) = 1/fdm_mass*(sum(U)*cos(X(FDM_STHETA))-fdm_mass*fdm_g);
- Xdot(FDM_STHETAD) = 1/fdm_inertia*(U(FDM_MOTOR_RIGHT) - U(FDM_MOTOR_LEFT));
+ Xdot(FDM_SX:FDM_STHETA) = X(FDM_SXD:FDM_STHETAD);
+
+ // forces :
+ gspeed_ltp = X(FDM_SXD:FDM_SZD);
+ airspeed_ltp = gspeed_ltp - fdm_wind;
+
+ stheta = sin(X(FDM_STHETA));
+ ctheta = cos(X(FDM_STHETA));
+
+ DCM = [ctheta stheta ; -stheta ctheta];
+ airspeed_body = DCM * airspeed_ltp;
+
+ lift_body = [0; sum(U) * fdm_Ct0 * ( 1 - abs(1/fdm_V0 *
airspeed_body(AXIS_Z)))];
+ lift_ltp = DCM'*lift_body;
+ weight_ltp = [0; -fdm_g * fdm_mass];
+ drag_ltp = -fdm_Cd * norm(airspeed_ltp) * airspeed_ltp;
+ Xdot(FDM_SXD:FDM_SZD) = 1/fdm_mass*(lift_ltp+weight_ltp+drag_ltp);
+
+ // moments
+ Xdot(FDM_STHETAD) = fdm_la * fdm_Ct0 / fdm_inertia*(U(FDM_MOTOR_RIGHT) -
U(FDM_MOTOR_LEFT));
+
+// pause
+
endfunction
Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce 2010-02-28
23:07:09 UTC (rev 4600)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce 2010-03-01
17:06:47 UTC (rev 4601)
@@ -6,13 +6,14 @@
exec('q3d_fo_traj_misc.sci');
exec('q3d_diff_flatness.sci');
+exec('q3d_ctl.sci');
exec('q3d_fdm.sci');
exec('q3d_display.sci');
exec('q3d_povray.sci');
t0 = 0;
-t1 = 10.;
+t1 = 3.;
dt = 1/512;
time = t0:dt:t1;
@@ -23,18 +24,19 @@
[coefs] = poly_get_coef_from_bound(time, b0, b1);
[fo_traj] = poly_gen_traj(time, coefs);
end
-if (0)
-// differential equation
+if (1)
+ // differential equation
dyn = [rad_of_deg(500) 0.7; rad_of_deg(500) 0.7];
max_speed = [5 2.5];
max_accel = [ 9.81*tan(rad_of_deg(29.983325)) 0.5*9.81];
b0 = [-5 0];
- b1 = [ 5 -2];
+ b1 = [ 5 -5];
[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
printf('xfinal %f, zfinal:%f\n',fo_traj(1,1,$), fo_traj(2,1,$));
end
-if (1)
- [fo_traj] = fo_traj_circle(time, [0 0], 2, rad_of_deg(45));
+if (0)
+ // analytic definition
+ [fo_traj] = fo_traj_circle(time, [0 0], 2, rad_of_deg(45));
end
@@ -45,13 +47,14 @@
diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
end
-fdm_init(time, df_state_of_fo(fo_traj(:,:,1)));
+fdm_init(time, df_state_of_fo(fo_traj(:,:,1)));
+fb_cmd = zeros(2,length(time));
+motor_cmd = zeros(2,length(time));
for i=2:length(time)
- u1 = diff_flat_cmd(1,i-1);
- u2 = diff_flat_cmd(2,i-1);
- m1 = 0.5*(u1+u2);
- m2 = 0.5*(u1-u2);
- fdm_run(i, [m1 m2]')
+ fb_cmd(:,i-1) = ctl_compute_feeback(fdm_state(:,i-1),diff_flat_ref(:,i-1));
+ MotorsOfCmds = 0.5*[1 -1 ; 1 1];
+ motor_cmd(:,i-1) = MotorsOfCmds * ( diff_flat_cmd(:,i-1) + fb_cmd(:,i-1));
+ fdm_run(i, motor_cmd(:,i-1));
end
set("current_figure",0);
@@ -60,23 +63,27 @@
f.figure_name="Flat Outputs Trajectory";
display_fo(time, fo_traj);
-set("current_figure",1);
-clf();
-f=get("current_figure");
-f.figure_name="Commands";
-display_commands(time, diff_flat_cmd);
+if 0
+ set("current_figure",1);
+ clf();
+ f=get("current_figure");
+ f.figure_name="Commands";
+ display_commands(time, diff_flat_cmd);
-set("current_figure",2);
-clf();
-f=get("current_figure");
-f.figure_name="Reference";
-display_fo_ref(time, diff_flat_ref);
+ set("current_figure",2);
+ clf();
+ f=get("current_figure");
+ f.figure_name="Reference";
+ display_fo_ref(time, diff_flat_ref);
-set("current_figure",3);
-clf();
-f=get("current_figure");
-f.figure_name="FDM";
-display_fdm();
+ set("current_figure",3);
+ clf();
+ f=get("current_figure");
+ f.figure_name="FDM";
+ display_fdm();
+end
+set("current_figure",1);
+display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd,
motor_cmd );
-povray_draw(time, diff_flat_ref);
+//povray_draw(time, diff_flat_ref);
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4601],
antoine drouin <=