forked from Archive/PX4-Autopilot
Fixed mavlink start / stop to ensure process is in a sane state once NSH return
This commit is contained in:
parent
56a35cc889
commit
c3bb6960e6
|
@ -743,7 +743,7 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
|
||||
thread_running = false;
|
||||
|
||||
exit(0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void
|
||||
|
@ -767,7 +767,7 @@ int mavlink_main(int argc, char *argv[])
|
|||
|
||||
/* this is not an error */
|
||||
if (thread_running)
|
||||
errx(0, "mavlink already running\n");
|
||||
errx(0, "mavlink already running");
|
||||
|
||||
thread_should_exit = false;
|
||||
mavlink_task = task_spawn_cmd("mavlink",
|
||||
|
@ -776,15 +776,25 @@ int mavlink_main(int argc, char *argv[])
|
|||
2048,
|
||||
mavlink_thread_main,
|
||||
(const char **)argv);
|
||||
|
||||
while (!thread_running) {
|
||||
usleep(200);
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
|
||||
/* this is not an error */
|
||||
if (!thread_running)
|
||||
errx(0, "mavlink already stopped");
|
||||
|
||||
thread_should_exit = true;
|
||||
|
||||
while (thread_running) {
|
||||
usleep(200000);
|
||||
printf(".");
|
||||
warnx(".");
|
||||
}
|
||||
|
||||
warnx("terminated.");
|
||||
|
|
|
@ -755,5 +755,7 @@ receive_start(int uart)
|
|||
|
||||
pthread_t thread;
|
||||
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
|
||||
|
||||
pthread_attr_destroy(&receiveloop_attr);
|
||||
return thread;
|
||||
}
|
||||
|
|
|
@ -829,5 +829,7 @@ uorb_receive_start(void)
|
|||
|
||||
pthread_t thread;
|
||||
pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
|
||||
|
||||
pthread_attr_destroy(&uorb_attr);
|
||||
return thread;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue