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 JSBSim
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_JSBSIM_ENABLED
22 :
23 : #include "SIM_JSBSim.h"
24 :
25 : #include <arpa/inet.h>
26 : #include <errno.h>
27 : #include <fcntl.h>
28 : #include <stdio.h>
29 : #include <sys/stat.h>
30 : #include <sys/types.h>
31 :
32 : #include <AP_HAL/AP_HAL.h>
33 :
34 : extern const AP_HAL::HAL& hal;
35 :
36 : namespace SITL {
37 :
38 : // the asprintf() calls are not worth checking for SITL
39 : #pragma GCC diagnostic ignored "-Wunused-result"
40 :
41 : #define DEBUG_JSBSIM 1
42 :
43 0 : JSBSim::JSBSim(const char *frame_str) :
44 : Aircraft(frame_str),
45 0 : sock_control(false),
46 0 : sock_fgfdm(true),
47 0 : initialised(false),
48 0 : jsbsim_script(nullptr),
49 0 : jsbsim_fgout(nullptr),
50 0 : created_templates(false),
51 0 : started_jsbsim(false),
52 0 : opened_control_socket(false),
53 0 : opened_fdm_socket(false),
54 0 : frame(FRAME_NORMAL)
55 : {
56 0 : if (strstr(frame_str, "elevon")) {
57 0 : frame = FRAME_ELEVON;
58 0 : } else if (strstr(frame_str, "vtail")) {
59 0 : frame = FRAME_VTAIL;
60 : } else {
61 0 : frame = FRAME_NORMAL;
62 : }
63 0 : const char *model_name = strchr(frame_str, ':');
64 0 : if (model_name != nullptr) {
65 0 : jsbsim_model = model_name + 1;
66 : }
67 0 : }
68 :
69 : /*
70 : create template files
71 : */
72 0 : bool JSBSim::create_templates(void)
73 : {
74 0 : control_port = 5505 + instance*10;
75 0 : fdm_port = 5504 + instance*10;
76 :
77 0 : printf("JSBSim backend started: instance=%u control_port=%u fdm_port=%u\n",
78 0 : instance, control_port, fdm_port);
79 :
80 0 : if (created_templates) {
81 0 : return true;
82 : }
83 :
84 0 : asprintf(&jsbsim_script, "%s/jsbsim_start_%u.xml", autotest_dir, instance);
85 0 : asprintf(&jsbsim_fgout, "%s/jsbsim_fgout_%u.xml", autotest_dir, instance);
86 :
87 0 : printf("JSBSim_script: '%s'\n", jsbsim_script);
88 0 : printf("JSBSim_fgout: '%s'\n", jsbsim_fgout);
89 :
90 0 : FILE *f = fopen(jsbsim_script, "w");
91 0 : if (f == nullptr) {
92 0 : AP_HAL::panic("Unable to create jsbsim script %s", jsbsim_script);
93 : }
94 0 : fprintf(f,
95 : "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n"
96 : "<?xml-stylesheet type=\"text/xsl\" href=\"http://jsbsim.sf.net/JSBSimScript.xsl\"?>\n"
97 : "<runscript xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\"\n"
98 : " xsi:noNamespaceSchemaLocation=\"http://jsbsim.sf.net/JSBSimScript.xsd\"\n"
99 : " name=\"Testing %s\">\n"
100 : "\n"
101 : " <description>\n"
102 : " test ArduPlane using %s and JSBSim\n"
103 : " </description>\n"
104 : "\n"
105 : " <use aircraft=\"%s\" initialize=\"reset\"/>\n"
106 : "\n"
107 : " <!-- we control the servos via the jsbsim console\n"
108 : " interface on TCP 5124 -->\n"
109 : " <input port=\"%u\"/>\n"
110 : "\n"
111 : " <run start=\"0\" end=\"10000000\" dt=\"%.6f\">\n"
112 : " <property value=\"0\"> simulation/notify-time-trigger </property>\n"
113 : "\n"
114 : " <event name=\"start engine\">\n"
115 : " <condition> simulation/sim-time-sec le 0.01 </condition>\n"
116 : " <set name=\"propulsion/engine[0]/set-running\" value=\"1\"/>\n"
117 : " <notify/>\n"
118 : " </event>\n"
119 : "\n"
120 : " <event name=\"Trim\">\n"
121 : " <condition>simulation/sim-time-sec ge 0.01</condition>\n"
122 : " <set name=\"simulation/do_simple_trim\" value=\"2\"/>\n"
123 : " <notify/>\n"
124 : " </event>\n"
125 : " </run>\n"
126 : "\n"
127 : "</runscript>\n"
128 : "",
129 : jsbsim_model,
130 : jsbsim_model,
131 : jsbsim_model,
132 0 : control_port,
133 0 : 1.0/rate_hz);
134 0 : fclose(f);
135 :
136 0 : f = fopen(jsbsim_fgout, "w");
137 0 : if (f == nullptr) {
138 0 : AP_HAL::panic("Unable to create jsbsim fgout script %s", jsbsim_fgout);
139 : }
140 0 : fprintf(f, "<?xml version=\"1.0\"?>\n"
141 : "<output name=\"127.0.0.1\" type=\"FLIGHTGEAR\" port=\"%u\" protocol=\"UDP\" rate=\"%f\">\n"
142 : " <time type=\"simulation\" resolution=\"1e-6\"/>\n"
143 : "</output>",
144 0 : fdm_port, rate_hz);
145 0 : fclose(f);
146 :
147 : char *jsbsim_reset;
148 0 : asprintf(&jsbsim_reset, "%s/aircraft/%s/reset.xml", autotest_dir, jsbsim_model);
149 :
150 0 : printf("JSBSim_reset: '%s'\n", jsbsim_reset);
151 :
152 0 : f = fopen(jsbsim_reset, "w");
153 0 : if (f == nullptr) {
154 0 : AP_HAL::panic("Unable to create jsbsim reset script %s", jsbsim_reset);
155 : }
156 : float r, p, y;
157 0 : dcm.to_euler(&r, &p, &y);
158 0 : fprintf(f,
159 : "<?xml version=\"1.0\"?>\n"
160 : "<initialize name=\"Start up location\">\n"
161 : " <latitude unit=\"DEG\" type=\"geodetic\"> %f </latitude>\n"
162 : " <longitude unit=\"DEG\"> %f </longitude>\n"
163 : " <altitude unit=\"M\"> 1.3 </altitude>\n"
164 : " <vt unit=\"FT/SEC\"> 0.0 </vt>\n"
165 : " <gamma unit=\"DEG\"> 0.0 </gamma>\n"
166 : " <phi unit=\"DEG\"> 0.0 </phi>\n"
167 : " <theta unit=\"DEG\"> 13.0 </theta>\n"
168 : " <psi unit=\"DEG\"> %f </psi>\n"
169 : "</initialize>\n",
170 0 : home.lat*1.0e-7,
171 0 : home.lng*1.0e-7,
172 0 : degrees(y));
173 0 : fclose(f);
174 :
175 0 : created_templates = true;
176 0 : return true;
177 : }
178 :
179 :
180 : /*
181 : start JSBSim child
182 : */
183 0 : bool JSBSim::start_JSBSim(void)
184 : {
185 0 : if (started_jsbsim) {
186 0 : return true;
187 : }
188 0 : if (!open_fdm_socket()) {
189 0 : return false;
190 : }
191 :
192 : int p[2];
193 0 : int devnull = open("/dev/null", O_RDWR|O_CLOEXEC);
194 0 : if (devnull == -1) {
195 0 : AP_HAL::panic("Unable to open /dev/null");
196 : }
197 0 : if (pipe(p) != 0) {
198 0 : AP_HAL::panic("Unable to create pipe");
199 : }
200 0 : pid_t child_pid = fork();
201 0 : if (child_pid == 0) {
202 : // in child
203 0 : setsid();
204 0 : dup2(devnull, 0);
205 0 : dup2(p[1], 1);
206 0 : close(p[0]);
207 0 : for (uint8_t i=3; i<100; i++) {
208 0 : close(i);
209 : }
210 : char *logdirective;
211 : char *script;
212 : char *nice;
213 : char *rate;
214 :
215 0 : asprintf(&logdirective, "--logdirectivefile=%s", jsbsim_fgout);
216 0 : asprintf(&script, "--script=%s", jsbsim_script);
217 0 : asprintf(&nice, "--nice=%.8f", 10*1e-9);
218 0 : asprintf(&rate, "--simulation-rate=%f", rate_hz);
219 :
220 0 : if (chdir(autotest_dir) != 0) {
221 0 : perror(autotest_dir);
222 0 : exit(1);
223 : }
224 :
225 0 : int ret = execlp("JSBSim",
226 : "JSBSim",
227 : "--suspend",
228 : rate,
229 : nice,
230 : logdirective,
231 : script,
232 : nullptr);
233 0 : if (ret != 0) {
234 0 : perror("JSBSim");
235 : }
236 0 : exit(1);
237 : }
238 0 : close(p[1]);
239 0 : jsbsim_stdout = p[0];
240 :
241 : // read startup to be sure it is running
242 : char c;
243 0 : if (read(jsbsim_stdout, &c, 1) != 1) {
244 0 : AP_HAL::panic("Unable to start JSBSim");
245 : }
246 :
247 0 : if (!expect("JSBSim Execution beginning")) {
248 0 : AP_HAL::panic("Failed to start JSBSim");
249 : }
250 0 : if (!open_control_socket()) {
251 0 : AP_HAL::panic("Failed to open JSBSim control socket");
252 : }
253 :
254 0 : fcntl(jsbsim_stdout, F_SETFL, fcntl(jsbsim_stdout, F_GETFL, 0) | O_NONBLOCK);
255 :
256 0 : started_jsbsim = true;
257 0 : check_stdout();
258 0 : close(devnull);
259 0 : return true;
260 : }
261 :
262 : /*
263 : check for stdout from JSBSim
264 : */
265 0 : void JSBSim::check_stdout(void) const
266 : {
267 : char line[100];
268 0 : ssize_t ret = ::read(jsbsim_stdout, line, sizeof(line));
269 0 : if (ret > 0) {
270 : #if DEBUG_JSBSIM
271 0 : write(1, line, ret);
272 : #endif
273 : }
274 0 : }
275 :
276 : /*
277 : a simple function to wait for a string on jsbsim_stdout
278 : */
279 0 : bool JSBSim::expect(const char *str) const
280 : {
281 0 : const char *basestr = str;
282 0 : while (*str) {
283 : char c;
284 0 : if (read(jsbsim_stdout, &c, 1) != 1) {
285 0 : return false;
286 : }
287 0 : if (c == *str) {
288 0 : str++;
289 : } else {
290 0 : str = basestr;
291 : }
292 : #if DEBUG_JSBSIM
293 0 : write(1, &c, 1);
294 : #endif
295 : }
296 0 : return true;
297 : }
298 :
299 : /*
300 : open control socket to JSBSim
301 : */
302 0 : bool JSBSim::open_control_socket(void)
303 : {
304 0 : if (opened_control_socket) {
305 0 : return true;
306 : }
307 0 : if (!sock_control.connect("127.0.0.1", control_port)) {
308 0 : return false;
309 : }
310 0 : printf("Opened JSBSim control socket\n");
311 0 : sock_control.set_blocking(false);
312 0 : opened_control_socket = true;
313 :
314 0 : char startup[] =
315 : "info\n"
316 : "resume\n"
317 : "iterate 1\n"
318 : "set atmosphere/turb-type 4\n";
319 0 : sock_control.send(startup, strlen(startup));
320 0 : return true;
321 : }
322 :
323 : /*
324 : open fdm socket from JSBSim
325 : */
326 0 : bool JSBSim::open_fdm_socket(void)
327 : {
328 0 : if (opened_fdm_socket) {
329 0 : return true;
330 : }
331 0 : if (!sock_fgfdm.bind("127.0.0.1", fdm_port)) {
332 0 : check_stdout();
333 0 : return false;
334 : }
335 0 : sock_fgfdm.set_blocking(false);
336 0 : sock_fgfdm.reuseaddress();
337 0 : opened_fdm_socket = true;
338 0 : return true;
339 : }
340 :
341 :
342 : /*
343 : decode and send servos
344 : */
345 0 : void JSBSim::send_servos(const struct sitl_input &input)
346 : {
347 0 : char *buf = nullptr;
348 0 : float aileron = filtered_servo_angle(input, 0);
349 0 : float elevator = filtered_servo_angle(input, 1);
350 0 : float throttle = filtered_servo_range(input, 2);
351 0 : float rudder = filtered_servo_angle(input, 3);
352 0 : if (frame == FRAME_ELEVON) {
353 : // fake an elevon plane
354 0 : float ch1 = aileron;
355 0 : float ch2 = elevator;
356 0 : aileron = (ch2-ch1)/2.0f;
357 : // the minus does away with the need for RC2_REVERSED=-1
358 0 : elevator = -(ch2+ch1)/2.0f;
359 0 : } else if (frame == FRAME_VTAIL) {
360 : // fake a vtail plane
361 0 : float ch1 = elevator;
362 0 : float ch2 = rudder;
363 : // this matches VTAIL_OUTPUT==2
364 0 : elevator = (ch2-ch1)/2.0f;
365 0 : rudder = (ch2+ch1)/2.0f;
366 : }
367 0 : float wind_speed_fps = input.wind.speed / FEET_TO_METERS;
368 0 : asprintf(&buf,
369 : "set fcs/aileron-cmd-norm %f\n"
370 : "set fcs/elevator-cmd-norm %f\n"
371 : "set fcs/rudder-cmd-norm %f\n"
372 : "set fcs/throttle-cmd-norm %f\n"
373 : "set atmosphere/psiw-rad %f\n"
374 : "set atmosphere/wind-mag-fps %f\n"
375 : "set atmosphere/turbulence/milspec/windspeed_at_20ft_AGL-fps %f\n"
376 : "set atmosphere/turbulence/milspec/severity %f\n"
377 : "iterate 1\n",
378 : aileron, elevator, rudder, throttle,
379 0 : radians(input.wind.direction),
380 : wind_speed_fps,
381 0 : wind_speed_fps/3,
382 0 : input.wind.turbulence);
383 0 : ssize_t buflen = strlen(buf);
384 0 : ssize_t sent = sock_control.send(buf, buflen);
385 0 : free(buf);
386 0 : if (sent < 0) {
387 0 : if (errno != EAGAIN) {
388 0 : fprintf(stderr, "Fatal: Failed to send on control socket: %s\n",
389 0 : strerror(errno));
390 0 : exit(1);
391 : }
392 : }
393 0 : if (sent < buflen) {
394 0 : fprintf(stderr, "Failed to send all bytes on control socket\n");
395 : }
396 0 : }
397 :
398 : /* nasty hack ....
399 : JSBSim sends in little-endian
400 : */
401 0 : void FGNetFDM::ByteSwap(void)
402 : {
403 0 : uint32_t *buf = (uint32_t *)this;
404 0 : for (uint16_t i=0; i<sizeof(*this)/4; i++) {
405 0 : buf[i] = ntohl(buf[i]);
406 : }
407 : // fixup the 3 doubles
408 0 : buf = (uint32_t *)&longitude;
409 : uint32_t tmp;
410 0 : for (uint8_t i=0; i<3; i++) {
411 0 : tmp = buf[0];
412 0 : buf[0] = buf[1];
413 0 : buf[1] = tmp;
414 0 : buf += 2;
415 : }
416 0 : }
417 :
418 : /*
419 : receive an update from the FDM
420 : This is a blocking function
421 : */
422 0 : void JSBSim::recv_fdm(const struct sitl_input &input)
423 : {
424 : FGNetFDM fdm;
425 0 : check_stdout();
426 :
427 : do {
428 0 : while (sock_fgfdm.recv(&fdm, sizeof(fdm), 100) != sizeof(fdm)) {
429 0 : send_servos(input);
430 0 : check_stdout();
431 : }
432 0 : fdm.ByteSwap();
433 0 : } while (fdm.cur_time == time_now_us);
434 :
435 0 : accel_body = Vector3f(fdm.A_X_pilot, fdm.A_Y_pilot, fdm.A_Z_pilot) * FEET_TO_METERS;
436 :
437 : double p, q, r;
438 0 : SIM::convert_body_frame(degrees(fdm.phi), degrees(fdm.theta),
439 0 : degrees(fdm.phidot), degrees(fdm.thetadot), degrees(fdm.psidot),
440 : &p, &q, &r);
441 0 : gyro = Vector3f(p, q, r);
442 :
443 0 : velocity_ef = Vector3f(fdm.v_north, fdm.v_east, fdm.v_down) * FEET_TO_METERS;
444 0 : location = {
445 0 : int32_t(RAD_TO_DEG_DOUBLE * fdm.latitude * 1.0e7),
446 0 : int32_t(RAD_TO_DEG_DOUBLE * fdm.longitude * 1.0e7),
447 0 : int32_t(fdm.agl*100 + home.alt),
448 : Location::AltFrame::ABSOLUTE
449 : };
450 0 : dcm.from_euler(fdm.phi, fdm.theta, fdm.psi);
451 0 : airspeed = fdm.vcas * KNOTS_TO_METERS_PER_SECOND;
452 0 : airspeed_pitot = airspeed;
453 :
454 : // update magnetic field
455 0 : update_mag_field_bf();
456 :
457 0 : rpm[0] = fdm.rpm[0];
458 0 : rpm[1] = fdm.rpm[1];
459 :
460 0 : time_now_us = fdm.cur_time;
461 0 : }
462 :
463 0 : void JSBSim::drain_control_socket()
464 : {
465 0 : const uint16_t buflen = 1024;
466 : char buf[buflen];
467 : ssize_t received;
468 : do {
469 0 : received = sock_control.recv(buf, buflen, 0);
470 0 : } while (received > 0);
471 0 : }
472 : /*
473 : update the JSBSim simulation by one time step
474 : */
475 0 : void JSBSim::update(const struct sitl_input &input)
476 : {
477 0 : while (!initialised) {
478 0 : if (!create_templates() ||
479 0 : !start_JSBSim() ||
480 0 : !open_control_socket() ||
481 0 : !open_fdm_socket()) {
482 0 : time_now_us = 1;
483 0 : return;
484 : }
485 0 : initialised = true;
486 : }
487 0 : send_servos(input);
488 0 : recv_fdm(input);
489 0 : update_battery();
490 0 : adjust_frame_time(rate_hz);
491 0 : sync_frame_time();
492 0 : drain_control_socket();
493 : }
494 :
495 : } // namespace SITL
496 :
497 : #endif // AP_SIM_JSBSIM_ENABLED
|
