Mavlink: allow to stop (WIP)

6 files changed, 54 insertions, 25 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index cbbe3c31f..0d9d06190 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -250,6 +250,43 @@ Mavlink* Mavlink::get_instance(unsigned instance)
 	return inst;
 }
 
+int Mavlink::destroy_all_instances()
+{
+	/* start deleting from the end */
+	Mavlink *inst_to_del = nullptr;
+	Mavlink *next_inst = ::_head;
+
+	unsigned iterations = 0;
+
+	warnx("waiting for instances to stop");
+	while (next_inst != nullptr) {
+
+		inst_to_del = next_inst;
+		next_inst = inst_to_del->_next;
+
+		/* set flag to stop thread and wait for all threads to finish */
+		inst_to_del->_task_should_exit = true;
+		while (inst_to_del->thread_running) {
+			printf(".");
+			usleep(10000);
+			iterations++;
+
+			if (iterations > 10000) {
+				warnx("ERROR: Couldn't stop all mavlink instances.");
+				return ERROR;
+			}
+		}
+		delete inst_to_del;
+	}
+
+	/* reset head */
+	::_head = nullptr;
+
+	printf("\n");
+	warnx("all instances stopped");
+	return OK;
+}
+
 bool Mavlink::instance_exists(const char *device_name, Mavlink *self)
 {
 	Mavlink* inst = ::_head;
@@ -1495,9 +1532,9 @@ Mavlink::task_main(int argc, char *argv[])
 			device_name = optarg;
 			break;
 
-		case 'e':
-			mavlink_link_termination_allowed = true;
-			break;
+//		case 'e':
+//			mavlink_link_termination_allowed = true;
+//			break;
 
 		case 'o':
 			_mode = MODE_ONBOARD;
@@ -1749,7 +1786,7 @@ Mavlink::task_main(int argc, char *argv[])
 	tcsetattr(_uart, TCSANOW, &uart_config_original);
 
 	/* destroy log buffer */
-	//mavlink_logbuffer_destroy(&lb);
+	mavlink_logbuffer_destroy(&lb);
 
 	thread_running = false;
 
@@ -1782,7 +1819,7 @@ Mavlink::status()
 
 static void usage()
 {
-	errx(1, "usage: mavlink {start|stop|status}");
+	errx(1, "usage: mavlink {start|stop|status} [-d device] [-b baudrate] [-o]");
 }
 
 int mavlink_main(int argc, char *argv[])
@@ -1830,21 +1867,16 @@ int mavlink_main(int argc, char *argv[])
 		// }
 
 		return 0;
-	}
-
-	// if (mavlink::g_mavlink == nullptr)
-	// 	errx(1, "not running");
 
-	// if (!strcmp(argv[1], "stop")) {
-	// 	delete mavlink::g_mavlink;
-	// 	mavlink::g_mavlink = nullptr;
+	} else if (!strcmp(argv[1], "stop")) {
+		return Mavlink::destroy_all_instances();
 
 	// } else if (!strcmp(argv[1], "status")) {
 	// 	mavlink::g_mavlink->status();
 
-	// } else {
-	// 	usage();
-	// }
+	 } else {
+	 	usage();
+	 }
 
 	return 0;
 }
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index bf7675267..d5bbb746b 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -168,6 +168,8 @@ public:
 
 	static Mavlink* get_instance(unsigned instance);
 
+	static int	destroy_all_instances();
+
 	static bool	instance_exists(const char *device_name, Mavlink *self);
 
 	static int get_uart_fd(unsigned index);
@@ -263,6 +265,8 @@ public:
 	/** Position setpoint triplet */
 	struct position_setpoint_triplet_s pos_sp_triplet;
 
+	bool		_task_should_exit;		/**< if true, mavlink task should exit */
+
 protected:
 	/**
 	 * Pointer to the default cdev file operations table; useful for
@@ -273,7 +277,6 @@ protected:
 
 private:
 	int		_mavlink_fd;
-	bool		_task_should_exit;		/**< if true, mavlink task should exit */
 	bool		thread_running;
 	int		_mavlink_task;			/**< task handle for sensor task */
 
diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp
index cd9408f2f..a2b71c931 100644
--- a/src/modules/mavlink/mavlink_orb_listener.cpp
+++ b/src/modules/mavlink/mavlink_orb_listener.cpp
@@ -83,7 +83,6 @@ cm_uint16_from_m_float(float m)
 
 MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
 
-	thread_should_exit(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")),
 	_mavlink(parent),
 	_listeners(nullptr),
@@ -678,7 +677,7 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
 		/* Invoke callback to set initial state */
 		//listeners[i].callback(&listener[i]);
 
-	while (!thread_should_exit) {
+	while (!_mavlink->_task_should_exit) {
 
 		int poll_ret = poll(fds, _n_listeners, timeout);
 
diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h
index 560b47423..317a489d4 100644
--- a/src/modules/mavlink/mavlink_orb_listener.h
+++ b/src/modules/mavlink/mavlink_orb_listener.h
@@ -116,8 +116,6 @@ public:
 
 private:
 
-	bool		thread_should_exit;		/**< if true, sensor task should exit */
-
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	Mavlink*	_mavlink;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 89dcb1d7b..16a1b9aff 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -106,8 +106,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	telemetry_status_pub(-1),
 	lat0(0),
 	lon0(0),
-	alt0(0),
-	thread_should_exit(false)
+	alt0(0)
 {
 
 }
@@ -807,7 +806,7 @@ MavlinkReceiver::receive_thread(void *arg)
 
 	ssize_t nread = 0;
 
-	while (!thread_should_exit) {
+	while (!_mavlink->_task_should_exit) {
 		if (poll(fds, 1, timeout) > 0) {
 			if (nread < sizeof(buf)) {
 				/* to avoid reading very small chunks wait for data before reading */
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index d8d3b5452..6614e13f4 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -101,8 +101,6 @@ public:
 
 private:
 
-	bool		thread_should_exit;		/**< if true, sensor task should exit */
-
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	Mavlink*	_mavlink;