diff options
author | Julian Oes <julian@oes.ch> | 2014-03-29 20:36:39 +0100 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-03-29 20:36:39 +0100 |
commit | 9da8e249fd2d1504b01571965c4dd5acdbf77ff1 (patch) | |
tree | cde1d18f4a8922cf056683390e9621fa9d0d57b4 | |
parent | d422d443eebbba24d785f96de5b74fab143e6652 (diff) | |
download | px4-firmware-9da8e249fd2d1504b01571965c4dd5acdbf77ff1.tar.gz px4-firmware-9da8e249fd2d1504b01571965c4dd5acdbf77ff1.tar.bz2 px4-firmware-9da8e249fd2d1504b01571965c4dd5acdbf77ff1.zip |
bottle_drop: added simple commands to drop bottle
-rw-r--r-- | src/modules/bottle_drop/bottle_drop.cpp | 209 |
1 files changed, 202 insertions, 7 deletions
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 7672ace9e..1dcad2144 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -55,7 +55,9 @@ #include <drivers/drv_hrt.h> #include <arch/board/board.h> #include <uORB/uORB.h> -#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/actuator_controls.h> +#include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <geo/geo.h> @@ -96,10 +98,30 @@ public: void status(); private: - bool _task_should_exit; /**< if true, sensor task should exit */ - int _main_task; /**< task handle for sensor task */ - + bool _task_should_exit; /**< if true, task should exit */ + int _main_task; /**< handle for task */ int _mavlink_fd; + + int _command_sub; + struct vehicle_command_s _command; + + orb_advert_t _actuator_pub; + struct actuator_controls_s _actuators; + + bool _open_door; + bool _drop; + hrt_abstime _doors_opened; + + void task_main(); + + void handle_command(struct vehicle_command_s *cmd); + + void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); }; namespace bottle_drop @@ -111,7 +133,14 @@ BottleDrop::BottleDrop() : _task_should_exit(false), _main_task(-1), - _mavlink_fd(-1) + _mavlink_fd(-1), + _command_sub(-1), + _command({}), + _actuator_pub(-1), + _actuators({}), + _open_door(false), + _drop(false), + _doors_opened(0) { } @@ -140,13 +169,179 @@ BottleDrop::~BottleDrop() bottle_drop::g_bottle_drop = nullptr; } -int BottleDrop::start() +int +BottleDrop::start() +{ + ASSERT(_main_task == -1); + + /* start the task */ + _main_task = task_spawn_cmd("bottle_drop", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&BottleDrop::task_main_trampoline, + nullptr); + + if (_main_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + + +void +BottleDrop::status() +{ + warnx("Doors: %s", _open_door ? "OPEN" : "CLOSED"); + warnx("Dropping: %s", _drop ? "YES" : "NO"); +} + +void +BottleDrop::task_main() +{ + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); + + _command_sub = orb_subscribe(ORB_ID(vehicle_command)); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* Setup of loop */ + fds[0].fd = _command_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + /* vehicle commands updated */ + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); + handle_command(&_command); + } + + /* update door actuators */ + if (_open_door) { + _actuators.control[0] = -1.0f; + _actuators.control[1] = 1.0f; + + if (_doors_opened == 0) { + _doors_opened = hrt_absolute_time(); + } + } else { + _actuators.control[0] = 0.5f; + _actuators.control[1] = -0.5f; + + _doors_opened = 0; + } + + /* update drop actuator, wait 0.5s until the doors are open before dropping */ + if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 500000) { + _actuators.control[2] = 0.5f; + } else { + _actuators.control[2] = -0.5f; + } + + /* 2s after drop, reset and close everything again */ + if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 2000000) { + _open_door = false; + _drop = false; + } + + /* lazily publish _actuators only once available */ + if (_actuator_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuator_pub, &_actuators); + + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators); + } + } + + warnx("exiting."); + + _main_task = -1; + _exit(0); +} + +void +BottleDrop::handle_command(struct vehicle_command_s *cmd) { + switch (cmd->command) { + case VEHICLE_CMD_CUSTOM_0: + + /* + * param1 and param2 set to 1: open and drop + * param1 set to 1: open + * else: close (and don't drop) + */ + if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { + _open_door = true; + _drop = true; + mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); + } + else if (cmd->param1 > 0.5f) { + _open_door = true; + _drop = false; + mavlink_log_info(_mavlink_fd, "#audio: open doors"); + } else { + _open_door = false; + _drop = false; + mavlink_log_info(_mavlink_fd, "#audio: close doors"); + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + default: + break; + } } +void +BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command); + break; + + default: + break; + } +} -void BottleDrop::status() +void +BottleDrop::task_main_trampoline(int argc, char *argv[]) { + bottle_drop::g_bottle_drop->task_main(); } static void usage() |