Move instance to new task context

1 files changed, 15 insertions, 18 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 6c04d2aba..f70c3ce75 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -156,7 +156,7 @@ namespace mavlink
 	Mavlink	*g_mavlink;
 }
 
-Mavlink::Mavlink(const char *port, unsigned baud_rate) : 
+Mavlink::Mavlink() : 
 
 	_task_should_exit(false),
 	_mavlink_task(-1),
@@ -215,9 +215,9 @@ int Mavlink::instance_count()
 	return inst_index + 1;
 }
 
-Mavlink* Mavlink::new_instance(const char *port, unsigned baud_rate)
+Mavlink* Mavlink::new_instance()
 {
-	Mavlink* inst = new Mavlink(port, baud_rate);
+	Mavlink* inst = new Mavlink();
 	Mavlink* parent = ::_head;
 	while (parent->_next != nullptr)
 		parent = parent->_next;
@@ -1740,21 +1740,23 @@ Mavlink::task_main(int argc, char *argv[])
 
 int Mavlink::start_helper(int argc, char *argv[])
 {
-	// This is beyond evil.. and needs a lock to be safe
-	return Mavlink::get_instance(Mavlink::instance_count() - 1)->task_main(argc, argv);
+	// Create the instance in task context
+	Mavlink *instance = Mavlink::new_instance();
+	// This will actually only return once MAVLink exits
+	return instance->task_main(argc, argv);
 }
 
 int
-Mavlink::start(Mavlink* mavlink)
+Mavlink::start()
 {
 
 	/* start the task */
 	char buf[32];
 	sprintf(buf, "mavlink if%d", Mavlink::instance_count());
 
-	mavlink->_mavlink_task = task_spawn_cmd(buf,
+	/*mavlink->_mavlink_task = */task_spawn_cmd(buf,
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 5,
+					 SCHED_PRIORITY_DEFAULT,
 					 2048,
 					 (main_t)&Mavlink::start_helper,
 					 NULL);
@@ -1763,10 +1765,10 @@ Mavlink::start(Mavlink* mavlink)
 	// 		usleep(200);
 	// }
 
-	if (mavlink->_mavlink_task < 0) {
-		warn("task start failed");
-		return -errno;
-	}
+	// if (mavlink->_mavlink_task < 0) {
+	// 	warn("task start failed");
+	// 	return -errno;
+	// }
 
 	return OK;
 }
@@ -1790,13 +1792,8 @@ int mavlink_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "start")) {
 
-		Mavlink *instance = Mavlink::new_instance("/dev/ttyS0", 57600);
-
-		if (mavlink::g_mavlink == nullptr)
-			mavlink::g_mavlink = instance;
-
 		// Instantiate thread
-		Mavlink::start(instance);
+		Mavlink::start();
 
 		// if (mavlink::g_mavlink != nullptr) {
 		// 	errx(1, "already running");