dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Remove useless modulo throttling, which is a nice source of potential non-trivial non-determinism / timing issues.

This commit is contained in:
Lorenz Meier 2014-09-11 01:04:02 +02:00
parent bc880a3ff9
commit 1512bf727c
1 changed files with 36 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
src/modules/bottle_drop/bottle_drop.cpp
View File

@ -517,53 +517,49 @@ BottleDrop::task_main()
case DROP_STATE_TARGET_VALID:
{
// Update drop point at 10 Hz
if (counter % 10 == 0) {
az = g; // acceleration in z direction[m/s^2]
vz = 0; // velocity in z direction [m/s]
z = 0; // fallen distance [m]
h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m]
h = h_0; // height over target [m]
ax = 0; // acceleration in x direction [m/s^2]
vx = groundspeed_body;// XXX project // ground speed in x direction [m/s]
x = 0; // traveled distance in x direction [m]
vw = 0; // wind speed [m/s]
vrx = 0; // relative velocity in x direction [m/s]
v = groundspeed_body; // relative speed vector [m/s]
Fd = 0; // Drag force [N]
Fdx = 0; // Drag force in x direction [N]
Fdz = 0; // Drag force in z direction [N]
az = g; // acceleration in z direction[m/s^2]
vz = 0; // velocity in z direction [m/s]
z = 0; // fallen distance [m]
h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m]
h = h_0; // height over target [m]
ax = 0; // acceleration in x direction [m/s^2]
vx = groundspeed_body;// XXX project // ground speed in x direction [m/s]
x = 0; // traveled distance in x direction [m]
vw = 0; // wind speed [m/s]
vrx = 0; // relative velocity in x direction [m/s]
v = groundspeed_body; // relative speed vector [m/s]
Fd = 0; // Drag force [N]
Fdx = 0; // Drag force in x direction [N]
Fdz = 0; // Drag force in z direction [N]
// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
while (h > 0.05f) {
// z-direction
vz = vz + az * dt_freefall_prediction;
z = z + vz * dt_freefall_prediction;
h = h_0 - z;
// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
while (h > 0.05f) {
// z-direction
vz = vz + az * dt_freefall_prediction;
z = z + vz * dt_freefall_prediction;
h = h_0 - z;
// x-direction
vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
vx = vx + ax * dt_freefall_prediction;
x = x + vx * dt_freefall_prediction;
vrx = vx + vw;
// x-direction
vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
vx = vx + ax * dt_freefall_prediction;
x = x + vx * dt_freefall_prediction;
vrx = vx + vw;
// drag force
v = sqrtf(vz * vz + vrx * vrx);
Fd = 0.5f * rho * A * cd * (v * v);
Fdx = Fd * vrx / v;
Fdz = Fd * vz / v;
// drag force
v = sqrtf(vz * vz + vrx * vrx);
Fd = 0.5f * rho * A * cd * (v * v);
Fdx = Fd * vrx / v;
Fdz = Fd * vz / v;
// acceleration
az = g - Fdz / m;
ax = -Fdx / m;
}
// compute drop vector
x = groundspeed_body * t_signal + x;
// acceleration
az = g - Fdz / m;
ax = -Fdx / m;
}
// compute drop vector
x = groundspeed_body * t_signal + x;
x_t = 0.0f;
y_t = 0.0f;