diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 6c04d2aba0..f70c3ce759 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");
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index e50b0f0c0f..9e2475e2ba 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -143,7 +143,7 @@ public:
 	/**
 	 * Constructor
 	 */
-	Mavlink(const char *port, unsigned baud_rate);
+	Mavlink();
 
 	/**
 	 * Destructor, also kills the mavlinks task.
@@ -155,7 +155,7 @@ public:
 	 *
 	 * @return		OK on success.
 	 */
-	static int		start(Mavlink* mavlink);
+	static int		start();
 
 	/**
 	 * Display the mavlink status.
@@ -164,7 +164,7 @@ public:
 
 	static int	instance_count();
 
-	static Mavlink* new_instance(const char *port, unsigned baud_rate);
+	static Mavlink* new_instance();
 
 	static Mavlink* get_instance(unsigned instance);