Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : simulator connector for the last_letter_lib flight dynamics library
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_LAST_LETTER_ENABLED
22 :
23 : #include "SIM_last_letter.h"
24 :
25 : #include <errno.h>
26 : #include <fcntl.h>
27 : #include <signal.h>
28 : #include <stdio.h>
29 : #include <stdlib.h>
30 : #include <string.h>
31 : #include <sys/stat.h>
32 : #include <sys/types.h>
33 : #include <sys/wait.h>
34 : #include <unistd.h>
35 :
36 : #include <AP_HAL/AP_HAL.h>
37 :
38 : extern const AP_HAL::HAL& hal;
39 :
40 : namespace SITL {
41 :
42 : // The default aircraft when --model is given without a ":<aircraft>" suffix.
43 : static const char *default_model_name = "ardupilot_plane";
44 :
45 : // The simulator process, so it can be reaped when SITL exits. SITL::Aircraft has
46 : // no destructor to hook into, hence the file-static plus atexit().
47 : static pid_t last_letter_pid = -1;
48 :
49 : // A SITL reboot is an execv() of ourselves, which discards both the atexit()
50 : // handler and the pid above while leaving the child running. execv() does keep
51 : // the environment, so the pid is handed to the next incarnation through it, the
52 : // same way SITL already passes SITL_WATCHDOG_RESET across a reboot.
53 : static const char *child_pid_env = "LAST_LETTER_CHILD_PID";
54 :
55 : /*
56 : Stop a last_letter child and wait for it to go away, so that it has released
57 : the UDP port before we return. Bounded, so a wedged child cannot hold up
58 : SITL indefinitely.
59 : */
60 0 : static void stop_and_reap(pid_t pid, uint16_t timeout_ms)
61 : {
62 0 : if (pid <= 0) {
63 0 : return;
64 : }
65 0 : kill(pid, SIGTERM);
66 0 : for (uint16_t i=0; i<timeout_ms/10; i++) {
67 0 : const pid_t ret = waitpid(pid, nullptr, WNOHANG);
68 0 : if (ret == pid) {
69 0 : return;
70 : }
71 0 : if (ret < 0 && errno == ECHILD) {
72 : // not ours to reap; watch it instead
73 0 : if (kill(pid, 0) < 0) {
74 0 : return;
75 : }
76 : }
77 0 : usleep(10000);
78 : }
79 0 : printf("last_letter: child %d will not exit, killing it\n", int(pid));
80 0 : kill(pid, SIGKILL);
81 0 : waitpid(pid, nullptr, 0);
82 : }
83 :
84 0 : static void stop_last_letter(void)
85 : {
86 0 : stop_and_reap(last_letter_pid, 1000);
87 0 : last_letter_pid = -1;
88 0 : }
89 :
90 0 : last_letter::last_letter(const char *_frame_str) :
91 : // We deviate from the JSON model convention by interpreting the frame
92 : // string as "last_letter:<model_name>" instead of "last_letter:<sim_ip>".
93 : // It is not used by JSON anyways, it is an API for its subclasses.
94 : // The constraint is that last_letter will be launched on localhost.
95 : JSON("last_letter"),
96 0 : sim_port(0),
97 0 : want_log(false)
98 : {
99 0 : const char *colon = strchr(_frame_str, ':');
100 0 : const char *name = (colon != nullptr && colon[1] != '\0') ? colon+1 : default_model_name;
101 0 : strncpy(model_name, name, sizeof(model_name)-1);
102 0 : model_name[sizeof(model_name)-1] = '\0';
103 :
104 0 : printf("Starting SITL: last_letter, aircraft '%s'\n", model_name);
105 0 : }
106 :
107 : /*
108 : Create the socket and record the port the child must bind. port_out is the
109 : port ArduPilot sends servo packets to.
110 : */
111 0 : void last_letter::set_interface_ports(const char* address, const int port_in, const int port_out)
112 : {
113 0 : JSON::set_interface_ports(address, port_in, port_out);
114 0 : sim_port = (uint16_t)port_out;
115 0 : }
116 :
117 : /*
118 : Options are a comma-separated list of flags:
119 :
120 : log record an MCAP log, named after the date and time
121 :
122 : Unknown flags are reported and ignored.
123 : */
124 0 : void last_letter::set_config(const char *config)
125 : {
126 0 : Aircraft::set_config(config);
127 :
128 : // strtok_r() writes into its input, and config points into argv.
129 : char options[256];
130 0 : strncpy(options, config, sizeof(options)-1);
131 0 : options[sizeof(options)-1] = '\0';
132 :
133 0 : char *saveptr = nullptr;
134 0 : for (char *tok = strtok_r(options, ",", &saveptr);
135 0 : tok != nullptr;
136 0 : tok = strtok_r(nullptr, ",", &saveptr)) {
137 0 : if (strcmp(tok, "log") == 0) {
138 0 : want_log = true;
139 : } else {
140 0 : printf("last_letter: ignoring unknown config option '%s'\n", tok);
141 : }
142 : }
143 0 : }
144 :
145 : /*
146 : Start the last_letter child process. The base class calls this once the model
147 : is fully configured, to pass the config to the command line.
148 : */
149 0 : void last_letter::launch_external_sim(void)
150 : {
151 : char port_str[8];
152 0 : snprintf(port_str, sizeof(port_str), "%u", unsigned(sim_port));
153 :
154 0 : const char *bin = getenv("LAST_LETTER_SITL_BIN");
155 0 : if (bin == nullptr) {
156 0 : bin = "last_letter_ardupilot";
157 : }
158 :
159 : const char *args[8];
160 0 : uint8_t argc = 0;
161 0 : args[argc++] = bin;
162 0 : args[argc++] = "--model";
163 0 : args[argc++] = model_name;
164 0 : args[argc++] = "--port";
165 0 : args[argc++] = port_str;
166 0 : if (want_log) {
167 0 : args[argc++] = "--log";
168 : }
169 0 : args[argc++] = nullptr;
170 :
171 : // Stop the child from before a SITL reboot. Without this it keeps running
172 : // and holding the UDP port, the child forked below dies on bind, and the
173 : // pre-reboot simulator serves the new incarnation instead -- which works,
174 : // but leaves one simulator spanning several boots. Wait for it to be gone
175 : // before forking, or the new child races it for the port.
176 0 : const char *prev = getenv(child_pid_env);
177 0 : if (prev != nullptr) {
178 0 : stop_and_reap((pid_t)atoi(prev), 3000);
179 0 : unsetenv(child_pid_env);
180 : }
181 :
182 0 : pid_t child_pid = fork();
183 0 : if (child_pid == 0) {
184 : // in child
185 : // Close stdin and set /dev/null to sit on it.
186 0 : close(0);
187 0 : if (open("/dev/null", O_RDONLY|O_CLOEXEC) == -1) {
188 0 : perror("/dev/null");
189 : }
190 : // Close all the other descriptors from the child side, so the child
191 : // cannot keep ArduPilot's sockets or log files alive.
192 : // Closing up to 100 descriptors.
193 0 : for (uint8_t i=3; i<100; i++) {
194 0 : close(i);
195 : }
196 0 : execvp(bin, const_cast<char *const *>(args));
197 : // only reached if the exec failed
198 0 : perror(bin);
199 0 : _exit(1);
200 : }
201 :
202 0 : if (child_pid < 0) {
203 0 : perror("fork");
204 0 : return;
205 : }
206 :
207 0 : last_letter_pid = child_pid;
208 :
209 : // Hand the pid to the incarnation on the far side of the next reboot. Set
210 : // after the fork(), so the child does not inherit its own pid in its
211 : // environment.
212 : char pid_str[16];
213 0 : snprintf(pid_str, sizeof(pid_str), "%d", int(child_pid));
214 0 : setenv(child_pid_env, pid_str, 1);
215 :
216 0 : atexit(stop_last_letter);
217 : }
218 :
219 : } // namespace SITL
220 :
221 : #endif // AP_SIM_LAST_LETTER_ENABLED
|
