mavlink: channel ID allocation fixed

3 files changed, 53 insertions, 6 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 788fe5732..4bc1055d1 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -159,8 +159,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 
 static void usage(void);
 
-Mavlink::Mavlink() :
+Mavlink::Mavlink(int instance_id) :
 	next(nullptr),
+	_instance_id(instance_id),
 	_device_name(DEFAULT_DEVICE_NAME),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
@@ -179,6 +180,40 @@ Mavlink::Mavlink() :
 {
 	_wpm = &_wpm_s;
 	fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
+
+	/* set channel according to instance id */
+	switch (_instance_id) {
+	case 0:
+		_channel = MAVLINK_COMM_0;
+		break;
+	case 1:
+		_channel = MAVLINK_COMM_1;
+		break;
+	case 2:
+		_channel = MAVLINK_COMM_2;
+		break;
+	case 3:
+		_channel = MAVLINK_COMM_3;
+		break;
+#ifdef MAVLINK_COMM_4
+	case 4:
+		_channel = MAVLINK_COMM_4;
+		break;
+#endif
+#ifdef MAVLINK_COMM_5
+		case 5:
+		_channel = MAVLINK_COMM_5;
+		break;
+#endif
+#ifdef MAVLINK_COMM_6
+	case 6:
+		_channel = MAVLINK_COMM_6;
+		break;
+#endif
+	default:
+		errx(1, "instance ID is out of range");
+		break;
+	}
 }
 
 Mavlink::~Mavlink()
@@ -226,7 +261,9 @@ Mavlink::instance_count()
 Mavlink *
 Mavlink::new_instance()
 {
-	Mavlink *inst = new Mavlink();
+	int id = Mavlink::instance_count();
+
+	Mavlink *inst = new Mavlink(id);
 	Mavlink *next = ::_mavlink_instances;
 
 	/* create the first instance at _head */
@@ -353,6 +390,12 @@ Mavlink::get_uart_fd()
 	return _uart_fd;
 }
 
+int
+Mavlink::get_instance_id()
+{
+	return _instance_id;
+}
+
 mavlink_channel_t
 Mavlink::get_channel()
 {
@@ -1881,7 +1924,7 @@ int
 Mavlink::start(int argc, char *argv[])
 {
 	// Instantiate thread
-	char buf[32];
+	char buf[24];
 	sprintf(buf, "mavlink_if%d", Mavlink::instance_count());
 
 	task_spawn_cmd(buf,
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index b52c12796..c606da504 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -105,7 +105,7 @@ public:
 	/**
 	 * Constructor
 	 */
-	Mavlink();
+	Mavlink(int instance_id);
 
 	/**
 	 * Destructor, also kills the mavlinks task.
@@ -182,6 +182,8 @@ public:
 
 	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
 
+	int get_instance_id();
+
 	mavlink_channel_t get_channel();
 
 	bool		_task_should_exit;		/**< if true, mavlink task should exit */
@@ -190,6 +192,8 @@ protected:
 	Mavlink	*next;
 
 private:
+	int _instance_id;
+
 	int		_mavlink_fd;
 	bool		_task_running;
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index ef1a747da..c9fcc4cd8 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -877,8 +877,8 @@ MavlinkReceiver::receive_thread(void *arg)
 	mavlink_message_t msg;
 
 	/* set thread name */
-	char thread_name[18];
-	sprintf(thread_name, "mavlink_uart_rcv_%d", _mavlink->get_channel());
+	char thread_name[24];
+	sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
 	prctl(PR_SET_NAME, thread_name, getpid());
 
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));