Merged upstream

4 files changed, 25 insertions, 28 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c29d9957d..fa0575866 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),
 	thread_running(false),
@@ -216,9 +216,9 @@ int Mavlink::instance_count()
 	return inst_index;
 }
 
-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* next = ::_head;
 	while (next != nullptr)
 		next = next->_next;
@@ -1712,22 +1712,24 @@ 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,
-					 4096,
+					 SCHED_PRIORITY_DEFAULT,
+					 2048,
 					 (main_t)&Mavlink::start_helper,
 					 NULL);
 
@@ -1735,10 +1737,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;
 }
@@ -1762,13 +1764,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 34e2b59ee..bf8c63d38 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);
 
diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp
index a6bcd4c0a..ffff1838c 100644
--- a/src/modules/mavlink/mavlink_orb_listener.cpp
+++ b/src/modules/mavlink/mavlink_orb_listener.cpp
@@ -726,13 +726,13 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
 
 void * MavlinkOrbListener::uorb_start_helper(void *context)
 {
-	return ((MavlinkOrbListener *)context)->uorb_receive_thread(NULL);
+	MavlinkOrbListener* urcv = new MavlinkOrbListener(((Mavlink *)context));
+	return urcv->uorb_receive_thread(NULL);
 }
 
 pthread_t
 MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink)
 {
-	MavlinkOrbListener* urcv = new MavlinkOrbListener(mavlink);
 
 	/* --- SENSORS RAW VALUE --- */
 	mavlink->get_subs()->sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -840,7 +840,7 @@ MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink)
 	pthread_attr_setstacksize(&uorb_attr, 2048);
 
 	pthread_t thread;
-	pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, urcv);
+	pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, mavlink);
 
 	pthread_attr_destroy(&uorb_attr);
 	return thread;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 0f1d5293e..3752bde10 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -843,13 +843,13 @@ void MavlinkReceiver::print_status()
 
 void * MavlinkReceiver::start_helper(void *context)
 {
-	return ((MavlinkReceiver *)context)->receive_thread(NULL);
+	MavlinkReceiver *rcv = new MavlinkReceiver(((Mavlink *)context));
+	return rcv->receive_thread(NULL);
 }
 
 pthread_t
 MavlinkReceiver::receive_start(Mavlink *mavlink)
 {
-	MavlinkReceiver *rcv = new MavlinkReceiver(mavlink);
 
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
@@ -866,7 +866,7 @@ MavlinkReceiver::receive_start(Mavlink *mavlink)
 	pthread_attr_setstacksize(&receiveloop_attr, 3000);
 
 	pthread_t thread;
-	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, rcv);
+	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, mavlink);
 
 	pthread_attr_destroy(&receiveloop_attr);
 	return thread;