From b3839afbbc904fe0009610dae57a4626b3306fb6 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 11 Mar 2014 11:12:39 +0400
Subject: [PATCH] mavlink: channel ID allocation fixed
---
src/modules/mavlink/mavlink_main.cpp | 49 ++++++++++++++++++++++--
src/modules/mavlink/mavlink_main.h | 6 ++-
src/modules/mavlink/mavlink_receiver.cpp | 4 +-
3 files changed, 53 insertions(+), 6 deletions(-)
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 788fe5732d..4bc1055d1e 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 b52c12796d..c606da5045 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 ef1a747da0..c9fcc4cd84 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));