diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 54 |
1 files changed, 25 insertions, 29 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index fa0575866..b5bf9ece0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -144,7 +144,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length #endif } - write(uart, ch, (size_t)(sizeof(uint8_t) * length)); + size_t desired = (size_t)(sizeof(uint8_t) * length); + int ret = write(uart, ch, desired); + + if (ret != desired) + warn("write err"); } @@ -1509,11 +1513,11 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ // MavlinkReceiver rcv(this); - receive_thread = MavlinkReceiver::receive_start(this); + //receive_thread = MavlinkReceiver::receive_start(this); /* start the ORB receiver */ //MavlinkOrbListener listener(this); - uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); + //uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); /* initialize waypoint manager */ mavlink_wpm_init(wpm); @@ -1595,11 +1599,6 @@ Mavlink::task_main(int argc, char *argv[]) /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) { - continue; - } - /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); @@ -1722,26 +1721,6 @@ int Mavlink::start() { - /* start the task */ - char buf[32]; - sprintf(buf, "mavlink if%d", Mavlink::instance_count()); - - /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - NULL); - - // while (!this->is_running()) { - // usleep(200); - // } - - // if (mavlink->_mavlink_task < 0) { - // warn("task start failed"); - // return -errno; - // } - return OK; } @@ -1765,7 +1744,24 @@ int mavlink_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { // Instantiate thread - Mavlink::start(); + char buf[32]; + sprintf(buf, "mavlink if%d", Mavlink::instance_count()); + + /*mavlink->_mavlink_task = */task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); + + // while (!this->is_running()) { + // usleep(200); + // } + + // if (mavlink->_mavlink_task < 0) { + // warn("task start failed"); + // return -errno; + // } // if (mavlink::g_mavlink != nullptr) { // errx(1, "already running"); |