diff options
Diffstat (limited to 'src/modules/bottle_drop/bottle_drop.c')
-rw-r--r-- | src/modules/bottle_drop/bottle_drop.c | 68 |
1 files changed, 52 insertions, 16 deletions
diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index 69720b439..09bbf4687 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -42,13 +42,16 @@ #include <nuttx/sched.h> #include <unistd.h> #include <stdio.h> +#include <poll.h> #include <systemlib/systemlib.h> #include <systemlib/err.h> #include <uORB/uORB.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_attitude.h> #include <drivers/drv_rgbled.h> +#include <drivers/drv_hrt.h> static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ @@ -101,7 +104,7 @@ int bottle_drop_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn_cmd("daemon", + daemon_task = task_spawn_cmd("bottle_drop", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 4096, @@ -143,38 +146,71 @@ int bottle_drop_thread_main(int argc, char *argv[]) { int rgbleds = open(RGBLED_DEVICE_PATH, 0); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + orb_set_interval(vehicle_attitude_sub, 20); + struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); - orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + + uint64_t drop_start = 0; + bool open_now = false; + while (!thread_should_exit) { // warnx("in while!\n"); // values from -1 to 1 - if (drop) { - // drop here - drop = false; + int ret = poll(fds, 1, 500); - ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_ON); - ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_RED); - } + if (ret < 0) { + /* poll error, count it in perf */ + warnx("poll error"); + + } else if (ret > 0) { + /* attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + if (drop) { + // drop here + open_now = true; + drop = false; + drop_start = hrt_absolute_time(); + } + if (open_now && (drop_start + 2e6 > hrt_absolute_time())) { // open door - actuators.control[0] = 0.5f; - actuators.control[1] = 0.5f; - actuators.control[2] = 0.5f; - actuators.control[3] = 0.5f; + actuators.control[0] = -1.0f; + actuators.control[1] = 1.0f; + } else if (open_now) { // unlock bottle + ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_ON); + ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_RED); + actuators.control[2] = 0.5f; - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); + } else { - usleep(50); + // leave closed + // warnx( "%4.4f", att.pitch); + actuators.control[0] = 0.5f; + actuators.control[1] = -0.5f; + actuators.control[2] = -0.5f; + } + + + + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); + + i++; + } - i++; } warnx("exiting.\n"); |