Browse Source

Bottle drop: Fix signs and comments

sbg
Lorenz Meier 11 years ago
parent
commit
d604985e08
  1. 48
      src/modules/bottle_drop/bottle_drop.cpp

48
src/modules/bottle_drop/bottle_drop.cpp

@ -462,7 +462,7 @@ BottleDrop::task_main() @@ -462,7 +462,7 @@ BottleDrop::task_main()
hrt_abstime last_run = hrt_absolute_time();
float dt_runs = 1e6f / sleeptime_us;
/* switch to faster updates during the drop */
// switch to faster updates during the drop
while (_drop_state > DROP_STATE_INIT) {
// Get wind estimate
@ -474,17 +474,17 @@ BottleDrop::task_main() @@ -474,17 +474,17 @@ BottleDrop::task_main()
// Get vehicle position
orb_check(vehicle_global_position_sub, &updated);
if (updated) {
/* copy global position */
// copy global position
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
}
// Get parameter updates
orb_check(parameter_update_sub, &updated);
if (updated) {
/* copy global position */
// copy global position
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
/* update all parameters */
// update all parameters
param_get(param_gproperties, &z_0);
param_get(param_turn_radius, &turn_radius);
param_get(param_precision, &precision);
@ -500,20 +500,10 @@ BottleDrop::task_main() @@ -500,20 +500,10 @@ BottleDrop::task_main()
mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real);
}
//warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING
//warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING
switch (_drop_state) {
case DROP_STATE_TARGET_VALID:
{
//warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING
// drop here
//open_now = true;
//drop = false;
//drop_start = hrt_absolute_time();
// Update drop point at 10 Hz
if (counter % 10 == 0) {
@ -547,18 +537,18 @@ BottleDrop::task_main() @@ -547,18 +537,18 @@ BottleDrop::task_main()
x = x + vx * dt_freefall_prediction;
vrx = vx + vw;
//Drag force
// 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
// acceleration
az = g - Fdz / m;
ax = -Fdx / m;
}
// Compute drop vector
// compute drop vector
x = groundspeed_body * t_signal + x;
}
@ -580,9 +570,6 @@ BottleDrop::task_main() @@ -580,9 +570,6 @@ BottleDrop::task_main()
y_drop = y_t + x * wind_direction_e;
map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon);
_drop_position.alt = _target_position.alt + _alt_clearance;
//warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING
// Compute flight vector
map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e,
@ -591,25 +578,8 @@ BottleDrop::task_main() @@ -591,25 +578,8 @@ BottleDrop::task_main()
map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e,
&flight_vector_e.lat, &flight_vector_e.lon);
flight_vector_e.altitude = _drop_position.alt;
//warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING
// Drop Cancellation if terms are not met
// warnx("latitude:%.2f", _global_pos.lat);
// warnx("longitude:%.2f", _global_pos.lon);
// warnx("drop_position.lat:%.2f", drop_position.lat);
// warnx("drop_position.lon:%.2f", drop_position.lon);
//warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING
// if (counter % 10 ==0) {
// warnx("x: %.4f", x);
// warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon);
// warnx("latitude %.4f, longitude: %.4f", _global_pos.lat, _global_pos.lon);
// warnx("future_distance: %.2f, precision: %.2f", future_distance, precision);
// }
/* Save WPs in datamanager */
// Save WPs in datamanager
const ssize_t len = sizeof(struct mission_item_s);
if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) {
@ -662,7 +632,7 @@ BottleDrop::task_main() @@ -662,7 +632,7 @@ BottleDrop::task_main()
x_f = x_l + _global_pos.vel_n * dt_runs;
y_f = y_l + _global_pos.vel_e * dt_runs;
map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED);
future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon));
future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon);
warnx("Distance real: %.2f", (double)distance_real);

Loading…
Cancel
Save