fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 14:51:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Bottle drop: Fix signs and comments

Browse Source
This commit is contained in:
Lorenz Meier
2014-08-22 17:22:35 +02:00
parent 3f37efc046
commit d604985e08
1 changed files with 9 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/bottle_drop/bottle_drop.cpp
+9 -39
View File
@@ -462,7 +462,7 @@ BottleDrop::task_main()
hrt_abstime last_run = hrt_absolute_time(); hrt_abstime last_run = hrt_absolute_time();
float dt_runs = 1e6f / sleeptime_us; 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) { while (_drop_state > DROP_STATE_INIT) {
// Get wind estimate // Get wind estimate
@@ -474,17 +474,17 @@ BottleDrop::task_main()
// Get vehicle position // Get vehicle position
orb_check(vehicle_global_position_sub, &updated); orb_check(vehicle_global_position_sub, &updated);
if (updated) { if (updated) {
/* copy global position */ // copy global position
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
} }
// Get parameter updates // Get parameter updates
orb_check(parameter_update_sub, &updated); orb_check(parameter_update_sub, &updated);
if (updated) { if (updated) {
/* copy global position */ // copy global position
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); 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_gproperties, &z_0);
param_get(param_turn_radius, &turn_radius); param_get(param_turn_radius, &turn_radius);
param_get(param_precision, &precision); param_get(param_precision, &precision);
@@ -500,20 +500,10 @@ BottleDrop::task_main()
mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); 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) { switch (_drop_state) {
case DROP_STATE_TARGET_VALID: 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 // Update drop point at 10 Hz
if (counter % 10 == 0) { if (counter % 10 == 0) {
@@ -547,18 +537,18 @@ BottleDrop::task_main()
x = x + vx * dt_freefall_prediction; x = x + vx * dt_freefall_prediction;
vrx = vx + vw; vrx = vx + vw;
//Drag force // drag force
v = sqrtf(vz * vz + vrx * vrx); v = sqrtf(vz * vz + vrx * vrx);
Fd = 0.5f * rho * A * cd * (v * v); Fd = 0.5f * rho * A * cd * (v * v);
Fdx = Fd * vrx / v; Fdx = Fd * vrx / v;
Fdz = Fd * vz / v; Fdz = Fd * vz / v;
//acceleration // acceleration
az = g - Fdz / m; az = g - Fdz / m;
ax = -Fdx / m; ax = -Fdx / m;
} }
// Compute drop vector // compute drop vector
x = groundspeed_body * t_signal + x; x = groundspeed_body * t_signal + x;
} }
@@ -580,9 +570,6 @@ BottleDrop::task_main()
y_drop = y_t + x * wind_direction_e; y_drop = y_t + x * wind_direction_e;
map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon);
_drop_position.alt = _target_position.alt + _alt_clearance; _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 // Compute flight vector
map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, 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()
map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, 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.lat, &flight_vector_e.lon);
flight_vector_e.altitude = _drop_position.alt; 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 // Save WPs in datamanager
// 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 */
const ssize_t len = sizeof(struct mission_item_s); 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) { 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()
x_f = x_l + _global_pos.vel_n * dt_runs; x_f = x_l + _global_pos.vel_n * dt_runs;
y_f = y_l + _global_pos.vel_e * 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); 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); warnx("Distance real: %.2f", (double)distance_real);