bottle_drop: added simple commands to drop bottle

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()