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 27 Packages Projects Releases Wiki Activity

Browse Source
This commit is contained in:
Antoine Drouin
2010-02-24 12:22:05 +00:00
parent 141c92e40e
commit 6b81bd7b9b
1 changed files with 13 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/scilab/q6d/q6d_sbb.sci
+13 -13
View File
@@ -6,29 +6,29 @@
SBB_TOLERANCE = 0.025;
function [step_dt, step_ampl, traj_dt] = compute_step(dist, dyn, max_accel, max_speed)
function [pulse_dt, pulse_ampl, traj_dt] = compute_step(dist, dyn, max_accel, max_speed)
if (dist < 0.01)
step_dt = 0;
step_ampl = 0;
pulse_dt = 0;
pulse_ampl = 0;
traj_dt = 1;
else
omega = dyn(1);
xsi = dyn(2);
step_dt = -log(SBB_TOLERANCE)/(omega*xsi*sqrt(1-xsi^2));
step_ampl = dist / (2*step_dt^2);
if step_ampl < max_accel
if step_ampl > max_speed/step_dt
step_ampl = max_speed/step_dt;
pulse_dt = -log(SBB_TOLERANCE)/(omega*xsi*sqrt(1-xsi^2));
pulse_ampl = dist / (2*pulse_dt^2);
if pulse_ampl < max_accel
if pulse_ampl > max_speed/pulse_dt
pulse_ampl = max_speed/pulse_dt;
end
else
step_ampl = max_accel;
if step_ampl <= max_speed/step_dt
step_dt = max_speed/step_ampl;
pulse_ampl = max_accel;
if pulse_ampl <= max_speed/pulse_dt
pulse_dt = max_speed/pulse_ampl;
else
step_ampl = max_speed/step_dt;
pulse_ampl = max_speed/pulse_dt;
end
end
traj_dt = (dist - 2*step_dt*(step_ampl*step_dt))/step_ampl*step_dt + 4*step_dt;
traj_dt = (dist - 2*pulse_dt*(pulse_ampl*pulse_dt))/pulse_ampl*pulse_dt + 4*pulse_dt;
end
endfunction