diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-03-11 11:12:39 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-03-11 11:12:39 +0400 |
| commit | b3839afbbc904fe0009610dae57a4626b3306fb6 (patch) | |
| tree | 381e1377c84405c53ca8363b5991cbcf0d83769d /src/modules/mavlink/mavlink_main.cpp | |
| parent | a6d9b7e864334381a99f4438aeea1173229bc2dd (diff) | |
| download | px4-firmware-b3839afbbc904fe0009610dae57a4626b3306fb6.tar.gz px4-firmware-b3839afbbc904fe0009610dae57a4626b3306fb6.tar.bz2 px4-firmware-b3839afbbc904fe0009610dae57a4626b3306fb6.zip | |
mavlink: channel ID allocation fixed
Diffstat (limited to 'src/modules/mavlink/mavlink_main.cpp')
| -rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 49 |
1 files changed, 46 insertions, 3 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, |