/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ /* simulator connector for ardupilot version of JSBSim */ #include "SIM_JSBSim.h" #include #include #include #include #include #include #include extern const AP_HAL::HAL& hal; namespace SITL { // the asprintf() calls are not worth checking for SITL #pragma GCC diagnostic ignored "-Wunused-result" #define DEBUG_JSBSIM 1 JSBSim::JSBSim(const char *home_str, const char *frame_str) : Aircraft(home_str, frame_str), sock_control(false), sock_fgfdm(true), initialised(false), jsbsim_script(nullptr), jsbsim_fgout(nullptr), created_templates(false), started_jsbsim(false), opened_control_socket(false), opened_fdm_socket(false), frame(FRAME_NORMAL) { if (strstr(frame_str, "elevon")) { frame = FRAME_ELEVON; } else if (strstr(frame_str, "vtail")) { frame = FRAME_VTAIL; } else { frame = FRAME_NORMAL; } const char *model_name = strchr(frame_str, ':'); if (model_name != nullptr) { jsbsim_model = model_name + 1; } control_port = 5505 + instance*10; fdm_port = 5504 + instance*10; printf("JSBSim backend started: control_port=%u fdm_port=%u\n", control_port, fdm_port); } /* create template files */ bool JSBSim::create_templates(void) { if (created_templates) { return true; } asprintf(&jsbsim_script, "%s/jsbsim_start_%u.xml", autotest_dir, instance); asprintf(&jsbsim_fgout, "%s/jsbsim_fgout_%u.xml", autotest_dir, instance); printf("JSBSim_script: '%s'\n", jsbsim_script); printf("JSBSim_fgout: '%s'\n", jsbsim_fgout); FILE *f = fopen(jsbsim_script, "w"); if (f == nullptr) { AP_HAL::panic("Unable to create jsbsim script %s", jsbsim_script); } fprintf(f, "\n" "\n" "\n" "\n" " \n" " test ArduPlane using %s and JSBSim\n" " \n" "\n" " \n" "\n" " \n" " \n" "\n" " \n" " simulation/notify-time-trigger \n" "\n" " \n" " simulation/sim-time-sec le 0.01 \n" " \n" " \n" " \n" "\n" " \n" " simulation/sim-time-sec ge 0.01\n" " \n" " \n" " \n" " \n" "\n" "\n" "", jsbsim_model, jsbsim_model, jsbsim_model, control_port, 1.0/rate_hz); fclose(f); f = fopen(jsbsim_fgout, "w"); if (f == nullptr) { AP_HAL::panic("Unable to create jsbsim fgout script %s", jsbsim_fgout); } fprintf(f, "\n" "\n" " ", fdm_port, rate_hz); fclose(f); char *jsbsim_reset; asprintf(&jsbsim_reset, "%s/aircraft/%s/reset.xml", autotest_dir, jsbsim_model); printf("JSBSim_reset: '%s'\n", jsbsim_reset); f = fopen(jsbsim_reset, "w"); if (f == nullptr) { AP_HAL::panic("Unable to create jsbsim reset script %s", jsbsim_reset); } float r, p, y; dcm.to_euler(&r, &p, &y); fprintf(f, "\n" "\n" " %f \n" " %f \n" " 1.3 \n" " 0.0 \n" " 0.0 \n" " 0.0 \n" " 13.0 \n" " %f \n" "\n", home.lat*1.0e-7, home.lng*1.0e-7, degrees(y)); fclose(f); created_templates = true; return true; } /* start JSBSim child */ bool JSBSim::start_JSBSim(void) { if (started_jsbsim) { return true; } if (!open_fdm_socket()) { return false; } int p[2]; int devnull = open("/dev/null", O_RDWR|O_CLOEXEC); if (pipe(p) != 0) { AP_HAL::panic("Unable to create pipe"); } pid_t child_pid = fork(); if (child_pid == 0) { // in child setsid(); dup2(devnull, 0); dup2(p[1], 1); close(p[0]); for (uint8_t i=3; i<100; i++) { close(i); } char *logdirective; char *script; char *nice; char *rate; asprintf(&logdirective, "--logdirectivefile=%s", jsbsim_fgout); asprintf(&script, "--script=%s", jsbsim_script); asprintf(&nice, "--nice=%.8f", 10*1e-9); asprintf(&rate, "--simulation-rate=%f", rate_hz); if (chdir(autotest_dir) != 0) { perror(autotest_dir); exit(1); } int ret = execlp("JSBSim", "JSBSim", "--suspend", rate, nice, logdirective, script, nullptr); if (ret != 0) { perror("JSBSim"); } exit(1); } close(p[1]); jsbsim_stdout = p[0]; // read startup to be sure it is running char c; if (read(jsbsim_stdout, &c, 1) != 1) { AP_HAL::panic("Unable to start JSBSim"); } if (!expect("JSBSim Execution beginning")) { AP_HAL::panic("Failed to start JSBSim"); } if (!open_control_socket()) { AP_HAL::panic("Failed to open JSBSim control socket"); } fcntl(jsbsim_stdout, F_SETFL, fcntl(jsbsim_stdout, F_GETFL, 0) | O_NONBLOCK); started_jsbsim = true; check_stdout(); close(devnull); return true; } /* check for stdout from JSBSim */ void JSBSim::check_stdout(void) { char line[100]; ssize_t ret = ::read(jsbsim_stdout, line, sizeof(line)); if (ret > 0) { #if DEBUG_JSBSIM write(1, line, ret); #endif } } /* a simple function to wait for a string on jsbsim_stdout */ bool JSBSim::expect(const char *str) { const char *basestr = str; while (*str) { char c; if (read(jsbsim_stdout, &c, 1) != 1) { return false; } if (c == *str) { str++; } else { str = basestr; } #if DEBUG_JSBSIM write(1, &c, 1); #endif } return true; } /* open control socket to JSBSim */ bool JSBSim::open_control_socket(void) { if (opened_control_socket) { return true; } if (!sock_control.connect("127.0.0.1", control_port)) { return false; } printf("Opened JSBSim control socket\n"); sock_control.set_blocking(false); opened_control_socket = true; char startup[] = "info\n" "resume\n" "iterate 1\n" "set atmosphere/turb-type 4\n"; sock_control.send(startup, strlen(startup)); return true; } /* open fdm socket from JSBSim */ bool JSBSim::open_fdm_socket(void) { if (opened_fdm_socket) { return true; } if (!sock_fgfdm.bind("127.0.0.1", fdm_port)) { check_stdout(); return false; } sock_fgfdm.set_blocking(false); sock_fgfdm.reuseaddress(); opened_fdm_socket = true; return true; } /* decode and send servos */ void JSBSim::send_servos(const struct sitl_input &input) { char *buf = nullptr; float aileron = filtered_servo_angle(input, 0); float elevator = filtered_servo_angle(input, 1); float throttle = filtered_servo_range(input, 2); float rudder = filtered_servo_angle(input, 3); if (frame == FRAME_ELEVON) { // fake an elevon plane float ch1 = aileron; float ch2 = elevator; aileron = (ch2-ch1)/2.0f; // the minus does away with the need for RC2_REV=-1 elevator = -(ch2+ch1)/2.0f; } else if (frame == FRAME_VTAIL) { // fake a vtail plane float ch1 = elevator; float ch2 = rudder; // this matches VTAIL_OUTPUT==2 elevator = (ch2-ch1)/2.0f; rudder = (ch2+ch1)/2.0f; } float wind_speed_fps = input.wind.speed / FEET_TO_METERS; asprintf(&buf, "set fcs/aileron-cmd-norm %f\n" "set fcs/elevator-cmd-norm %f\n" "set fcs/rudder-cmd-norm %f\n" "set fcs/throttle-cmd-norm %f\n" "set atmosphere/psiw-rad %f\n" "set atmosphere/wind-mag-fps %f\n" "set atmosphere/turbulence/milspec/windspeed_at_20ft_AGL-fps %f\n" "set atmosphere/turbulence/milspec/severity %f\n" "iterate 1\n", aileron, elevator, rudder, throttle, radians(input.wind.direction), wind_speed_fps, wind_speed_fps/3, input.wind.turbulence); ssize_t buflen = strlen(buf); ssize_t sent = sock_control.send(buf, buflen); free(buf); if (sent < 0) { if (errno != EAGAIN) { fprintf(stderr, "Fatal: Failed to send on control socket: %s\n", strerror(errno)); exit(1); } } if (sent < buflen) { fprintf(stderr, "Failed to send all bytes on control socket\n"); } } /* nasty hack .... JSBSim sends in little-endian */ void FGNetFDM::ByteSwap(void) { uint32_t *buf = (uint32_t *)this; for (uint16_t i=0; i 0); } /* update the JSBSim simulation by one time step */ void JSBSim::update(const struct sitl_input &input) { while (!initialised) { if (!create_templates() || !start_JSBSim() || !open_control_socket() || !open_fdm_socket()) { time_now_us = 1; return; } initialised = true; } send_servos(input); recv_fdm(input); adjust_frame_time(rate_hz); sync_frame_time(); drain_control_socket(); } } // namespace SITL