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

Fix bottle drop logic

Browse Source
This commit is contained in:
Lorenz Meier
2014-08-18 16:47:30 +02:00
parent 789f949e74
commit 47b3f1253e
1 changed files with 18 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/bottle_drop/bottle_drop.cpp
+18 -25
View File
@@ -126,10 +126,9 @@ private:
float _alt_clearance; float _alt_clearance;
struct position_s { struct position_s {
double lat; //degrees double lat; ///< degrees
double lon; //degrees double lon; ///< degrees
float alt; //m float alt; ///< m
bool initialized;
} _target_position, _drop_position; } _target_position, _drop_position;
enum DROP_STATE { enum DROP_STATE {
@@ -646,7 +645,7 @@ BottleDrop::task_main()
(double)distance_open_door, (double)distance_open_door,
(double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); (double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east))));
if (isfinite(distance_real) && distance_real < distance_open_door && _drop_approval) { if (isfinite(distance_real) && distance_real < distance_open_door) {
open_bay(); open_bay();
_drop_state = DROP_STATE_BAY_OPEN; _drop_state = DROP_STATE_BAY_OPEN;
} }
@@ -655,27 +654,21 @@ BottleDrop::task_main()
case DROP_STATE_BAY_OPEN: case DROP_STATE_BAY_OPEN:
{ {
map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); if (_drop_approval) {
x_f = x_l + _global_pos.vel_n * dt_runs; map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l);
y_f = y_l + _global_pos.vel_e * dt_runs; x_f = x_l + _global_pos.vel_n * dt_runs;
map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); y_f = y_l + _global_pos.vel_e * dt_runs;
future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); 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));
warnx("Distance real: %.2f", (double)distance_real); warnx("Distance real: %.2f", (double)distance_real);
if (isfinite(distance_real) && (((distance_real < precision) && (distance_real < future_distance)) || if (isfinite(distance_real) &&
/* we drop as well if we're really close */ (distance_real < precision / 10.0f))) { // Drop only if the distance between drop point and actual position is getting larger again (((distance_real < precision) && (distance_real < future_distance)) ||
// XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal (distance_real < precision / 10.0f))) {
drop();
// if(fabsf(_wrap_pi(_global_pos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop _drop_state = DROP_STATE_DROPPED;
// { }
drop();
_drop_state = DROP_STATE_DROPPED;
// }
// else
// {
// state_run = true;
// }
} }
} }
break; break;
@@ -684,6 +677,7 @@ BottleDrop::task_main()
/* 2s after drop, reset and close everything again */ /* 2s after drop, reset and close everything again */
if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) {
_drop_state = DROP_STATE_INIT; _drop_state = DROP_STATE_INIT;
_drop_approval = false;
lock_release(); lock_release();
close_bay(); close_bay();
} }
@@ -759,7 +753,6 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
_target_position.lat = cmd->param5; _target_position.lat = cmd->param5;
_target_position.lon = cmd->param6; _target_position.lon = cmd->param6;
_target_position.alt = cmd->param7; _target_position.alt = cmd->param7;
_target_position.initialized;
_drop_state = DROP_STATE_TARGET_VALID; _drop_state = DROP_STATE_TARGET_VALID;
mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat,
(double)_target_position.lon, (double)_target_position.alt); (double)_target_position.lon, (double)_target_position.alt);