PX4 Firmware
PX4 Autopilot Software http://px4.io
nshterm.c
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
4  * Author: Andrew Tridgell
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions
8  * are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in
14  * the documentation and/or other materials provided with the
15  * distribution.
16  * 3. Neither the name PX4 nor the names of its contributors may be
17  * used to endorse or promote products derived from this software
18  * without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  ****************************************************************************/
34 
35 /**
36  * @file nshterm.c
37  */
38 
39 #include <px4_platform_common/px4_config.h>
40 #include <px4_platform_common/module.h>
41 #include <termios.h>
42 #include <stdbool.h>
43 #include <stdio.h>
44 #include <stdarg.h>
45 #include <unistd.h>
46 #include <stdlib.h>
47 #include <errno.h>
48 #include <nshlib/nshlib.h>
49 #include <fcntl.h>
50 #include <systemlib/err.h>
51 #include <drivers/drv_hrt.h>
52 
54 
55 __EXPORT int nshterm_main(int argc, char *argv[]);
56 
57 
58 static void print_usage(void)
59 {
60  PRINT_MODULE_DESCRIPTION("Start an NSH shell on a given port.\n"
61  "\n"
62  "This was previously used to start a shell on the USB serial port.\n"
63  "Now there runs mavlink, and it is possible to use a shell over mavlink.\n"
64  );
65 
66  PRINT_MODULE_USAGE_NAME_SIMPLE("nshterm", "command");
67  PRINT_MODULE_USAGE_ARG("<file:dev>", "Device on which to start the shell (eg. /dev/ttyACM0)", false);
68 }
69 
70 int
71 nshterm_main(int argc, char *argv[])
72 {
73  if (argc < 2) {
74  print_usage();
75  return 1;
76  }
77 
78  unsigned retries = 0;
79  int fd = -1;
80  int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
81  struct actuator_armed_s armed;
82 
83  /* back off 1800 ms to avoid running into the USB setup timing */
84  while (hrt_absolute_time() < 1800U * 1000U) {
85  usleep(50000);
86  }
87 
88  /* try to bring up the console - stop doing so if the system gets armed */
89  while (true) {
90 
91  /* abort if an arming topic is published and system is armed */
92  bool updated = false;
93  orb_check(armed_fd, &updated);
94 
95  if (updated) {
96  /* the system is now providing arming status feedback.
97  * instead of timing out, we resort to abort bringing
98  * up the terminal.
99  */
100  orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);
101 
102  if (armed.armed) {
103  /* this is not an error, but we are done */
104  return 0;
105  }
106  }
107 
108  /* the retries are to cope with the behaviour of /dev/ttyACM0 */
109  /* which may not be ready immediately. */
110  fd = open(argv[1], O_RDWR);
111 
112  if (fd != -1) {
113  close(armed_fd);
114  break;
115  }
116 
117  usleep(100000);
118  retries++;
119  }
120 
121  if (fd == -1) {
122  perror(argv[1]);
123  return 1;
124  }
125 
126  /* set up the serial port with output processing */
127 
128  /* Try to set baud rate */
129  struct termios uart_config;
130  int termios_state;
131 
132  /* Back up the original uart configuration to restore it after exit */
133  if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
134  warnx("ERR get config %s: %d\n", argv[1], termios_state);
135  close(fd);
136  return -1;
137  }
138 
139  /* Set ONLCR flag (which appends a CR for every LF) */
140  uart_config.c_oflag |= (ONLCR | OPOST);
141 
142  if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
143  warnx("ERR set config %s\n", argv[1]);
144  close(fd);
145  return -1;
146  }
147 
148  /* setup standard file descriptors */
149  close(0);
150  close(1);
151  close(2);
152  dup2(fd, 0);
153  dup2(fd, 1);
154  dup2(fd, 2);
155 
156  nsh_consolemain(0, NULL);
157 
158  close(fd);
159 
160  return 0;
161 }
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
Definition: I2C.hpp:51
High-resolution timer with callouts and timekeeping.
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
#define warnx(...)
Definition: err.h:95
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
static void print_usage(void)
Definition: nshterm.c:58
__EXPORT int nshterm_main(int argc, char *argv[])
Definition: nshterm.c:58
int fd
Definition: dataman.cpp:146
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).