diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-24 14:58:14 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-24 14:58:14 +0200 |
commit | d12c09cc869c09de4cf670e58f900e42f7fc5f0f (patch) | |
tree | be4f2bc7c8943abd76bb19a12669d0b1c773bb84 /apps/mavlink/mavlink.c | |
parent | 45e178eaa3ba620dfc8364aa73a1deeb9b609a2b (diff) | |
download | px4-firmware-d12c09cc869c09de4cf670e58f900e42f7fc5f0f.tar.gz px4-firmware-d12c09cc869c09de4cf670e58f900e42f7fc5f0f.tar.bz2 px4-firmware-d12c09cc869c09de4cf670e58f900e42f7fc5f0f.zip |
improvements / debugging on I2C drivers
Diffstat (limited to 'apps/mavlink/mavlink.c')
-rw-r--r-- | apps/mavlink/mavlink.c | 25 |
1 files changed, 11 insertions, 14 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 4f3a8295d..aa7351153 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -94,7 +94,6 @@ static int mavlink_task; /* terminate MAVLink on user request - disabled by default */ static bool mavlink_link_termination_allowed = false; -static bool mavlink_exit_requested = false; mavlink_system_t mavlink_system = {100, 50, MAV_TYPE_FIXED_WING, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255 static uint8_t chan = MAVLINK_COMM_0; @@ -459,8 +458,6 @@ static void *receiveloop(void *arg) while (!thread_should_exit) { - if (mavlink_exit_requested) break; - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; if (poll(fds, 1, timeout) > 0) { @@ -704,7 +701,6 @@ static void *uorb_receiveloop(void *arg) const int timeout = 1000; while (!thread_should_exit) { - if (mavlink_exit_requested) break; int poll_ret = poll(fds, fdsc_count, timeout); @@ -1090,12 +1086,8 @@ void handleMessage(mavlink_message_t *msg) fflush(stdout); usleep(50000); - /* terminate other threads */ - mavlink_exit_requested = true; - pthread_cancel(receive_thread); - pthread_cancel(uorb_receive_thread); - - pthread_exit(NULL); + /* terminate other threads and this thread */ + thread_should_exit = true; } else { @@ -1425,7 +1417,7 @@ int mavlink_thread_main(int argc, char *argv[]) if (uart < 0) { printf("[mavlink] FAILED to open %s, terminating.\n", uart_name); - return ERROR; + goto exit_cleanup; } /* Flush UART */ @@ -1511,9 +1503,9 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } - while (!thread_should_exit) { + thread_running = true; - if (mavlink_exit_requested) break; + while (!thread_should_exit) { /* get local and global position */ orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); @@ -1606,9 +1598,15 @@ int mavlink_thread_main(int argc, char *argv[]) printf("[mavlink] Restored original UART config, exiting..\n"); } +exit_cleanup: + /* close uart */ close(uart); + /* close subscriptions */ + close(mavlink_subs.global_pos_sub); + close(local_pos_sub); + fflush(stdout); fflush(stderr); @@ -1641,7 +1639,6 @@ int mavlink_main(int argc, char *argv[]) thread_should_exit = false; mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4400, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; exit(0); } |