Browse Source

mavlink: channel ID allocation fixed

sbg
Anton Babushkin 11 years ago
parent
commit
b3839afbbc
  1. 49
      src/modules/mavlink/mavlink_main.cpp
  2. 6
      src/modules/mavlink/mavlink_main.h
  3. 4
      src/modules/mavlink/mavlink_receiver.cpp

49
src/modules/mavlink/mavlink_main.cpp

@ -159,8 +159,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length @@ -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() : @@ -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() @@ -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() @@ -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 @@ -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,

6
src/modules/mavlink/mavlink_main.h

@ -105,7 +105,7 @@ public: @@ -105,7 +105,7 @@ public:
/**
* Constructor
*/
Mavlink();
Mavlink(int instance_id);
/**
* Destructor, also kills the mavlinks task.
@ -182,6 +182,8 @@ public: @@ -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: @@ -190,6 +192,8 @@ protected:
Mavlink *next;
private:
int _instance_id;
int _mavlink_fd;
bool _task_running;

4
src/modules/mavlink/mavlink_receiver.cpp

@ -877,8 +877,8 @@ MavlinkReceiver::receive_thread(void *arg) @@ -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));

Loading…
Cancel
Save