diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 784c64bf1f..56d4f9f3af 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -59,87 +59,92 @@ __EXPORT int nshterm_main(int argc, char *argv[]); int nshterm_main(int argc, char *argv[]) { - if (argc < 2) { - printf("Usage: nshterm \n"); - exit(1); - } - unsigned retries = 0; - int fd = -1; - int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); - struct actuator_armed_s armed; + if (argc < 2) { + printf("Usage: nshterm \n"); + exit(1); + } - /* back off 1800 ms to avoid running into the USB setup timing */ - while (hrt_absolute_time() < 1800U * 1000U) { - usleep(50000); - } + unsigned retries = 0; + int fd = -1; + int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); + struct actuator_armed_s armed; - /* try to bring up the console - stop doing so if the system gets armed */ - while (true) { + /* back off 1800 ms to avoid running into the USB setup timing */ + while (hrt_absolute_time() < 1800U * 1000U) { + usleep(50000); + } - /* abort if an arming topic is published and system is armed */ - bool updated = false; - orb_check(armed_fd, &updated); - if (updated) { - /* the system is now providing arming status feedback. - * instead of timing out, we resort to abort bringing - * up the terminal. - */ - orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); + /* try to bring up the console - stop doing so if the system gets armed */ + while (true) { - if (armed.armed) { - /* this is not an error, but we are done */ - exit(0); - } - } + /* abort if an arming topic is published and system is armed */ + bool updated = false; + orb_check(armed_fd, &updated); - /* the retries are to cope with the behaviour of /dev/ttyACM0 */ - /* which may not be ready immediately. */ - fd = open(argv[1], O_RDWR); - if (fd != -1) { - close(armed_fd); - break; - } - usleep(100000); - retries++; - } - if (fd == -1) { - perror(argv[1]); - exit(1); - } + if (updated) { + /* the system is now providing arming status feedback. + * instead of timing out, we resort to abort bringing + * up the terminal. + */ + orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); - /* set up the serial port with output processing */ - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; + if (armed.armed) { + /* this is not an error, but we are done */ + exit(0); + } + } - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { - warnx("ERR get config %s: %d\n", argv[1], termios_state); - close(fd); - return -1; - } + /* the retries are to cope with the behaviour of /dev/ttyACM0 */ + /* which may not be ready immediately. */ + fd = open(argv[1], O_RDWR); - /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST); + if (fd != -1) { + close(armed_fd); + break; + } - if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERR set config %s\n", argv[1]); - close(fd); - return -1; - } + usleep(100000); + retries++; + } - /* setup standard file descriptors */ - close(0); - close(1); - close(2); - dup2(fd, 0); - dup2(fd, 1); - dup2(fd, 2); + if (fd == -1) { + perror(argv[1]); + exit(1); + } - nsh_consolemain(0, NULL); + /* set up the serial port with output processing */ - close(fd); + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; - return OK; + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { + warnx("ERR get config %s: %d\n", argv[1], termios_state); + close(fd); + return -1; + } + + /* Set ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag |= (ONLCR | OPOST); + + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR set config %s\n", argv[1]); + close(fd); + return -1; + } + + /* setup standard file descriptors */ + close(0); + close(1); + close(2); + dup2(fd, 0); + dup2(fd, 1); + dup2(fd, 2); + + nsh_consolemain(0, NULL); + + close(fd); + + return OK; }