Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-25 23:46:04 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

playing around

Browse Source
This commit is contained in:
Antoine Drouin
2010-03-15 15:43:16 +00:00
parent 5c2b8f9450
commit 11326f436c
10 changed files with 174 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q6d/povray/Makefile
+3 -1
View File
@@ -1,3 +1,5 @@
test:
povray test.pov +Oimg001.png Display=false +W800 +H600 +Q9 +A0.3 +R5
clean:
rm -f *.png
rm -f *~ img*.png test.avi q3d.pov
sw/simulator/scilab/q6d/q6d_algebra.sci
+7
View File
@@ -128,6 +128,13 @@ function [euler] = euler_of_quat(quat)
endfunction
//
//
//
function [qo] = quat_normalize(qi)
qo = qi / norm(qi);
endfunction
//
//
//
sw/simulator/scilab/q6d/q6d_ctl.sci
+46
View File
@@ -0,0 +1,46 @@
CTL_UT = 1;
CTL_UP = 2;
CTL_UQ = 3;
CTL_UR = 4;
CTL_USIZE = 4;
global ctl_diff_flat_cmd;
global ctl_diff_flat_ref;
global ctl_fb_cmd;
global ctl_u;
global ctl_motor_cmd;
function ctl_init(time)
global ctl_diff_flat_cmd;
ctl_diff_flat_cmd = zeros(CTL_USIZE,length(time));
global ctl_diff_flat_ref;
ctl_diff_flat_ref = zeros(DF_REF_SIZE, length(time));
global ctl_fb_cmd;
ctl_fb_cmd = zeros(CTL_USIZE,length(time));
global ctl_u;
ctl_u = zeros(CTL_USIZE, length(time));
global ctl_motor_cmd;
ctl_motor_cmd = zeros(CTL_USIZE,length(time));
endfunction
function ctl_run(i, fo_tra)
global ctl_diff_flat_cmd;
ctl_diff_flat_cmd(:,i) = df_input_of_fo(fo_tra);
global ctl_diff_flat_ref;
ctl_diff_flat_ref(:,i) = df_ref_of_fo(fo_tra);
global ctl_u;
ctl_u(:,i) = ctl_diff_flat_cmd(:,i);
motor_of_cmd = [ 0.25 0. 0.5 -0.25
0.25 -0.5 0. 0.25
0.25 0. -0.5 -0.25
0.25 0.5 0. 0.25 ];
global ctl_motor_cmd;
ctl_motor_cmd(:,i) = 1/fdm_Ct0 * motor_of_cmd * ctl_u(:,i);
endfunction
sw/simulator/scilab/q6d/q6d_diff_flatness.sci
+19 -19
View File
@@ -28,20 +28,20 @@ DF_L = 0.25;
DF_CM_OV_CT = 0.1;
// state from flat output
function [state] = df_state_of_fo(fo)
function [ref] = df_ref_of_fo(fo)
state = zeros(DF_REF_SIZE, 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);
ref = zeros(DF_REF_SIZE, 1);
ref(DF_REF_X) = fo(DF_FO_X,1);
ref(DF_REF_Y) = fo(DF_FO_Y,1);
ref(DF_REF_Z) = fo(DF_FO_Z,1);
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);
ref(DF_REF_XD) = fo(DF_FO_X,2);
ref(DF_REF_YD) = fo(DF_FO_Y,2);
ref(DF_REF_ZD) = fo(DF_FO_Z,2);
state(DF_REF_PSI) = fo(DF_FO_PSI,1);
ref(DF_REF_PSI) = fo(DF_FO_PSI,1);
psi = state(DF_REF_PSI);
psi = ref(DF_REF_PSI);
cpsi = cos(psi);
spsi = sin(psi);
@@ -54,8 +54,8 @@ function [state] = df_state_of_fo(fo)
z2dmg = z2d - DF_G;
av = sqrt(axpsi^2 + z2dmg^2);
state(DF_REF_PHI) = sign(z2dmg)*atan(aypsi/av);
state(DF_REF_THETA) = atan(axpsi/z2dmg);
ref(DF_REF_PHI) = sign(z2dmg)*atan(aypsi/av);
ref(DF_REF_THETA) = atan(axpsi/z2dmg);
x3d = fo(1,4);
y3d = fo(2,4);
@@ -80,14 +80,14 @@ function [state] = df_state_of_fo(fo)
phid = sign(z2dmg)*(adypsi*av-adv*aypsi)/(aypsi^2+av^2);
thetad = (adxpsi*z2dmg-z3d*axpsi)/(axpsi^2+z2dmg^2);
cphi = cos(state(DF_REF_PHI));
sphi = sin(state(DF_REF_PHI));
ctheta = cos(state(DF_REF_THETA));
stheta = sin(state(DF_REF_THETA));
cphi = cos(ref(DF_REF_PHI));
sphi = sin(ref(DF_REF_PHI));
ctheta = cos(ref(DF_REF_THETA));
stheta = sin(ref(DF_REF_THETA));
state(DF_REF_P) = phid - stheta*psid;
state(DF_REF_Q) = cphi*thetad + sphi*ctheta*psid;
state(DF_REF_R) = -sphi*thetad + cphi*ctheta*psid;
ref(DF_REF_P) = phid - stheta*psid;
ref(DF_REF_Q) = cphi*thetad + sphi*ctheta*psid;
ref(DF_REF_R) = -sphi*thetad + cphi*ctheta*psid;
endfunction
sw/simulator/scilab/q6d/q6d_display.sci
+44
View File
@@ -333,3 +333,47 @@ function display_control(time, fdm_state, fdm_euler, diff_flat_ref)
endfunction
function display_sensors(time)
f=get("current_figure");
f.figure_name="Sensors";
subplot(3,3,1);
plot2d(time, deg_of_rad(sensor_gyro(1,:)), 2);
xtitle('gyro p');
subplot(3,3,2);
plot2d(time, deg_of_rad(sensor_gyro(2,:)), 2);
xtitle('gyro q');
subplot(3,3,3);
plot2d(time, deg_of_rad(sensor_gyro(3,:)), 2);
xtitle('gyro r');
subplot(3,3,4);
plot2d(time, sensor_accel(1,:), 2);
xtitle('accel x');
subplot(3,3,5);
plot2d(time, sensor_accel(2,:), 2);
xtitle('accel y');
subplot(3,3,6);
plot2d(time, sensor_accel(3,:), 2);
xtitle('accel z');
subplot(3,3,7);
plot2d(time, sensor_mag(1,:), 2);
xtitle('mag x');
subplot(3,3,8);
plot2d(time, sensor_mag(2,:), 2);
xtitle('mag y');
subplot(3,3,9);
plot2d(time, sensor_mag(3,:), 2);
xtitle('mag z');
endfunction
sw/simulator/scilab/q6d/q6d_fdm.sci
+1
View File
@@ -75,6 +75,7 @@ global fdm_raccel;
function fdm_init(time, X0)
global fdm_time;
fdm_time = time;
global fdm_state;
sw/simulator/scilab/q6d/q6d_io.sci
+20
View File
@@ -0,0 +1,20 @@
function io_dump_fdm_sensor_dat(time, filename)
fid = mopen(filename+'.txt', "w");
for i=1:length(time)
mfprintf(fid, "%f [%.16f %.16f %.16f %.16f] [%.16f %.16f %.16f] [%.16f %.16f %.16f] [%.16f %.16f %.16f] [%.16f %.16f %.16f]\n", ...
time(i),...
fdm_state(FDM_SQI,i), fdm_state(FDM_SQX,i), fdm_state(FDM_SQY,i), fdm_state(FDM_SQZ,i),...
fdm_state(FDM_SP,i), fdm_state(FDM_SQ,i), fdm_state(FDM_SR,i),...
sensor_gyro(1,i), sensor_gyro(2,i), sensor_gyro(3,i),...
sensor_accel(1,i), sensor_accel(2,i), sensor_accel(3,i),...
sensor_mag(1,i), sensor_mag(2,i), sensor_mag(3,i) );
end
mclose(fid);
save(filename+'.dat',time, fdm_state, sensor_gyro, sensor_accel, sensor_mag);
endfunction
sw/simulator/scilab/q6d/q6d_sbb.sci
+3 -2
View File
@@ -41,13 +41,14 @@ function [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1)
fo_traj = zeros(DF_FO_SIZE, DF_FO_ORDER, length(time));
// psi
omega = rad_of_deg(45);
if 1
omega = rad_of_deg(45);
for i=1:length(time)
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
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));
sw/simulator/scilab/q6d/q6d_sensors.sci
+26 -11
View File
@@ -2,29 +2,36 @@
// Sensors model
//
sensor_noise_gyro = rad_of_deg(3);
sensor_bias_gyro = [rad_of_deg(2.5) rad_of_deg(-2.5) rad_of_deg(-1.25)]';
//sensor_bias_gyro = [rad_of_deg(0.5) rad_of_deg(-0.5) rad_of_deg(0.25)]';
//sensor_bias_gyro = [rad_of_deg(0) rad_of_deg(0) rad_of_deg(0)]';
sensor_noise_accel = 1.;
sensor_noise_mag = 0.5;
global sensor_gyro;
global sensor_accel;
global sensor_mag;
global sensor_baro;
global sensor_gps_pos;
global sensor_gps_speed;
function sensors_init()
function sensors_init(time)
global fdm_time;
global sensor_accel;
sensor_accel = zeros(AXIS_NB, length(fdm_time));
global sensor_gyro;
sensor_gyro = zeros(AXIS_NB, length(fdm_time));
sensor_gyro = sensor_noise_gyro * rand(AXIS_NB, length(time),'normal');
global sensor_accel;
sensor_accel = sensor_noise_accel * rand(AXIS_NB, length(time),'normal');
global sensor_mag;
sensor_mag = sensor_noise_mag * rand(AXIS_NB, length(time),'normal');
global sensor_baro;
sensor_baro = zeros(1, length(fdm_time));
sensor_baro = zeros(1, length(time));
global sensor_gps_pos;
sensor_gps_pos = zeros(AXIS_NB, length(fdm_time));
sensor_gps_pos = zeros(AXIS_NB, length(time));
global sensor_gps_speed;
sensor_gps_speed = zeros(AXIS_NB, length(fdm_time));
sensor_gps_speed = zeros(AXIS_NB, length(time));
endfunction
@@ -36,14 +43,22 @@ function sensors_run(i)
global fdm_accel;
global sensor_gyro;
global sensor_accel;
global sensor_mag;
global sensor_baro;
global sensor_gps_pos;
global sensor_gps_speed;
sensor_gyro(:,i) = fdm_state(FDM_SP:FDM_SR, i);
sensor_gyro(:,i) = sensor_gyro(:,i) + fdm_state(FDM_SP:FDM_SR, i) + sensor_bias_gyro;
sensor_accel(:,i) = quat_vect_mult(fdm_state(FDM_SQI:FDM_SQZ,i), (fdm_accel(:,i)-[0; 0; 9.81]));
accel_earth = fdm_accel(:,i)-[0; 0; 9.81];
accel_body = quat_vect_mult(fdm_state(FDM_SQI:FDM_SQZ,i), accel_earth);
sensor_accel(:,i) = sensor_accel(:,i) + accel_body;
// mag_earth = [1 0 1]';
mag_earth = [0.4912 0.1225 0.8624]';
mag_body = quat_vect_mult(fdm_state(FDM_SQI:FDM_SQZ,i), mag_earth);
sensor_mag(:,i) = sensor_mag(:,i) + mag_body;
sensor_gps_pos(:,i) = fdm_state(FDM_SX:FDM_SZ, i);
sensor_gps_speed(:,i) = fdm_state(FDM_SXD:FDM_SZD, i);
sw/simulator/scilab/q6d/test_stop_stop.sce
+5 -5
View File
@@ -21,8 +21,8 @@ b0 = [ -5 0 0];
b1 = [ 5 0 0];
//b0 = [ 0 -5 0 ];
//b1 = [ 0 5 0 ];
//[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
[fo_traj] = fo_traj_circle(time);
[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
//[fo_traj] = fo_traj_circle(time);
set("current_figure",0);
clf();
@@ -33,14 +33,14 @@ diff_flat_cmd = zeros(4,length(time));
diff_flat_ref = zeros(DF_REF_SIZE, length(time));
for i=1:length(time)
diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i));
diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
diff_flat_ref(:,i) = df_ref_of_fo(fo_traj(:,:,i));
end
set("current_figure",1);
clf();
display_df_ref(time, diff_flat_ref);
povray_draw( time, diff_flat_ref );
//povray_draw( time, diff_flat_ref );
set("current_figure",2);
clf();
@@ -76,4 +76,4 @@ clf();
//display_fdm(time, fdm_state, fdm_euler)
display_control(time, fdm_state, fdm_euler, diff_flat_ref);
povray_draw(time,diff_flat_ref);
//povray_draw(time,diff_flat_ref);