fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 16:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Robustify bottle drop

Browse Source
This commit is contained in:
Lorenz Meier
2014-08-21 15:14:55 +02:00
parent e5f5f5e2cc
commit 2a99ff39dc
1 changed files with 11 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/bottle_drop/bottle_drop.cpp
+11 -7
View File
@@ -459,7 +459,7 @@ BottleDrop::task_main()
continue; continue;
} }
const unsigned sleeptime_us = 10000; const unsigned sleeptime_us = 50000;
hrt_abstime last_run = hrt_absolute_time(); hrt_abstime last_run = hrt_absolute_time();
dt_runs = 1e6f / sleeptime_us; dt_runs = 1e6f / sleeptime_us;
@@ -648,6 +648,7 @@ BottleDrop::task_main()
if (isfinite(distance_real) && distance_real < distance_open_door) { 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;
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
} }
} }
break; break;
@@ -664,10 +665,11 @@ BottleDrop::task_main()
warnx("Distance real: %.2f", (double)distance_real); warnx("Distance real: %.2f", (double)distance_real);
if (isfinite(distance_real) && if (isfinite(distance_real) &&
(((distance_real < precision) && (distance_real < future_distance)) || (distance_real < precision) && ((distance_real < future_distance) ||
(distance_real < precision / 10.0f))) { (distance_real < precision / 10.0f))) {
drop(); drop();
_drop_state = DROP_STATE_DROPPED; _drop_state = DROP_STATE_DROPPED;
mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
} }
} }
} }
@@ -680,6 +682,7 @@ BottleDrop::task_main()
_drop_approval = false; _drop_approval = false;
lock_release(); lock_release();
close_bay(); close_bay();
mavlink_log_info(_mavlink_fd, "#audio: closing bay");
} }
break; break;
} }
@@ -688,7 +691,7 @@ BottleDrop::task_main()
// update_actuators(); // update_actuators();
// run at roughly 100 Hz // run at roughly 20 Hz
usleep(sleeptime_us); usleep(sleeptime_us);
dt_runs = 1e6f / hrt_elapsed_time(&last_run); dt_runs = 1e6f / hrt_elapsed_time(&last_run);
@@ -719,12 +722,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
} else if (cmd->param1 > 0.5f) { } else if (cmd->param1 > 0.5f) {
open_bay(); open_bay();
mavlink_log_info(_mavlink_fd, "#audio: open doors"); mavlink_log_info(_mavlink_fd, "#audio: opening bay");
} else { } else {
lock_release(); lock_release();
close_bay(); close_bay();
mavlink_log_info(_mavlink_fd, "#audio: close doors"); mavlink_log_info(_mavlink_fd, "#audio: closing bay");
} }
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
@@ -735,11 +738,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
switch ((int)(cmd->param1 + 0.5f)) { switch ((int)(cmd->param1 + 0.5f)) {
case 0: case 0:
_drop_approval = false; _drop_approval = false;
mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval");
break; break;
case 1: case 1:
_drop_approval = true; _drop_approval = true;
mavlink_log_info(_mavlink_fd, "#audio: prepare deploy approval"); mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval");
break; break;
default: default:
@@ -768,7 +772,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
case 1: case 1:
_drop_approval = true; _drop_approval = true;
mavlink_log_info(_mavlink_fd, "#audio: control deploy approval"); mavlink_log_info(_mavlink_fd, "#audio: got drop approval");
break; break;
default: default: