From 89374b2e1daab1386b75075390f74ec212731f50 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 25 Apr 2022 11:17:30 -0400
Subject: [PATCH] mavlink: delete Mavlink instance if early startup fails

---
 src/modules/mavlink/mavlink_main.cpp | 51 ++++++++++++++++------------
 src/modules/mavlink/mavlink_main.h   |  2 +-
 2 files changed, 31 insertions(+), 22 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 26b159829e..43c87f612e 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -199,50 +199,50 @@ Mavlink::mavlink_update_parameters()
 	}
 }
 
-void
-Mavlink::set_channel()
+bool Mavlink::set_channel()
 {
 	/* set channel according to instance id */
 	switch (_instance_id) {
 	case 0:
 		_channel = MAVLINK_COMM_0;
-		break;
+		return true;
 
 	case 1:
 		_channel = MAVLINK_COMM_1;
-		break;
+		return true;
 
 	case 2:
 		_channel = MAVLINK_COMM_2;
-		break;
+		return true;
 
 	case 3:
 		_channel = MAVLINK_COMM_3;
-		break;
+		return true;
 #ifdef MAVLINK_COMM_4
 
 	case 4:
 		_channel = MAVLINK_COMM_4;
-		break;
+		return true;
 #endif
 #ifdef MAVLINK_COMM_5
 
 	case 5:
 		_channel = MAVLINK_COMM_5;
-		break;
+		return true;
 #endif
 #ifdef MAVLINK_COMM_6
 
 	case 6:
 		_channel = MAVLINK_COMM_6;
-		break;
+		return true;
 #endif
 
 	default:
-		PX4_WARN("instance ID %d is out of range", _instance_id);
-		px4_task_exit(1);
+		PX4_ERR("instance ID %d is out of range", _instance_id);
 		break;
 	}
+
+	return false;
 }
 
 bool
@@ -2135,18 +2135,21 @@ Mavlink::task_main(int argc, char *argv[])
 
 #endif // MAVLINK_UDP
 
-	if (!set_instance_id()) {
-		PX4_ERR("no instances available");
-		return PX4_ERROR;
+	if (set_instance_id()) {
+		if (!set_channel()) {
+			PX4_ERR("set channel failed");
+			return PX4_ERROR;
+		}
 
-	} else {
 		// set thread name
 		char thread_name[13];
 		snprintf(thread_name, sizeof(thread_name), "mavlink_if%d", get_instance_id());
 		px4_prctl(PR_SET_NAME, thread_name, px4_getpid());
-	}
 
-	set_channel();
+	} else {
+		PX4_ERR("no instances available");
+		return PX4_ERROR;
+	}
 
 	/* initialize send mutex */
 	pthread_mutex_init(&_send_mutex, nullptr);
@@ -2160,7 +2163,7 @@ Mavlink::task_main(int argc, char *argv[])
 		 */
 		if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
 			PX4_ERR("msg buf alloc fail");
-			return 1;
+			return PX4_ERROR;
 		}
 
 		/* initialize message buffer mutex */
@@ -2238,6 +2241,8 @@ Mavlink::task_main(int argc, char *argv[])
 
 	_mavlink_start_time = hrt_absolute_time();
 
+	_task_running.store(true);
+
 	while (!should_exit()) {
 		/* main loop */
 		px4_usleep(_main_loop_delay);
@@ -2606,6 +2611,8 @@ Mavlink::task_main(int argc, char *argv[])
 
 	PX4_INFO("exiting channel %i", (int)_channel);
 
+	_task_running.store(false);
+
 	return OK;
 }
 
@@ -2765,10 +2772,12 @@ int Mavlink::start_helper(int argc, char *argv[])
 		PX4_ERR("OUT OF MEM");
 
 	} else {
-		/* this will actually only return once MAVLink exits */
-		instance->_task_running.store(true);
+		/* this will actually only return once MAVLink exits, unless there's a startup error */
 		res = instance->task_main(argc, argv);
-		instance->_task_running.store(false);
+
+		if (res != PX4_OK) {
+			delete instance;
+		}
 	}
 
 	return res;
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 536928d9f0..23c2fd5e4b 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -747,7 +747,7 @@ private:
 #endif // MAVLINK_UDP
 
 
-	void set_channel();
+	bool set_channel();
 
 	bool set_instance_id();